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

机械手臂的运动学公式推导图13关节平面机械臂1. 3关节平面机械臂3关节平面机械臂有3个自由度，关节1有1个自由度，关节2有1个 自由度，关节3有1个自由度机器人手臂的几何尺寸（mm）:关节1长度：L1关节2长度：L2关节3长度：L3关节的运动范围（右手）：如表1所示。表1关节运动范围关节123最大值Max10Max20Max3最小值Min1Min2Min32.机器人手臂的坐标系建立参考坐标系（1）为了对3关节平面机械臂进行控制，同时也便于描述机器人的动作状态， 必须建立适当的初始坐标系。我们设定机械臂的初始姿态：关节1、关节2和关节3均处于水平姿态，与世界坐标系（xo,yo）的xo轴的夹角为0度。参考坐标系（实验室坐标系）的设定如图 1所示：X轴：从关节i到关节i+1的方向定义为X轴，即沿连杆方向y轴：根据X轴和Z轴的方向，以右手螺旋法则确定Z轴：沿关节轴方向，即垂直纸面，从里向外为Z轴正方向(2)连杆参数连杆参数列表如表2所示C1-S1000T =lS00!丨AI 010001 一C21_S20LJ1；T =IS21C2001101010.0001 一C3_S30呵2t =IS3C30031I00100001 一(si C2C3 C1S2C3 C1C2S _ S1S2S3)qC2C3 1 S1S2C3 -sqq 1 qC2S300C1C2C_ SIS2C_C1S2S _ S1C2S3SIC2C*C1S2C*C1C2S3 _ S1S2S30I00010L1c +Lc1cs1s2 )Ll5 + L2 ( sc2 +5% )01(1)C123S1230I -0_S123L1C1C123L2c12L2SI2表2连杆参数连杆iai-1a i-1diOi关节变量范围1000010Min1 Max12L100020Min2 0Max23L200O3OMin3 0Max3正解：连杆之间的齐次变换矩阵为:C0xl10yl1 01010.0001 一其中C1： cos(5)(6)c2： cos2s1: sinGs1: sinG(7)C - C123(8)s = si23(9)x = L)C| L2q2(10) Lisi L2s12(11)式(1)为3关节平面机械臂的变换矩阵，式(2)为采用三角函数和差角公式化 简得到的，式(3)为式的简化表示，式(4)-式(11)为简化符号的详细表示。反解：几何解：*y0图2 3关节平面机械臂的平面几何关系图2给出了 3关节平面机械臂的几何关系，可以看出，在世界坐标系(xo,yo)下,由连杆Li,L2以及关节1和关节3的连线构成三角形。图中虚线所示为构成三 角形的另一种情况，对于实线构成的三角形，采用余弦定理可得X2 y2 = L22 -2IhL2COS(180 二2)(12)由于 cos(180+62)=-cos62,所以L12 L2：2L丄2(13)5三角形成立的条件为 2边之和大于第三边，因此L1+L2必须大于,x2y2。可利用上式检验反解是否存在，当上述条件不成立时，反 解不存在。当反解存在时，即可由(13)式得出G2的值。为求，可先求出B和収根据三角函数与三角形各边的关系，(14)(15)(16)应用2幅角反正切公式得：:=Ata n 2( y, x)cos?=X2 y2 _L12 _L222L1 ,x2 y2再利用余弦定理求出式中，+-号根据G2的符号取，当620,取正号，反之取负号。平面内旋转角度可加和，因此3个连杆的旋转角度之和即为末端连杆 的姿态，也即机械臂末端的姿态。玉八(17)由以上式(1)-式(17)，可反解出所有连杆在世界坐标系的变换矩阵，即姿态。#