第四章 机器人学逆运动学方程

第四章 逆运动学方程
Chapter Ⅳ Inverse Kinematic Equations
4.1 引言 4.2 逆运动学方程的解 4.3 斯坦福机械手的逆运动学解 4.4 欧拉变换的逆运动学解
4.5 RPY变换的逆运动学解
4.6 球坐标变换的逆运动学解 4.7 本章小结
4.1
引言 (Introduction)
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

（4.28）
注意：
根据式（4.1） T6 = A1 A2 A3 A4 A5 A6
分别用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 ) （4.2） （4.3） （4.4） （4.5） （4.6）
x y x y x y
－ ＋
y
x y
＋ ＋
θ x
－ －
＋ －
就不难确定欧拉角所在的象限。
为此，我们采用本章第二节的方法，用Rot (z, ø )－1左乘 式（4.31）有 Rot－1(z,ø ) T ＝ Rot (y, θ) Rot (z, ψ) （4.46）
图4.1 正切函数所在象限
即
cos sin 0 0 sin cos 0 0 0 0 1 0 0 n x n 0 y 0 n z 1 0 ox oy oz 0 ax ay az 0 p x cos cos sin py p z sin cos 1 0 cos sin cos sin sin 0 sin 0 cos 0 0 0 （4.47） 0 1
4.4 欧拉变换的逆运动学解 （Inverse solution of Euler Angles ）
由第三章知欧拉变换为 Euler (ø , θ,ψ) ＝ Rot (z, ø ) Rot (y, θ) Rot (z,ψ) 我们用T来表示欧拉变换的结果，即 T ＝ Euler (ø , θ , ψ) 或 T ＝ Rot (z, ø ) Rot (y, θ) Rot (z,ψ) 其中 （4.31） （4.30） （4.29）
d3 S 2 C1 px S1 p y C2 pz
（4.25）
（4.26）
4 tan 1
1
C2 C1a x S1a y S 2 a z
S1a x C1a y
6 tan 1
tan （4.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
Hale Waihona Puke py d2 1 1 tan tan p 2 x r 2 d2
1
（4.23）
根据同样的方法，利用式（4.9）和式（4.13）矩阵元素相等建立的相关的方程 组，可得到其它各关节变量如下：
2 tan
1
C1 p x S1 p y pz
（4.24）
根据上述五个矩阵方程对应元素相等，可得到若干个可解的代数方程，便可
求出关节变量θn或 dn。
4.3 斯坦福机械手的逆运动学解
（ Inverse solution of Stanford manipulator）
在第三章我们推导出 Stanford Manipulator 的运动方程和各关节齐次变换式。 下面应用式（4.2）～（4.6）进行求解：
所谓逆运动学方程的解，就是已知机械手直角坐标空间的位姿 （pose）T6，求出各节变量θn or dn 。 T6 = A 1 A 2 A 3 A 4 A 5 A 6 逆运动学方程解的步骤如下： （1）根据机械手关节坐标设置确定An （4.1）
An为关节坐标的齐次坐标变换，由关节变量和参数确定。关节变量 和参数有：
由式（4.20）可得
1
＝ d2/r
（ 0< d2/r ≤1 ） < ）
（4.20）
sin(Φ －θ 1)＝ d2/r con(Φ －θ 1)＝
（0< Φ －θ
2
1
（4.21） （4.22）
d 1 2r
这里±号表示机械手是右肩结构（＋）还是左肩结构（－）。
由式（4.21）、（4.22）和（4.18）可得到第一个关节变量θ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。
4.2 逆运动学方程的解（Solving inverse kinematic equations）
2
- S1 px＋C1 py = d2 px = r cosΦ py = r sinΦ
r px p y
1
2
（4.18） （4.19）
py tan p x 将式（4.16）和式（4.17）代入式（4.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 由第三章得到的斯坦福机械手运动学方程式（3.48）为 C2( C4C5C6 - S4S6 ) - S2S5C6 -C2( C4C5S6 + S4C6 )+ S2S5S6 S2( C4C5C6 - S4S6 ) + C2S5C6 -S2( C4C5 S6+ S4C6 )- C2S5S6 S4C5C6 + C4C6 -S4C5S6 + C4C6 0 0
cos 1 (a z )
由式（4.40）和式（4.43）可解出φ角
（4.43）
ax cos 1 sin
由式（4.36）和式（4.43）可解出Ψ角
（4.44）
nz cos 1 sin
（4.45）
这里需要指出的是，在我们采用式（ 4.43 ） ~ 式（ 4.45 ） 来计算 θ 、 φ 、 Ψ 时都是采用反余弦函数，而且式（ 4.43 ）和 式（4.45）的分母为sinθ，这会带来如下问题： 1 ）由于绝对值相同的正负角度的余弦相等，如 cosθ ＝ cos（-θ），因此不能确定反余弦的结果是在那个象限； 2）当sinθ接近于0时，由式（4.43）和式（4.45）所求出 的角度φ和Ψ是不精确的； 3 ）当 θ ＝ 0 或±180º 时，式（ 4.43 ）和式（ 4.45 ）无数值 解。 为此，我们必须寻求更为合理的求解方法。 由三角函数的知识我们知道，反正切函数θ＝tan－1（x / y ）所在的象限空间可由自变量的分子和分母的符号确定 （如图4.1所示），因此如果我们得到欧拉角的正切表达式，
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
比较式（4.32）和式（4.33）有
n x cos cos cos sin sin
n y sin cos cos cos sin
（4.34）
（4.35）
（4.36） （4.37） （4.38） （4.39） （4.40） （4.41）
nz sin cos
将上式写成如下形式
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
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
（4.42）
由式（4.42）可解出θ角
n x n T y nz 0
ox oy oz 0
ax ay az 0
px py pz 1
（4.32）
Rot ( z , ) Rot ( y, ) Rot ( z , ) cos sin 0 0 sin cos 0 0 0 0 cos 0 0 0 1 0 sin 0 1 0 0 sin 1 0 cos 0 0 0 cos sin 0 0 0 1 0 sin cos 0 0 0 0 0 0 1 0 0 1 cos sin sin sin con 0 0 0 （4.33） 0 1