用平面二连杆机器人为例贯穿运动学雅可比动力学轨迹规划甚至控制与编程.docx
《用平面二连杆机器人为例贯穿运动学雅可比动力学轨迹规划甚至控制与编程.docx》由会员分享，可在线阅读，更多相关《用平面二连杆机器人为例贯穿运动学雅可比动力学轨迹规划甚至控制与编程.docx（22页珍藏版）》请在冰豆网上搜索。
用平面二连杆机器人为例贯穿运动学雅可比动力学轨迹规划甚至控制与编程
.
一、平面二连杆机器人手臂运动学
平面二连杆机械手臂如图1所示，连杆1长度l1，连杆2长度l2。
建立如图1所示的坐
标系，其中，（x0,y0）为基础坐标系，固定在基座上，（x1,y1）、（x2,y2）为连体坐标系，
分别固结在连杆1和连杆2上并随它们一起运动。
关节角顺时针为负逆时针为正。
y0
y2
C
D
P
B
2
x2
2
x1
1
y11
A
x0
图1平面双连杆机器人示意图
1、用简单的平面几何关系建立运动学方程
连杆2末段与中线交点处一点P在基础坐标系中的位置坐标：
xp
l1cos
1
l2cos（1
2）
yp
l1sin
l2sin（1
（1）
1
2）
2、用D-H方法建立运动学方程
假定z0、z1、z2垂直于纸面向里。
从（x0,y0,z0）到（x1,y1,z1）的齐次旋转变换矩阵为：
cos
01Tsin
0
0
1
1
sin
cos
0
0
1
1
10
10
（2）
10
11
从（x1,y1,z1）到（x2,y2,z2）的齐次旋转变换矩阵为：
cos
12Tsin
0
0
2
2
sin
cos
0
0
2
0
l1
2
0
0
（3）
10
01
从（x0,y0,z0）到（x2,y2,z2）的齐次旋转变换矩阵为：
..
.
cos
20T01T21T
sin
0
0
1
1
sin
cos
0
0
1
1
0
0
cos
0
0
sin
1
0
0
0
1
0
2
2
sin
cos
0
0
20l1
200
10
01
（4）
cos（1
2）
sin（
1
2）
0
l1cos
1
sin（1
2）
cos（1
2）
0
l1sin
1
0
0
1
0
0
0
0
1
那么，连杆
2末段与中线交点处一点
P在基础坐标系中的位置矢量为：
cos（1
2）
sin（1
2）
0
l1cos
1
l2
0P20T2P
sin（
1
2）
cos（1
2）
0
l1sin
1
0
0
0
1
0
0
0
0
0
1
1
l1cos1
l2cos（1
2）
xp
（5）
l1sin
1
l2sin（1
2）
yp
0
zp
1
1
即，
xp
l1cos
1
l2cos（1
2）
（6）
yp
l1sin
l2sin（1
2）
1
与用简单的平面几何关系建立运动学方程（
1）相同。
建立以上运动学方程后，若已知个连杆的关节角
1、2
，就可以用运动学方程求出机
械手臂末端位置坐标，这可以用于运动学仿真。
3、平面二连杆机器人手臂逆运动学
建立以上运动学方程后，若已知个机械臂的末端位置，可以用运动学方程求出机械手臂
二连杆的关节角1、2，这叫机械臂的逆运动学。
逆运动学可以用于对机械臂关节角和末
端位置的控制。
对于本例中平面二连杆机械臂，其逆运动学方程的建立就是已知末端位置
（xp,yp）求相应关节角1、2的过程。
推倒如下。
（1）问题
xpl1cos1l2cos（12）
ypl1sin1l2sin（12）
已知末端位置坐标（xp,yp），求关节角1、2。
（2）求1
..
.
由（6）式得到：
（xp
l1cos
1）2
（ypl1sin1）2
l22
（7）
整理得到：
x2p
y2pl12
l22
2l1（xpcos1
ypsin1）
（8）
令
xp
tg
sin
yp
p
cos
p
（9）
p
由（8）式得到：
x2p
yp2
l12
l22
2l1xp（cos1cosp
sin
1sinp）
cos
p
x2p
yp2
l12
l22
2l1xp
p）
（10）
cos
cos（1
p
由此可解出
1。
arccos
x2p
yp2
l12
l22
arctg
yp
1
2l1xp
cos
p
（11）
xp
（3）求2
由（6）式得到：
[xp
l2cos（1
2）]2
[ypl2sin（1
2）]2
l12
（12）
整理得到：
x2p
yp2
l22
l12
2l2[xpcos（12）
ypsin（12）]
（13）
令
xp
tg
sin
yp
p
cos
p
（14）
p
由（14）式得到：
x2pyp2
l22
l122l2xp
[cos（1
2）cospsin（1
2）sinp]
cosp
（15）
2l2xp
cos（1
2
p）
cosp
由此可解出2。
..
.
xp2
yp2
l22
l12
cosp
arctg
yp
（16）
2arccos
2l2xp
1
xp
二、平面二连杆机器人手臂的速度雅可比矩阵
速度雅可比矩阵的定义：
从关节速度向末端操作速度的线性变换。
现已二连杆平面机器人为例推导速度雅可比矩阵。
xp
l1cos1
l2cos（1
2）
yp
l1sin1
l2sin（1
2）
上面的运动学方程两边对时间求导，得到下面的速度表达式：
dxp
l1sin
l2sin（1
2）
（1
2）
dt
1
1
（17）
dyp
l1cos
l2cos（1
2）
（1
2）
dt
1
1
把上式写成如下的矩阵形式：
xp
l1sin
1
l2sin（
1
2）
l2sin（1
2）
1
（18）
yp
l1cos
l2cos（1
2）
l2cos（1
2）
1
2
令上式中的末端位置速度矢量
xp
X，
yp
关节角速度矢量
1
，
2
矩阵
l1sin
1
l2sin（
1
2）
l2sin（
1
2）
J（1,2）
l1cos1
l2cos（1
2）
l2cos（1
2）
J（1,2）就是速度雅可比矩阵，实现从关节角速度向末端位置速度的转变。
（18）式可
以写成：
XJ（1,2）
速度雅可比矩阵可以进一步写成：
J（1,2）
l1sin
1
l2sin（1
2）
l2sin（1
2）
l1cos
l2cos（1
2）
l2cos（1
2）
1
J11
J12
（19）
J21
J22
其中，
..
.
J11
xp
l1
sin
l2sin（1
2）
1
1
J12
xp
l2sin（
2）
1
2
（20）
yp
J21
l1cos1
l2cos（1
2）
1
J22
yp
l2cos（1
2）
2
由此可知雅可比矩阵的定义：
xp
xp
J（1,
2）
J11
J12
1
2
（21）
J22
yp
yp
J21
1
2
三、平面二连杆机器人手臂的动力学方程
推倒动力学方程的方法很多，各有优缺点。
拉格朗日方法思路清晰、不考虑连杆之间的内力，是推倒动力学方程的常用方法。
下面推导图1所示的平面双连杆机器人的动力学方程。
图1中所示连杆均为均质杆，其转动惯量分别是I1和I2。
1、求两连杆的拉格朗日函数
（1）求系统总动能
连杆1的动能为：
1
2
K1I
A
1
2
（21）
1
1
2
2
1
2
2
（m1l1
）1
6
m1l11
2
3
求连杆2
质心D处的线速度：
对连杆
2质心位置求导得到其线速度。
连杆
2质心位置
为：
xD
l1cos
1
2）
1
l2cos（
1
2
（22）
1l2sin（
yD
l1sin
1
1
2）
2
连杆2质心速度为：
xD
l1sin
1
1
1l2sin（
1
2）
（
1
2）
2
（23）
1l2cos（
YD
l1cos
1
1
1
2）
（1
2）
2
2
2
2
2
1
2
2
12
2
12
VD
xD
yD
（l1
4
l2
l1l2cos
2）
1
4
l2
2
（
2
l2
l1l2cos2）12
..
.
（24）
连杆2的动能：
K2
1ID（
1
2）2
1m2VD2
2
2
1
（
1
m2l
2
）（
2）
2
1
m2
[（l
2
1
2
l1l
2cos
2）
2
1
2
2
（
1
2
l1l2cos
2）1
2]
2
12
2
1
2
1
4
l2
1
4
l2
2
l
2
2
1
2
1
2
2
1
2
2
1
2
2
2
m2（l1
3l2
l1l2
cos
2）1
6
m2l2
2
2
m2
（
3l2
l1l2cos
2）
1
2
（25）
系统总动能：
K
K1
K2
1
2
1
2
2
1
2
2
1
22
2
m2（l1
3
l
2
l1l2cos
2）
1
6m2l2
2
2
m2（
3
l2
l1l2cos
2）
1
2
（1ml2
1ml
2
1ml
2
1mll
cos
）
2
1ml
2
2
（1ml
2
1mll
cos
）
2
2
1
6
1
1
6
2
2
2
21
2
2
1
6
2
2
2
3
2
2
2
2
12
2
1
2
（26）
（2）求系统总势能系统总势能为：
1
1
P
2
m1gl1sin1
m2g（l1sin12
l2sin（12））
（27）
（3）求拉格朗日函数
LK
P
1
2
1
2
1
2
1
cos
2
1
2
2
1
2
1
cos2）12
（
m2l1
6
m1l1
6
m2l2
m2l1l2
2）1
6
m2l2
2
（
m2l2
m2l1l2
2
2
3
2
1
1m2g[l1sin
1
2）]
m1gl1sin
1l2sin（1
2
2
（28）
（4）列写动力学方程
按照拉格朗日方程，对应关节
1、2的驱动力矩分别为：
1
L
L
t
1
1
（29）
L
L
2
t
2
2
L
（m2l12
1
m1l12
1
m2l22
m2l1l2cos2）1
（
1
m2l22
1
m2l1l2cos2）2
1
3
3
3
2
..
.
t
L
（m2l12
1
m1l12
1
m2l22
m2l1l2cos
2）
1
（
1
m2l22
1
m2l1l2cos
2）
2
1
3
3
3
2
m2l1l2sin
1
m2l1l2sin
2
2
12
2
2
2
L
（1m1
m2）gl1cos1
1m2gl2cos（1
2）
1
2
2
1（m2l12
1m1l12
1m2l
22
m2l1l2cos2）
1
（1m2l22
1m2l1l2cos
2）2
3
3
3
2
m2l1l2sin
1
2
1
m2）gl1cos
1
2）
2
1
2
m2l1l2sin
2
2
（
m1
1
m2gl2cos（1
2
2
2
（30）
同理：
L
1m2l22
2
升级会员 