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

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

2、里。从（Xo,yo,Zo）到（为，,乙）的齐次旋转变换矩阵为:cos®-si n®0010sin qcosd0000100001 一从 （X1, y1, z1）到（X2, y2, Z2）的齐次旋转变换矩阵为:-COS 日 2-si n 日20l1；T =sin 日 2cosd0000100001从（Xo，yo，Zo）到（X2,y2,Z2）的齐次旋转变换矩阵为:cosq-sin 30 O'cosB2_sinT20I1 12t =0t 2t =sinqco眇0 0sin 日 2cos 日 200001 00010000 1_0001一-cos（也+ 日 2)-sin(&

3、#174; +日,2）011 cos®si n(d十日2）cos(d +日2)0l1 si n®00100001 _那么，连杆2末段与中线交点处一点 P在基础坐标系中的位置矢量为:0P =0COS(哥 丁2) si n但 1 + 日2)I0I0-si n(円 6)cos(寸rJ?)000010hcosy1211 si n010|0丄(5)即,l1 sind +l2 sin但 1 十日2)yp0Zp- 1 一1 11 一1 cosy l2 cos1COS齐 Jcos&i丁2）=h sin R 12Sin（比 二2）与用简单的平面几何关系建立运动学方程（Xpyp1）相同

4、。(6)建立以上运动学方程后，若已知个连杆的关节角片、二2，就可以用运动学方程求出机械手臂末端位置坐标，这可以用于运动学仿真。3、平面二连杆机器人手臂逆运动学建立以上运动学方程后，若已知个机械臂的末端位置，可以用运动学方程求出机械手臂 二连杆的关节角 齐、二2，这叫机械臂的逆运动学。逆运动学可以用于对机械臂关节角和末 端位置的控制。对于本例中平面二连杆机械臂，其逆运动学方程的建立就是已知末端位置（Xp, yp）求相应关节角 齐、r2的过程。推倒如下。（1）问题Xp = 11 cost l2cos（弓 “2）yp “1 sin1 I2Sin（齐 1）已知末端位置坐标（xp,yp），求关节角 片、

5、二2。（2）求齐3由（6）式得到:2 2 2(XpliCOS) (ypliSi n)=l2整理得到:2 2 2 2xp yp l112 /(XpCOSR ypsin“)ypsin pCOS dp(8)(9)由（8）式得到:2 2. 2. 2xpyph122liXp(cos cospsin sinp)cos dp2 2 2 2Xp yp li -l22liXpcos %COS® - r)(10)5#由此可解出韦。弓二 arccosx： +y： +l； i；2liXpcos 1 + arctg 如Xp(11)(3)求-2由(6)式得到：Xp2 cos(y 匕)2 yp-丨2 Sin

6、74;E)2=li2整理得到：xpy： l； - li2 =2l2XpCos(6 r) ypSin匕)(12)(13)Xp讨ypsin珀 cos%(14)由(14)式得到：xp yp lj-l；氾cos(3 llcosdp si n( Ds in 入cosf pcosG =2 - = p)COS dp(15)由此可解出去。#日2 =arccos2 2 . 2 . 2Xp+yp+JhCOS 0 p2l2xpy parctg 哥Xp二、平面二连杆机器人手臂的速度雅可比矩阵现已二连杆平面机器速度雅可比矩阵的定义： 从关节速度向末端操作速度的线性变换。 人为例推导速度雅可比矩阵。xl1 co S1 +

7、l2co S( +日2)yp =h si n1 卅 2si n +e2)dXp上面的运动学方程两边对时间求导，得到下面的速度表达式:-l1 sin 哥 l2sin(円J2)(円)2)dyp二 l1 cos3 l2 cos(k V2) C v2)(17)#把上式写成如下的矩阵形式：(18)Xp-l1sin 3l2sin(冇 v2) -12 sin(十 二2) 刊Gcos®+l2 cosd + 日2)l2cosd+日2)日2令上式中的末端位置速度矢量和；ypj关节角速度矢量- 1 2 一lebI -h sin R - 丨2 sin二2) 矩阵l1 co1 JcosG2)-12 si n(

8、y二 2)12込何出)Jdd2)JC1门2）就是速度雅可比矩阵，实现从关节角速度向末端位置速度的转变。（18）式可以写成：X十1门2）心速度雅可比矩阵可以进一步写成：11 si门刊一J2Sin（刊 R） -2 si门（二1 匕）| ILh COSKl2COS（K 二2） l2COS（y二2）J12J 22J11IL j 21(19)其中,#cXpJ11 二 =li sin * 一I2 sin（r ：爲）SXp.（20）J1212 sin（"i ：；，-.2）釦J21l1 COSE l2 C0S（3j2）'1J22l2 COS（刊V2）.-2由此可知雅可比矩阵的定义：7#Jg）

9、.J11.J 21J12J 22-：Yp01cXp 1如賀2#三、平面二连杆机器人手臂的动力学方程推倒动力学方程的方法很多，各有优缺点。拉格朗日方法思路清晰、不考虑连杆之间的 内力，是推倒动力学方程的常用方法。 下面推导图1所示的平面双连杆机器人的动力学方程。图1中所示连杆均为均质杆，其转动惯量分别是I1和|21、求两连杆的拉格朗日函数（1 ）求系统总动能连杆1的动能为：1（21）Q =2崇12= 1（1m1l12p12 =1 m1l12122 36求连杆2质心D处的线速度：对连杆 2质心位置求导得到其线速度。连杆2质心位置为：xD = l1 COSl 2cos（片亠 2 ）（22）21Yd

