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

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

《完整版用平面二连杆机器人为例贯穿运动学雅可比动力学轨迹规划甚至控制与编程.docx》由会员分享，可在线阅读，更多相关《完整版用平面二连杆机器人为例贯穿运动学雅可比动力学轨迹规划甚至控制与编程.docx（23页珍藏版）》请在冰豆网上搜索。

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

一、平面二连杆机器人手臂运动学

平面二连杆机械手臂如图1所示，连杆1长度l1，连杆2长度l2。

建立如图1所示的坐标系，其中，（x0,y0）为基础坐标系，固定在基座上，（x1,y1）、（x2,y2）为连体坐标系，

分别固结在连杆1和连杆2上并随它们一起运动。

关节角顺时针为负逆时针为正。

y0

y2

B

2

C

x1

1

x0

图1平面双连杆机器人示意图

1、用简单的平面几何关系建立运动学方程

连杆2末段与中线交点处一点P在基础坐标系中的位置坐标：

xpl1cos1l2cos（12）

ypl1sin1l2sin（12）

2、用D-H方法建立运动学方程

假定z0、z1、

z2垂直

于纸面向

里。

从（x0,y0,z0）到（x1,y1,z1）的齐次旋转变换矩阵为：

cos1

sin

10

0

0sin1

cos

10

0

01T1

（2）

10

0

1

0

0

0

0

1

从（x1,y1,z1）

到（x2,

y2,z2）

的齐次旋转变换矩阵为：

cos2

sin

20

l1

1sin2

cos

20

0

12T2

（3）

20

0

1

0

0

0

0

1

从（x0,y0,z0）到（x2,y2,z2）的齐次旋转变换矩阵为：

cos

1

sin

10

0

cos

2

sin2

0

l1

001sin

20T01T21T

1

cos

10

0

sin

2

cos2

0

0

2120

0

1

0

0

0

1

0

0

0

0

1

0

0

0

1

（4）

cos（

1

2）

sin（

1

2）

0

l1cos1

sin（

1

2）

cos（

1

2）

0

l1sin1

0

0

1

0

0

0

0

1

cos（1

2）sin（1

0P02T2P

sin（

1

2）cos（

1

0

0

0

0

l1cos

1

l2cos（1

2）

l1sin

1

l2sin（1

2）

0

1

即，

xp

l1cos

1

l2cos（1

2）

yp

l1sin

1

l2sin（1

2）

6）

那么，连杆2末段与中线交点处一点P在基础坐标系中的位置矢量为：

与用简单的平面几何关系建立运动学方程

（1）相同。

械手臂末端位置坐标，这可以用于运动学仿真。

3、平面二连杆机器人手臂逆运动学

建立以上运动学方程后，若已知个机械臂的末端位置，可以用运动学方程求出机械手臂二连杆的关节角1、2，这叫机械臂的逆运动学。

逆运动学可以用于对机械臂关节角和末

端位置的控制。

对于本例中平面二连杆机械臂，其逆运动学方程的建立就是已知末端位置

（xp,yp）求相应关节角1、2的过程。

推倒如下。

（1）问题

xpl1cos1l2cos（12）

ypl1sin1l2sin（12）

已知末端位置坐标（xp,yp），求关节角1、2

2）求1

由（6）式得到：

（xpl1cos1）2

（ypl1sin1）2l22

7）

整理得到：

2

xp

2

yp

l12

l22

2l1（xpcos1

ypsin

1）

8）

xp

yp

tg

sin

9）

cos

由（8）式得到：

2

x2p

2

y2p

l12

l22

2l1xp（cos1cosp

cosp

sin1sin

p）

2

x2p

2

y2p

l12

l22

2l1xp

cos（1

cosp

p）

10）

由此可解出

1。

1arccos

2

x2p

222

ypl1l2

cos

2l1xp

arctgyp

xp

11）

3）求2

由（6）式得到：

[xpl2cos（

2）]2[ypl2sin（1

2）]2l12

12）

整理得到：

2

x2p

2

y2p

l22

l12

2l2[xpcos（1

2）

ypsin（1

2）]

13）

xp

tg

sin

yp

cos

14）

由（14）式得到：

2222

x2py2pl22l12

2l2xp

[cos（1

cosp

2l2xp

cos（1

cosp

2）cospsin（

12）sin

p]

15）

由此可解出

2。

2xparccos

yp2l22l12

cos

2l2xp

arctgyp

