0
0
0
ny
c1 0
1 0
0 1
nz 0
oy oz 0
ay az 0
py pz 1
1
T6
令矩阵方程两端的元素（2, 4）对应相等，可得：
s1px c1py d2
利用三角代换： px cos py sin
式中， px2 py2 ； a tan 2( py , px ) 。带入可以得出θ1 的解：
23 2 3 a tan 2[(a3 a2c3) pz (c1 px s1 py )(a2s3 d4), (d4 a2s3) pz (c1 px s1 py )(a2c3 a3)]
根据1 和3 解的四种可能组合，由上式可以得到相应的四种可能值23 ，于
是可得到2 的四种可能解：
2 23 3
2
sin( 1) d2 / ; cos( 1) 1 d2 /
1
a
tan
2
d2
,
1
d2
2
1 a tan 2( py , px ) a tan 2(d2, px2 py2 d22 )
式中，正、负号对应于θ1 的两个可能解。 2. 求θ3
在选定1 的一个解之后，再令矩阵方程两端的元素（1,4）和（3,4）分别对
arctan
cos
ax
az
sin
a
y
arctan(cos ax
sin ay , az )
类型 3 的逆运动学解
nx ox ax cos
ny
oy
ay
=
sin
nz oz az 0
sin cos
0
0 cos
0
0
1 sin
0 sin 1 0
1
0
0
cos
0 cos 0 sin
解：（1）如图建立两个坐标系o1x1y1z1 、o2x2 y2z2分别固结在
两个楔形物体上，如下图
对楔块 1 进行的变换矩阵为：
T1 Rot(z,90 )Rot(x,90 ) ；
对楔块 2 进行的变换矩阵为：
T2 Trans(3, 0,9)Rot(Z , 90 )Trans(0,5, 0)Rot( X ,90 )Rot(Z,180。)
(6
)
则求关节变量θ1，θ2，θ3，θ4，θ5，θ6 的值。具体求解步骤如下： 1. 求θ1
用逆变换
T0 1 1
1
左乘（式
1）两边：
T0 1 1
1
0 T6
1 T2 (2 )2T3 (3 )3T4 (4 )4T5 (5 )5T6 (6 )
c1 s1 0 0 nx ox ax px
0
s1 0
应相等，即得两方程：
c1 px s1 py a3c23 d4s23 a2c2 px a3s23 d4c23 a2s2
结合式 2 与上式，消去2 ，可以求解得θ3 为：
3 a tan 2(a3, d4) a tan 2(k, a32 d42 k2 )
式中正、负号对应3 的两种可能解。
得
arctan
cos
nz nx sin
ny
arctan(nz , cosnx
sinny )
又有
sin ox cosoy =cos sin ax cosay = sin 得
=
arctan
sinax cosay sinox cosoy
=
arctan(sin ax
cosay
cos sin
cos sincos sin sin
sin
sin cos
cos
sin
cos cos
类型 2 的逆运动学解
nx ox ax cos sin 0 cos 0 sin cos sin 0
ny
oy
ay
=
sin
cos
0
0
1
0
sin
cos
0
nz oz az 0
,
sinox
cosoy
)
4. 求 PUMA560 机器人的逆运动学方程的解。 解：建立 PUMA 560 机器人的 D-H 坐标系如图如下：
求各连杆变换矩阵如下：
c1 s1 0 0
0
T1
s1 0
c1 0
0 0 1 0
0
0 0 1
c2 s2 0 0
1T2
0 s2 0
0 c2 0
1 0 0
s1c4
s1c23c4 c1s4 s1c23s4 c1c4
s1s23 0
根据矩阵两边元素分别对应相等，可得：
s23c4 s23s4 c23 0
a2c3c4 d2s4 a3c4
a2c3s4
d2c4
a3s4
a2s3 d4 1
ax (c1c23c4 s1s4 ) ay (s1c23c4 c1s4 ) az (s23c4 ) s5
,
ax
)
的正负由定义知
由
sin ox cosoy =cos sin nx cosny = sin 则
=
arctan
sinnx sinox
cosny cosoy
=
arctan( sin nx
cosny
,
sin ox
cosoy
)
由
cosax sin ay = sin az = cos 则
sin sin
cos sin
sin
sin
cos
类型 3 的正运动学解
cos
R=
sin
0
sin cos
0
0 cos
0
0
1 sin
0 sin 1 0
1
0
0
cos
0 cos 0 sin
0
sin
cos
cos cos
sin
cos
sin
cos sin sin sincos sin sin sin coscos
式中，2 取与3 相对应的值。
4. 求θ4 因为式 4-3 的左边均为已知，令两边元素（1,3）和（3,3）分别对应相等， 则可得：
axc1c23 ay s1c23 az s23 c4s5 axs1 ayc1 s4s5
只要 s5≠0，便可求出 4 ：
4 a tan 2(axs1 ayc1, axc1c23 ays1c23 azs23)
0
nz
cosox sinoy sinox cosoy
oz
由（2,3）元素相等，则
cosax sinay coscos
sin
ax
cosay
sin
az
sincos
cos sin cos
sin sin
sin
0
cos
sinax cosay =0
得
arctan
ay
ax
arctan(ay
py pz 1
3
T6
联立求解得 s23 和 c23 ：
s
(a3
a2c3 ) pz pz2
(c1 px s1 py )(a2s3 (c1 p s1 p)2
d4 )
c
(d4
a2s3 )
pz (c1 px s1 py )(a2c3 pz2 (c1 p s1 p)2
a3 )
由上式得
nx[(c1c23c4 s1s4 )c5 c1s23s5] ny[(s1c23c4 c1s4 )c5 s1s23s5 ] nz (s23c4c5 c23s5 ) c6
从而可求出θ6 的解：
6 a tan 2(s6 , c6 )
以上求解过程即是 PUMA560 机器人的逆运动学方程求解过程 至此，1、2、3、4、5、6 都以求得。 5. 对于下图所示三自由度机械手，其关节 1 与关节 2 轴线相交， 关节 2 与关节 3 轴线平行，各关节的正向转动角度如图标示，请建立该机械手 的 D-H 坐标系，并求其变换矩阵 0 A1 ， 1A2 ， 2A3 。
工业机器人课后作业
姓 名： 班 级： 学 号：
2014 年 4 月
第三章作业
1. 初始时坐标系{B}与参考系{A}重合，现将{B}先绕 ZB 轴旋转 θ 角，然后 再绕 XB 轴旋转φ角，求转动后的{B}对于{A}的旋转矩阵 解：R=Rot（z，θ）Rot（x，φ）
cos
=
sin
0
sin cos
nx ox ax px
0 T6
ny
nz 0
oy oz 0
ay az 0
p
y
pz 1
其中 px
0 T6
ny
nz 0
oy oz 0
ay az 0
py pz 1
0
T1(1)1T2
(2
)2T3
(3
)3T4
(4
)4T5
(5
)5T6
5.求θ5
据求出的
4 ，可进一步解出
T0 1
5 ，将式 1 两端左乘逆变换 4
1,2 ,3 ,4
得：
T0 1 4
(1
,2
,3
,4
)
0T6
4T5 (5 ) 5T6 (6 )
T0 1
逆变换 4
1,2 ,3 ,4
为：
c1c23c4 s1s4
T41
(1
,2
,3
,4
)
c1c23s4
c1s23 0
0 1 sin 0 cos 0
0 1
则
cos sin 0 nx ox ax cos 0 sin cos sin 0