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

1、一、平面二连杆机器人手臂运动学平面二连杆机械手臂如图1所示，连杆1长度l，连杆2长度/。建立如图1所示的坐12标系，其中，(x,y)为基础坐标系，固定在基座上，(x,y)、(x,y)为连体坐标系，001122分别固结在连杆1和连杆2上并随它们一起运动。关节角顺时针为负逆时针为正。图1平面双连杆机器人示意图1、用简单的平面几何关系建立运动学方程连杆2末段与中线交点处一点P在基础坐标系中的位置坐标:x=lcos0+lcos(0+0)p11212y=lsin0+lsin(0+0)p112122、用D-H方法建立运动学方程1)假定z。、z2垂直于纸面向里。从(x0，八z0)到(八z)的齐次旋转变换矩阵

2、为:0T=1cos01sin0100-sin01cos0100001000012)从(xi，yi，G到(X，y22，Z2)的齐次旋转变换矩阵为:cos02sin020-sin02cos02000010l100从(X0，y0，Z0)到y22,z)的齐次旋转变换矩阵为：3)0114)cos0-sin000cos0-sin00厂11221sin0cos000sin0cos000oT=oT-1T=i1-222120010001000010001cos(e+0)-sin(e+0)0lcos0121211sin(0+0)cos(0+0)0lsin012121100100001那么，连杆2末段与中线交点处一

3、点P在基础坐标系中的位置矢量为:cos(0+0)12sin(0+0)1200-sin(0+0)12cos(0+0)20010lcos0+lcos(0+0)11212lsin0+lsin(0+0)112101xpy=pzp15)2即，6)就可以用运动学方程求出机x=lcos0+lcos(0+0)p11212y=lsin0+lsin(0+0)p112121)相同。与用简单的平面几何关系建立运动学方程建立以上运动学方程后，若已知个连杆的关节角01、02械手臂末端位置坐标，这可以用于运动学仿真。3、平面二连杆机器人手臂逆运动学建立以上运动学方程后，若已知个机械臂的末端位置，可以用运动学方程求出机械手臂

4、二连杆的关节角0、0，这叫机械臂的逆运动学。逆运动学可以用于对机械臂关节角和末12端位置的控制。对于本例中平面二连杆机械臂，其逆运动学方程的建立就是已知末端位置(x,y)求相应关节角0、0的过程。推倒如下。pp12(1)问题x=lcos0+lcos(0+0)p11212y=lsin0+lsin(0+0)p11212已知末端位置坐标(x,y)，求关节角0、0。pp122) 求01由（6）式得到：(x一lcos0)2+(y一lsin0)2=12p11p112整理得到：x2+y2+12一12=21(xcos0+ysin0)pp121p1p1令xsin0p=tg0=pypcos0pp8）式得到：x2p

5、+12-121221x1Pcos0p(cos0cos0+sin0sin0)1p1px2p+12一121221x1cos(0-0)cos01pp由此可解出01。0=arccos1x2+y2+12一12Pp1cos021xp1p（3）求02由（6）式得到y+arctgpxpx一1cos(0+0)2+y一1sin(0+0)2=12p212p2121整理得到：x2+y2+12一12=21xcos(0+0)+ysin(0+0)pp212p12p12令xsin0=tg0=pypcos0pp由(14)式得到：21xx2+y2+12-12=cos(0+0)cos0+sin(0+0)sin0pp21cos012

6、p12pp21x=匕亠cos(0+00)cos012pp由此可解出0。27）8）（9）（10）（11）（12）（13）（14）（15）416)x2+y2+12-12pp2121x2p二、平面二连杆机器人手臂的速度雅可比矩阵速度雅可比矩阵的定义：从关节速度向末端操作速度的线性变换。现已二连杆平面机器人为例推导速度雅可比矩阵。x=1cos9+1cos(9+9)p11212y=1sin9+1sin(9+9)p11212上面的运动学方程两边对时间求导，得到下面的速度表达式dx亠=-1sin99-1sin(9+9)-(9+9)dt11121212dyp=1cos99+1cos(9+9)-(9+9)dt1

