机器人学简明教程 (5)

第4章 机器人逆运动学
机器人逆运动学问题涉及一个复杂的非线性方程组求解， 而从数学角度分析一般的非线性方程组经常没有封闭（解析） 解。不过对于机械臂逆运动学问题存在合适的解决方案，因 为机械臂是人造机构，只需将其设计成存在封闭解的结构即 可解决该问题。理论上已经证明，对于6自由度机械臂，存 在封闭解的充分条件是有相邻的三个关节轴相交于一点。因 此，已经设计出来的6自由度机械臂几乎都有三个相交的关 节轴，例如PUMA560的4、5、6轴交于一点。
,
1 d32
2
第4章 机器人逆运动学
最后θ1的解可以写为
1 atan2(px , py ) atan2(d3, px2 py2 d32 ) (4－10)
式（4－10）的正负号表明θ1有两种解。现在θ1已知，因此式 （4－6）的左边均为已知。令式（4－6）两边的元素（1，4）
和（3，4）对应相等，得
第4章 机器人逆运动学
与欧拉角求解类似，根据第3章PUMA560运动学式（3 －21）和式（3－23）得
c1 s1 0 0 r11 r12 r13 px 1r11 1r12 1r13 1 px
s1 c1
0 0 r21
r22
r23
py
1r21
1r22
1r23
1
py
0
0
0 0
1 0 r31
/(2a2
)
采用与解式（4－8）相同的方法可以得到θ3的两种解。
3 atan2(a3,d4 ) atan2(k, a32 d42 k2 ) (4－13)
在θ1和θ3均已知，根据运动学关系可以得到下面等式
即
[ 03T (2 )]1[ 06T ] 36T
c1c23 s1c23 s23 a2c3 r11 r12 r13 px c4c5c6 s4s6 c4c5s6 s4c6 c4s5 a3
c1s23 s1s23 c23
a2 s3
r21
r22
r23
p
y
s5c6
s5s6
c5
d4
s1
c1
0 d3 r31 r32 r33 pz s4c5c6 c4s6 s4c5s6 c4c6 s4s5 0
0
0
0
1
0
0
0
1
0
0
0
1
(4－14)
第4章 机器人逆运动学
令式（4－14）两端的元素（1，4）和（2，4）相等，得到
对应（2，1）和（2，2）元素相等得 s -nxs+nyc，c-oxs+oyc
所以有
=atan2(-nxs+nyc, -oxs+oyc)
（4-5）
因此，欧拉变换解计算过程并不复杂，根据以上计算过程我们知道该
问题一般存在两个解。应当指出，当 时，根据（4-1）知 ay=ax=0， 此时（4-3）将不能确定 的值。此时对应绕 Z 轴连续做两次旋转，
第4章 机器人逆运动学
图4－2 平面机械臂有两个解
第4章 机器人逆运动学
3.逆运动学问题解法 前面强调，从运动学方程中求解关节变量θ1,θ2,…,θn是 一个非线性方程组求解问题。而非线性方程组求解方法分为 封闭（解析）解法和数值解法两大类。数值解法随着计算机 技术的发展已经成为非线性方程组求解的基本方法。然而， 对于逆运动学问题，数值解法并不适用，一是机械臂操作需 要频繁求解逆运动学问题，数值解法计算量比较大；二是数 值解法不能保证求出全部解。所以逆运动学问题一般只采用 封闭（解析）解法。
pxc1 p sy 1 a2c2 a3c23 d4s23 px a3s23 a2s2 d4c23
(4－11)
将式（4－8）和式（4－11）平方后相加，经复杂的运算得
a3c3 d4s3 k
(4－12)
第4章 机器人逆运动学
式中，
k
( px2
py2
pz2
a22
a32
d32
d
2 4
)
4 atan2( r13s1 r23c1, r13c1c23 r23s1c23 r33s23)
(4－19)
当θ5=0时，和欧拉角方程求解一样，属于奇异状态，可以任
意指定θ4的值。
[ 04T ]1[ 06T ] 46T
(4－20)
令式（4－20）两边的元素（1，2）和元素（3，3）相等得
d4 )
s23
(a2 s3
d4 ) pz (a3 a2c3 )(c1 px pz2 (c1 px s1 py )2
s1
py
)
(4－16)
第4章 机器人逆运动学
两式中分母相等，且为正值，因此可以得θ23的值:
23 atan2{(a3 a2c3) pz (c1 px s1 py )(a2s3 d4 ), (a2s3 d4 ) pz (a3 a2c3)(c1 px s1 py )}
式中，矩阵两边对应（2，3）元素相等得
s
0Baidu Nhomakorabea
c
(4－2)
-axs+ayc=0tan=ay/ax
第4章 机器人逆运动学
所以得 的两个解
=atan2(ay,ax)，= +
（4-3）
对应（1，3）和（3，3）元素相等得 s axc+ays，caz 所以有
=atan2(cax+say, az)
（4-4）
不能确定每次转的角度，属于欧拉角奇异情况。
第4章 机器人逆运动学
4.3 PUMA560逆运动学
本节将研究PUMA560的逆运动学封闭解，一般的6自由 度工业机器人逆运动学问题可以参考该方法进行求解。已知
06T ，计算各关节变量θ1,θ2,…,θ6。各连杆坐标系变
换关系如下:
06T [ 01T (1)][ 21T (2 )][ 23T (3 )][ 34T (4 )][ 45T (5 )][ 56T (6 )] [ 01T (1)]1[ 06T ] 61T
第4章 机器人逆运动学
第4章 机器人逆运动学
4.1 逆运动学问题的可解性 4.2 欧拉变换解 4.3 PUMA560逆运动学
第4章 机器人逆运动学
4.1 逆运动学问题的可解性
1.解的存在性 逆运动学问题解是否存在完全取决于机械臂的工作空间。 所谓工作空间是指机械臂末 端执行器所能达到的空间位姿的集合。一般来说，对于给 定的机械臂，其工作空间是固定的。而对于少于6个自由度的 机械臂，它在三维空间内不能达到全部位姿。所以通用工业机 器人一般都设计成6个自由度。当期望位姿位于机械臂的工作 空间之外时，逆运动学问题无解。如图4－1所示期望平面机械 臂末端达到B点，显然该逆运动学问题是无解的。
因此，PUMA560的逆运动学问题共有8组解。由于实际 系统关节运动范围的限制，其中一些解需要舍去，在余下的 有效解中，通常选取与当前机械臂关节角位置最近的解。
两个方程
c1c23 px s1c23 py s23 pz a2c3 a3 c1s23 px s1s23 py c23 pz a2s3 d4
(4－15)
联立上述两个方程可以解出s23和c23，结果为
s23
(a3
a2c3 ) pz (c1 px s1 py )(a2s3 pz2 (c1 px s1 py )2
0
1
0
r32 0
r33 0
pz
1
1r31
0
1r32 0
1r33 0
1
pz
1
(4－6)
第4章 机器人逆运动学
式（3－22）的最后三个数如下:
1 1
px py
a2c2 d3
a3c23
d4s23
1 pz a3s23 a2s2 d4c23
令式（4－6）两边元素（2,4）相等，得到
-pxs1+pyc1=d3
nx ox ax
Rzyz
ny
oy
a
y
nz oz ax
第4章 机器人逆运动学 根据欧拉变换方程式（2－40）可得如下9个方程:
nnxy
cc c sc c
s s c s
nz s c
ooxy
cc s sc s
sc cc
oazx
s s c s
ay ss
az c
(4－1)
第4章 机器人逆运动学
1.双变量反正切函数 在三角函数求解时，通常采用双变量反正切函数 atan2(y,x)来确定角度。atan2提供两个自变量，即纵坐标和 横坐标，见图4－3。当-π≤θ≤π时，由atan2反求角度过程中， 同时检查y和x的符号来确定其所在象限。该函数也能检验什 么时候x或y为0，并反求出正确的角度。atan2的精确程度对 其整个定义域都是一样的。高级编程语言如C和Matlab等提 供标准库函数供编程者调用。
对于平面机械臂的逆运动学问题，可以采用3.3节介绍 的几何方法进行求解。下面首先介绍欧拉变换的求解方法， 然后以PUMA560为例介绍6自由度机械臂的逆运动学问题求 解方法。
第4章 机器人逆运动学
4.2 欧拉变换解
式（2－43）给出了采用欧拉角表示的坐标变换，其逆 问题是给定旋转矩阵Rzyz，确定对应的欧拉角。假设给定 的旋转矩阵如下:
r21[(s1c23c4 c1s4 )c5 s1s23s5 ] r31(s23c4c5 c23s5 )
第4章 机器人逆运动学
由于式（4－10）和式（4－13）的θ1和θ3各有两个解， 另外机械臂腕关节“翻转”可以得到θ4~θ6的另一组解
54''
4 180o 5
6' 6 180o
(4－26)
为了求解式（4－8），做三角恒等变换
px=cos py=sin
(4－7) （4－8） （4－9）
第4章 机器人逆运动学
式中，
p2 p2
x
y
atan2(py ,
px
)
将式（4－9）代入式（4－8）得
所以
c s1
sc1
d3
sin(
1 )
d3
因此
cos( 1)
1 d32
2
1
atan2
d3
因θ23=θ2+θ3,故可得θ2的值:
2=23-3
（4－17）
现在式（4－14）的左边均为已知。令式（4－14）两
边的元素（1，3）和（3，3）对应相等，得
r13c1c23 r23s1c23 r33s23 c4s5
r13s1 r23c1 s4s5
(4－18)
第4章 机器人逆运动学
若s5≠0，可以解出
第4章 机器人逆运动学
图4－1 期望机械臂末端达到B点
第4章 机器人逆运动学
2.多解问题 逆运动学求解的另一个问题是多解问题。如图4－2所示 的平面机械臂有两个解，虚线表示另外一个解。逆运动学解 的个数取决于机械臂关节的数量，同时与连杆参数和关节运 动范围有关。PUMA560工业机器人一般存在8个解。
r13 (c1c23c4 s1s4 ) r23(s1c23c4 c1s4 ) r33s23c4 s5
r13c1s23 r23s1s23 r33c23 c5
(4－21)
可以确定θ5的值
5 atan2(s5, c5 )
(4－22)
第4章 机器人逆运动学
现在θ1~θ5均已知，根据运动学关系可以得到下面等式
第4章 机器人逆运动学
图4－3 双变量反正切函数
第4章 机器人逆运动学
2.欧拉变换解 根据式（2－39）和式（2－40）可知:
Rot(z,)1 Rzyz Rot( y, )Rot(z, )
即
c s 0 nx ox ax c c c s
s
c
0
ny
oy
ay
s
c
0 0 1 nz oz az s c s s
[ 05T ]1[ 06T ] 56T (6 )
(4－23)
令式（4－23）两边元素（1，1）和（3，1）相等可以得到 θ6的解
6 atan2(s6, c6 )
s6 r11(c1c23s4 s1c4 ) r21(s1c23s4 c1c4 ) r31s23s4 c6 r11[(c1c23c4 s1s4 )c5 c1s23s5 ]