用平面二连杆机器人为例贯穿运动学雅可比动力学轨迹规划甚至控制与编程Word文档下载推荐.docx

1、l1 sinl 2 sin（ 1（1）2、用 D-H 方法建立运动学方程假定 z0 、z1 、 z2 垂直于纸面向里。 从 （ x0 , y0 , z0 ） 到 （ x1 , y1 , z1 ） 的齐次旋转变换矩阵为：cos01T sinsin10（ 2）11从 （ x1 , y1 , z1 ） 到 （x2 , y2 , z2 ） 的齐次旋转变换矩阵为：12T sinl1（ 3）0 1从 （ x0 , y0 , z0 ） 到 （ x2 , y2 , z2 ） 的齐次旋转变换矩阵为：.20T 01T 21T2 0 l120 01 0（ 4）cos（ 1sin（sin（ 1那么，连杆2 末段与中线

2、交点处一点P 在基础坐标系中的位置矢量为：l 20P 20T2Pl 1 sinl1 cos 1（ 5）zp即，l1 cos（ 6）与用简单的平面几何关系建立运动学方程（1）相同。建立以上运动学方程后，若已知个连杆的关节角1、 2，就可以用运动学方程求出机械手臂末端位置坐标，这可以用于运动学仿真。3、平面二连杆机器人手臂逆运动学建立以上运动学方程后， 若已知个机械臂的末端位置， 可以用运动学方程求出机械手臂二连杆的关节角 1、 2 ，这叫机械臂的逆运动学。逆运动学可以用于对机械臂关节角和末端位置的控制。对于本例中平面二连杆机械臂，其逆运动学方程的建立就是已知末端位置（ x p , y p ） 求

3、相应关节角 1、 2 的过程。推倒如下。（ 1）问题x p l 1 cos 1 l 2 cos（ 1 2 ）y p l1 sin 1 l 2 sin（ 1 2 ）已知末端位置坐标 （ x p , y p ） ，求关节角 1、 2 。（2）求 1由（ 6）式得到：（x p1 ）2（ y p l1 sin 1 ） 2l 22（7）整理得到：x2py2p l122l1 （ x p cos 1y p sin 1 ）（ 8）令tgp（ 9）由（ 8）式得到：yp2l122l1 xp （cos 1 cos p1 sin p ）2l1 xpp ）（ 10）由此可解出1 。arccosy p2arctgyp（

4、 11）xp（3）求 2 x p2）2 y p l 2 sin（ 1（12）l22l 122l 2 x p cos（ 1 2 ）y p sin（ 1 2 ）（ 13）（ 14）由（ 14）式得到：x2p yp2l 122l 2 xpcos（ 12 ） cos p sin（ 12 ） sin p cos p（ 15）2l 2 xp由此可解出 2 。xp2（ 16）2 arccos2l 2 x p二、平面二连杆机器人手臂的速度雅可比矩阵速度雅可比矩阵的定义： 从关节速度向末端操作速度的线性变换。 现已二连杆平面机器人为例推导速度雅可比矩阵。l 1 cos 1l1 sin 1上面的运动学方程两边对时

5、间求导，得到下面的速度表达式：dxp（ 1dt（ 17）dyp把上式写成如下的矩阵形式：l 2 sin（ 18）令上式中的末端位置速度矢量X ，关节角速度矢量，矩阵J（ 1, 2）J （ 1 , 2 ） 就是速度雅可比矩阵，实现从关节角速度向末端位置速度的转变。 （ 18）式可以写成：X J（ 1, 2）速度雅可比矩阵可以进一步写成：J11J12（19）J 21J22其中，（ 20）J 22由此可知雅可比矩阵的定义：J（ 1,（21）三、平面二连杆机器人手臂的动力学方程推倒动力学方程的方法很多， 各有优缺点。 拉格朗日方法思路清晰、 不考虑连杆之间的内力，是推倒动力学方程的常用方法。 下面推导

6、图 1 所示的平面双连杆机器人的动力学方程。图 1 中所示连杆均为均质杆，其转动惯量分别是 I 1 和 I 2 。1、求两连杆的拉格朗日函数（1）求系统总动能连杆 1 的动能为：K 1I（ 21）（ m1l1） 16m1l1 13求连杆 2质心 D 处的线速度：对连杆2 质心位置求导得到其线速度。连杆2 质心位置为：xDl 2 cos（ 22）1 l 2 sin（y D连杆 2 质心速度为：（ 23）1 l 2 cos（YD1 2VDyD（l14l 1l 2 cosl1l 2 cos 2 ） 1 2（24）连杆 2 的动能：K 21ID（2 ） 21 m2VD2m2l）（m2（ ll1l2 c

7、osl1l 2 cos2 ） 12 12lm2 （l13 l 2l 1l 2m2 l 2l1 l 2 cos（25）系统总动能：KK 12 26 m2 l2m2 （l2（ 1 m l 21 m l1 m l l）（1 m l2 1（26）（2）求系统总势能系统总势能为：m1gl1 sin 1m2g（l1 sin 1 2l 2 sin（ 1 2 ）（ 27）（3）求拉格朗日函数L Kcos 2 ） 1 2m2 l1m1 l1m2 l1 l 21m2 gl 1 sin2 ）m1 gl1 sin1l 2 sin（ 1（28）（4）列写动力学方程按照拉格朗日方程，对应关节1、 2 的驱动力矩分别为：Lt（ 29）（m2 l12m1l12m2l 22m2 l1 l 2 cos 2 ） 1m2 l22m2 l1l 2 cos 2 ） 2（m2l12m1l 12m2l1l 2 cosm2l 1l 2 sin（1 m1m2）gl1 cos 11 m2gl2 cos（ 12）1 （m2 l121 m1l 121 m2l22m 2l1l 2 cos 2 ）（1 m2 l 221 m2l 1l 2 cosm2 l1l 2 sinm2 ）gl 1 cosm1m2 gl 2 cos（ 1（ 30）同理：1 m2l 22