《机器人原理及控制技术》第04章 逆运动学方程

所谓逆运动学方程的解，就是已知机械手直角坐标空间的位姿 （pose）T6，求出各节变量θn or dn 。
T6 = A1 A2 A3 A4 A5 A6
（4.1）
逆运动学方程解的步骤如下：
（1）根据机械手关节坐标设置确定An
An为关节坐标的齐次坐标变换，由关节变量和参数确定。关节变量 和参数有：
an－连百度文库长度;
tan
1
py px
将式（4.16）和式（4.17）代入式（4.15）有
sinΦconθ1－conΦsinθ1 ＝ d2/r 由式（4.20）可得
（ 0< d2/r ≤1 ）
sin(Φ－θ1)＝ d2/r
（0< Φ－θ1 < ）
con(Φ－θ1)＝ 1 d 2 r 2
这里±号表示机械手是右肩结构（＋）还是左肩结构（－）。
第四章 逆运动学方程
Chapter Ⅳ Inverse Kinematic Equations
4.1 引言 4.2 逆运动学方程的解 4.3 斯坦福机械手的逆运动学解 4.4 欧拉变换的逆运动学解 4.5 RPY变换的逆运动学解 4.6 球坐标变换的逆运动学解 4.7 本章小结
4.1 引言 (Introduction)
（4.2） （4.3） （4.4） （4.5） （4.6）
根据上述五个矩阵方程对应元素相等，可得到若干个可解的代数方程，便可 求出关节变量θn或 dn。
4.3 斯坦福机械手的逆运动学解 （ Inverse solution of Stanford manipulator）
在第三章我们推导出 Stanford Manipulator 的运动方程和各关节齐次变换式。 下面应用式（4.2）～（4.6）进行求解：
l 在求解关节变量过程中如出现反正切函数的分子和分母太小， 则计算结果误差会很大，此时应重新选择矩阵元素建立新的方 程组再进行计算，直到获得满意的结果为止。同样，如果计算 结果超出了机械手关节的运动范围，也要重新计算，直到符合 机械手关节的运动范围。
l 由于机械手各关节变量的相互耦合，后面计算的关节变量与前 面的关节变量有关，因此当前面关节变量的计算结果发生变化 时，后面关节变量计算的结果也会发生变化，所以逆运动方程 的解不是唯一的，我们应该根据机械手的组合形态和各关节的 运动范围，经过多次反覆计算，从中选择一组合理解。由此可 见，求解机械手的逆运动方程是一个十分复杂的过程。
组，可得到其它各关节变量如下：
2
tan 1
C1 px S1 py pz
d3 S2 C1 px S1 py C2 pz
4
tan 1
C2
S1ax C1ay C1ax S1ay S2az
（4.24） （4.25） （4.26）
5
tan 1
C4
C2
C1ax
S1ay S2az S2 C1ax S1ay
C2C4S5 + S2C5 S2C4S5 - C2C5
S4S5 0
S2d3 -C2d3
d2 1
（4.13）
比较式（4.9）和式（4.13）矩阵中的第三行第四列元素相等得到
f13（p）= d2
或
- S1 px＋C1 py = d2
令
px = r cosΦ
其中
py = r sinΦ r px2 py2
（4.14） （4.15） （4.16） （4.17） （4.18） （4.19）
（4.20）
（4.21） （4.22）
由式（4.21）、（4.22）和（4.18）可得到第一个关节变量θ1的值
1
tan 1
py px
tan 1
d2 r2 d22
（4.23）
根据同样的方法，利用式（4.9）和式（4.13）矩阵元素相等建立的相关的方程
αn－连杆扭转角;
dn－相邻两连杆的距离;
θn－相邻两连杆的夹角。
对于旋转关节θn为关节变量，而对于滑动关节dn为关节变量。其余为 连杆参数，由机械手的几何尺寸和组合形态决定。
（2） 根据任务确定机械手的位姿T6
T6为机械手末端在直角坐标系（参考坐标或基坐标）中的位姿，由 任务确定，即式（ 3.37 ）给出的表达式T6 = Z-1 X E-1确定。它是由三个 平移分量构成的平移矢量P（确定空间位置）和三个旋转矢量n，o，a （确定姿态）组成的齐次变换矩阵描述。
1T6 =
C2( C4C5C6 - S4S6 ) - S2S5C6 S2( C4C5C6 - S4S6 ) + C2S5C6
S4C5C6 + C4C6 0
-C2( C4C5S6 + S4C6 )+ S2S5S6 -S2( C4C5 S6+ S4C6 )- C2S5S6
-S4C5S6 + C4C6 0
分别用An（n＝1，2，…，5）的逆左乘式（4.1）有
A1-1 T6 = 1T6 A2-1 A1-1 T6 = 2T6 A3-1A2-1 A1-1 T6 = 3T6 A4-1 A3-1A2-1 A1-1 T6 = 4T6 A5-1 A4-1 A3-1A2-1 A1-1 T6 = 5T6
( 1T6 = A2 A3 A4 A5 A6 ) ( 2T6 = A3 A4 A5 A6 ) ( 3T6 = A4 A5 A6 ) ( 4T6 = A5 A6 ) ( 5T6 = A6 )
S4 S1ax C2az
C1ay
（4.27）
6
tan 1
C5
C4
C2
C1ox S1oy S4 C2
S2oz C1ox
S4 S1oy
S1ox C1oy S5 S2oz C4 S1ox
S2 C1ox C1oy
S1oy
C2oz
（4.28）
注意：
这里 其中
f11 = C1 x＋S1 y f12 = - z f13 = - S1 x＋C1 y
(4.10) (4.11) (4.12)
x =[ nx ox ax px ]T， y =[ ny oy ay py ]T， z =[ nz oz az pz ]T 由第三章得到的斯坦福机械手运动学方程式（3.48）为
nx ox ax px
T6
ny
nz 0
oy oz 0
ay az 0
p
y
pz 1
（3）由T6和An（n＝1，2，…，6）和式（4.1）求出相应的关节变量θn 或 dn。
4.2 逆运动学方程的解（Solving inverse kinematic equations）
根据式（4.1）
T6 = A1 A2 A3 A4 A5 A6