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

1、、平面二连杆机器人手臂运动学平面二连杆机械手臂如图 1所示，连杆1长度li ,连杆2长度12 ,连杆3长度为13。建 立如图1所示的坐标系，其中，(xo，yo)为基础坐标系，固定在基座上，(xi, yi)、(x2,y2)、 (x3,y3)为连体坐标系，分别固结在连杆 1、连杆2、连杆3上并随它们一起运动。 关节角顺 时针为负逆时针为正。图1平面双连杆机器人示意图1、用简单的平面几何关系建立运动学方程连杆2末段与中线交点处一点p在基础坐标系中的位置坐标：xp =l1cos9 l 2 cos(4 -2)+1 3cos&-2 -3)yp = i1 sin 4 l 2 sing 丁2)+1 3

2、sin(1 "2 %)(1)2、用d-h方法建立运动学方程假定z0、z1、z2垂直于纸面向外。从(% ,y°, zo)到(为，y1，4)的齐次旋转变换矩阵为:80t =cos 仇-sin0100sin 01cos61001 0010一 0001 -(2)从（xi, y，z1）到（x2 ,丫22）的齐次旋转变换矩阵为:2t =cos% sin % i 0i 0一sin 二2cos%000010lj001(3)从（x2, y2, z2）至u （&，y3, z3 ）的齐次旋转变换矩阵为:cos：3-sin% 0 l22t =sin飞0cos飞0 001 0(3)cos &

3、amp;-sin 6 0 0 1cosq-sin 80 i1cos63 -sin 630 l23t =0t 2t .3t =sin 6|cos 20 0fsin 62cos®0 0rsin 2cos&0 0001 0001 0001 0000 1000 1000 1cos(日地十 -sin(日+6+e3) 0 11cosh+l2 cos(日 +2)sin(6|+62+&)cos(h 坳怕)0 i1 sin日+l2sin(6| 地)00100001从（x。，yo, z°）到（x3, y3, z3）的齐次旋转变换矩阵为:那么，连杆2末段与中线交点处一点 p在基础

4、坐标系中的位置矢量为:i1 cosq +l2cos(g +62)1 i3(4)0p = t 3p =即,cos(a+8)sin(8 +a +2)0-sin(口 口2)cosgi2%)00001011sin &+l2 sin(8 1+62)0(5)i1 cos。+i2 cos© +82) +lcos佃+a)i1 sin 十l2 sin( 十)十l3sin(& +w +63)0i1一 jvp zpj 一xp =i1cosi1 i 2 cosc1 12)+l 3 cos(11 12 %) yp=l1sinw jsing 4)+l3sin(n 4 /)(6)结论：（6）与用简

5、单的平面几何关系建立运动学方程（1）相同。补充：正解用于仿真，逆解用于控制建立以上运动学方程后，若已知个连杆的关节角仇、仇、仇，就可以用运动学方程求出机械手臂末端位置坐标，这可以用于运动学仿真。3、平面二连杆机器人手臂逆运动学二、平面二连杆机器人手臂的速度雅可比矩阵速度雅可比矩阵的定义： 从关节速度向末端操作速度的线性变换。现已二连杆平面机器人为例推导速度雅可比矩阵。xp = 1 cos1 + l 2 cos（+0 2 ） +3 c601（8 才 6）yp = 1si n1 + l 2 si n（i+e 2）+3 si it（9 2 0 3 ）上面的运动学方程两边对时间求导，得到下面的速度表达

6、式："p- -1 sin j -l2sin（口 4） m4）-l3sin（口力%）（% 42&3）dtdy 二11 cos工 t 12cos（1 ） （斗 为）12cos 12 13）什1 +2 也）dt（17）把上式写成如下的矩阵形式：(18)xp| - l1sin % - lsinq%） -l2sin（z） %yp111cos81+l2 cos（31 +优）12 cos圾+%） mxp令上式中的末端位置速度矢量.p = x ,:ypi电j-关节角速度矢量|，=,-l1sin11 -l2sin（1、）-12sin（% z）矩阵=jg1,?2）iu1 cos 12cos011

7、2）12 cosp1 z）j（1,0）就是速度雅可比矩阵，实现从关节角速度向末端位置速度的转变。（18）式可以写成：x =j（"2）。速度雅可比矩阵可以进一步写成：-11sin %-12sin（l %） -心冶性汽）jg1,三）：'cos-112cos（超- %）12cos（禺 12）j1111（19）j12j21j 22 1其中，cxpj11 - -li sin u -12 sinci ： ： 2)"-1xpj12 = = -12 sin( -1-2)(20)"2npj21 '=l1cosf12 cosg%), 孤j 22 = = 12 cos。

