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

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

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

l1sin

l2sin（1

（1）

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

假定z0、z1、z2垂直于纸面向里。

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

cos

01Tsin

sin

10

（2）

11

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

12Tsin

l1

（3）

01

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

..

20T01T21T

20l1

200

10

（4）

cos（1

sin（

sin（1

那么，连杆

2末段与中线交点处一点

P在基础坐标系中的位置矢量为：

l2

0P20T2P

l1sin

l1cos1

（5）

zp

即，

l1cos

（6）

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

1）相同。

建立以上运动学方程后，若已知个连杆的关节角

1、2

，就可以用运动学方程求出机

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

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

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

二连杆的关节角1、2，这叫机械臂的逆运动学。

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

端位置的控制。

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

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

推倒如下。

（1）问题

xpl1cos1l2cos（12）

ypl1sin1l2sin（12）

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

（2）求1

由（6）式得到：

（xp

1）2

（ypl1sin1）2

l22

（7）

整理得到：

x2p

y2pl12

2l1（xpcos1

ypsin1）

（8）

令

tg

p

（9）

由（8）式得到：

yp2

l12

2l1xp（cos1cosp

1sinp）

2l1xp

p）

（10）

由此可解出

1。

arccos

yp2

arctg

yp

（11）

xp

（3）求2

[xp

2）]2

[ypl2sin（1

（12）

l22

l12

2l2[xpcos（12）

ypsin（12）]

（13）

（14）

由（14）式得到：

x2pyp2

l122l2xp

[cos（1

2）cospsin（1

2）sinp]

cosp

（15）

2l2xp

由此可解出2。

xp2

（16）

2arccos

2l2xp

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

速度雅可比矩阵的定义：

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

现已二连杆平面机器人为例推导速度雅可比矩阵。

l1cos1

l1sin1

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

dxp

（1

dt

（17）

dyp

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

l2sin（

（18）

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

X，

关节角速度矢量

，

矩阵

J（1,2）

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

（18）式可

以写成：

XJ（1,2）

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

J11

J12

（19）

J21

J22

其中，

（20）

J22

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

J（1,

（21）

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

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

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

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

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

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

（1）求系统总动能

连杆1的动能为：

K1I

（21）

（m1l1

）1

6

m1l11

3

求连杆2

质心D处的线速度：

对连杆

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

连杆

2质心位置

为：

xD

l2cos（

（22）

1l2sin（

yD

连杆2质心速度为：

（

（23）

1l2cos（

YD

12

VD

yD

（l1

4

l1l2cos

l1l2cos2）12

（24）

连杆2的动能：

K2

1ID（

2）2

1m2VD2

m2l

）（

m2

[（l

l1l

2cos

l1l2cos

2）1

2]

12

l

m2（l1

3l2

l1l2

m2l2

l1l2cos

（25）

系统总动能：

K

K1

22

6m2l2

m2（

l2

（1ml2

1ml

1mll

）

（1ml

21

（26）

（2）求系统总势能系统总势能为：

m1gl1sin1

m2g（l1sin12

l2sin（12））

（27）

（3）求拉格朗日函数

LK

cos2）12

m2l1

m1l1

m2l1l2

1m2g[l1sin

2）]

m1gl1sin

1l2sin（1

（28）

（4）列写动力学方程

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

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

L

t

（29）

（m2l12

m1l12

m2l22

m2l1l2cos2）1

m2l22

m2l1l2cos2）2

（m2l12

m1l12

m2l1l2cos

m2l1l2sin

（1m1

m2）gl1cos1

1m2gl2cos（1

2）

1（m2l12

1m1l12

1m2l

22

m2l1l2cos2）

（1m2l22

1m2l1l2cos

m2l1l2sin

m2）gl1cos

m1

m2gl2cos（1

（30）

同理：

1m2l22