用平面二连杆机器人为例贯穿运动学、雅可比、动力学、轨迹规划甚至控制与编程分析

1、一、平面二连杆机器人手臂运动学平面二连杆机械手臂如图1所示，连卞f1长度li,连日f2长度12。建立如图1所示的坐标系，其中，（Xo，y0）为基础坐标系，固定在基座上，（Xi,yi）、（X2,y2）为连体坐标系，分别固结在连杆1和连杆2上并随它们一起运动。关节角顺时针为负逆时针为正。yopB2CXo图1平面双连杆机器人示意图1、用简单的平面几何关系建立运动学方程连杆2末段与中线交点处一点P在基础坐标系中的位置坐标：xp=11cos%12cos（吊B2）（1）yp=11sin12sin（%朋）2、用D-H方法建立运动学方程假定Zo、z1、Z2垂直于纸面向里。从（Xo,yo,Zo）到（X1,y1，

2、乙）的齐次旋转变换矩阵为:cos日-sin1OOosin4cos91OO1T=（2）O010_0001_从（Xi,y1，乙）到（X2,y2,Z2）的齐次旋转变换矩阵为:cos-2-sin%O111sin日2cos*OO2T=（3）0010-00011从（Xo，yo,2。）到（X2,y2,z2）的齐次旋转变换矩阵为:2T=：T2T=cossin0I0-sin力cosi0000i00cos与sinu200cos。12)sin(01+%)0I0-sin(由队)cosQ12)0000i0-sin%cosu2001icos1isind0i00i0lil00i(4)那么，连杆2末段与中线交点处一点P在基础

3、坐标系中的位置矢量为:0P=0T2P=cosR12)sin(q+82)I0I0-sinQ12)cos(二2)0000i011c0slil21isini0i0|0即,11c0sli12cos(即1isin6i+12sin(0iI0Ii叫)十%)-xp!(5)xp二11cos%12cos(i%)yp=1isiniLsinCiZ)(6)与用简单的平面几何关系建立运动学方程(i)相同。建立以上运动学方程后，若已知个连杆的关节角乙、%,就可以用运动学方程求出机械手臂末端位置坐标，这可以用于运动学仿真。3、平面二连杆机器人手臂逆运动学可以用运动学方程求出机械手臂建立以上运动学方程后，若已知个机械臂的末端位

4、置,二连杆的关节角4、02,这叫机械臂的逆运动学。逆运动学可以用于对机械臂关节角和末端位置的控制。对于本例中平面二连杆机械臂，其逆运动学方程的建立就是已知末端位置(xp,yp)求相应关节角4、%的过程。推倒如下。(i)问题xp=1icos912cos(9B2)yp=1isin%LsinGi%)已知末端位置坐标(xp,yp),求关节角日仆可。pp由(6)式得到:(Xp-licos)2(ypliSin%)2=l；整理得到:22XpVpli,2一l2=2l1(xp0081ypSin)Xp,=tgiVp由(8)式得到:sin1Pp00slpp2xp222ypl1-122liXp(cos/cos入sin

5、sin")pp，cos?p2XP222yp1-122liXpcosi-7)cos1pp(8)(9)(10)由此可解出仇。Xp+yp+li2-l2=arccos2liXpcos3卜arctg"Xp(ii)(3)求-2由(6)式得到：Xp-12cos口z)2yp-l2sinCi矗)2f2整理得到：Xpypl2-li2=2l2XpcosQiz)ypsinQiz)Xpyp=tg%sin%cospp(I3)(I4)由(I4)式得到：Xpypl2-li2cosOi2)cospsinfiZTin%cos-pp2l2Xpcos-pp(I5)由此可解出日2。cos(玛121p)2=arcco

