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

1、一、平面二连杆机器人手臂运动学平面二连杆机械手臂如图 1所示，连杆1长度h，连杆2长度丨2。建立如图1所示的坐标系，其中，（Xo，y。）为基础坐标系，固定在基座上，（Xyj、（X2,y2）为连体坐标系，分别固结在连杆1和连杆2上并随它们一起运动。关节角顺时针为负逆时针为正。yoy22B21y1Xo，2D1#图1平面双连杆机器人示意图1、用简单的平面几何关系建立运动学方程连杆2末段与中线交点处一点 P在基础坐标系中的位置坐标:(1)Xp = l1 cos 十 l2cos(齐 T2) y p = l1 si 12si n(K )2)2、用D-H方法建立运动学方程假定Zo、Z1、Z2垂直于纸面向里。

2、从（Xo,yo,Zo）到（X1,y1,zj的齐次旋转变换矩阵为:cos 齐-sin 片 0 00 sin qcosd0 0iT =（ 2）001 0.000 1 一从（X1, y1, Z1）到（X2, y2, Z2）的齐次旋转变换矩阵为：cos -sin 二2 0 l11 sin 日2cos°20 02（ 3）001 0-000 1 一从（X0,y°, Z0）到（X2,y2,Z2）的齐次旋转变换矩阵为：cosd-sin 30 0cosB2_sinT2011 12t =0t 2t =sin qco眇0 0sin 日 2cos日 200001 00010000 1_0001 一

3、-cos(也+ 日 2)-sin(® + 02)0l1 cos® sin (q十日2)cos(d +日2)0l1 si n®0010I0001 _那么，连杆2末段与中线交点处一点 P在基础坐标系中的位置矢量为:0P =0COS(哥 丁2) si n但 1 + 日2)I0I0-si n(円v2)cos()i丁2)000010hcosyl2l1 si n010|0J.(5)即,11 sin d +l2 sin® +82)yp0Zp. 1 一1 11 一*1 cos l2 cos(1Xp COS l2COS1二 2)yp sinl2Sin(十出)(6)与用简单

4、的平面几何关系建立运动学方程(1)相同。建立以上运动学方程后，若已知个连杆的关节角二1、二2，就可以用运动学方程求出机械手臂末端位置坐标，这可以用于运动学仿真。3、平面二连杆机器人手臂逆运动学建立以上运动学方程后，若已知个机械臂的末端位置，可以用运动学方程求出机械手臂 二连杆的关节角 齐、二2，这叫机械臂的逆运动学。逆运动学可以用于对机械臂关节角和末 端位置的控制。对于本例中平面二连杆机械臂，其逆运动学方程的建立就是已知末端位置 (Xp, yp)求相应关节角 齐、I的过程。推倒如下。(1)问题xp = l1 cos 齐 Jcosd T2)yp “1 sin1 Jsin(31)已知末端位置坐标(

5、xp,yp)，求关节角3、二2。(2)求齐2由（6）式得到:3#(Xp -Il cos冃)2 (yp hsin)2 =1；整理得到:2 2. 2, 2Xpypli12 = 2l1(xpcos1ypsin 山)Xp-=tgr yp由(8)式得到:sin Jpp COSdp2Xp2 . . 2 . 2yp l1 - I 22hXpcos dp(cosy cospsin sinrp)2Xp21凶cost pcos® -齐)(8)(9)(10)#由此可解出哥。弓二 arccosxp +y： +I； -I；2IiXpcos 1 + arctg 如Xp(11)#(12)(13)(3)求-2 由（

6、6）式得到：Xp2 cos® 出）2 yp2 Sin（弓二2）2整理得到：Xpyp二 tgp Sinpcos%(14)由（14）式得到：xp y2 丄2-“款c吨1 讣0口 sind）si"p 2l2Xpcos%(15)由此可解出去。cosG =2 - dp)xpy： l； -l； =2l2XpCosU 出）ypSind 出）4（16）=arccos*+y2+l2li21COS 0 p2*Xparctg 竺xp二、平面二连杆机器人手臂的速度雅可比矩阵现已二连杆平面机器速度雅可比矩阵的定义： 从关节速度向末端操作速度的线性变换。 人为例推导速度雅可比矩阵。xl1 coS1 +

7、l2co S( +日2) yp =h si n1 H2si n +e2)dXp上面的运动学方程两边对时间求导，得到下面的速度表达式:-l1 sin 哥 l2sin(円丁2)(円)2)5（16）dypdT二 l1 cos3 l2 cos(k V2) C v2)把上式写成如下的矩阵形式：Xp -l1sin-l2sin(tE)G ' cos® +l2 cos(6 + 日2)-l2s in (d +)甲 l2cos(6 + 日2)血令上式中的末端位置速度矢量关节角速度矢量I -h sin R -12 sin(K 二2) 矩阵| li cos 丨2 cos&i R)-12 si

