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

1、一、平面二连杆机器人手臂运动学平面二连杆机械手臂如图1所示，连杆1长度11 ,连杆2长度丨2。建立如图1所示的坐标系，其中，（Xo，y）为基础坐标系，固定在基座上，（X1,yj、（X2,y2）为连体坐标系，分别固结在连杆1和连杆2上并随它们一起运动。关节角顺时针为负逆时针为正。图1平面双连杆机器人示意图1、用简单的平面几何关系建立运动学方程连杆2末段与中线交点处一点P在基础坐标系中的位置坐标:(1)Xp =丨1 C0S1 丨 2COS（ “2）yp =丨1 sin 哥 12Sin（R寸2）2、用D-H方法建立运动学方程(2)假定zo、Z1、Z2垂直于纸面向里。从（Xo,yo,Zo）到（羽1,乙

2、）的齐次旋转变换矩阵为:-cosd-si nd0011t =sin qcos0000100001 一从（X1, y1, zO到（X2, y2,Z2）的齐次旋转变换矩阵为：cosi s ini 0 丨11si n 日2cos 日 20 0J =（ 3）001 0-000 1 一从（X0，y,Z0）到（X2,y2,Z2）的齐次旋转变换矩阵为：2t =0t 2t 二co1-sin 30 0-cosB2_sinT20Usi ndco眇0 0sin 日 2cos 日 200001 00010.000 1_0001一_cos(也+ 日 2)-sin( +日,2）011 cossi n(d十日2）cos(d

3、 +日2)0l1 si n00101 0001那么，连杆2末段与中线交点处一点 P在基础坐标系中的位置矢量为:0p =0cos(哥 v2)si n但 1 + 日2)I0-si n(円v2)cos()i丁2)000010hcosyl2l1 si01010丄(5)即,Xpl1 sind +l2 sin但 1 十日2)yp0Zp- 1 一1 11 一I01 cosy l2 cos1= hcosK bcosG= 1sin 十 12sin(可 丁2)=2)(6)与用简单的平面几何关系建立运动学方程（1）相同。建立以上运动学方程后，若已知个连杆的关节角二1、二2，就可以用运动学方程求出机械手臂末端位置坐标

4、，这可以用于运动学仿真。3、平面二连杆机器人手臂逆运动学建立以上运动学方程后，若已知个机械臂的末端位置，可以用运动学方程求出机械手臂 二连杆的关节角 齐、二2，这叫机械臂的逆运动学。逆运动学可以用于对机械臂关节角和末 端位置的控制。对于本例中平面二连杆机械臂，其逆运动学方程的建立就是已知末端位置 （Xp, yp）求相应关节角 齐、r2的过程。推倒如下。（1）问题Xp = I1 cos弓 12cos（弓专）yp =l1 si nt 12Si n（t如已知末端位置坐标（xp,yp），求关节角 片、二2。（2）求齐由（6）式得到:2 2 2(Xp-liC0S3) (ypjsi门切 J整理得到:2 2

5、 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 -I22IiXpcos %COS - r)（10）由此可解出韦。弓二 arccosx： +y： +l； i；2liXpcos 1 + arctg 如Xp(11)(3)求-2由(6)式得到：(12)(13)Xp2 cosQi 匕)2 yp2 sin(y E)2 =li2整理得到：sin珀 cos%(14)Xpy： I； - I； =2l2XpCos(6R) yp

