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

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

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

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

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

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

建立如图1所示的坐

标系，其中，（,）

x0y为基础坐标系，固定在基座上，（x1,y1）、（x2,y2）为连体坐标系，

0

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

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

y

0

y

2

B

D

x

2P

2

2

C

x

1

1

y

1

1

A

x

0

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

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

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

x

p

y

p

l

1

l

1

cos

sin

1

1

l

l

cos（

2

sin（

2

1

1

）

2

2

）

（1）

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

假定

z、z1、z2垂直于纸面向里。

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

0

cossin00

11

sincos00011

1T

（2）

0010

0001

从（x1,y,z）到（x2,y2,z2）的齐次旋转变换矩阵为：

11

cossin0

22

l

1

sincos00122

T（3）2

0010

0001

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

00

1

cossin00cossin0

1122

l

1

0

T

2

0

T

1

1

2

T

sin

0

1

cos

0

1

0

1

0

0

sin

0

2

cos

0

2

0

1

0

0

0

cos（

12

0

）

0

sin（

1

1

2

0

）0l

1

00

cos

1

1

（4）

sin（）cos（）0lsin

121211

0010

0001

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

cos（）sin（）0lcos

121211

l

2

0

P

0

2

2

TP

sin（

1

0

2

）

cos（

）

0

l

121

0

1

sin

0

1

0

0

l

1

cos

0

1

l

2

cos（

1

0

）

2

x

p

011

（5）

l

1

sin

1

lsin（

2

1

2

）

y

p

0

z

p

11

即，

x

y

p

p

l

1

l

1

cos

sin

1

1

l

l

cos（

2

sin（

2

1

1

）

2

2

）

（6）

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

（1）相同。

建立以上运动学方程后，若已知个连杆的关节角1、2，就可以用运动学方程求出机

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

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

建立以上运动学方程后，若已知个机械臂的末端位置，可以用运动学方程求出机械手臂

二连杆的关节角

1、，这叫机械臂的逆运动学。

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

2

端位置的控制。

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

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

推倒如下。

（1）问题

x

p

l

1

coslcos（）

1212

y

p

lsin

1

1

l

sin（

2

1

2

）

已知末端位置坐标（,）

xpy，求关节角1、2。

p

（2）求

1

2

由（6）式得到：

222（xl1cos）（ylsin）l

p（7）

1p112

整理得到：

2222

xpyll2l1（xcos1ysin1）（8）

p12pp

令

x

y

ptg

p

p

sin

cos

p

p

（9）

由（8）式得到：

2

p

x

2

p

y

2

l

1

l

2

2

2l

x

p

p

1

cos

（cos

cossinsin

1p1p

）

2lx22221p

xyllcos（）（10）

pp121p

cos

p

由此可解出

1。

2222

xylly

pp12p

arccoscosarctg

1（11）

p

2lxx

1pp

（3）求

2

由（6）式得到：

222

[xpl2cos（）][yplsin（）]l（12）

122121

整理得到：

2222

xpyll2l2[xcos（12）ysin（12）]（13）

p21pp

令

x

y

ptg

p

p

sin

cos

p

p

（14）

由（14）式得到：

2

p

x

2

p

y

2

l

2

l

2

1

2l

2

cos

2l

2

cos

x

p

p

x

p

p

[cos（

）

cossin（sin

12p12p

cos（）

12p

）

]

（15）

由此可解出

2。

3

2222

xyll

pp21

2arccoscos

2lx

2p

p

arctg

y

p

x

p

1

（16）

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

速度雅可比矩阵的定义：

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

现已二连杆平面机器

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

x

p

l

1

cos

1

l

cos（

212

）

y

p

lsin

1

1

l

sin（

21

2

）

上面的运动学方程两边对时间求导，得到下面的速度表达式：

dx

p

dt

dy

dt

p

l

1

l

1

cos

sinlsin（）（）

1121212

lcos（）（

）

1121212

（17）

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

x

y

p

p

l

l

1

sin

1

cos

1

1

l

l

2

sin（

2

cos（

1

1

）

2

）

2

l

l

2

sin（

2

cos（

1

1

2

）

2

）

1

2

（18）

x

p

令上式中的末端位置速度矢量X

y

p

，

关节角速度矢量

1，

2

lsinlsin（）lsin（）

1J

1212212

矩阵（,）

12

lcoslcos（）lcos（）

11212212

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

（18）式可

以写成：

XJ（1,2）

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

J（

1

2

）

J

11

l

l

1

sin

1

cos

J

12

1

1

l

l

2

sin（

2

cos（

1

1

）

2

2

）

l

l

2

sin（

2

cos（

1

1

2

）

2

）

（19）

J

