操作手的逆运动学问题

---

这里以Stanford机器人为例，说明逆运动学问题的解法。

Stanford机器人的D-H变换矩阵为(参见图5．13-33)：

求逆运动学问题时臂矩阵T的各元素是已知的。由的第四列得W点在定系中的坐标为

再从(5．13－23)式右边合成矩阵和的第四列得W点在第一杆系中的坐标为

由第三式得 。

设，于是，

解得t，并求出

由第一、二式得／ (5．13－28)

由第一、二式的平方和得

θ₁有双解，θ₂因θ₁有双解而有双解，d₃总大于零，因此负号无意义。下一步求θ₄、θ₅和θ₅。设：

由于θ₁、θ₂和θ₃已知，由此计算出T₁，再由(5．136)式计算。T是给定的，故由(5．13－30)式可计算出T₂的诸元素的数值。由于r₂=³A₄⁴A5⁵A₆，左乘：，得 (5．13－31)

从(3，3)元素相等，得

由(1，3)和(2，3)元素相等，得

由(3，1)和(3，2)元素相等，得

如果θ₄、θ₅和θ₆都能旋转360°，则θ₄、θ₅和θ₆有双解，其间的关系为