机器人学得一个正运动学举例说明

PUMA 560 运动分析（表示）
1 正解
PUMA 560 是属于关节式机器人，6 个关节都是转动关节。前 3 个关节确定手腕参 考点的位置，后 3 个关节确定手腕的方位。
各连杆坐标系如图 1 所示。相应的连杆参数列于表 1。
图 1 机器人模型
PUMA560 每个关节均有角度零位与正负方向限位开关，机器人的回转机体实现机 器人机体绕 z0 轴的回转（角1 ），它由固定底座和回转工作台组成。安装在轴中心的驱 动电机经传动装置，可以实现工作台的回转。大臂、小臂的平衡由机器人中的平衡装置 控制，在机器人的回转工作台上安装有大臂台座，将大臂下端关节支承在台座上，大臂 的上端关节用于支承小臂。大臂臂体的下端安有直流伺服电机，可控制大臂上下摆动（角 2 ）。小臂支承于大臂臂体的上关节处，其驱动电机可带动小臂做上下俯仰（角3 ），以 及小臂的回转（4 ）。机器人的腕部位于小臂臂体前端，通过伺服电动机传动，可实现

腕部摆动（5 ）和转动（6 ）。 下图为简化模型：
T i1 6
Ai Ai1 A6
图 2 机器人简化模型
表1
机械手的末端装置即为连杆
6
的坐标系，它与连杆坐标系的关系可由
T i1 6
表示：
T i 1 6
Ai Ai1 A6
(1)
可得连杆变换通式为 ：
ci
si
0
ai1
T i1 i

si
c
i
1
si si1
cici1 ci si1
si1 ci1
di
si1

dici1
(2)
0
0
0
1
据连杆变换通式式(2)和表 1 所示连杆参数，可求得各连杆变换矩阵如下：

c1 s1 0 0
0T1

s1
0
c1 0
0 0 1 0
0 0 0 1
c2 s2 0 0
1T2

0
s2
0 c2
1 0
d
2

0
0 0 0 1
c3 s3 0 a2
2T3

s3
0
c3 0
0
0

1 0
0 0 0 1
c4 s4 0 a3
3T4

0
s4
0 c4
1 0
d4

0
0 0 0 1
c5 s5 0 0
4T5

0
s5
0 c5
1 0 0 0
0 0 0 1
c6 s6 0 0
5T6

0
s6
0 c6
1 0
0 0
0 0 0 1
各连杆变换矩阵相乘，得 PUMA 560 的机械手变换的 T 矩阵：
0T6 0T1(1)1T2 (2 ) 2T3 (3 ) 3T4 (4 ) 4T5 (5 ) 5T6 (6 )
(3)
即为关节变量1，2，3，，6 的函数。 该矩阵描述了末端连杆坐标系{6}相对
基坐标系{0}的位姿。
于是，可求得机械手的 T 变换矩阵：
nx ox ax px
0T6
0T1 1T6

ny
nz
oy oz
ay az
py

pz
(4)
0 0 0 1
nx c1 c23 (c4c5c6 s4s6 ) s23s5c6 s1(s4c5c6 c4s6 ),
ny s1 c23 (c4c5c6 s4s6 ) s23s5c6 c1(s4c5c6 c4s6 ),
nz s23 (c4c5c6 s4s6 ) c23s5c6;
ox c1[c23 (c4c5s6 s4c6 ) s23s5s6 ] s1(c4c6 s4c5s6 ),
oy s1[c23 (c4c5s6 s4c6 ) s23s5s6 ] c1(c4c6 s4c5c6 ),
oz s23 (c4c5s6 s4c6 ) c23s5s6 ,
(5)
ax c1(c23c4s5 s23c5 ) s1s4s5 ,
ay s1(c23c4s5 s23c5 ) c1s4s5 ,
az s23c4s5 c23c5;
px c1[a2c2 a3c23 d4s23 ] d2s1,
py s1[a2c2 a3c23 d4s23 ] d2c1
pz a3s23 a2s2 d4c23.

2 逆解
由上面可得：
nx ox ax px
0T6

ny
nz
oy oz
ay az
py pz

0T1(1)1T2 (2 ) 2T3 (3 ) 3T4 (4 ) 4T5 (5 ) 5T6 (6 )
(6)
0 0 0 1
若末端连杆的位姿已经给定，即为已知，则求关节变量1，2，3，，6 的值称
为运动反解。用未知的连杆逆变换左乘方程(6)两边，把关节变量分离出来，从而求得
1，2，3，，6 的解。
2.1 求1
用逆变换
T0 1 1
1
左乘式(6)两边：
T0 1 1
1
0T6 1T2 (2 ) 2T3 (3 ) 3T4 (4 ) 4T5 (5 ) 5T6 (6 )
c1 s1 0 0 nx ox ax px
1nx 1ox 1ax 1 px

s1
c1
0
0 ny
0 0 1 0 nz
oy oz
ay az
py pz

1T6

1ny
1nz
1oy 1oz
1ay 1az
1
p
y

1 pz
(7)
0 0 0 1 0 0 0 1
0 0 0 1
两边(2,4)项元素对应相等：
s1 px c1 py 1 py d2
(8)
利用三角代换：
px cos; py sin
(9)
其中
px2
p
2 y
;
atan2
py , px
，（9）代入（8），得：
sin(
1 )
d2
/
;
cos( 1) 1 (d2 / )2

1
atan2

d2
,
1

d2
2


1 atan2( py , px ) atan2(d2 ,
px2
p
2 y
d
2 2
)
式中，正、负号对应于1 的两个可能解。