机械臂运动学方程.docVIP

机械手臂的运动学公式推导 仿人机器人手臂模型 仿人机器人的手臂有6个自由度，肩部（shoulder）3个，肘部（elbow）2个，腕部（wrist）1个，如图1所示。 机器人手臂的几何尺寸（mm）： 上臂长度：216 小臂长度：173.5 关节的运动范围（右手）：如表1所示。 表1 关节运动范围 关节 1 2 3 4 5 6 最大值 160 30 180 30 180 90 最小值 -120 -80 -180 -150 -180 -90 机器人手臂的坐标系建立 ⑴ 参考坐标系 为了对仿人机器人进行控制，同时也便于描述机器人的动作状态，必须建立适当的初始坐标系。我们设定机器人手臂的初始姿态：大臂从肩垂直向下，小臂向前平伸，与大臂成。 参考坐标系（实验室坐标系）的设定以机器人本身的初始位置与实验室坐标系相一致的原则设定，如图2所示。 X轴：以机器人初始（状态）位置的右侧方向作为实验室坐标系的X轴； y轴：设定y轴使其为右手系坐标系，即正前方为y轴正向。 Z轴：以机器人初始（状态）位置的上方向作为实验室坐标系的Z轴；按D-H坐标建立的方法，各个关节的轴线与各关节坐标系的Z轴共线. (2) 关节坐标系 各关节坐标系的建立如图3所示。 （3）连杆参数 连杆参数列表如表2所示。 表2 连杆参数 连杆 ai-1 αi-1 di-1 关节变量范围 1 0 0 0 -120～160 2 0 90 0 -80～30 3 0 -90 l0 -180～180 4 0 90 0 -150～30 5 0 -90 l1 -180～180 6 0 90 0 -90～90 连杆之间的齐次变换矩阵为： 从而可以确定： = [ (((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)+cos(t1)*sin(t2)*cos(t4))*cos(t6),