第三章 工业机器人的运动学-3
主要内容
数学基础——齐次坐标变换
机器人运动学方程的建立（正运动学）
机器人逆运动学分析（逆运动学）
三、逆运动学方程
（ Inverse Kinematic Equations ）
3.1 引言 3.2 逆运动学方程的解 3.3 斯坦福机械手的逆运动学解 3.4 欧拉变换的逆运动学解
根据上述五个矩阵方程对应元素相等，可得到若干个可解的代数方程，便可
求出关节变量θn或 dn。
3.3 斯坦福机械手的逆运动学解
（ Inverse solution of Stanford manipulator）
在第三章我们推导出 Stanford Manipulator 的运动方程和各关节齐次变换式。 下面应用式（3.2）～（3.6）进行求解：
x y x y x y
－ ＋
y
x y
＋ ＋
θ x
－ －
＋ －
就不难确定欧拉角所在的象限。
为 此 ， 我 们 采 用 前 节的 方 法 ， 用 Rot (z, ø － 1 左 乘 式 ) （3.31）有 Rot－1(z,ø T ＝ Rot (y, θ) Rot (z, ψ) ) （3.46）
图3.1 正切函数所在象限
nx n T6 y nz 0
ox oy oz 0
ax ay az 0
px py pz 1
（3）由T6 和An（n＝1，2，…，6）和式（4.1）求出相应的关节变量θn 或 dn。
3.2 逆运动学方程的解（Solving inverse kinematic equations）
(3.10) (3.11) (3.12)
1T
6
=
C2C4S5 + S2C5 S2C4S5 - C2C5 S4S5 0
S2d3 -C2d3 d2 1
（3.13）
比较式（3.9）和式（3.13）矩阵中的第三行第四列元素相等得到 f13（p）= d2 或 令 其中 （3.14） （3.15） （3.16） （3.17）
比较式（3.32）和式（3.33）有
n x cos cos cos sin sin
n y sin cos cos cos sin
（3.34）
（3.35）
（3.36） （3.37） （3.38） （3.39） （3.40） （3.41）
nz sin cos
由式（3.20）可得
1
＝ d2/r
（ 0< d2/r ≤1 ） < ）
（3.20）
sin(Φ －θ 1)＝ d2/r con(Φ －θ 1)＝
（0< Φ －θ
2
1
（3.21） （3.22）
d 1 2 r
这里±号表示机械手是右肩结构（＋）还是左肩结构（－）。
由式（3.21）、（3.22）和（3.18）可得到第一个关节变量θ1的值
2
- S1 px＋C1 py = d2 px = r cosΦ py = r sinΦ
r px p y
2
1
（3.18） （3.19）
py tan p x 将式（3.16）和式（3.17）代入式（3.15）有
sinΦ conθ 1－conΦ sinθ
这里
f11 = C1 x＋S1 y f12 = - z f13 = - S1 x＋C1 y
其中 x =[ nx ox ax px ]T， y =[ ny oy ay py ]T， z =[ nz oz az pz ]T 由前节得到的斯坦福机械手运动学方程式（2.48）为 C2( C4C5C6 - S4S6 ) - S2S5C6 -C2( C4C5S6 + S4C6 )+ S2S5S6 S2( C4C5C6 - S4S6 ) + C2S5C6 -S2( C4C5 S6+ S4C6 )- C2S5S6 S4C5C6 + C4C6 -S4C5S6 + C4C6 0 0
将上式写成如下形式
f11 (n) f ( n) 12 f13 (n) 0 f11 (o) f12 (o) f13 (o) 0 f11 (a) f12 (a) f13 (a) 0 f11 ( p) cos cos f12 ( p) sin f13 ( p) sin cos 1 0 cos sin cos sin sin 0 sin 0 cos 0 0 0 0 1
py d2 1 tan tan 1 p 2 x r 2 d2
1
（3.23）
根据同样的方法，利用式（3.9）和式（3.13）矩阵元素相等建立的相关的方程 组，可得到其它各关节变量如下：
2 tan
1
C1 p x S1 p y pz
（3.24）

