3关节平面机械臂运动学方程.doc

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