机器人学得一个正运动学的例子

PUMA 560 运动分析（表示）

1 正解

PUMA 560是属于关节式机器人，6个关节都是转动关节。前3个关节确定手腕参考点的位置，后3个关节确定手腕的方位。

各连杆坐标系如图1所示。相应的连杆参数列于表1。

图1机器人模型

PUMA560每个关节均有角度零位与正负方向限位开关，机器人的回转机体实现机器人机体绕0z 轴的回转（角1θ），它由固定底座和回转工作台组成。安装在轴中心的驱动电机经传动装置，可以实现工作台的回转。大臂、小臂的平衡由机器人中的平衡装置控制，在机器人的回转工作台上安装有大臂台座，将大臂下端关节支承在台座上，大臂的上端关节用于支承小臂。大臂臂体的下端安有直流伺服电机，可控制大臂上下摆动（角2θ）

。小臂支承于大臂臂体的上关节处，其驱动电机可带动小臂做上下俯仰（角3θ），以及小臂的回转（4θ）。机器人的腕部位于小臂臂体前端，通过伺服电动机传动，可实现

腕部摆动（5θ）和转动（6θ）。

下图为简化模型：

图2机器人简化模型

表1

机械手的末端装置即为连杆6的坐标系，它与连杆坐标系的关系可由16i T -表示：

1

616i i i T A A A -+= (1)

可得连杆变换通式为：

111111111100001i

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所示连杆参数，可求得各连杆变换矩阵如下：

1

616

i i i T A A A -+=

11221

120

1

12223324433

342

3

3444554

555000

00000100100000010001000000100100000010001000010000

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 变换矩阵：

001

6160

00

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) [][]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

00

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 θθθθθθ-=

11

1

1

111

111

1116111

1

000000100

10

0010

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)

其中()atan2,y x p p ρφ=

=，（9）代入（8），得：

1212112sin()/;cos()atan2,atan2(,)atan2(,y x d d p p d φθρφθφθρθ⎧-=-=⎪⎪⎡⎪

⎢=>-=⎨⎢⎪⎣⎪

⎪=-⎩

式中，正、负号对应于1θ的两个可能解。