第4章 机器人逆运动学方程
合集下载
相关主题
- 1、下载文档前请自行甄别文档内容的完整性，平台不提供额外的编辑、内容补充、找答案等附加服务。
- 2、"仅部分预览"的文档,不可在线预览部分如存在完整性等问题,可反馈申请退款(可完整预览的文档不适用该条件!)。
- 3、如文档侵犯您的权益，请联系客服反馈,我们会尽快为您处理(人工客服工作时间：9:00-18:30)。
根据式（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）
在求解关节变量过程中如出现反正切函数的分子和分母太小， 则计算结果误差会很大，此时应重新选择矩阵元素建立新的方 程组再进行计算，直到获得满意的结果为止。同样，如果计算 结果超出了机械手关节的运动范围，也要重新计算，直到符合 机械手关节的运动范围。
由于机械手各关节变量的相互耦合，后面计算的关节变量与前 面的关节变量有关，因此当前面关节变量的计算结果发生变化 时，后面关节变量计算的结果也会发生变化，所以逆运动方程 的解不是唯一的，我们应该根据机械手的组合形态和各关节的 运动范围，经过多次反覆计算，从中选择一组合理解。由此可 见，求解机械手的逆运动方程是一个十分复杂的过程。
这里
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
将上式写成如下形式
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
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）
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
an－连杆长度; αn－连杆扭转角;
dn－相邻两连杆的距离;
θn－相邻两连杆的夹角。
对于旋转关节θn为关节变量，而对于滑动关节dn为关节变量。其余为 连杆参数，由机械手的几何尺寸和组合形态决定。
（2） 根据任务确定机械手的位姿T6 T6为机械手末端在直角坐标系（参考坐标或基坐标）中的位姿，由 任务确定，即式（ 3.37 ）给出的表达式T6 = Z-1 X E-1确定。它是由三个 平移分量构成的平移矢量P（确定空间位置）和三个旋转矢量n，o，a（ 确定姿态）组成的齐次变换矩阵描述。
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θ
(4.10) (4.11) (4.12)
1T
6
=
C2C4S5 + S2C5 S2C4S5 - C2C5 S4S5 0
S2d3 -C2d3 d2 1
（4.13）
比较式（4.9）和式（4.13）矩阵中的第三行第四列元素相等得到 f13（p）= d2 或 令 其中 （4.14） （4.15） （4.16） （4.17）
（4.48）
式中
f11 cos x sin y
（4.49） （4.50） （4.51） （4.52）
f12 sin x cos y
f13 z
f12 (a) sin a x cos a y
比较式（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
由式（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的值
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
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）
注意：
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）
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
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）
Βιβλιοθήκη Baidu
由式（4.42）可解出θ角
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）
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所示），因此如果我们得到欧拉角的正切表达式，
4.1
引言 (Introduction)
所谓逆运动学方程的解，就是已知机械手直角坐标空间的位姿 （pose）T6，求出各节变量θn or dn 。 T6 = A 1 A 2 A 3 A 4 A 5 A 6 逆运动学方程解的步骤如下： （1）根据机械手关节坐标设置确定An （4.1）
An为关节坐标的齐次坐标变换，由关节变量和参数确定。关节变量 和参数有：
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
根据上述五个矩阵方程对应元素相等，可得到若干个可解的代数方程，便可
求出关节变量θn或 dn。
4.3 斯坦福机械手的逆运动学解
（ Inverse solution of Stanford manipulator）
在第三章我们推导出 Stanford Manipulator 的运动方程和各关节齐次变换式。 下面应用式（4.2）～（4.6）进行求解：
分别用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）
在求解关节变量过程中如出现反正切函数的分子和分母太小， 则计算结果误差会很大，此时应重新选择矩阵元素建立新的方 程组再进行计算，直到获得满意的结果为止。同样，如果计算 结果超出了机械手关节的运动范围，也要重新计算，直到符合 机械手关节的运动范围。
由于机械手各关节变量的相互耦合，后面计算的关节变量与前 面的关节变量有关，因此当前面关节变量的计算结果发生变化 时，后面关节变量计算的结果也会发生变化，所以逆运动方程 的解不是唯一的，我们应该根据机械手的组合形态和各关节的 运动范围，经过多次反覆计算，从中选择一组合理解。由此可 见，求解机械手的逆运动方程是一个十分复杂的过程。
这里
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
将上式写成如下形式
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
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）
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
an－连杆长度; αn－连杆扭转角;
dn－相邻两连杆的距离;
θn－相邻两连杆的夹角。
对于旋转关节θn为关节变量，而对于滑动关节dn为关节变量。其余为 连杆参数，由机械手的几何尺寸和组合形态决定。
（2） 根据任务确定机械手的位姿T6 T6为机械手末端在直角坐标系（参考坐标或基坐标）中的位姿，由 任务确定，即式（ 3.37 ）给出的表达式T6 = Z-1 X E-1确定。它是由三个 平移分量构成的平移矢量P（确定空间位置）和三个旋转矢量n，o，a（ 确定姿态）组成的齐次变换矩阵描述。
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θ
(4.10) (4.11) (4.12)
1T
6
=
C2C4S5 + S2C5 S2C4S5 - C2C5 S4S5 0
S2d3 -C2d3 d2 1
（4.13）
比较式（4.9）和式（4.13）矩阵中的第三行第四列元素相等得到 f13（p）= d2 或 令 其中 （4.14） （4.15） （4.16） （4.17）
（4.48）
式中
f11 cos x sin y
（4.49） （4.50） （4.51） （4.52）
f12 sin x cos y
f13 z
f12 (a) sin a x cos a y
比较式（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
由式（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的值
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
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）
注意：
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）
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
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）
Βιβλιοθήκη Baidu
由式（4.42）可解出θ角
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）
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所示），因此如果我们得到欧拉角的正切表达式，
4.1
引言 (Introduction)
所谓逆运动学方程的解，就是已知机械手直角坐标空间的位姿 （pose）T6，求出各节变量θn or dn 。 T6 = A 1 A 2 A 3 A 4 A 5 A 6 逆运动学方程解的步骤如下： （1）根据机械手关节坐标设置确定An （4.1）
An为关节坐标的齐次坐标变换，由关节变量和参数确定。关节变量 和参数有：
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
根据上述五个矩阵方程对应元素相等，可得到若干个可解的代数方程，便可
求出关节变量θn或 dn。
4.3 斯坦福机械手的逆运动学解
（ Inverse solution of Stanford manipulator）
在第三章我们推导出 Stanford Manipulator 的运动方程和各关节齐次变换式。 下面应用式（4.2）～（4.6）进行求解：