最新用平面二连杆机器人为例贯穿运动学雅可比动力学轨迹规划甚至控制与编程文件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）式，将动力学方程写成如下矩阵形式
升级会员 