工业机器人运动学-逆运动学

由于机械手各关节变量的相互耦合，后面计算的关节变量与前 面的关节变量有关，因此当前面关节变量的计算结果发生变化 时，后面关节变量计算的结果也会发生变化，所以逆运动方程 的解不是唯一的，我们应该根据机械手的组合形态和各关节的 运动范围，经过多次反覆计算，从中选择一组合理解。由此可 见，求解机械手的逆运动方程是一个十分复杂的过程。
d3 S2 CLeabharlann px S1 py C2 pz（3.24） （3.25）
4
tan 1
C2
S1ax C1ay C1ax S1ay S2az
（3.26）
5
tan 1
C4
C2
C1ax
S1ay S2az S2 C1ax S1ay
S4 S1ax C2az
C1ay
1T6 =
C2( C4C5C6 - S4S6 ) - S2S5C6 S2( C4C5C6 - S4S6 ) + C2S5C6
S4C5C6 + C4C6
0
-C2( C4C5S6 + S4C6 )+ S2S5S6 -S2( C4C5 S6+ S4C6 )- C2S5S6
-S4C5S6 + C4C6
0
C2C4S5 + S2C5 S2C4S5 - C2C5
7
这里 其中
f11 = C1 x＋S1 y
f12 = - z f13 = - S1 x＋C1 y
(3.10) (3.11) (3.12)
x =[ nx ox ax px ]T， y =[ ny oy ay py ]T， z =[ nz oz az pz ]T 由前节得到的斯坦福机械手运动学方程式（2.48）为
3
3.1 引言 (Introduction)
所谓逆运动学方程的解，就是已知机械手直角坐标空间的位姿
（pose）T6，求出各节变量θn or dn 。
T6 = A1 A2 A3 A4 A5 A6
（3.1）
逆运动学方程解的步骤如下：
（1）根据机械手关节坐标设置确定An
An为关节坐标的齐次坐标变换，由关节变量和参数确定。关节变量 和参数有：
an－连杆长度;
αn－连杆扭转角;
dn－相邻两连杆的距离;
θn－相邻两连杆的夹角。
对于旋转关节θn为关节变量，而对于滑动关节dn为关节变量。其余为 连杆参数，由机械手的几何尺寸和组合形态决定。
4
（2） 根据任务确定机械手的位姿T6
T6为机械手末端在直角坐标系（参考坐标或基坐标）中的位姿，由 任务确定，即式（ 2.37 ）给出的表达式T6 = Z-1 X E-1确定。它是由三个 平移分量构成的平移矢量P（确定空间位置）和三个旋转矢量n，o，a （确定姿态）组成的齐次变换矩阵描述。
第三章 工业机器人的运动学-3
1
主要内容
数学基础——齐次坐标变换 机器人运动学方程的建立（正运动学） 机器人逆运动学分析（逆运动学）
2
三、逆运动学方程
（ Inverse Kinematic Equations ）
3.1 引言 3.2 逆运动学方程的解 3.3 斯坦福机械手的逆运动学解 3.4 欧拉变换的逆运动学解 3.5 RPY变换的逆运动学解 3.6 球坐标变换的逆运动学解 3.7 本章小结
S4S5
0
S2d3 -C2d3
d2
1
（3.13）
8
比较式（3.9）和式（3.13）矩阵中的第三行第四列元素相等得到
f13（p）= d2
或
- S1 px＋C1 py = d2
令 其中
px = r cosΦ py = r sinΦ
r px2 py2
tan 1
py px
将式（3.16）和式（3.17）代入式（3.15）有
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。
5
3.2 逆运动学方程的解（Solving inverse kinematic equations）
根据式（3.1）
T6 = A1 A2 A3 A4 A5 A6
（3.20）
（3.21） （3.22）
9
由式（3.21）、（3.22）和（3.18）可得到第一个关节变量θ1的值
1
tan 1
py px
tan 1
d2 r2 d22
（3.23）
根据同样的方法，利用式（3.9）和式（3.13）矩阵元素相等建立的相关的方程
组，可得到其它各关节变量如下：
2
tan 1
C1 px S1 py pz
（3.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
（3.28）
10
注意：
在求解关节变量过程中如出现反正切函数的分子和分母太小， 则计算结果误差会很大，此时应重新选择矩阵元素建立新的方 程组再进行计算，直到获得满意的结果为止。同样，如果计算 结果超出了机械手关节的运动范围，也要重新计算，直到符合 机械手关节的运动范围。
sinΦconθ1－conΦsinθ1 ＝ d2/r 由式（3.20）可得
（ 0< d2/r ≤1 ）
sin(Φ－θ1)＝ d2/r
（0< Φ－θ1 < ）
con(Φ－θ1)＝
1
d2
r
2
这里±号表示机械手是右肩结构（＋）还是左肩结构（－）。
（3.14） （3.15） （3.16） （3.17） （3.18） （3.19）
（3.2） （3.3） （3.4） （3.5） （3.6）
根据上述五个矩阵方程对应元素相等，可得到若干个可解的代数方程，便可
求出关节变量θn或 dn。
6
3.3 斯坦福机械手的逆运动学解 （ Inverse solution of Stanford manipulator）
在第三章我们推导出 Stanford Manipulator 的运动方程和各关节齐次变换式。 下面应用式（3.2）～（3.6）进行求解：
分别用An（n＝1，2，…，5）的逆左乘式（3.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 )