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

1、最新用平面二连杆机器人为例贯穿运动学雅可比动力学轨迹规划甚至控制与编程文件doc一、平面二连杆机器人手臂运动学平面二连杆机械手臂如图 1 所示， 连杆 1 长度 l1 ，连杆 2 长度 l 2 。建立如图 1 所示的坐标系，其中， ( , )x0 y 为基础坐标系，固定在基座上， (x1 ,y1) 、 ( x2, y2 ) 为连体坐标系，0分别固结在连杆 1 和连杆 2 上并随它们一起运动。关节角顺时针为负逆时针为正。y0y2BDx2 P22Cx11y11Ax0图 1 平面双连杆机器人示意图1、用简单的平面几何关系建立运动学方程连杆 2 末段与中线交点处一点 P 在基础坐标系中的位置坐标：xp

2、ypl1l1cossin11llcos(2sin(211)22)（1）2、用 D-H 方法建立运动学方程假定z 、z1、z2 垂直于纸面向里。 从( x0 , y0 , z0 )到( x1 , y1, z1 ) 的齐次旋转变换矩阵为：0cos sin 0 01 1sin cos 0 0 0 1 11T （2）0 0 1 00 0 0 1从(x1, y , z ) 到 (x2, y2, z2 ) 的齐次旋转变换矩阵为：1 1cos sin 02 2l1sin cos 0 0 1 2 2T （3） 20 0 1 00 0 0 1从(x0 ,y , z )到( x2 , y2 ,z2 ) 的齐次旋转

3、变换矩阵为：0 01cos sin 0 0 cos sin 01 1 2 2l10T20T112Tsin01cos010100sin02cos0201000cos(1 20)0sin(1120) 0 l10 0cos11（4）sin( ) cos( ) 0 l sin1 2 1 2 1 10 0 1 00 0 0 1那么，连杆 2 末段与中线交点处一点 P 在基础坐标系中的位置矢量为：cos( ) sin( ) 0 l cos1 2 1 2 1 1l20P022T Psin(102)cos()0l1 2 101sin0100l1cos01l2cos(10)2xp0 1 1（5）l1sin1l

4、sin(212)yp0zp1 1即，xyppl1l1cossin11llcos(2sin(211)22)（6）与用简单的平面几何关系建立运动学方程（ 1）相同。建立以上运动学方程后，若已知个连杆的关节角 1、 2 ，就可以用运动学方程求出机械手臂末端位置坐标，这可以用于运动学仿真。3、平面二连杆机器人手臂逆运动学建立以上运动学方程后， 若已知个机械臂的末端位置， 可以用运动学方程求出机械手臂二连杆的关节角1、 ，这叫机械臂的逆运动学。逆运动学可以用于对机械臂关节角和末2端位置的控制。对于本例中平面二连杆机械臂，其逆运动学方程的建立就是已知末端位置(xp , yp ) 求相应关节角 1、 2 的

5、过程。推倒如下。（1）问题xpl1cos l cos( )1 2 1 2ypl sin11lsin(212)已知末端位置坐标 ( , )xp y ，求关节角 1、 2 。p（2）求12由（ 6）式得到：2 2 2 (x l1 cos ) ( y l sin ) lp （7）1 p 1 1 2整理得到：2 2 2 2xp y l l 2l1(x cos 1 y sin 1) （8）p 1 2 p p令xyp tgppsincospp（9）由（ 8）式得到：2 px2py2l1l222lxpp1cos(coscos sin sin1 p 1 p)2l x 2 2 2 2 1 px y l l co

6、s( ) （10）p p 1 2 1 pcosp由此可解出1 。2 2 2 2x y l l yp p 1 2 parccos cos arctg1 （11）p2l x x1 p p（3）求2由（ 6）式得到：2 2 2xp l2 cos( ) yp l sin( ) l （12）1 2 2 1 2 1整理得到：2 2 2 2xp y l l 2l2x cos( 1 2 ) y sin( 1 2 ) （ 13）p 2 1 p p令xyp tgppsincospp（14）由（ 14）式得到：2 px2py2l2l212l2cos2l2cosx ppxppcos()cos sin( sin1 2

7、p 1 2 pcos( )1 2 p)（15）由此可解出2 。32 2 2 2x y l lp p 2 12 arccos cos2l x2 pparctgypxp1（16）二、平面二连杆机器人手臂的速度雅可比矩阵速度雅可比矩阵的定义： 从关节速度向末端操作速度的线性变换。 现已二连杆平面机器人为例推导速度雅可比矩阵。xpl1co s1lco s(2 1 2)ypl si n11lsi n (2 12)上面的运动学方程两边对时间求导，得到下面的速度表达式：dxpdtdydtpl1l1cossin l sin( ) ( )1 1 2 1 2 1 2l cos( ) ()1 1 2 1 2 1 2

