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

1、 一、平面二连杆机器人手臂运动学平面二连杆机械手臂如图 1 所示，连杆 1 长度l ，连杆 2 长度l 。建立如图 1 所示的坐12(x , y )(x , y ) (x , y )、 为连体坐标系，标系，其中，为基础坐标系，固定在基座上，001122分别固结在连杆 1 和连杆 2 上并随它们一起运动。关节角顺时针为负逆时针为正。y0y2dxpb222cx11y11ax0图 1 平面双连杆机器人示意图1、用简单的平面几何关系建立运动学方程连杆 2 末段与中线交点处一点 p 在基础坐标系中的位置坐标：x = l cos +lpq+cos(q q )11212（1）y = l sin +lq+si

2、n(q q )p112122、用 d-h 方法建立运动学方程假定 z 、z 、 垂直于纸面向里。从到的齐次旋转变换矩阵为：z(x , y , z ) (x , y , z )012000111cosq - sinq 0 011sinqcosq0 0=01t11（2） 0001 000 1(x , y , z ) (x , y , z )的齐次旋转变换矩阵为：从到111222cosq - sinq 0 l221sinqcosq0 0=12t22（3） 0001 000 1(x , y , z ) (x , y , z )从到的齐次旋转变换矩阵为：0002221 cosq - sinq 0 0 c

3、osq - sinq 0 l 11221 sinqcosq 0 0 sinqcosq0 0 = t t =02t01121122 0001 0 0001 0 00 100 1（4）cos(q q )+-sin(q q ) 0+lcosq121211sin(q +q ) cos(q +q ) 0 l sinq=10212110100 001那么，连杆 2 末段与中线交点处一点 p 在基础坐标系中的位置矢量为：cos(q q )+-sin(q q ) 0 l cosq+ l 1212112 0sin(q +q ) cos(q +q ) 0 l sinq = t p =0p0221021211010

4、0 0 0011 （5）cosq + l cos(q q )+ x l11212p l sin + lq+sin(q q )y =11212p0 z p11 即，x = l cos +lpq+cos(q q )11212（6）y = l sin +lq+sin(q q )p11212与用简单的平面几何关系建立运动学方程（1）相同。q 、q1建立以上运动学方程后，若已知个连杆的关节角，就可以用运动学方程求出机2械手臂末端位置坐标，这可以用于运动学仿真。3、平面二连杆机器人手臂逆运动学建立以上运动学方程后，若已知个机械臂的末端位置，可以用运动学方程求出机械手臂q 、q1二连杆的关节角，这叫机械臂的

5、逆运动学。逆运动学可以用于对机械臂关节角和末2端位置的控制。对于本例中平面二连杆机械臂，其逆运动学方程的建立就是已知末端位置(x , y )q 、q1求相应关节角的过程。推倒如下。2pp（1）问题x = l cos +lpq+cos(q q )11212y = l sin +lqsin(q q )+p11212已知末端位置坐标(x , y )，求关节角q 、q1。2pp（2）求q12 由（6）式得到：(x - l cosq ) + (y - l sinq ) = l2222（7）（8）p11p11整理得到：x2+ y2+ l21-l22= 2l (x cos + yq1sinq )pp1pp1