7、112129=arccos2cos9+arctgp人-ex1p17)12把上式写成如下的矩阵形式：-lsin9-lsin(9+9)11212lcos9+lcos(9+9)11212xp=yp-1sin(9+9)92121 cos(9+9)92 1218)x令上式中的末端位置速度矢量yp关节角速度矢量i92=0,矩阵-lsin9-lsin(9+9)11212lcos9+lcos(9+9)11212-lsin(9+9)2121 cos(9+9)2 12=J(91,92)J(9,9)就是速度雅可比矩阵，12以写成：实现从关节角速度向末端位置速度的转变。(18)式可X=J(9,9)012速度雅可比矩阵

8、可以进一步写成：-1sin9-1sin(9+9)112121cos9+1cos(9+9)1J(91,92)=J21J12J221212-1sin(9+9)2121 cos(9+9)2 1219)其中，624)11二貳一a61-12Sin(0i+02)112=sin(°i+打)29J=p=1cos0+1cos(°+0)21301121213yJ=p=1cos(0+0)22302122由此可知雅可比矩阵的定义：3xJ(0,01211J211222303xp03yp30221)三、平面二连杆机器人手臂的动力学方程推倒动力学方程的方法很多，各有优缺点。拉格朗日方法思路清晰、不考虑连

9、杆之间的内力，是推倒动力学方程的常用方法。下面推导图1所示的平面双连杆机器人的动力学方程。图1中所示连杆均为均质杆，其转动惯量分别是I和I。121、求两连杆的拉格朗日函数（1）求系统总动能连杆1的动能为：1K=I0212A1111=（一ml2）02=_ml20223111611121)求连杆2质心D处的线速度：对连杆2质心位置求导得到其线速度。连杆2质心位置为：x=1cos0+1cos(0+0)D11221222)y=1sin0+1sin(0+0)D112212连杆2质心速度为：X=-1sin00-1sin(0+0)-(0+0)D11122121212(23)Y=1cos00+1cos(0+0

10、)-(0+0')D111221212V2D=x2+y2DD111=(12+12+11COs0)02+1202+(一12+11COS0)0014212214222212212连杆2的动能：11K=_I(0+0)2+mV222D122111111=(m12)(0+0)2+m(12+12+11cos0)02+1202+(12+11cos0)00212221211=m(12+12+11cos0)02+2213212212D11严、/22142m120262221221422221221212+m(12+11cos0)00223212212(25)系统总动能：K=K+K1211=m(12+一12

11、+11cos0)02221321221(1111=(m12+m12+m12+m2216116222+m12O262222(12+11cos0)0023212212111cos0)02+m1202+62222122111(m12+_m11cos0)00322221221226)2）求系统总势能系统总势能为：1+-1sin0+0)1221227)P=mg1sin0+mg(1sin02111213）求拉格朗日函数1+m221221g1sin0+11sin(0+0)112212(丄m12+m11cos03222212L=KP(171717=(m/2+m/2+m/2221611622mg1sin0m21

12、112111cos0)02+m1202+62228)4）列写动力学方程对应关节1、2的驱动力矩分别为：按照拉格朗日方程66L6L29)6t60601166L6L6t6060226L.=(m12+丄m1260213111.111cos0)0+(m12212213221+m/221211cos0)022133L111=(m12+m12+m12+m11cos0)0+(30213113222122131m12+m11cos0)0222212221一milsin000一一milsin002212212221222=-(m+m)glcos0一一mglcos0+0)3021211222121111t=(m1

13、2+m12+m12+mllcos0)0+(m121213113222122131m21-milsin0002121一一m11sin002+212221222221+m/2212llcos0)022+m)glcos0211+-mglcos(0+0)2221230)同理：3110m120303211+(一m12+m11cos0)0222123221ddL(-m123221+m/2212llcos0)0211+m12032221m11sin0002212212百二丄mll22121sin002一m11212212sin0002121一一mglcos(0+0)2221211t=(一m12+m11cos

14、0)023222212211+-m120322211sin00221221+mglcos(0+0)2221231)联合（30）、（31）式，将动力学方程写成如下矩阵形式：T1T2m12+-m12+m12+m11cos023111zm12322322212+丄m11cos02212222m11cos022121zm12322.0.102llsin02122一milsin02120一丄m11sin022120001200012四、平面二连杆机器人手臂的轨迹规划一02.102m+m)glcos02gl221mglcos(0+0)22212cos(0+0)1232)11轨迹规划就是已知起点和终点的位置

