机械臂运动学方程

1. 仿人机器人手臂模型

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

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

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

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

表1 关节运动范围

1

2 3 4 5 6 l1 l0 图1 手臂模型 5 180 -180 Z X

O

Y

图2 参考坐标系

关节 最大值 最小值 1 160 -120 2 30 -80 3 180 -180 4 30 -150 6 90 -90 2. 机器人手臂的坐标系建立 ⑴ 参考坐标系

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

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

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

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

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

(2) 关节坐标系 各关节坐标系的建立如图3所示。 X 2

O2 X1 shoulder Y1 1、2、3

Z2

Y2 Z1

O 1

L 0 Z Y 4 3

Z4 O 3 Y 3

O4

4 、 5 elbow X4

X3

Z 5 Y 6

L1

Z6 Y5 O5 O6

6 wrist X6 X 5

图3 关节坐标系

1

（3）连杆参数

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

表2 连杆参数

连杆 1 2 3 4 5 6 ai-1 0 0 0 0 0 0 αi-1 0 90 -90 90 -90 90 di-1 0 0 l0 0 l1 0 关节变量范围 -120～160 -80～30 -180～180 -150～30 -180～180 -90～90

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

cisici1i1iTsisi10从而可以确定：

sicici1cisi100si1ci10ai1disi1

dici11c1s1 0T100c302 3Ts30

c2s100c100 1T02s20100010s20c20s40c40s60c60010 0001010 0001010 0001000s30c30c40001l03 4T00s4010c504 5Ts50

06s50c500100c600l15 6T0s61012345T0T4T5T6T = 1T2T3

[ (((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))*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),

2

-((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)*cos(t2)*sin(t3)-cos(t1)*cos(t3))*sin(t5))*cos(t6)-(-(sin(t1)*cos(t2)*cos(t3)-cos(t1)*sin(t3))*sin(t4)+sin(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(t2)*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]

101T01Tcos1sin100cos2sin200cos3sin300cos4sin400sin1cos1000100100100000 100100 0100 l0100 01sin200sin300sin4002111T2T0cos232TT2310cos3

431T34T0cos4 3

cos50sin505T410cos5045Tsin5010l 10001cos60sin606T5T1sin60cos60560100 0001以“6”为参考，1、2、3三个关节交点“0”的位置由4、5、6三个关节决定，因此有6P32663TP26

px0其中，6Ppy333026已知；P26为T2的第4 p列，即P26zl0116T6T54354T3T p=633TP26(c6c5s4s6c4)l0s6l1(ss)lT6c54c6c40c6l1s4s5l01

6PT266P226pxp22ypz1

PTpl220l12c4l1l01 22222cl0l1pxpypz42la

1l0令utan41u22，则c41u2a，有

u1a1a 1a42arctan1a或2arctan1a41a（4） θ4的范围为 –150----30 由pzs4s5l0得： sp5zl其中s0s440

4

令tan52u，则sin52u1u2，即

2up1u2zl 0s4u22l0s4pu10z ul0s4122pl0s41

zpzl0s42252arctan(p1l0s41)zpzl0s42252arctan(p11)或

zpl0s4

z （5）

52arctan(l0s41pl220s41)zpzθ5的范围为 –180----180 pxc6c5c4l0s6c4l0s6l1 则有：

(pcu2x5c4l0)(2c4l02l0)upxc5c4l00

u(2c4l02l0)(2c4l02l20)4(pxc5c4l0)(pxc5c4l0)2(pxc5c4l0)因此6有两个值： 22222 (c4l0l0)l0(c42c4c5c41)px62arctanp或

xc5c4l0l22222 0(c42c4c5c41)px62arctan(c4l0l0)pxc5c4l

0二、手腕方位的反解

1、2、3 决定手腕的方位。

nxoxaxpxnyoyaypynzozap3212T1T0T zz0001 5

c1c2c3s1s3s1c2c3c1s3s2c30 321c1c2s3s1c3s1c2s3c1c3s2s302T1T0Tc1s2s1s2c2l 00001

ayatan3 当sin20时

x 3atan2(ay,ax)

ozntan1 z 1atan2(oz,nz)

tan2ozasin

z12atan2(oz,azsin1)

6