机械臂运动学方程

机械手臂的运动学公式推导

1. 仿人机器人手臂模型

● 仿人机器人的手臂有6个自由度，肩部（shoulder ）3个，肘

部（elbow ）2个，腕部（wrist ）1个，如图1所示。 ● 机器人手臂的几何尺寸（mm ）：

上臂长度：216 小臂长度：173.5

● 关节的运动范围（右手）：如表1所示。

表1 关节运动范围

⑴ 参考坐标系

为了对仿人机器人进行控制，同时也便于描述机器人的动作状态，

必须建立适当的初始坐标系。我们设定机器人手臂的初始姿态：大臂从肩垂直向下，小臂向前平伸，与大臂成

90。

参考坐标系（实验室坐标系）的设定以机器人本身的初始位置与实验室坐标系相一致的原则设定，如图2所示。

X 轴：以机器人初始（状态）位置的右侧方向作为实验室坐标系的X 轴； y 轴：设定y 轴使其为右手系坐标系，即正前方为y 轴正向。

Z 轴：以机器人初始（状态）位置的上方向作为实验室坐标系的Z 轴；按D-H 坐标建立的方法，各个关节的轴线与各关节坐标系的Z 轴共线.

(2) 关节坐标系

各关节坐标系的建立如图3所示。

1

2 3 4 5 6

图1 手臂模型 X 5 X

O

Z Y

图2 参考坐标系

X 3 O 3 Z 3

Y 3Z 4 O 4

Y 4X 4

O 5

Z 5Y 5Z 6 O 6

Y 6 X 6 X 1 O 1 Z 1

Y 1 Y 2 O 2 X 2Z 2shoulder 1、2、3

elbow wrist 4、5 6 图3 关节坐标系

（3）连杆参数

连杆参数列表如表2所示。

表2 连杆参数

连杆之间的齐次变换矩阵为：

⎥⎥⎥⎥

⎦

⎤⎢⎢⎢

⎢⎣⎡---=----------10

00

0111

1111111i i i i i i i i c d c s c s s s d s c c c s a s c T i i i i i i i i

i i i αααααααα

从而可以确定：

⎥⎥

⎥⎦

⎤⎢⎢⎢⎣⎡-=10000100

000011

11

01c s s c T ⎥⎥

⎥⎥⎦⎤⎢⎢⎢⎢⎣⎡--=1000

000100

00

2222

12

c s s c T ⎥⎥⎥⎦

⎤

⎢⎢

⎢⎣⎡---=100

0010000330332

3c s l s c T ⎥⎥

⎥⎥⎦⎤⎢⎢⎢⎢⎣⎡--=1000

000100

00

4444

3

4c s s c T

⎥⎥⎥⎦

⎤

⎢⎢

⎢⎣⎡---=10

00

0010000551554

5c s l s c T ⎥⎥

⎥⎥⎦⎤⎢⎢⎢⎢⎣⎡--=100

000100

006666

5

6c s s c T

T T T T T T T 5

6453423

120106

= =

[ (((cos(t1)*cos(t2)*cos(t3)+sin(t1)*sin(t3))*cos(t4)+cos(t1)*sin(t2)*sin(t4))*cos(t5)-(-cos(t1)*c os(t2)*sin(t3)+sin(t1)*cos(t3))*sin(t5))*cos(t6)-(-(cos(t1)*cos(t2)*cos(t3)+sin(t1)*sin(t3))*sin(t4)+cos(t1)*sin(t2)*cos(t4))*sin(t6),

-(((cos(t1)*cos(t2)*cos(t3)+sin(t1)*sin(t3))*cos(t4)+cos(t1)*sin(t2)*sin(t4))*cos(t5)-(-cos(t1)*cos (t2)*sin(t3)+sin(t1)*cos(t3))*sin(t5))*sin(t6)-(-(cos(t1)*cos(t2)*cos(t3)+sin(t1)*sin(t3))*sin(t4)+c

os(t1)*sin(t2)*cos(t4))*cos(t6),

-((cos(t1)*cos(t2)*cos(t3)+sin(t1)*sin(t3))*cos(t4)+cos(t1)*sin(t2)*sin(t4))*sin(t5)-(-cos(t1)*cos(t

2)*sin(t3)+sin(t1)*cos(t3))*cos(t5), (-(cos(t1)*cos(t2)*cos(t3)+sin(t1)*sin(t3))*sin(t4)+cos(t1)*sin(t2)*cos(t4))*l1-cos(t1)*sin(t2)*l0] [ (((sin(t1)*cos(t2)*cos(t3)-cos(t1)*sin(t3))*cos(t4)+sin(t1)*sin(t2)*sin(t4))*cos(t5)-(-sin(t1)*co s(t2)*sin(t3)-cos(t1)*cos(t3))*sin(t5))*cos(t6)-(-(sin(t1)*cos(t2)*cos(t3)-cos(t1)*sin(t3))*sin(t4)+s in(t1)*sin(t2)*cos(t4))*sin(t6),

-(((sin(t1)*cos(t2)*cos(t3)-cos(t1)*sin(t3))*cos(t4)+sin(t1)*sin(t2)*sin(t4))*cos(t5)-(-sin(t1)*cos(t 2)*sin(t3)-cos(t1)*cos(t3))*sin(t5))*sin(t6)-(-(sin(t1)*cos(t2)*cos(t3)-cos(t1)*sin(t3))*sin(t4)+sin(

t1)*sin(t2)*cos(t4))*cos(t6), -((sin(t1)*cos(t2)*cos(t3)-cos(t1)*sin(t3))*cos(t4)+sin(t1)*sin(t2)*sin(t4))*sin(t5)-(-sin(t1)*cos(t2

)*sin(t3)-cos(t1)*cos(t3))*cos(t5), (-(sin(t1)*cos(t2)*cos(t3)-cos(t1)*sin(t3))*sin(t4)+sin(t1)*sin(t2)*cos(t4))*l1-sin(t1)*sin(t2)*l0] [((-sin(t2)*cos(t3)*cos(t4)+cos(t2)*sin(t4))*cos(t5)-sin(t2)*sin(t3)*sin(t5))*cos(t6)-(sin(t2)*cos(t

3)*sin(t4)+cos(t2)*cos(t4))*sin(t6), -((-sin(t2)*cos(t3)*cos(t4)+cos(t2)*sin(t4))*cos(t5)-sin(t2)*sin(t3)*sin(t5))*sin(t6)-(sin(t2)*cos(t3

)*sin(t4)+cos(t2)*cos(t4))*cos(t6), -(-sin(t2)*cos(t3)*cos(t4)+cos(t2)*sin(t4))*sin(t5)-sin(t2)*sin(t3)*cos(t5), (sin(t2)*cos(t3)*sin(t4)+cos(t2)*cos(t4))*l1-cos(t2)*l0] [0,0,0,1]

⎥⎥

⎥⎥⎦

⎤⎢⎢⎢⎢⎣⎡-==-1000010000cos sin 00sin cos 1

1

11

1

0110θθ

θθT

T ⎥⎥⎥⎥⎦⎤⎢⎢⎢⎢⎣⎡---==-100000100cos 0sin 0sin 0cos 2

2

2

211221θθ

θθT

T ⎥⎥⎥⎥⎦⎤⎢⎢⎢⎢⎣⎡----==-10000100cos 0sin 0sin 0cos 03

3

3

31

23

32

l T

T θθ

θθ

⎥⎥⎥⎥⎦

⎤⎢⎢⎢⎢⎣⎡---==-10

00100cos 0sin 0sin 0cos 4

4

4

41

3443θθ

θθT

T