8、（17）把上式写成如下的矩阵形式：xyppll1sin1cos11ll2sin(2cos(11)2)2ll2sin(2cos(112)2)12（18）xp令上式中的末端位置速度矢量 Xyp，关节角速度矢量1 ，2l sin l sin( ) l sin( )1 J1 2 1 2 2 1 2矩阵 ( , )1 2l cos l cos( ) l cos( )1 1 2 1 2 2 1 2J( 1, 2 ) 就是速度雅可比矩阵，实现从关节角速度向末端位置速度的转变。 （18）式可以写成：X J( 1 , 2 )速度雅可比矩阵可以进一步写成：J(,12)J11ll1sin1cosJ1211ll2si

9、n(2cos(11)22)ll2sin(2cos(112)2)（19）JJ 21 22其中，4J11xpl1sin l sin( )1 2 1 21J12xpl sin( )2 1 22（20）J21yp1l cos l cos( )1 1 2 1 2J22yp2l2cos( )1 2由此可知雅可比矩阵的定义：xpxpJ J11 121 2J( 1, ) （21）2J y yJp p21 221 2三、平面二连杆机器人手臂的动力学方程推倒动力学方程的方法很多， 各有优缺点。 拉格朗日方法思路清晰、 不考虑连杆之间的内力，是推倒动力学方程的常用方法。 下面推导图 1 所示的平面双连杆机器人的动力

10、学方程。图 1 中所示连杆均为均质杆，其转动惯量分别是 I1 和 I 2 。1、求两连杆的拉格朗日函数（1）求系统总动能连杆 1 的动能为：1K I A121 1 (2 3212m l1 1)21162m l1 121（21）求连杆 2 质心 D 处的线速度：对连杆 2 质心位置求导得到其线速度。连杆 2 质心位置为：xDyDl1l1cossin111 l21 l222cos(sin(11)22)（22）连杆 2 质心速度为：xDl sin1 1 112l2sin(12)(12)YDl1cos1 112l cos( ) ( )2 1 2 1 2（23）1 1 12 cos )2 2 2 2 2

11、 2 2 2VD xD yD (l l l l cos ) l ( l l l1 2 1 2 2 1 2 2 2 1 2 24 4 21 25（24）连杆 2 的动能：1 12 2K I D ( ) m VD2 1 2 22 2121(12m l22)(21 22)12m (2l2114 22ll l cos )1 2 22114l22221 2(l22l l12cos2)12122m (l2 113l22l l12cos)22116m l222221 2m2(23l22l l12cos2)1 2(25)系统总动能：KK1K212m22(l113l 22l l cos1 22)2116m l2

12、222212m (223l22l l12cos2)1 212(m22l1162m l1 116m l2 2212m l l2 12cos )221162m l2 2221 3(m l22212m2l l cos1 22)12（26）（2）求系统总势能系统总势能为：1 1P m1gl sin m g(l sin l sin( ) （27）1 1 2 1 1 2 1 22 2（3）求拉格朗日函数L K P(122m l2 1162m l1 116m l22212m2l l cos1 22)2116m2l2222(13m l22212m l l2 12cos2)1 212m1gl1sinm gl s

13、in1 2 1 112l2sin(12)（28）（4）列写动力学方程按照拉格朗日方程，对应关节 1、2 的驱动力矩分别为：L L1t1L1L（29）2t2 2L11 1 1 12 2 2 2(m l m l m l m l l c o s ) ( m l m l l2 1 1 1 2 2 2 1 2 2 1 2 2 2 1 23 3 3 2c o s2)26L12(m l2 1132m l1 113m l2 22m l l2 12cos )211 3(2m l2 212m l l cos2 1 22)2tm l l2 12sin21212m l l2 12sin222L11 1( m1 m2)

14、 gl1 cos 1 m2gl2 cos(1 2 2 2)2(m l2 113m l12113m l222m l l2 12cos )211 3(m22l212m l l cos2 1 22)1 2m l l2 12sin21212m l l sin2 1 2222(12m m )gl cos1 2 1112m2gl2cos(12)（30）同理：L213m l2 2221 3(m l22212m l l2 1 2cos )21L21 1 1 12 2( m l m l l cos ) m l m l l2 2 2 1 2 2 1 2 2 2 2 13 2 3 22sin2 1 2tL212m l l2 12sin22112m l l2 12sin21212m2gl2cos(12)1 1 1 1 12 2 22 ( m l m l l cos ) m l m l l sin m2 gl2 cos( 1 2)2 2 2 1 2 2 1 2 2 2 2 1 2 2 13 2 3 2 2（31）联合（ 30）、（31）式，将动力学方程写成如下矩阵形式