在求解关节变量过程中如出现反正切函数的分子和分母太小， 则计算结果误差会很大，此时应重新选择矩阵元素建立新的方 程组再进行计算，直到获得满意的结果为止。同样，如果计算 结果超出了机械手关节的运动范围，也要重新计算，直到符合 机械手关节的运动范围。

由于机械手各关节变量的相互耦合，后面计算的关节变量与前 面的关节变量有关，因此当前面关节变量的计算结果发生变化 时，后面关节变量计算的结果也会发生变化，所以逆运动方程 的解不是唯一的，我们应该根据机械手的组合形态和各关节的 运动范围，经过多次反覆计算，从中选择一组合理解。由此可 见，求解机械手的逆运动方程是一个十分复杂的过程。
d 3 S 2 C1 px S1 p y C2 pz
（3.25）
（3.26）
4 tan 1
1
C2 C1a x S1a y S 2 a z S1a Fra bibliotek C1a y
6 tan 1
tan （3.27） S C a S a C a C C C C o S o S o S S o C o S S C o S o C o S C C o S o S o C S o C o
C4 C2 C1a x S1a y S 2 a z S 4 S1a x C1a y
2 1 x 1 y 2 z 5
5 4 2 1 x 1 y 2 z 4 1 x 1 y 4 5 2 1 x 1 y 2 z 4 2 1 x 1 y 2 z 1 x 1 y

（3.28）
注意：
cos cos cos sin sin sin cos cos cos sin sin cos 0
cos cos sin sin cos sin cos sin cos cos sin sin 0
即
cos sin 0 0 sin cos 0 0 0 0 n x 0 0 n y 1 0 n z 0 1 0 ox oy oz 0 ax ay az 0 p x cos cos p y sin p z sin cos 1 0 cos sin cos sin sin 0 sin 0 cos 0 0 0 （3.47） 0 1
3.5 RPY变换的逆运动学解
3.6 球坐标变换的逆运动学解 3.7 本章小结
3.1
引言 (Introduction)
所谓逆运动学方程的解，就是已知机械手直角坐标空间的位姿 （pose）T6，求出各节变量θn or dn 。 T6 = A1 A2 A3 A4 A5 A6 逆运动学方程解的步骤如下： （1）根据机械手关节坐标设置确定An （3.1）
An为关节坐标的齐次坐标变换，由关节变量和参数确定。关节变量 和参数有：
an－连杆长度; αn－连杆扭转角;
dn－相邻两连杆的距离;
θn－相邻两连杆的夹角。
对于旋转关节θn为关节变量，而对于滑动关节dn为关节变量。其余为 连杆参数，由机械手的几何尺寸和组合形态决定。
（2） 根据任务确定机械手的位姿T6 T6为机械手末端在直角坐标系（参考坐标或基坐标）中的位姿，由 任务确定，即式（ 2.37 ）给出的表达式T6 = Z-1 X E-1确定。它是由三个 平移分量构成的平移矢量P（确定空间位置）和三个旋转矢量n，o，a （确定姿态）组成的齐次变换矩阵描述。
o x cos cos sin sin cos
o y sin cos sin cos cos
oz sin sin
a x cos sin
a y sin sin
a z cos
（3.42）
由式（3.42）可解出θ角
根据式（3.1） T6 = A1 A2 A3 A4 A5 A6
分别用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 ) （3.2） （3.3） （3.4） （3.5） （3.6）
3.4 欧拉变换的逆运动学解 （Inverse solution of Euler Angles ）
由前节知欧拉变换为 Euler (ø θ,ψ) ＝ Rot (z, ø Rot (y, θ) Rot (z,ψ) , ) 我们用T来表示欧拉变换的结果，即 T ＝ Euler (ø θ,ψ) , 或 T ＝ Rot (z, ø Rot (y, θ) Rot (z,ψ) ) 其中 （3.31） （3.30） （3.29）