《用平面二连杆机器人为例贯穿运动学、雅可比、动力学、轨迹规划甚至控制与编程》由会员分享，可在线阅读，更多相关《用平面二连杆机器人为例贯穿运动学、雅可比、动力学、轨迹规划甚至控制与编程（15页珍藏版）》请在金锄头文库上搜索。
1、一、平面二连杆机器人手臂运动学平面二连杆机械手臂如图1所示，连杆1长度l ,连杆2长度l。建立如图1所示的坐12标系，其中，(x ,y )为基础坐标系，固定在基座上，(x,y )、(x ,y )为连体坐标系，0 0 1 1 2 2 分别固结在连杆1 和连杆2上并随它们一起运动。关节角顺时针为负逆时针为正。图 1 平面双连杆机器人示意图1、用简单的平面几何关系建立运动学方程连杆2末段与中线交点处一点P在基础坐标系中的位置坐标:x = / cos0 +1 COS(e +0 )p 1 1 2 1 2y = l sin0 +l sin(0 +0 )p 1 1 2 1 22、用D-H方法建立运动学方程假
2、定 z0 、 z1Z 2垂直于纸面向里。从(xo, y o, Z 0)到(xi, yi, Zi)的齐次旋转变换矩阵为:cos0 sin 00 01112)sin0cos0001100100001从（xi, yi, Zi）到（x2, y2，2)的齐次旋转变换矩阵为:cos0 sin 02 2sin0cos0101T 二 222 0 0从（x0，y0，Z0）到（x2, y2，Z2)的齐次旋转变换矩阵为:cos 0- sin 0iisin0cos0oT=o T -1T = ii2 12 0 00000001001cos02sin0200-sin02cos02000l1001001cos(0 +0 )
3、- sin(0 +0 )0 l cos01 2 1 2 1 1 sin(0 +0 )cos(0 +0 )0 l sin01 2 1 2 1 10 0 1 00 0 0 1那么，连杆2末段与中线交点处一点P在基础坐标系中的位置矢量为:4)cos(0 +0 )12sin(0 +0 )1200- sin(0 +0 )12 cos(0 +0 )20010l cos0 +l cos(0 +0 )11212l sin0 +l sin(0 +0 )112101xpy=pzp15)即，x = / cos0 +l cos(0 +0 )p 1 1 2 1 2y = l sin0 +l sin(0 +0 )p 1
4、1 2 1 2与用简单的平面几何关系建立运动学方程(1)相同。6)建立以上运动学方程后，若已知个连杆的关节角0 、0 ，就可以用运动学方程求出机12 械手臂末端位置坐标，这可以用于运动学仿真。3、平面二连杆机器人手臂逆运动学 建立以上运动学方程后，若已知个机械臂的末端位置，可以用运动学方程求出机械手臂 二连杆的关节角0 、0 ，这叫机械臂的逆运动学。逆运动学可以用于对机械臂关节角和末12 端位置的控制。对于本例中平面二连杆机械臂，其逆运动学方程的建立就是已知末端位置 (x ,y )求相应关节角0 、0 的过程。推倒如下。p p 1 2(1)问题x = l cos0 +l cos(0 +0 )p
5、 11 21 2y = l sin0 +l sin(0 +0 )p 1 1 21 2已知末端位置坐标(x ,y )，求关节角0、0。p p 1 22)求01由（6）式得到：(x 一 l cos0 )2 + (y 一 l sin0 )2 = 12 p 1 1 p 1 1 2整理得到：x2 + y 2 +12 一 12 = 21 (x cos0 + y sin0 )p p 1 2 1 p 1 p 1 令xsin 0p = tg0=py p cos 0pp8）式得到：x2p+ 12 121221 x1 pcos0p(cos0 cos0 + sin0 sin0 )1p1px2p+ 12 121221
6、x1 cos(0 0 ) cos01 pp由此可解出01。0 = arccos1x 2 + y 2 +12 一 12pp 1 cos 021 xp1py+ arctg pxp(3)求02由(6)式得到：x 一 1 cos(0 +0 )2 + y 一 1 sin(0 +0 )2 = 12 p 212p 2121整理得到：x2 +y2 +12 一12 =21 x cos(0 +0 )+y sin(0 +0 ) p p 212 p12p12令xsin0p = tg0=pyp cos0pp由(14)式得到：21 xx2 + y2 +12 12 =cos(0 +0 )cos0 + sin(0 +0 )s
7、in0 pp 21cos012p12pp21 x =匕亠cos(0 +0 0 ) cos012 pp由此可解出0 。27）8）（9）（10）（11）（12）（13）（14）（15）0 = arccos2x 2 + y 2 +12 一 12PP2L COS 021 xp2P+ arctgx1P16)二、平面二连杆机器人手臂的速度雅可比矩阵 速度雅可比矩阵的定义：从关节速度向末端操作速度的线性变换。现已二连杆平面机器 人为例推导速度雅可比矩阵。x = 1 cos0 +1 cos(0 +0 )P 11212y = 1 sin0 +1 sin(0 +0 )P 11212上面的运动学方程两边对时间求导，
8、得到下面的速度表达式：dxp = 一1 sin0 0 -1 sin(0 +0 )-(0 +0 )17)dt 11121212dyp = 1 cos0 0 +1 cos(0 +0 ) - (0 +0 )dt 11121212把上式写成如下的矩阵形式：xp=yPP一 1 sin0 一 1 sin(0 +0 )1 1 2 1 21 cos0 +1 cos(0 +0 )1 1 2 1 2一 1 sin(0 + 0 )2 1 21 cos(0 +0 )2 1 2010218)x .令上式中的末端位置速度矢量P = Xy ,P关节角速度矢量川=,02一 1 sin0 一 1 sin(0 +0 )1 1 2
9、 1 21 cos0 +1 cos(0 +0 )1 1 2 1 2一1 爲 I； ? = J Pl,0 2)2 1 2J (0102)就是速度雅可比矩阵，实现从关节角速度向末端位置速度的转变。(18)式可以写成：X = J(0 ,0 )-012速度雅可比矩阵可以进一步写成：J (0 ,012一 1 sin0 一 1 sin(0 + 0 )1 1 2 1 21 cos0 + 1 cos(0 +0 )1 1 2 1 2-1 sin(0 +0 )2 1 21 cos(0 +0 )2 1 219)1112JJ 2122其中，1112= p = -l sin0 -1 sin(e +0 )30112 i
10、213x=p = -1 sin(0 +0 )30 2 1 2220)J = p = 1 cos0 +1 cos(0 +0 )21301 1 2 1 213yJ = p = 1 cos(0 +0 )22302 1 22由此可知雅可比矩阵的定义：J (0 ,01211J 2112223xp03yp3013xp30三、平面二连杆机器人手臂的动力学方程21)推倒动力学方程的方法很多，各有优缺点。拉格朗日方法思路清晰、不考虑连杆之间的内力，是推倒动力学方程的常用方法。下面推导图1 所示的平面双连杆机器人的动力学方程。图1中所示连杆均为均质杆，其转动惯量分别是I和I。 121 、求两连杆的拉格朗日函数（1
11、 ）求系统总动能连杆 1 的动能为：=2G叫汨=6叫件21 )求连杆2质心D处的线速度:对连杆2质心位置求导得到其线速度。连杆2质心位置为:x = 1 cos0 + 1 cos(0 +0 )D 1 1 2 2 1 222)y = 1 sin0 + 1 sin(0 +0 )D 1 1 2 2 1 2连杆2 质心速度为:1x = -1 sin0 0 - 1 sin(0 +0 ) - (0 +0 )D 1 1 1 22 1 2 1 212(23)Y = 1 cos0+ 1 cos(0 +0 ) (0 +0 )D 1 1 1 22 1 2 1 2V2 = x2 + y2DDD1 1 1=(12 + 1
12、2 + 1 1 cos0 )0 2 + 120 2 + (- 12 + 1 1 cos0 )0 01 4 2 1 2 2 1 4 2 2 2 2 1 2 2 1 224)连杆 2 的动能：K = I (0 +0 )2 + 丄m V22 2 D 1 2 2 2D=丄(丄m 12)(0 +0 )2 +丄m2 12 2 2 1 2 2 2=m (12 + -12 +11 cos 0 )02 2 1 3 2 1 2 2 111(12 + 12 +11 cos0 )0 2 + 120 2 + ( 12 +11 cos0 )0 0 1 4 2 1 2 2 1 4 2 2 2 21 1 22 + m 120 2 + m ( 12 +11 cos0 )0 06 22 2 2 2 3 2 12 2 1 24 2 2 2 2 1 2 2 1 2(25)系统总动能：K =K +K12=m (12 + 12 +11 cos0 )0 22 2 1 3 2 1 2 2 1(1 1 1 1=(m 12 + m 12 + m 12 + m2
