【发那科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 θθθθθθ-=