10、T1 sin 弓sinQ飞）2连杆2质心速度为:Xd 一 -hsin 齐片in（片 1）（弓比）j（ 23）Yd =l1COSR 耳丨2 C0S（3 匕）（刁 6）2*4"*4"*4"vf =xD +yD =（h2 +T； Ujcos日2）日12 +T；时 +（T； Ujcos日2）日 1日2442#(24)9(24)连杆2的动能:11K2 Id(R r)2m2V；11 2 2 1 2 1 2 . .2 1 2 1 2 - (m2l2)(R v2)m2(l-Il2 hl2cosy)»12 -2 ( l2 hJcosR)71 存22 1224421 2 1

11、 2 2 1 2 2 1 2 2m2(hl2 l1l2 co21m2l2d2m2( l2 l1l2cos2p223623(25)系统总动能：K = K1 K21 211221221/22m2(hl2 l1l2 co21m2l2r2m2( l2 l1l2cosi2p22 36231 212121212 2121-(m2l；m1lfm2l；m2l1l2 cos21m2l；r2(m2l；m2l1l2cos2< 22 662632(26)(2)求系统总势能系统总势能为：1m1gl1sinr mhsinr fl2sin®讪(27)(3 )求拉格朗日函数=(丄 m2l； 1 m112 61

12、21212 2121m2l2m2hl2 cos® m2l2 2 ( m2l2m2l1l2 co2p262632m1gl1 sin 3 - m2gl1 sin 耳2l2s in (可屯)2(28)(4 )列写动力学方程按照拉格朗日方程，对应关节1、2的驱动力矩分别为:.:L(29):L2石厂(吋1- 1 2 1 m2l1l2 co 2p1 ( 12m2m2cos2尸232#.:L_ 2 1 2 1 2 1 2 1 (m? hml 1m212"m? h 12 cos v 2) E' (m212m?l 112cos v 2)二 23332 1'2-m2l1l2 s

13、in 七北七 m2l1l2sin 二2r22:L111-(m2)gl1cos1m2gl2cos£ 七)221 - m2l1l2 cosv2户221 2 1 2 1 2 m1l1m2l2 m2hl2 cosv2 )3( m2l2333- 1 2 1 1-m2l1l2 sin * 刊 丁2m2l1l2 sin®®( m1 m2 )gl1 cosm2gl2 cos <2)2 2 2(30)同理:(1 m2l2231m2l112 cos2 k12:L.1. 2-1 iThbl22 COS K 门1m2hl2sin Rm211#1一m211l 2 sin -l2121

14、 一m211l 2 sin 出二存？21m2gl2cos(yI)2#1 2 1 1 2 " 1-212=(m2l2m2l1l2cos2ym2l2*m2l1l2sin)2 »m2gl2 cos(-1 v2)32322(31)联合（30）、（ 31）式，将动力学方程写成如下矩阵形式:m2l121m1l123 1 1 1m2l2?3 2 21 2m2l2 m2l1l2 cos2311 m2l；3 2 21+ - m2l112 cos日21 m2l|3 2 2_- 15 2 一le-J0m2l1l2 sin 021m2l1l2 sin 日20A十诃;(尹1 +m2)gh cos日i

15、00齐2m2l1l2 cos j2,.Hm2l1l2 sin2!01+?m2gl2 cos® +日2)12m2gl2cos® + 日2)#(32)四、平面二连杆机器人手臂的轨迹规划#轨迹规划就是已知起点和终点的位置速度加速度等参数确定中间点的相应参数的过程。 轨迹规划是机器人完成规定任务所必需的。 它分为关节空间的轨迹规划和直角坐标空间的轨 迹规划、以及基于动力学的轨迹规划等几种类型。关节空间的轨迹规划就是已知某连杆起点和终点的角位置角速度角加速度等参数确定 中间点的相应参数的过程。 如图所示，一两自由度机械手，已知两连杆起点和终点的关节角， 确定中间位置的关节角。（1）非

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

17、地为 + 衣矽）=&Uj = 772由此得到位跟速度和加I速度的多顶武方程如下：昨）= 30 + 5Aa - 072/'则=10L& - 2A6i2 fl（/）二 10.fi 432/ft人时间求得：协=34.68*袖=必開”&=59.16D。=皿妙80位置60 才，40五次多项式轨迹规划已知机器人上某关节在运动开始时刻心=0的角度为§、速度矗、 加速度玄，在时刻&运动.製角康&八速度为方八加速度为昂。要求 用五次多项式规划该关节的运动。屮8()=q + qf+cf + c/ + C5F 屮初始条件与末端条件：卫innnai&(

18、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进而得到如下运动方程：8=30 + 2.5t2 + l.fir1 一 0.5S? + 0.046 4沙e(t) = 5r + 4

19、.8尸-232尹 + 0.232/*d(f) = 5 + 9.6r - 6.96r1 + 0.928/3图5.12是机器人关节的位置、速度和加遽度曲线，其最大加速度为度/秒S图工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坐标之差简单地加以分劃得到，然后通过求 解逆运动学方程得到对应每个中间点的两个关节角。结