8、n(y 2) JcosOi )(17)(18)J（弓门2）就是速度雅可比矩阵，实现从关节角速度向末端位置速度的转变。（18）式可以写成：速度雅可比矩阵可以进一步写成:I1Sin 円一 Jsin(1 匕)1, 2ILh cos I2 cos°1 二2),J11 J12=I-J 21J221-l2 sin(2)LcosG屯)(19)其中,6(20)cXpJ11l1 sinml2 sin(“r)7(20)#(20)J12J21J22-Xp_ = -l2si n( rr)-7p14 cos t I2cos(r -3-=l2 cos(t 亍2) r由此可知雅可比矩阵的定义:5p5p 1J 11

9、 J 12日1日21 _-J21 J 22cYp创pIc61J（“，）二(21)三、平面二连杆机器人手臂的动力学方程推倒动力学方程的方法很多，各有优缺点。拉格朗日方法思路清晰、不考虑连杆之间的 内力，是推倒动力学方程的常用方法。 下面推导图1所示的平面双连杆机器人的动力学方程。图1中所示连杆均为均质杆，其转动惯量分别是I,和|21、求两连杆的拉格朗日函数（1）求系统总动能连杆1的动能为：1=(-m1l12/12 = - m1l12'122 36(21)求连杆2质心D处的线速度：对连杆 2质心位置求导得到其线速度。连杆2质心位置为：xD = l1 cos12cos(片亠 2 )21Yd

10、T1 sin 弓Jsin(飞)2(22)连杆2质心速度为:Xd 一 -hsint 已 一1jsin(片 1)(弓比)j( 23)Yd =11遇十已l2 COS(t 出)Q工)2彳M彳M彳nav；+yD =(l； +;l； +l1l2cos02)d2 +：l；时 +(-l2 +hl2cos 日 2)日卩 24428(24)9(24)连杆2的动能:1 2 1 2K2 I D (R R)m2VD2 2d d""dd" d "d""II22 I2 I 22l22l2(m2l；)(m 咗2)2rni2(l：12 I1I2 co2i2( 12 I

11、1I2COS氏戶门22 1224421 2 1 2 2 1 2 2 1 2 2m2(hl2 Ucos r)刁m2I2 *m2( l2hl2cosj2)YR23623(25)系统总动能：K = K1 K21 2 1 2 2 1 2 2 1 2 2 m2(h12 hJcos*)刊m2l22m2( l2 l1l2cos2p21 21212112121-(m2l；m1lfm2l；m2l1l2 cos2.,m2lp2(m2l；m2l1l2cos2< 22 662632(26)(2)求系统总势能系统总势能为：1m1gl1sinr mhsinr fl2sin®讪(27)(3 )求拉格朗日函数

12、1 212121. 212-2121.=(m2l1m1l1m2l2m2hl2 cos)2)Rm2122(m2l2m2hl2 cosv2)牛22 6626321m1gl1 sin 3 -m2gl1 sin112 sin( K *)2(28)(4 )列写动力学方程按照拉格朗日方程，对应关节_L_L1acCE 口=1-1LL2H -21、2的驱动力矩分别为:(29)丄21212壬厂(讪1尹13叫2- 1 2 1 'mzllcoS?尸 1 ( m?l2m2hl2CoS2 尸 23210-汇21| 21| 21.21l = (m? + ml 1 * m?l 2 + mzh 12 cos日 2)&

13、#174; + ( m?l 2 * mzh12 cosH2)2 寸：U3332-m?l 112 sin 2v 1 21m?l * 2 sin 22；L：r11-(g m2)gl1cos1m2gl2cos£ v2)22.1 = (m2l11 2 1 2 1 2 1m1l1m2l2 m2hl2 cosv2) (m2l2m2l1l2 co223332- 1"21 1-m2l1l2sin *刊 丁2m2l1l2sin 丁2丁2 ( m1 m2)gl1 cos1m2gl2cos(宀 丁2)2 2 2(30)同理:二-mzEm (mzl； m2l1l2co21：%332:L1111&#

14、39;'( m2l2m2l1l2 cos2m2l| 2m2l1l2 sin RE R3 ：T23232:l1-1- -1m2l1l2 sin v2 #m2l1l2 sin R J存2 m2gl 2 cos 2)J22221 21 1 2 "1 -212 =( m2l2 m2l1l2cos2y1 m2l2 * m2l1l2sin)2» m2gl2cos(-1 v2)32322(31)联合（30）、（ 31）式，将动力学方程写成如下矩阵形式:m2l1m1l12312m2l231 2m2l2 m2l1l2 co2 31m2l1l2 cos j221+ - m2l112 c

15、os日221- 2-m2l23- 15 2 一le-J12#0一一 m2l1l2 sin 日221m2l1l2 sin 日20t2 一十；11 口剪 mg®910毎0千2一1 01+ m2gl2 cos(6 +日2)21-m2gl 2cos(E +甘2)2 一(32)四、平面二连杆机器人手臂的轨迹规划轨迹规划就是已知起点和终点的位置速度加速度等参数确定中间点的相应参数的过程。 轨迹规划是机器人完成规定任务所必需的。 它分为关节空间的轨迹规划和直角坐标空间的轨 迹规划、以及基于动力学的轨迹规划等几种类型。关节空间的轨迹规划就是已知某连杆起点和终点的角位置角速度角加速度等参数确定 中间点

16、的相应参数的过程。 如图所示，一两自由度机械手，已知两连杆起点和终点的关节角， 确定中间位置的关节角。（1）非归一化和归一化问题（2）末端位置的轨迹、关节空间轨迹 规划的缺点。隔自由度机器扎犬审空同鴨非旧化込劝两自由庭机器人关节空间的归一优运动三次多项式轨迹规划娶求：机器人上某关节在运动开始时刻心前销度为耳在时刻与运动到角度用三次多项式规划该关节的运初始条件与未端祭件：-贾G = g 啲）=&严确定系数钿兮6 以确定型人方方的表达式a卫举例：要求一个5轴机器人的第一关节在5秒之内从初始角30度运动到终端角75 度，用三次多项式计算在第1、2、3、4秒时的关节角。S（fj） = c0 =

17、 30- 30卿+ G+心俘"珊刃-75- A " Ci （Ipi= 5-4鮒 Ci + 地为 + 衣矽）=&Uj = 772由此得到位跟速度和加I速度的多顶武方程如下：昨）= 30 + 5Aa - 072/'则=10L& - 2A6i2 fl（/）二 10.fi 432/ft人时间求得：协=34.68*袖=必開”&=59.16D。=皿妙80位置60 才，40五次多项式轨迹规划已知机器人上某关节在运动开始时刻心=0的角度为§、速度矗、 加速度玄，在时刻&运动.製角康&八速度为方八加速度为昂。要求 用五次多项式规划该关

18、节的运动。屮8()=q + qf+cf + c/ + C5F 屮初始条件与末端条件：卫innnai&(fy) =、 0(f” = 0、巩) +'确定系数 5巾、 cP ",以确定日(t)4(t)诺(t)的表达式。-M 同例5.1,且巳知初始加速度和末端减速度均为5度/秒2。解：由例5J和给出的加速度值得到：= 30*产0度/秒 g严5度/秒工8广矽& = Q度/砂&f= -5度/秒2猗初始和末端边界条件代入式(5.5 ) 式(51 )碍出:cn = 30Cj = 0c； = 2JCj = 16c4 = -0.58c5 = 0,0464进而得到如下运动方

19、程：8=30 + 2.5t2 + l.fir1 一 0.5S? + 0.046 4沙e(t) = 5r + 4.8尸-232尹 + 0.232/*d(f) = 5 + 9.6r - 6.96r1 + 0.928/3图5.12是机器人关节的位置、速度和加遽度曲线，其最大加速度为度/秒S80图工12例乞3的关节位置、速度和加速度抛物线过渡的线性运动轨迹规划（略）具有中间点以及用抛物线过渡的线性运动轨迹规划（略） 高次多项式运动轨迹规划（略）直角坐标空间的轨迹规划（1）所有用于关节空间的轨迹规划方法都可以用于直角坐标 空间轨迹规划；（2）直角坐标轨迹规划必须不断进行逆运动学运算，以便及时得到关节角。 这个过程可以归纳为以下计算循环：（a）将时间增加一个增量；（b）禾U用所选择的轨迹函数计算出手的位姿；（c）利用逆运动学方程计算相应的关节变量；（d）将关节变量信息送给控制器;（e）返回到循环的开始。图眾了具有加雀和减速段的勒迹规划例5.6 个两自由度平面机器人要求从起点（3* 10）沿亶线运动到终点（8* 14）。假此聲 径分为10段，求出机器人的关节变童 每一根连杆的长度为9英寸4解：直角坐桶空间中起点和终点间的直线可以描述为:y = O.ik + 7.6中间点的坐标町以通过将起点和终点的 y坐标之差简单地加以分劃得到，然后通过求 解逆运动学方程得到对应每个中间点的两个