J2122

其中，

4

J

11

x

p

l

1

sinlsin（）

1212

1

J

12

x

p

lsin（）

212

2

（20）

J

21

y

p

1

lcoslcos（）

11212

J

22

y

p

2

l

2

cos（）

12

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

x

p

x

p

JJ

1112

12

J（1,）（21）

2

Jyy

J

pp

2122

12

三、平面二连杆机器人手臂的动力学方程

推倒动力学方程的方法很多，各有优缺点。

拉格朗日方法思路清晰、不考虑连杆之间的

内力，是推倒动力学方程的常用方法。

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

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

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

（1）求系统总动能

连杆1的动能为：

1

KIA

1

2

11

（

23

2

1

2

ml

11

）

2

1

1

6

2

ml

11

2

1

（21）

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

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

连杆2质心位置

为：

x

D

y

D

l

1

l

1

cos

sin

1

1

1

l

2

1

l

2

2

2

cos（

sin（

1

1

）

2

2

）

（22）

连杆2质心速度为：

x

D

lsin

111

1

2

l

2

sin（

1

2

）

（

1

2

）

Y

D

l

1

cos

11

1

2

lcos（）（）

21212

（23）

111

2cos）

22222222

VDxDyD（llllcos）l（lll

121221222122

442

12

5

（24）

连杆2的动能：

11

22

KID（）mVD

2122

22

1

2

1

（

12

ml

2

2

）（

2

12

2

）

1

2

m[（

2

l

2

1

1

4

2

2

l

llcos）

122

2

1

1

4

l

2

2

2

2

1

2

（

l

2

2

ll

1

2

cos

2

）

1

2

]

1

2

2

m（l

21

1

3

l

2

2

ll

1

2

cos

）

2

2

1

1

6

ml

2

2

2

2

2

1

2

m

2

（

2

3

l

2

2

ll

1

2

cos

2

）

12

（25）

系统总动能：

K

K

1

K

2

1

2

m

2

2

（l

1

1

3

l

2

2

llcos

12

2

）

2

1

1

6

ml

2

2

2

2

2

1

2

m（

2

2

3

l

2

2

ll

1

2

cos

2

）

12

1

2

（

m

2

2

l

1

1

6

2

ml

11

1

6

ml

2

2

2

1

2

mll

21

2

cos）

2

2

1

1

6

2

ml

22

2

2

1

3

（

ml

2

2

2

1

2

m

2

llcos

12

2

）

1

2

（26）

（2）求系统总势能

系统总势能为：

11

Pm1glsinmg（lsinlsin（））（27）

11211212

22

（3）求拉格朗日函数

LKP

（

1

2

2

ml

21

1

6

2

ml

11

1

6

ml

2

2

2

1

2

m

2

llcos

12

2

）

2

1

1

6

m

2

l

2

2

2

2

（

1

3

ml

2

2

2

1

2

mll

21

2

cos

2

）

12

1

2

m

1

gl

1

sin

mg[lsin

1211

1

2

l

2

sin（

1

2

）]

（28）

（4）列写动力学方程

按照拉格朗日方程，对应关节1、2的驱动力矩分别为：

LL

1

t

1

L

1

L

（29）

2

t

22

L

1

1111

2222

（mlmlmlmllcos）（mlmll

2111222122122212

3332

cos

2

）

2

6

L

1

2

（ml

21

1

3

2

ml

11

1

3

ml

2

2

2

mll

21

2

cos）

2

1

1

3

（

2

ml

22

1

2

mllcos

212

2

）

2

t

mll

21

2

sin

2

1

2

1

2

mll

21

2

sin

2

2

2

L

1

11

（m1m2）gl1cos1m2gl2cos（12

22

）

2

（ml

21

1

3

ml

1

2

1

1

3

ml

2

2

2

mll

21

2

cos）

2

1

1

3

（

m

2

2

l

2

1

2

mllcos

212

2

）

12

mll

21

2

sin

2

1

2

1

2

mllsin

212

2

2

2

（

1

2

mm）glcos

121

1

1

2

m

2

gl

2

cos（

1

2

）

（30）

同理：

L

2

1

3

ml

2

2

2

2

1

3

（

ml

2

2

2

1

2

mll

212

cos）

2

1

L

2

1111

22

（mlmllcos）mlmll

222122122221

3232

2

sin

212

t

L

2

1

2

mll

21

2

sin

2

2

1

1

2

mll

21

2

sin

2

1

2

1

2

m

2

gl

2

cos（

1

2

）

11111

222

2（mlmllcos）mlmllsinm2gl2cos（12）

222122122221221

32322

（31）

联合（30）、（31）式，将动力学方程写成如下矩阵形式