xp

二、平面二连杆机器人手臂的速度雅可比矩阵

现已二连杆平面机器

速度雅可比矩阵的定义：

从关节速度向末端操作速度的线性变换。

人为例推导速度雅可比矩阵。

xpl1cos1l2cos（12）ypl1sin1l2sin（12）

把上式写成如下的矩阵形式：

yp

关节角速度矢量

18）式可

J（1,2）就是速度雅可比矩阵，实现从关节角速度向末端位置速度的转变。

以写成：

XJ（1,2）

速度雅可比矩阵可以进一步写成：

J（1,2）

l1sin1l2sin（1

l1cos1l2cos（1

2）l2sin（12）

2）l2cos（12）

19）

11

12

J21J22

其中，

J11

xp

1

l1sin1

l2sin（1

2）

J12

xp

l2sin（1

2）

2

J21

yp

1

l1cos1

l2cos（1

2）

J22

yp

l2cos（1

2）

xp

xp

J11J12

J（1,2）1112

1

2

J21J22

yp

yp

12

21）

由此可知雅可比矩阵的定义：

三、平面二连杆机器人手臂的动力学方程推倒动力学方程的方法很多，各有优缺点。

拉格朗日方法思路清晰、不考虑连杆之间的内力，是推倒动力学方程的常用方法。

下面推导图1所示的平面双连杆机器人的动力学方程。

图1中所示连杆均为均质杆，其转动惯量分别是I1和I2

1、求两连杆的拉格朗日函数

21）

（1）求系统总动能连杆1的动能为：

1

2

K1

IA1

2

1

122

122

（m1l1）1

m1l11

2

3

6

22）

求连杆2质心D处的线速度：

对连杆2质心位置求导得到其线速度。

连杆2质心位置为：

1

xD

l1cos1

l2cos（12）

2212

yD

l1sin1

l2sin（12）

2

连杆2质心速度为：

xD

l1sin11l2sin（

2

1

l1cos11l2cos（

11122

12）（1

2）

（23）

YD

12）（1

2）

VD2

22212xDyD（l1l2

4

l1l2cos2）1

122l22

4

（12l2

l1l2cos2）12

连杆2的动能：

K2ID（12

2

1122（12m2l2）（1

1212m2（l1l2

23

12

m2VD

2

212

2）2m2[（l12

2221

l1l2cos2）1

1l2

l2

4

12

m2l2

6

l1l2cos2）1

12m2（23l22

23

系统总动能：

KK1K2

1l22

3l2

1m1l12

611

12

2m2（l1

12（2m2l12

l1l2cos2）

1m2l22

622

12

m2l2

622

1m2l1l2cos2）

1l22

4

l1l2cos

122m2（l2

2232

12m2l2

622

12

（2l22l1l2cos2）12]

（25）

l1l2cos2）1

（13m2l22

3

26）

m2l1l2cos2）12

2）求系统总势能

系统总势能为：

1Pm1gl1sin

1

m2g（l1sin1l2sin（1

2））

27）

3）求拉格朗日函数

LKP

（1m2l121m1l12

221611

1

m1gl1sin1m2g[l1sin

1m2l22

622

m2l1l2cos2）1

221221

1

l2sin（12）]

2

1m2l22

622

（1m2l22

322

m2l1l2cos2）12

2212212

28）

4）列写动力学方程

按照拉格朗日方程，对应关节

1、2的驱动力矩分别为：

t1

L

29）

（m2l12

121

m1l1m2l

31132

12

m2l1l2cos2）1（m2l2

3

1

m2l1l2cos2）2

Lt1

1m1l12

3

1m2l22

3

m2l1l2cos2）1

（3m2l2

m2l1l2cos2）2

2

m2l1l2sin212

m2l1l2sin22

2

（m2l12

同理：

（12m1

1m1l12

3

m2l1l2sin

L1m2l222

3222

m2）gl1cos1

（1m2l22

322

m2gl2cos（1

12

m2l2

3

1

12m2l1l2sin2

2

m2l1l2cos2）

2）

1

m2l1l2cos2）2

2

12

（m2l2

3

1

（m1m2）gl1cos1

2

1

m2gl2cos（

2

12）

30）

（1m2l22

322

m2l1l2

2212

1

m2l1l2sin2

2

（1m2l221m2l1l2cos

32

1m2l1l2cos

2212

cos2）

1

m2l1l2

2

2）

1m2l22

322

sin