8、：“ *2):一力1cxpcxp 1 j11 j12j,%)=1 卜j21 j 22124yp8ypi，明1 戊与由此可知雅可比矩阵的定义:)三、平面二连杆机器人手臂的动力学方程推倒动力学方程的方法很多，各有优缺点。拉格朗日方法思路清晰、不考虑连杆之间的 内力，是推倒动力学方程的常用方法。 下面推导图1所示的平面双连杆机器人的动力学方程。图1中所示连杆均为均质杆，其转动惯量分别是卜和i21、求两连杆的拉格朗日函数(1)求系统总动能连杆1的动能为：1. ck1 = 2 i a%= 1(1m1112t=:2册22 36(21)求连杆2质心d处的线速度：对连杆 2质心位置求导得到其线速度。连杆2质心

9、位置为：xd1二11 cos12cos(玛 i 2 )21二11 sin 912sin(多, i2)2(22)连杆2质心速度为:xd - tsin 丁 丁 -12sin(1%) (% z):(23)1yd 二11 cos曰入 12 cos('2)( a -2)2v；2.22二xd yd =(11工 12 1112 cos712h2 ' - 122(, 122 1112 cosz)用u2442(24)连杆2的动能:1 o 1 ck2 = -id( 肌)2 5 m2 vd1.1 , 22121 , 221 .2 2,1.2=( m2l2)(x12)m2klil2 1112cosu2

10、”112 -2( 121112cos，2”f22 1224421 ,.21.221,2 21,2.2= m2(11121112cos12)xm21212m2 (-12 1112 cos匕尸由2 3623(25)系统总动能：k = k1 k21 ,21,2, 、. 21,2/212,2 、. .二一m2(11121112cos明)一m212i2 m2(121112cos2"f22 36231 .21.21.2121.2 2,1.21二( m211m111m212m21112 cos12)“m212 2 ( m212m21112 cosf2)l 22 662632(26)(2)求系统总势

11、能系统总势能为：(27)1 .1.p=3mi1glisin%m2g(11sin% -12sinp1 %)l =k -p二(1 m2112 1 m111226(3)求拉格朗日函数191.919.9101m212m21112cosr2 户1m212 % ( m212m21112 cos % 户1 %626321 12sin(z %)21. . . ri .m1gli sin % - m2g11 sin 12(28)(4)列写动力学方程按照拉格朗日方程，对应关节1、2的驱动力矩分别为:,:l.：l 1,2-二3111212m/1-m21233(29).121m21112 cos2 尸 1(|m212

12、 m21112 cos2尸 232.:l2 12 1212 1一 =(m211m111m212 m21112cosi2)1 ( m212m21112 cosi2)f333212-m21112 sin 2 -1 3|2 -m21112 sin 2 22:l,1、 ,1,、-( n m2)g11cos1m2g12 cos*1 i2)222 - 1 = (m2111,2 1,21, 2m111m212 m21112 cos12 )z ( m2123331.m21112 cos12 ”22.1,.2,1、，,1, 厂 .、m21112 sin 12nzm21112 sin n ( m1m2 )g11

13、cos 1m2g12 cosc112)222(30)同理:l j ml212u2c 3j 2(m>212 31m21112 cos 1 2 )112二(1 m21223- 1 m211122.12cos <2 ) 1ml2 121231, ,m)21112 sin 看。21.-m21112 sin 12121. mhlilsin "1。21m2 g12cos(上)2,1,21,. 、 .1,2.1 一 . . . 21, 八 .、2 =(-m212m21112cos2)9m212u2- m21112 sin?2am2g12cos(1 12)32322(31)联合（30）、

14、（31）式，将动力学方程写成如下矩阵形式:工01；=?2 1112 112 一m111m212 m21112 cos12331.21.m21；m21112 cos 12321 m21； 31,+ - m21112 cos%1 m2123 2 211 2话-m21112 sin 021， .-m21112 sin u20ml k2 j11+ -m21112sin02 0伴2+ qm1+m2)g11cos%+/2g12cos+%)1001 即21 .1m2g12 cos(%+为)(32)四、平面二连杆机器人手臂的轨迹规划 轨迹规划就是已知起点和终点的位置速度加速度等参数确定中间点的相应参数的过程。

15、轨迹规划是机器人完成规定任务所必需的。它分为关节空间的轨迹规划和直角坐标空间的轨迹规划、以及基于动力学的轨迹规划等几种类型。关节空间的轨迹规划就是已知某连杆起点和终点的角位置角速度角加速度等参数确定 中间点的相应参数的过程。 如图所示，一两自由度机械手，已知两连杆起点和终点的关节角, 确定中间位置的关节角。（1）非归一化和归一化问题（2）末端位置的轨迹、关节空间轨迹 规划的缺点。脚自由度机器人美审空间的非旧化注动两自由度机器人关节空间的归一化运动三次多项式轨迹规划要求：机器人上某关节在运动开始时刻片的角度为鸟，在时刻口运动到角度%.用三次多项式规划该关节的运动* *优分=%+卬+7产+夕初始条

16、件与末端条件; 目=4.加）=6'事&力=%»遍=0*确定系数. .勺j ,以确定我已启涝（0的表达式a 卡举例：要求一个5轴机器人的第一关节在5秒之内从初始角3。度运动到终端角7 5 度，用三次多项式计算在第1、2、3、4秒时的关节角。矶l）= c0 = 30ftfl - 30叩4二。+蝇5） +工小）+0）-75 - g产。前士） c 0 0工=5-4国力皿4+ 2cm5） +女6） = 01口=s由此得到位置速度和加速度的多项式方程如下：岫= 30 + 5aa - 072?财=10l& - 2,16?fl（/） = 10,fi - 432/代人时间求得：

17、刈1） = 34.68*&=必网” 网 3） = 5916d 矶% = 7也32"80图5.1u例5.1中的关节位置、速度和加速度五次多项式轨迹规划已知机器人上某关节在运动开始时亥以=0的角度为月、速度&、 加速度在时刻运动到庭i,度斗、速度为，八加速度为,。要求 用五次多项式规划该关节的运动口8（，）= c. + cl+g/2 + cjp + a/" + c#u 1 z 54 +j初始条件与末端条件± vs（g = &、改右）=。、我幻=孱/innnai日&）二 斗 £ 日7）-0 =&/ +-1确定系数/、”与

18、、今cp c5 ?以确定日（t）、3g前t）的表达式口 m 同例5.1,且巳知初始加速度和末端减速度均为5度/秒2c解：由例5和给出的加速度值得到：心= 3（r e-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& - 6.96r

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