用平面三连杆机器人为例贯穿运动学、雅可比、动力学、轨迹规划甚

1、一、平面二连杆机器人手臂运动学平面二连杆机械手臂如图1所示，连杆1长度l1，连杆2长度l2，连杆3长度为l3。建立如图1所示的坐标系，其中，(x0,y0)为基础坐标系，固定在基座上，(x1,y1)、(x2,y2)、(x3,y3)为连体坐标系，分别固结在连杆1、连杆2、连杆3上并随它们一起运动。关节角顺时针为负逆时针为正。1图1平面双连杆机器人示意图1、用简单的平面几何关系建立运动学方程连杆2末段与中线交点处一点P在基础坐标系中的位置坐标： xp=l1cos1+l2cos(1+2)+l3cos(1+2+3)yp=l1sin1+l2sin(1+2)+l3sin(1+2+3)（1）2、用D-H方法建

2、立运动学方程假定z0、z1、z2垂直于纸面向外。从(x0,y0,z0)到(x1,y1,z1)的齐次旋转变换矩阵为：cos1-sin1000sin1cos1001T=0010 （2） 0001从(x1,y1,z1)到(x2,y2,z2)的齐次旋转变换矩阵为：cos2-sin20l11sin2cos2002T=0010 （3） 0001从(x2,y2,z2)到(x3,y3,z3)的齐次旋转变换矩阵为：cos3-sin30l21sincos3002T=3010（3） 00001从(x0,y0,z0)到(x3,y3,z3)的齐次旋转变换矩阵为：cos1-sin100cos2-sin20l1cos3-s

3、in30l00cos2003T=1T122T3T=sin1cos100sin22cossin330000100010000001000110 0001cos(1+2+3)-sin(1+2+3)0l1cos1+l2cos(1+2)=sin(1+2+3)cos(1+2+3)0l1sin1+l2sin(1+2)01000001那么，连杆2末段与中线交点处一点P在基础坐标系中的位置矢量为：cos(1+2+3)-sin(1+2+3)0l1cos1+l2cos(1+2)l30P=03sin(+)cos(+2+3)0l1sin1+l2sin(1+2)3TP=123100010000011l1cos1+l2c

4、os(1+2)+l3cos(1+2+3)xp=l1sin+lsin(+2)+l3sin(1+2+3)y121=p0zp11即，xp=l1cos1+l2cos(1+2)+l3cos(1+2+3)y p=l1sin1+l2sin(1+2)+l3sin(1+2+3)结论：（6）与用简单的平面几何关系建立运动学方程（1）相同。4）（5） （6） （补充：正解用于仿真，逆解用于控制建立以上运动学方程后，若已知个连杆的关节角1、2、3，就可以用运动学方程求出机械手臂末端位置坐标，这可以用于运动学仿真。3、平面二连杆机器人手臂逆运动学二、平面二连杆机器人手臂的速度雅可比矩阵速度雅可比矩阵的定义：从关节速度向