15、速度加速度等参数确定中间点的相应参数的过程。轨迹规划是机器人完成规定任务所必需的。它分为关节空间的轨迹规划和直角坐标空间的轨迹规划、以及基于动力学的轨迹规划等几种类型。关节空间的轨迹规划就是已知某连杆起点和终点的角位置角速度角加速度等参数确定中间点的相应参数的过程。如图所示，一两自由度机械手，已知两连杆起点和终点的关节角，确定中间位置的关节角。（1）非归一化和归一化问题（2）末端位置的轨迹、关节空间轨迹规划的缺点。两徉由it机器人关书空间的奉曲化迄动flB网口由度肌器人关节咗问的归-化远动13三次多项式轨迹规划襲察；机器人上某关书在运动开始时憩U的埔度为在削刘片运动到角厦日厂用三庆务项成规划该

16、关节的运初始条件与末啪条件："虫需）=9严肌切=SJ*毗/=牛祐»=2礁;g賈:数"；i-0込、以碉定宛珈卜日©的我这式a*举例：要求一个5轴机器人的第一关节在5秒之内从初始角30度运动到终端角75度，用三次多项式计算在第1、2、3、4秒时的关节角。（艸j=m=30Ci=0c2-54Lcj=-W.72"（0=q,+C,（S）+亦刊+c/P）=75$勻©。=C|+笙+3c朋）=fl由此得到位置、遽度和加速世的茅项式方程如卜：呦-30+5.4?-O.72T3剜=10.8J-ll&r2代人时间求得：0（1）34.68a盹)=10.8

17、-4327tf2)=粘ET&=59.16e(4)=7032c80位置图50例5.1中的关节位置、速度和加速度五次多项式轨迹规划已知机器人上某关节在运动开始时刻勺=o的角度为坊、速度&、加速度左时刻冷运动0.1速度为初、加速度为N°要求用五次多项式规划该关节的运动。屮0(t)二q+qf+c?广+c+cf+初始条件与末端条件：Q日石)=&i=0'i&-tj)=0心I日(乡)=曰厂&(f”=0i&切)=力"确定系数%勺、勺、答以确定日®晁)、亦的表达式°+同例5.1,且已知初始加速度舸末端减連度均为5度F

18、渺2。解；由例5.1和给出的加速度值得到：=30'&=0度/秒度/秒2舟=75。亦=0度/秒爲三_5度f聖工将初蜡和末端边界亲件牝入式(5.5)-式m得出*<o-30C)=0c2=2-5巧=1百ct-0.5«cs»0.0464进而得到如下运动方稅：5(0=30+25tl+1血彳一(.5Kr4+00464r5«(/)=5r+4.8?-232fs+0.2321*苗=5+9.6r-6.96P+0528?图52呈机器人关节的位置”速度和加速度曲线，其最大加速度为度/秒打80图5.12例5.3的关节位置、速度和加速度抛物线过渡的线性运动轨迹规划（略）具

19、有中间点以及用抛物线过渡的线性运动轨迹规划（略）高次多项式运动轨迹规划（略）直角坐标空间的轨迹规划（1）所有用于关节空间的轨迹规划方法都可以用于直角坐标空间轨迹规划；（2）直角坐标轨迹规划必须不断进行逆运动学运算，以便及时得到关节角。这个过程可以归纳为以下计算循环：（a）将时间增加一个增量；（b）利用所选择的轨迹函数计算出手的位姿；（c）利用逆运动学方程计算相应的关节变量；（d）将关节变量信息送给控制器；（e）返回到循环的开始。图$.?Mfidu速和咸速匪的轨迹规星15JT14例5£个两自由度平面机器人要农从起点煮10)厝直錢运动到蚪点S,14)n腔分为10段+汞出机需人的关节娈量每一根连杆的弋度为9英寸札解：直角坐标空间申起点和终点间的直线可以描述为：或者y=0.8x+7.6中间点的坐标可以通过将起点和终点的F坐标之差简单地削以介割辱到*然后通対求解逆运动学方程得到时应每个中间点的两个关节角蜡果见我3和图5,17.表5.T例5.6的坐标和关节角#Xy纵罠131018,S10923.510.4j19104.03410.819.5100,444.51L220.295.85511621390.965.51222.585J7612.424.180+186.512.826742g713.228.267.8107.513.630.860+711S1433.952.8中间点数图5