机械手臂的运动学公式推导 y3y1y0y2x2x1x0x3
图1 3关节平面机械臂
1. 3关节平面机械臂
? 3关节平面机械臂有3个自由度，关节1有1个自由度，关节2有1个
自由度，关节3有1个自由度 ? 机器人手臂的几何尺寸（mm）：
关节1长度：L1 关节2长度：L2 关节3长度：L3
? 关节的运动范围（右手）：如表1所示。
表1 关节运动范围 关节 1 2 最大值 ΘMax1 ΘMax2 最小值 ΘMin1 ΘMin2 2. 机器人手臂的坐标系建立
参考坐标系
3 ΘMax3 ΘMin3 (1) 为了对3关节平面机械臂进行控制，同时也便于描述机器人的动作状态，必须建立适当的初始坐标系。我们设定机械臂的初始姿态：关节1、关节2和关节3均处于水平姿态，与世界坐标系（x0,y0）的x0轴的夹角为0度。
参考坐标系（实验室坐标系）的设定如图1所示: X轴：从关节i到关节i+1的方向定义为X轴，即沿连杆方向 y轴：根据X轴和Z轴的方向，以右手螺旋法则确定 Z轴：沿关节轴方向，即垂直纸面，从里向外为Z轴正方向
1
(2) 连杆参数
连杆参数列表如表2所示。
表2 连杆参数
连杆i 1 2 3 ? 正解：
连杆之间的齐次变换矩阵为：
ai-1 0 L1 L2 αi-1 di 0 0 0 Θi Θ1 Θ2 Θ3 关节变量范围 ΘMin1～ΘMax1 ΘMin2～ΘMax2 ΘMin3～ΘMax3 0 0 0 ?c1?s10T?1?0?0??c2?s21?T?2?0??0?c3?s32?T?3?0??0?c1c2c3?s1s2c3?c1s2s3?s1c2s3?0012?s1c2c3?c1s2c3?c1c2s3?s1s2s3T?TTT?3123?0?0??s1c100?s2c200?s3c30000100?0?0? 1??0L1?00??10?
?01?0L2?00??10?
?01??(s1c2c3?c1s2c3?c1c2s3?s1s2s3)0L1c1?L2?c1c2?s1s2???c1c2c3?s1s2c3?c1s2s3?s1c2s30L1s1?L2?s1c2?c1s2??(1)
?010?001??c123?s1230012?T?TTT?3123?0??0?s1230L1c1?L2c12?c1230L1s1?L2s12??? (2) 010?001? 2
?c??s?0012?T?TTT?3123?0??0其中
?s?c?000x?0y??10? (3)
?01?c1: cosΘ1 (4) c2: cosΘ2 (5) s1: sinΘ1 (6) s1: sinΘ1 (7)
c??c123 (8) s??s123 (9) x?L1c1?L2c12 (10) y?L1s1?L2s12 (11)
式(1)为3关节平面机械臂的变换矩阵，式(2)为采用三角函数和差角公式化简得到的，式(3)为式(2)的简化表示，式(4)- 式(11)为简化符号的详细表示。
? 反解：
几何解： y0yβψxx0
图2 3关节平面机械臂的平面几何关系
图2给出了3关节平面机械臂的几何关系，可以看出，在世界坐标系（x0,y0）
3
下,由连杆L1,L2以及关节1和关节3的连线构成三角形。图中虚线所示为构成三角形的另一种情况，对于实线构成的三角形，采用余弦定理可得
x2?y2?L12?L22?2L1L2cos(180??2) (12)
由于cos(180+Θ2)=-cosΘ2,所以
x2?y2?L12?L22c2?2L1L2 (13)
三角形成立的条件为2边之和大于第三边，因此L1+L2必须大于
x2?y2。可利用上式检验反解是否存在，当上述条件不成立时，反
解不存在。当反解存在时，即可由(13)式得出Θ2的值。
为求Θ1，可先求出β和ψ，根据三角函数与三角形各边的关系，应用2幅角反正切公式得：
??Atan2(y,x) (14)
再利用余弦定理求出ψ
cos??x2?y2?L12?L222L1x?y22 (15)
?1???? (16)
式中，+-号根据Θ2的符号取，当Θ2<0，取正号，反之取负号。 平面内旋转角度可加和，因此3个连杆的旋转角度之和即为末端连杆的姿态，也即机械臂末端的姿态。
?1??2??3?? (17)
由以上式(1) – 式(17)，可反解出所有连杆在世界坐标系的变换矩阵，即姿态。
4