【发那科FANUC机器人】机器人学得一个正运动学的例子

1
PUMA 560 运动分析（表示）
1 正解
PUMA 560是属于关节式机器人，6个关节都是转动关节。

前3个关节确定手腕参考点的位置，后3个关节确定手腕的方位。

各连杆坐标系如图1所示。

相应的连杆参数列于表1。

图 1 机器人模型
PUMA560每个关节均有角度零位与正负方向限位开关，机器人的回转机体实现机器人机体绕0z 轴的回转（角1 ），它由固定底座和回转工作台组成。

安装在轴中心的驱动电机经传动装置，可以实现工作台的回转。

大臂、小臂的平衡由机器人中的平衡装置控制，在机器人的回转工作台上安装有大臂台座，将大臂下端关节支承在台座上，大臂
2
的上端关节用于支承小臂。

大臂臂体的下端安有直流伺服电机，可控制大臂上下摆动（角
2θ）。

小臂支承于大臂臂体的上关节处，其驱动电机可带动小臂做上下俯仰（角3θ），以及小臂的回转（4θ）。

机器人的腕部位于小臂臂体前端，通过伺服电动机传动，可实现腕部摆动（5θ）和转动（6θ）。

下图为简化模型：
图 2 机器人简化模型
表1
机械手的末端装置即为连杆6的坐标系，它与连杆坐标系的关系可由16i T -表示：
1
616i i i T A A A -+= (1)
1
616
i i i T A A A -+=
3
可得连杆变换通式为 ：
1
1
1111
111100
00
1i
i i i i i i i i i i i i i i i i i i c s a s c c c s d s T s s c s c d c θθθαθαααθαθααα-----------⎡⎤
⎢⎥--⎢⎥=⎢⎥
⎢
⎥⎣⎦ (2) 据连杆变换通式式(2)和表1所示连杆参数，可求得各连杆变换矩阵如下：
11221
120
1
12223324433
342
3
3444554
555000
000001001000000100010000001001000000100010
00010000
c s c s s c
d T T s c c s a c s a s c d T T s c c s T s c θθθθθθθθθθθθθθθθθθθθ--⎡⎤⎡⎤⎢⎥⎢⎥⎢⎥⎢
⎥==⎢⎥
⎢⎥
--⎢⎥
⎢⎥⎣⎦⎣⎦--⎡⎤
⎡⎤⎢⎥⎢⎥⎢⎥⎢
⎥==⎢⎥
⎢⎥
--⎢⎥⎢⎥⎣⎦⎣⎦--=665
66600001000010
01c s T s c θθθθ-⎡⎤⎡⎤⎢⎥⎢⎥⎢
⎥⎢
⎥=⎢⎥
⎢⎥
--⎢⎥⎢⎥⎣⎦⎣⎦ 各连杆变换矩阵相乘，得PUMA 560的机械手变换的T 矩阵：
0123456112233445566()()()()()()T T T T T T T θθθθθθ= (3)
即为关节变量1236θθθθ，，，，的函数。

该矩阵描述了末端连杆坐标系{6}相对
基坐标系{0}的位姿。

于是，可求得机械手的T 变换矩阵：
016160
1x x x x y
y y y z z z z n o a p n
o a p T T T n o a p ⎡⎤⎢⎥⎢⎥==⎢⎥⎢⎥⎣⎦
(4)
4
[][]1234564623561456461234564623561456462345646235612345646235614645612345646235()(),()(),();
[()](),[()x y z x y n c c c c c s s s s c s s c c c s n s c c c c s s s s c c s c c c s n s c c c s s c s c o c c c c s s c s s s s c c s c s o s c c c s s c s s s =--++=---+=---=--++-=--+6146456234564623561234523514512345235145234523512232342321122323423213232242](),(),(),(),;
[],[]z x y z x y z c c c s c c o s c c s s c c s s a c c c s s c s s s a s c c s s c c s s a s c s c c p c a c a c d s d s p s a c a c d s d c p a s a s d c --=---+=-+-=-++=-=+--=+-+=---3.
(5)
2 逆解
由上面可得：
0123456112233445566()()()()()()0
1x x x x y
y y y z z z z n o a p n o a p T T T T T T T n o a p θθθθθθ⎡⎤⎢⎥⎢⎥==⎢⎥⎢⎥⎣⎦
(6) 若末端连杆的位姿已经给定，即为已知，则求关节变量1236θθθθ，，，
，的值称
为运动反解。

用未知的连杆逆变换左乘方程(6)两边，把关节变量分离出来，从而求得
1236θθθθ，，，
，的解。

2.1 求1θ
用逆变换()0111T θ-左乘式(6)两边：
()0
10123451162233445566()()()()()T T T T T T T θθθθθθ-=
5
11
1
1
111
111
111611
1
1000000100
100010
00
1x
x x x x
x
x x y
y y y y
y
y
y z
z z z z z z z c s n o a p n o a p s c n o a p n o a p T n o a p n o a p ⎡⎤⎡⎤⎡⎤⎢⎥⎢⎥⎢⎥-⎢⎥
⎢⎥⎢⎥==⎢⎥⎢⎥⎢⎥
⎢⎥⎢⎥⎢
⎥
⎣⎦⎣⎦⎣⎦
(7) 两边(2,4)项元素对应相等：
1112x y y s p c p p d =>
-+== (8)
利用三角代换：
cos ;
sin x y p p ρφρφ== (9)
其中()22
;atan2,x y y x p p p p ρφ=
+= ，
（9）代入（8），得： 2
1212
2221222
122sin()/;cos()1(/)atan2,1atan2(,)atan2(,)y x x y d d d d p p d p p d φθρφθρφθρρθ⎧-=-=-⎪⎪⎡⎛⎫⎪
⎢=>-=-⎨ ⎪⎢⎝⎭⎪⎣
⎪
⎪=-+-⎩
式中，正、负号对应于1θ的两个可能解。

2.2 求3θ
（7）式两边(1,4)项和(3,4)项元素对应相等，可得：
111323423221
32342322x y
x z z c p s p p a c d s a c p p a s d c a s ⎧+==-+⎪=>
⎨==---⎪⎩
(10) 式（8）和式（10）的平方和为：
3343a c d s k =>
-=
(11)
其中2222222
2324
2
2x y z p p p a a d d k a ++----=
式（11）中已经消去了2θ，且方程（11）和（8）有相同的形式，因而可用三角代。