5、末端操作速度的线性变换。现已二连杆平面机器人为例推导速度雅可比矩阵。xp=1lcos1+l2cos(1+yp=1lsin1+l2sin(1+22)l+3)l+3co+s+1(2 sin+1(23)上面的运动学方程两边对时间求导，得到下面的速度表达式：dxpdtdypdt -lsin(+)( + )-lsin(+)( + + )=-l1sin11212123123123 +lcos(+)( + )+lcos(+)( + + )=l1cos11212122123123（17）把上式写成如下的矩阵形式： p-l1sin1-l2sin(1+2)-l2sin(1+2)x1= （18） y lcos+lc

6、os(+)lcos(+)12122122p1px ， 令上式中的末端位置速度矢量=X py ， 关节角速度矢量1= 2矩阵-l1sin1-l2sin(1+2)-l2sin(1+2)=J(1,2) l1cos1+l2cos(1+2)l2cos(1+2)（18）式可J(1,2)就是速度雅可比矩阵，实现从关节角速度向末端位置速度的转变。以写成：=J(,) X12速度雅可比矩阵可以进一步写成：-l1sin1-l2sin(1+2)-l2sin(1+2)J(1,2)=l1cos1+l2cos(1+2)l2cos(1+2) （19） JJ12=11J21J22其中，J11=J12=J21=J22=xp1xp

7、2yp1yp2=-l1sin1-l2sin(1+2)=-l2sin(1+2) （20） =l1cos1+l2cos(1+2)=l2cos(1+2)由此可知雅可比矩阵的定义：J11J(1,2)=J21xpJ121=J22yp1xp2 （21） yp2三、平面二连杆机器人手臂的动力学方程推倒动力学方程的方法很多，各有优缺点。拉格朗日方法思路清晰、不考虑连杆之间的内力，是推倒动力学方程的常用方法。下面推导图1所示的平面双连杆机器人的动力学方程。图1中所示连杆均为均质杆，其转动惯量分别是I1和I2。1、求两连杆的拉格朗日函数（1）求系统总动能连杆1的动能为：K1=1 2IA12 （21） 111 2=

8、ml2 2=(m1l12)1111236求连杆2质心D处的线速度：对连杆2质心位置求导得到其线速度。连杆2质心位置为：1xD=l1cos1+l2cos(1+2)2 （22） 1yD=l1sin1+l2sin(1+2)2连杆2质心速度为：-1lsin(+)( + ) D=-l1sin1x1212122 （23） 1 +lcos(+)( + ) =lcosYD1112121221212222 2+1l2 2 D DVD=x+y=(l12+l2+l1l2cos2)122+(l2+l1l2cos2)12 442（24）连杆2的动能：1 + )2+1mV2ID(122D22112 + )2+1m(l2+

9、1l2+llcos) 2+1l2 2+(1l2+llcos) =(m2l2)(122121221222122122122442112 2+1ml2 2+1m(2l2+llcos) =m2(l12+l2+l1l2cos2)1222221221223623K2=(25)系统总动能：K=K1+K2112 2+1ml2 2+1m(2l2+llcos) m2(l12+l2+l1l2cos2)122222122122362311112 2+1ml2 2+(1ml2+1mllcos) =(m2l12+m1l12+m2l2+m2l1l2cos2)1222222122122662632=（26）（2）求系统总势

10、能系统总势能为：11P=m1gl1sin1+m2g(l1sin1+l2sin(1+2) （27） 22（3）求拉格朗日函数L=K-P11112 2+1ml2 2+(1ml2+1mllcos) =(m2l12+m1l12+m2l2+m2l1l2cos2)122222212212266263211-m1gl1sin1-m2gl1sin1+l2sin(1+2)22（28）（4）列写动力学方程按照拉格朗日方程，对应关节1、2的驱动力矩分别为：1=LL-t11LL-2t2 （29） 2=L112 +(1ml2+1mllco =(m2l12+m1l12+m2l2+m2l1l2cos2)s2)1222122

11、33321L112 +(1ml2+1mllcos) =(m2l12+m1l12+m2l2+m2l1l2cos2)12221222t33321 -1mllsin 2-m2l1l2sin212212222L11=-(m+m)glcos-m2gl2cos(1+2) 1211122+(ml2+mllcos) 1=(m2l12+m1l12+m2l22+m2l1l2cos2)12221222 -1mllsin 2+(1m+m)glcos+1mglcos(+)-m2l1l2sin2122122212112212222（30）同理：13131312L1112 2 =m2l22+(m2l2+m2l1l2cos2

12、)13223L112 +1ml2 1mllsin =(m2l2+m2l1l2cos2)1222-212212t32322L1 2-1mllsin -1mglcos(+) =-m2l1l2sin2121221222122222+ml2 2+mglcos(+)2=(m2l22+m2l1l2cos2)m2l1l2sin21222+122121312131212（31）联合（30）、（31）式，将动力学方程写成如下矩阵形式：11112222ml+ml+ml+mllcosml+mllcos 2122222122121311322132= 1112222m2l2+m2l1l2cos2m2l232310-m

13、llsin 2212212 +2 1m2l1l2sin220211(m+m)glcos+mglcos(+) 12112212-m2l1l2sin201222+ 10012m2gl2cos(1+2)2（32）四、平面二连杆机器人手臂的轨迹规划轨迹规划就是已知起点和终点的位置速度加速度等参数确定中间点的相应参数的过程。轨迹规划是机器人完成规定任务所必需的。它分为关节空间的轨迹规划和直角坐标空间的轨迹规划、以及基于动力学的轨迹规划等几种类型。关节空间的轨迹规划就是已知某连杆起点和终点的角位置角速度角加速度等参数确定中间点的相应参数的过程。如图所示，一两自由度机械手，已知两连杆起点和终点的关节角，确定中间位置的关节角。（1）非归一化和归一化问题（2）末端位置的轨迹、关节空间轨迹规划的缺点。三次多项式轨迹规划举例：要求一个轴机器人的第一关节在秒之内从初始角度运动到终端角度，用三次多项式计算在第、秒时的关节角。五次多项式轨迹规划抛物线过渡的线性运动轨迹规划（略）具有中间点以及用抛物线过渡的线性