机械臂运动学方程

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

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

O 1 2 shoulder X 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 di-1 0 0 l0 0 l1 0 关节变量范围 -120～160 -80～30 -180～180 -150～30 -180～180 -90～90 0 90 -90 90 -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?s0 1T??10?0??s1c1000010?c20??00? 1?2T??0?s2?1???0?c400??01l0?3 4T???00?s4??01??00100?s20c20?s40c40?s60c600?1000?1000?1000?0?? 0??1?0?0?? 0??1?0?0?? 0??1??c3?02 3T???s?3?0

?s30?c30?c5?04 5T???s?5?0

06?s50?c50?c60??0l1?5 6T???0?s6??1??012345T?0T3T4T5T6T = 1T2

[ (((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?0T1?cos?1??sin?1???0??0?cos?2??sin?2???0??0?cos?3??sin?3???0??0?cos?4??sin?4???0??0sin?100?cos?100?? 010??001?0?sin?20?cos?210000?sin?30?cos?310000?sin?40?cos?410000?0?? 0??1?0?0?? ?l0??1?0?0?? 0??1?211?1T?2T32?1T?23T

43?1T?34T 3

??cos?50?sin?50?5T?4?10?cos?50?45T???sin?5??010?l? 1??0001????cos?60?sin?60?6T?5?1???sin?60?cos?60?56T??0100?? ?0001??以“6”为参考，1、2、3三个关节交点“0”的位置由4、5、6三个关节决定，因此有

6P6326?3TP26

??px??0?其中，Pp??0?6y33326????已知；P26为T2的第? ?p4列，即P26??z???l0??1????1??63T?655T4T43T 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?42，则c1?u24?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