6、令xsinqcosqq= tg =（9）ppppyp由（8）式得到：2l xq cosq sinq sinq )xx2+ y+ y2+ l21- l- l22=1(cos+pcosqpp1p1pp2l xq q )+ l=cos( -1p（10）（11）222122cosqpp1pp由此可解出q 。1 + + -x2y2l21l22yq arccoscosq+ arctg=pppp2l xx1p1p（3）求q2由（6）式得到：x - l cos(q +q ) +y - l sin(q +q ) = l2221（12）（13）p212p212整理得到：x2+ y2+ l22- l21= 2l x

7、 cos( +q q )+ y+sin(q q )pp2p12p12令xsinqq= tg =（14）ppppycosqp由（14）式得到：2l xx2+ y2+ l22- l21=2cos( +q q ) cosq sin(q q ) sinq +pcosqpp12p12pp（15）2l x=cos(q +q -q )2pcosq12ppq由此可解出 。23 + + -x2y2l22l21yxq arccoscosq-q=+ arctg（16）pppp2l x2p12p二、平面二连杆机器人手臂的速度雅可比矩阵速度雅可比矩阵的定义：从关节速度向末端操作速度的线性变换。现已二连杆平面机器人为例推

8、导速度雅可比矩阵。x = l cos +lpq+cos(q q )11212y = l sin +lq+sin(q q )p11212上面的运动学方程两边对时间求导，得到下面的速度表达式：dx&1& &= -lsinq q- l sin(q +q ) (q +q )pdt1121212（17）（18）dy&1& &q +q ) (q q )= lcosq q+ l cos(+pdt1121212把上式写成如下的矩阵形式：&sinqsin(q q )sin(q +q )q -+-xl1ll= p1212212 1&lcosqcos(q q )+cos(q q )+ l+lqy p11212212

9、2& x&= x，令上式中的末端位置速度矢量p&y p&q &= q，关节角速度矢量1&q 2sinq - l sin(q +q ) - l sin(q +q )- l= j(q ,q )矩阵11212212l cos + lq1cos(q q )+l+cos(q q )121212212j(q ,q )就是速度雅可比矩阵，实现从关节角速度向末端位置速度的转变。（18）式可12以写成：&x = j (&q ,q ) q12速度雅可比矩阵可以进一步写成：sinq - l sin(q +q ) - l sin(q +q )- lj(q ,q )=11212212lcosq + l cos(q +q

10、 ) l cos(q +q )1211212212（19）jjj=111222j 21其中，4 x= -l sin - lq+sin(q q )j =11pq112121xj =12= -l sin(q +q )pq2122（20）yj = l cos + lqcos(q q )+pqyq21112121p2j = l cos(q +q )22212由此可知雅可比矩阵的定义：xx pppjqyqyjjj(q ,q )=111222（21）1212j 21pq q12三、平面二连杆机器人手臂的动力学方程推倒动力学方程的方法很多，各有优缺点。拉格朗日方法思路清晰、不考虑连杆之间的内力，是推倒动力学

11、方程的常用方法。下面推导图 1 所示的平面双连杆机器人的动力学方程。图 1 中所示连杆均为均质杆，其转动惯量分别是 和 。ii121、求两连杆的拉格朗日函数（1）求系统总动能连杆 1 的动能为：1&a 1qk = i221（21）1 11&1&21= ( m l ) = m lqq21 1221 12 36求连杆 2 质心 d 处的线速度：对连杆 2 质心位置求导得到其线速度。连杆 2 质心位置为：1x = l cos + ldq+cos(q q )211212（22）（23）1y = l sin + ldq+sin(q q )211212连杆 2 质心速度为：1&1& & = - sinq

12、q- l sin( +q q ) (q q )xl1 +2d1212121& &cosq q+ l cos(q q ) (q+q )y= l2d11121212111&1&2&cosq )q q= x& + y& = (l + l + l l cosq )q+ lq (+ l + l lv2222122222222442ddd1 221 221 25 （24）连杆 2 的动能：11& &(q q )k =i+ m v22222d122d1 11111& &1&2&1 2=( m l )(q q )+ m l + l + l l(cosq )q + l q + ( l + l l cosq )q

13、 q 22 2221222222222 12124421221 221 221112&2&= m (l + l + l l cosq )q+ m lq+ m(l + l lcosq )q q222222236232121 2212 2221 2212(25)系统总动能：k = k + k1211112&1&2&cosq )q q= m (l+ l+ l l cosq )q+ m lq+ m l(+ l l222222 222223623211 2221 221 21111111&1&2&cosq )q q= ( m l + m l + m l + m l l cosq )q+ m lq (+

14、m l + m l l22 121 122 2222 2222 226626322 1 222 1 221 2（26）（2）求系统总势能系统总势能为：11p = m gl sin m g lq + ( sinq + sin(q +q )l（27）22111211212（3）求拉格朗日函数l = k - p1111111&1&2&cosq )q q= ( m l + m l + m l + m l l cosq )q+ m lq (+ m l + m l l22 121 122 2222 2222 226626322 1 222 1 221211- m gl sin - m gl sinqq+

15、l+sin(q q )22111211212（28）（29）（4）列写动力学方程按照拉格朗日方程，对应关节 1、2 的驱动力矩分别为： l ltt=-&1t q q121 ll-&t qq22l1111&1&2=(m l + m l + m l + m l l cosq )q+ ( m l + m l l cosq )q22 121 122 222 2&q33322 1 222 1 2216 l&=1111&(m l + m l + m l + m l l cosq )q&q )q+ ( m l + m l l cos22 121 122 222 2t q33322 1 2212 1 2221

16、1&22- m l lsinq q q- m l lsinq q22 1 22 122 1 22l11= -( m + m )gl cos - m glq+cos(q q )q221211221211111&1&2t (cosq )q+ ( m l + m l l cosq )q= m l + m l + m l + m l l22 121 122 222 2333212 1 222 1 221211& &sinq q q -&22- m l lm l l2 1 2sinq q + ( m + m )gl cosq + m gl cos(q +q )222 1 22 12212112212（3

17、0）同理：l 111&2&1q(cosq )q= m l+ m l + m l l22 222 2&q3322 1 222 l11112&( m l + m l l cosq )q&2&sinq q qq -=+ m lm l l2 1 222 2&t q3232 1 2212 222122l111&1&= - m l lsinq q - m l l sinq q q- m gl cos(q +q )2q2222 1 222 1 22122212211111&1&2&21t (= m l + m l lcosq )qqsinq qcos(q q )+ m l3+ m l l+ m gl+22

18、222 2322222 1 222 1 222212（31）联合（30）、（31）式，将动力学方程写成如下矩阵形式：11131m l+ m l+ m l+ m l l cosm l+ m l lcosqq22 121 122 222 2&tq 2 313122 1 222 1 2=1 t 11&q 2 m l22 2+m l l2 1 2cosqm l22 22323210- m l l sinq& q 2222 1 2+ 1q1&sinq0 2m l l2 1 222211( m + m )gl cosq + m gl cos(q +q )&sinq 0 q q - m l l2212112212+2 1 2212 &0 q