6、Sin-)Xp=tg，yp由(14)式得到：Xp yp lj-l；氾cos(3 llcosdp si n( Ds in 入(15)cosf pcosG =2 - = p)COS dp由此可解出去。=arccosx2+y2+l2-h2a 1cos 0 p2l2Xpy p arctg 片xp二、平面二连杆机器人手臂的速度雅可比矩阵速度雅可比矩阵的定义：从关节速度向末端操作速度的线性变换。人为例推导速度雅可比矩阵。xp = l1 cosy l2COS（N v2）yp = l1 sin 耳 12sin（刁 v2）上面的运动学方程两边对时间求导，得到下面的速度表达式：（16）现已二连杆平面机器dxpdy

7、p二 l1 cos3 I2cos（k v2）（円 6）（17）-l1 sin % l2sin（円v2）（円6）把上式写成如下的矩阵形式:Xp lisi n： 从 一cosr _l2sin（R v2） l2cos（丁r2）-l2 sin（d +日2）1日1I2 cos +鶏）仏（18）令上式中的末端位置速度矢量和；ypj关节角速度矢量矩阵TW1 一以咱 如-Ssg 如（卄2） lUiCOSNI2COSU1二2） l 2 COS何二2）JU门2）就是速度雅可比矩阵，实现从关节角速度向末端位置速度的转变。（18）式可以写成：X十2）心速度雅可比矩阵可以进一步写成：11 si门刊一J2Sin（刊 R）

8、 -2 si门（二1 匕）| |1 COSK I2COSG 二2） I2COSQ1 二 2）J12J 22J11IL j 21（19）其中,cXpJ11 二 二 li sin * 一I2 sin（r ：爲）SXp.J1212 sin（i ：；，-.2）釦J21l1 COSE l2 C0S（3j2）1J22l2 COS（刊V2）.-2由此可知雅可比矩阵的定义：（20）Jg）.J11.J 21Jl2J 22-：Yp01cXp 1如賀2三、平面二连杆机器人手臂的动力学方程推倒动力学方程的方法很多，各有优缺点。拉格朗日方法思路清晰、不考虑连杆之间的 内力，是推倒动力学方程的常用方法。 下面推导图1所示

9、的平面双连杆机器人的动力学方程。图1中所示连杆均为均质杆，其转动惯量分别是I1和|21、求两连杆的拉格朗日函数（1 ）求系统总动能连杆1的动能为：1Q =2崇122 （ 21）= 1（1m1l12p12 =1 m1l12122 36求连杆2质心D处的线速度：对连杆2质心位置求导得到其线速度。 连杆2质心位置为:xD 二 l1 cos1l2COS 丁2 ）（22）2yDTsiz尹哨如连杆2质心速度为：Xd - -hsi nt 已 一 in （片 1）（弓比）2（ 23）Yd * Cost 已一J COS（3 D m）2*4*4*4V； =xD +yD =（h2 +T； +l1l2Cos82）d2

10、+（T； +H2COS 日 2）巧日 2442（24）连杆2的动能:1 2 1 2K2 IdGi r)2 m2vD=1 (丄 m2l；)( r v2 )2 m2 (l2 1| hl2 cosr)寸-l|-2 ( 12 hl 2 cosv2 Hu 2 1224421 21 2212212 2m2(liI2 I1I2 co2p1口2丨2二2m2( I2 hLcos)吋22 3623(25)系统总动能：K = K1 K21 21 2212 212 2m2(l-! -l2 l1l2 co2p1m2l2%m2( l2 l1l2cos2p22 36231 2 1 2 1 2 1 2 1 2 2 1 2 1

11、-(m2l1m1l1m2l2m2l1l2cos21m2l2 ( m2l2m2l1l2cos2 2(26)(2)求系统总势能系统总势能为：P=1gghsi1l2sin(r 七)(27)(3 )求拉格朗日函数L 二 K -P1 2121=(m2l1m1l1m2*2 6 611 2 1 2 m2l1l2COSR)眉口2丨2二2 61l2 sin(K乙)2(-m2l；31m2 hl 2 cost 2)斗2(28)(4 )列写动力学方程按照拉格朗日方程，对应关节1、2的驱动力矩分别为:,:Ld cL2(29):L=血112 3m11m2l|3 2 2- 1 2 1 m2l1l2 co2( m2l；m2l

12、1l 2 cos2232.:Li=(m2l1212 1 2 12 1m2l2m2l1l2 co2k1 ( m2l2m2hl2 cosr2)二23332-m?l 1122sin 二2巧2m2l1l2 sin 些22.L11-( m m2)gl1cosi1m2gl2 cos v2)2-1 2 1 2 1m2l2 m2hl2 cos v2)( m2l2m2l1l2 co22332- 1 2 1 1-m2l1l2sin 丁2刊 丁2m2l1l2sin 丁2丁2 ( m1m2)gl1 cosm2gl2cos(r 丁2)2 2 2(30)同理:(1 m2l2231m2l112 cos2 k12.1. 2-

13、1 mdil22 COS K 门 1 2 h m212 】231mhhl2sin R*221 . 1m2l1l2 sin J2冇m2gl2cos 2)2 2121 121-212=( m2l2m2l1l2cos2y1m2l2*m2l1l2sin)2m2gl2 cos(-1v2)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 日21m2l1l2

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

15、的关节角， 确定中间位置的关节角。(1)非归一化和归一化问题(2)末端位置的轨迹、关节空间轨迹 规划的缺点。哺自由度机器扎犬审空同鴨非旧化込劝两自由庭机器人关节空间的归一优运动三次多项式轨迹规划娶求：机器人上某关节在运动开始时刻包前销度为耳在时刻与运动到角度用三次多项式规划该关节的运 V)= +邙+C3几初始条杵与未端祭件：-述=2啲)=&严确定系数钿兮Cj .以确定理:；id方的表达式a卫举例：要求一个5轴机器人的第一关节在5秒之内从初始角30度运动到终端角75 度，用三次多项式计算在第1、2、3、4秒时的关节角。S(fj) = c0 = 30fcq - 30卿+ G+门俘珊刃-75如皿=

16、G 二。pi = 5-4鮒 q 5 +地为+女矽)=0Uj = 772由此得到位跟速度和加I速度的多顶武方程如下：&(0 = 30 + 5A1 -附=10L& - 2A6i2 fl(/)二 10.fi 432/ft人时间求得：协=34.68*袖=必開”&=59.16D。=皿妙五次多项式轨迹规划已知机器人上某关节在运动开始时刻专=0的角度为q、速度矗、 加速度在时刻兮运动到舷&八 速度为6、加速度为6 要求 用五次多项式规划该关节的运动。40t) =+cf + cf*初始条件与末端条件：*m(珀)=烤、总(鮎)=o、罠fj)=虽 !&(切=B f、日(fi = 0 i 段心、=19/ +-1确定

17、系数 %巾、令 6，以确定0(t)d(t)诺(t)的表达式。W 同例51,且巳知初始加速度和隶端减速度均为5度/抄3解：由例5和给出的加速度值得到：= 30* 反0度/秒 9, = 5度/秒2坷=方 & = 0度/妙场=-5度/秒丄游初始和末端边界条件代入式5,5)-式(5.7 ),得出:Co = 30Cj = 0c2 = 2.5Cj = 1.6c4 = T.58q = 0,0464进而得到如下运动方程；fl(r) = 30 + 2.5? + 1.6? - 0.58r4 + 0.0464rJ強 - Sr + 48尸-2J2tJ + 0.232?tf(f) = 5 + 9.6r -+ 0.928

18、/3图5.12是机器人关节的位置、速度利I加速度曲线，其疑大加速度为&7度/秒J图工12例乞3的关节位置、速度和加速度抛物线过渡的线性运动轨迹规划（略）具有中间点以及用抛物线过渡的线性运动轨迹规划（略） 高次多项式运动轨迹规划（略）直角坐标空间的轨迹规划（1）所有用于关节空间的轨迹规划方法都可以用于直角坐标 空间轨迹规划；（2）直角坐标轨迹规划必须不断进行逆运动学运算，以便及时得到关节角。 这个过程可以归纳为以下计算循环：（a）将时间增加一个增量；（b）禾U用所选择的轨迹函数计算出手的位姿；（c）利用逆运动学方程计算相应的关节变量；（d）将关节变量信息送给控制器;（e）返回到循环的开始。图眾了

19、具有加雀和减速段的勒迹規划例5.6个两自由度平面机篙人要求从起点(3+ 10)浴直线运动到终点(8, 14 )0假设弼径分为10段，求出机器人的关节变童 每一很连杆的长度为9英寸J解：直角坐标空间中起点和终点间的Jt线可以描述为:或者y = 0.8x + 7.6中间点的坐标可H通过将起点和终点的工、y坐标之差简单地加以分劃得到，然后通过求 解逆运动学方程得到对应毎个中间点的两个关节角。结果见表5和图5J7o表5.1例56的坐标和关节角#Xy%鬲131018.810923.510.419104.03410.819.5100.444.51L220.295.85511,621.390.965.51222.58577612424.180,186.512+826742971322&267.8107.513.630.860.71181433+95