可求得：
23 2 3 tan 1[
所以：
(a3 a2c3 ) px (c1 px s1 p y )(a2 s3 d 4 ) (d 4 a2 s3 ) px (c1 px s1 p y )(a2c3 a3 )
]
2 23 3
（4）求 4
点，那么坐标系的原点就在这一交点上。如果两轴线互相平行，那么选择原点 使对下一个连杆（其坐标原点已经确定）的距离 d i 1 为零。连杆 i 的 z 轴与关节
i 1 的轴线在一直线上，而 x 轴则在连杆 i 和 i +1 的公共法线上，其方向从 i 指向
i +1。
图 1 PUMA560 机器人坐标系图
1 0 0 0 Trans( x, ai 1 ) 0 0 1 0 1 0 Trans( z, di ) 0 0
0 1 0 0 0 1 0 0
0 ai 1 0 0 1 0 0 1 0 0 0 0 1 di 0 1
nz s23 (c4c5c6 s4 s6 ) c23 s5c6 oz s23 (c4c5c6 s4 s6 ) c23s5c6 az s23c4 s5 c23c5 px c1 (a2c2 a3c23 d 4 s23 ) d3 s1
py s1 (a2c2 a3c23 d4 s23 ) d3c1
(1-9)
式(1-7)和(1-9)的平方和
a3c3 d 4 s3 k
(1-10)
其中， k
px 2 p y 2 pz 2 a2 2 a32 d 2 2 d 4 2 2a2
同 1 算法，可求得：
3 tan 1 (
a3 k ) tan 1 ( ) 2 d4 a3 d 4 2 k 2
i 1
Ti Rot ( x, i 1 ) Trans( x, ai 1 ) Rot ( z,i ) Trans( z, di )
(1-1)
其中：
0 0 1 0 cos sin i 1 i 1 Rot ( x, i 1 ) 0 sin i 1 cos i 1 0 0 0
(1-11)
（3）求 2
在矩阵方程(1-4)两边左乘 0 T31 。
0
T310T6 3 T44T55T6
s23 c23 0 0 a2c3 nx a2 s3 ny d 2 nz 1 0 ox oy oz 0 ax ay az 0 px py 3 T6 pz 1
PUMA560 机器人正逆运动学推导及工作空间求解
题目：PUMA 机器人正逆运动学推导及运动空间解算
求解：①建立坐标系；②给出 D-H 参数表；③推导正运动学、逆运动学；④编 程得出工作空间。
1. 连杆坐标系的建立
机械臂可以看成由一系列连接在一起的连杆构成的。为描述相邻杆件间平 移和转动的关系，Denavit 和 Harten-berg 于 1955 年提出了一种为关节链中的每 一杆件建立附体坐标系的矩阵方法，D-H 方法是为每个关节处的杆件坐标系建 立 4×4 齐次变换矩阵(又叫 A 矩阵)来表示它与前一杆件坐标系的关系。通常连 杆坐标系的设定不固定，所以连杆参数也会随之不同，连杆参数和连杆坐标关 系如图 2-1 所示。刚性杆件的 D-H 表示法取决于此杆的 4 个几何参数，这 4 个几 何参数可完全描述任何转动或移动关节。各连杆的 4 个特征参数定义如下： ai 1 为从 zi 1 到 zi 沿 xi 1 测量的距离； i 1 为从 zi 1 到 zi 绕 xi 1 旋转的角度； d i 为从
c 4 0 3 T4 s 4 0
s 4 0 c 4 0
0 a3 1 d4 0 0 0 1 0 1 0 0 0 0 0 1
(1-4)
0 c 6 s 6 0 0 0 5 T6 s 6 c 6 0 0 1 0 各连杆变换矩阵相乘，得 PUMA560 的机械手变换矩阵 nx n 0 T6 y nz 0 ax ay az 0
n 自由度的机械手端部对基座的关系：
0
(1-2)
Tn 0T1 1T2 n1Tn
c 2 0 1 T2 s 2 0 s 2 0 c 2 0
(1-3)
根据式(1-2)和表 2-1 所示，可求得各连杆变换矩阵如下
c1 s1 s c1 0 T1 1 0 0 0 0
sin( 1 )
d2 R
d2 px 2 p y 2 d 2 2 )
(1-8)
1 tan 1 (
（2）求 3
py px
) tan 1 (
对式(1-6)两端的元素(1,4)和(3,4)分别对应相等，即得方程
c1 px s1 p y a3c23 d 4 s23 a2 c2 px a3 s23 d 4c23 a2 s2
(1-13)
式(1-12)的左边均为已知，令两边元素(1,3)和(3,3)分别对应相等，则可得：
ax c1c23 a y s1c23 az s23 c4 s5 ax s1 a y c1 s4 s5
只要 s5 0 ，可求出：
4 tan 1 (
ax s1 a y c1 ax c1c23 a y s1c23 az s23
cos i sin i Rot ( z,i ) 0 0
sin i cos i 0 0
0 0 1 0
0 0 0 1
将各子式带入式(1-1)可得连杆变换通式为
si 0 i 1ci ci s c ci c i 1 s i 1 di s i 1 i i 1 i 1 Ti si s i 1 ci s i 1 c i 1 di c i 1 0 0 1 0
)
(1-14)
s5 0 时，机械臂处于奇异形位。此时，可任选 4 的值。
（5）求 5
式(1-12)的两端左乘逆变换 0 T4 1 ：
0
T4 10T6 4 T55T6
(1-15)
同上述方法，将矩阵两边元素(1,3)和(3,3)分别对应相等，可得：
ax (c1c23c4 s1s4 ) a y ( s1c23c4 c1s4 ) az s23c4 s5 ax (c1s23 ) a y ( s1s23 ) az (c23 ) c5 ຫໍສະໝຸດ 1 =90o 2 =0
3 =90o
4 =0
5 =0
6 =0
其中， a2 =431.8mm
a3 =20.32mm d 2 =149.09mm d 4 =433.07mm d 6 =56.25mm
3. 正运动学推导
确定连杆坐标系之后，我们可以按照以下顺序由两个旋转和两个平移来建 立相邻连杆 i -1 与 i 之间的相对应关系： 1、绕 xi 1 轴旋转 i 1 角； 2、沿 xi 1 轴平移 ai 1 ； 3、绕 zi 轴旋转 i 角； 4、沿 zi 轴平移 d i 。 这种关系可表示为：
， 6 的求解。 器的位姿(位置和姿态) ,求解相应的关节变量，也就是对 1， 2，
（1）求 1
对方程(1-5)左乘逆变换 0 T11 两边，得：
0
T110T6 1 T22T33T44T55T6 1 T6
0 0 1 0 0 nx 0 ny 0 nz 1 0 ox oy oz 0 ax ay az 0 px py 1 T6 pz 1
c1 s1 s c 1 1 0 0 0 0
(1-6)
使矩阵方程两端的元素(2,4)对应相等，可以求得
s1 px c 1p y d
令R
2
(1-7)
px 2 py 2 ， cos
py py px 1 ， sin ， tan ( ) 代入式(4-6)，得： px R R
c1c23 c s 1 23 s1 0
s1c23 s1s23 c1 0
(1-12)
令方程(1-6)两边的元素(1,4)和(2,4)分别对应相等可得：
c1c23 px s1c23 p y s23 p y a2c3 a3 c1s23 px s1s23 p y c23 px a2 s3 d 4
ox c ( c4 c5 s 1[ c 23 6
s4)c6
s2 3s ] 5 s6 ( s1
s 4c 5 s 6 ) c 4c 6
ax c1 (c23c4 s5 s23c5 ) s1s4 s5
ny s1[c23 (c4c5c6 s4 s6 ) s23s5c6 ] c1 (s4c5c6 c4 s6 ) oy s1[c23 (c4c5 s6 s4c6 ) s23s5 s6 ] c1 (s4c5 s6 c4c6 ) ay s1 (c23c4 s5 s23c5 ) c1s4 s5
0 0 1 0
0 0 0 1
0 0 1 d2 0 0 0 1
c3 s 2 T3 3 0 0 c5 0 4 T5 s5 0 ox oy oz 0
s3 0 a2 c3 0 0 0 1 0 0 0 1 s5 0 c5 0 0 1 0 0
pz a3 s23 a2 s2 d 4 c23 c cos , s sin , s23 sin( 2 3 ), c23 cos( 2 3 )
4. 逆运动学推导
机器人逆运动学问题在机器人运动学、动力学及控制中占有非常重要的地 位,直接影响着控制的快速性与准确性。逆运动学问题就是根据已知的末端执行