6、s212xpypcosBp+arctg四xp二、平面二连杆机器人手臂的速度雅可比矩阵速度雅可比矩阵的定义：从关节速度向末端操作速度的线性变换。人为例推导速度雅可比矩阵。(16)现已二连杆平面机器xp=11c0sll2cos12)yp=l1sin112sin(%)上面的运动学方程两边对时间求导，得到下面的速度表达式:dXp"dTdyp"dT-l1sin。一12sin(x12)(丁-2)=l1cos1z12cos(f肌)G，)把上式写成如下的矩阵形式：Xp.|-11sin入一12sin(时？2)l2sin(“)1%Lp111cos“+l2cos(4+82)12cos(&

7、+%)1e2令上式中的末端位置速度矢量:XpXX*p一(17)(18)关节角速度矢量I1|一IisinF-12sin(F丁2)矩阵|IicosN12cos。z)一I2sin(F2)12cos1，)J0i2)J(4,4)就是速度雅可比矩阵，实现从关节角速度向末端位置速度的转变。(18)式可以写成：X=J(Ui2)。速度雅可比矩阵可以进一步写成：、厂.、lsin%LsinQ队)IzsinQ%)JCjF)二.Iicos-1I2cos(-1-2)I2cosp1-2)一-(19)1 .J11J12=I21J221其中，xpJ11=-11sin一l2s1ndi12)“1XpJ12=12sin(i'

8、-2)"2J21yp(20)J22"1-:Yp2=11cosj12cos(f二12cosQ12)%)('cXpEXp_J11J12e9J)=11=12J21J22gypQp1曾石。21、平面二连杆机器人手臂的动力学方程由此可知雅可比矩阵的定义:)推倒动力学方程的方法很多，各有优缺点。拉格朗日方法思路清晰、不考虑连杆之间的内力，是推倒动力学方程的常用方法。下面推导图1所示的平面双连杆机器人的动力学方程。图1中所示连杆均为均质杆，其转动惯量分别是I1和|21、求两连杆的拉格朗日函数(1)求系统总动能连杆1的动能为：凸与出2”；=111汨12236(21)求连杆2质心D处

9、的线速度：对连杆2质心位置求导得到其线速度。连杆2质心位置为：XD1二11cos12cosg22)21二11sin912sin(1-匕)2(22)连杆2质心速度为:Xd-11sin1-112sin(1%)(丁，)2(23)Yd=11cos111112cos(-1-2)(-1-2)2222212212T2/12Vd=XdyDu(11-121112cos2p1二1212(二121112cos12尸尸2442(24)连杆2的动能:1 .212K2=ID(内肌)m2VD2 21 ,1.22121.221.2.2,1.2(m2l2)(1u2)m2Klil21112cos12all22(l21112cos

10、r2Hl22 1224421 21.221,221,2,2=-m2(l1l21112cos冬)。m2l212m2(121112coslAll2 3623(25)系统总动能：K=K1K21,.21.221.2.21,2.2=-m2(11-121112cos引-m212；,5m2(§121112cosi2"f21 .21.21.2121.22,1.21二(m211m111m212m21112cos%)m21；u2(m212m21112cos明)、22 662632(26)(2)求系统总势能系统总势能为：1.1.P=m1glisin%m2g(11sin1-12sin(%)(3)求

11、拉格朗日函数(27)L=K-P二(1m21；1ml1；26121,212.2121m212m21112cos12)%m212%(m212m21112cos%)中2626321-m1g11sin-m2g11sina112sin(1r)2(28)(4)列写动力学方程按照拉格朗日方程，对应关节1、2的驱动力矩分别为:.L：L-1二-ac口cQ:-1:-1.L.L2二-32工(29)-L21,21,2-二(m211二mJ-m212.可33.121m21112cos2尸1(-m212-m21112cos2尸232L21,21,21,21=(m211m111m212m21112cos内(m212m2111

12、2cosi2)f23tl333212m21112sin-21-2m21112sin22:L2-1=(m2112,1、,1,、-(mim2)gl1cos1m2g12c0sqi12)221,21,2一、J,21,.、.m111m212m21112cos%)z(m212m21112cos12)%3332.12,1、，,1,、m21112sin12n12m21112sinn(m1m2)g11cosZm2g12cos(112)222（30）同理：:L12,121=-m21212(m212m21112cos2)332；:L121.12.1=(m212,m21112cos%)%m21212-m21112si

13、n121%t.33232:L1.21.1二一-m21112sin%m21112sin2耍2m2g12cos(川口2)f2222,1,21一.、.1,2.1一.21,八.、2=(-m212m21112cosi2)Zm212u2-m21112sin%m2g12cos(-1二2)32322(31)联合（30）、（31）式，将动力学方程写成如下矩阵形式:m12工讪=3211,21.2.m111m212m21112cos12331,21,m212m21112cos大321m21；31,+m21112cos%21.2二m2123112话Li,.Hm21112sin2.01.-m21112sin%2-m21

14、112sin021 11(-m1+m2)g11cos日1+一m2g12cos。+%)2 21m2g12cos(81+4).21(32)四、平面二连杆机器人手臂的轨迹规划轨迹规划就是已知起点和终点的位置速度加速度等参数确定中间点的相应参数的过程。轨迹规划是机器人完成规定任务所必需的。它分为关节空间的轨迹规划和直角坐标空间的轨迹规划、以及基于动力学的轨迹规划等几种类型。关节空间的轨迹规划就是已知某连杆起点和终点的角位置角速度角加速度等参数确定中间点的相应参数的过程。如图所示，一两自由度机械手，已知两连杆起点和终点的关节角,确定中间位置的关节角。（1）非归一化和归一化问题（2）末端位置的轨迹、关节空

15、间轨迹规划的缺点。脚自由度机器人美审空间的非旧化注动两自由度机器人关节空间的归一化运动三次多项式轨迹规划要求：机器人上某关节在运动开始时刻片的角度为鸟，在时刻口运动到角度%.用三次多项式规划该关节的运动*优分=%+卬+7产+夕初始条件与末端条件;目=4.加）=6'事&力=%»遍=0*确定系数.勺J,以确定我已启涝（0的表达式a卡举例：要求一个5轴机器人的第一关节在5秒之内从初始角3。度运动到终端角75度，用三次多项式计算在第1、2、3、4秒时的关节角。矶L）=c0=30ftfl-30叩4二。+蝇5）+工小）+0）-75-g产。前士）C00工=5-4国力皿4+2cM5）

16、+女6）=01口=S由此得到位置速度和加速度的多项式方程如下：岫=30+5Aa-072?财=10L&-2,16?fl（/）=10,fi-432/代人时间求得：刈1）=34.68*&=必网”网3）=5916D矶%=7也32"图5.1U例5.1中的关节位置、速度和加速度五次多项式轨迹规划已知机器人上某关节在运动开始时亥以=0的角度为月、速度&、加速度在时刻运动到庭I,度斗、速度为，八加速度为,。要求用五次多项式规划该关节的运动口8（，）=c.+cl+g/2+cjP+a/"+c#U1Z54+J初始条件与末端条件±vs（G=&、改右）=。、

17、我幻=孱/innnai日&）二斗£日7）-0=&/+-1确定系数/、”与、今cPc5?以确定日（t）、3g前t）的表达式口M同例5.1,且巳知初始加速度和末端减速度均为5度/秒2c解：由例5和给出的加速度值得到：心=3（re-o度/秒a=5度/秒2%=75"&=0度/秒%=7度/杪2将初始和末端边界条件代入式（5.5）式（5.7）,得出：5=30C=0Cy-25Cj=1.6g=-0-58c5=0.0464进而得到如下运动方程：ff（t）=30+2.5t2+1.6户-0.5S?+0,0464/期-5r+4.8户-232-+0.232/4=5+9&

18、;-6.96r1+5928.图5.12是机器人关节的位置，速度和加速度曲线，其最大加速度为&7度/秒，图5.12例5.3的关节位置，速度和加速度抛物线过渡的线性运动轨迹规划(略)具有中间点以及用抛物线过渡的线性运动轨迹规划(略)高次多项式运动轨迹规划(略)直角坐标空间的轨迹规划(1)所有用于关节空间的轨迹规划方法都可以用于直角坐标空间轨迹规划；(2)直角坐标轨迹规划必须不断进行逆运动学运算，以便及时得到关节角。这个过程可以归纳为以下计算循环：(a)将时间增加一个增量；(b)利用所选择的轨迹函数计算出手的位姿；(c)利用逆运动学方程计算相应的关节变量；(d)将关节变量信息送给控制器；(e)返回到循环的开始。图57具有加速和感速段的轨迹规划例5.6一个两自由度平面机器人要求从起点（3,0）沿直线运动到终点（8,14）0假谀蓝径分为10段，求出机器人的关节变量每一根连杆的长度为9英寸，解：直角坐标空间中起点和终点间的直线可以描述为:或者y=O.8bc+7.6中间点的坐标可以通过将起点和终点的工、y坐标之差简单地加以分割得到，然后通过求解逆运动学方程得到对应每个中间点的两个关节的©结果见表5和图547口表5.1例5.6的坐