平面双连杆机械臂动态模型
-
构建平面双连杆机械臂动态模型
1.
平面双连杆机械臂的分析
图
1
平面双连杆机械臂
平面双连杆机械臂如图
1
，图中
θ
1
为关节
1
转角，
θ
2
为关节
2
转角，
l1
为杆
1
的长度，
l2
为
杆
2
的长度，
r1
为关节
1
到杆
1
质心的距离，
r2
为关节
2
到杆
2
质心的距离
< br>,M1
为负载质量。以图
中的
O
为原点的
x
0
y
0
为基坐标。
2.
数学建模
2.1
寻找动力学
末端坐标
x
pl
l
1
c
os
1
l
2
cos(
1
2
)<
/p>
y
pl
l
1
sin
1
l
2
sin(
1
2
)
根据雅克比矩阵的形式
dx
1
< br>dy
1
dx
2
dy
2
<
/p>
J
对末端坐标进行微分得到末端速度方程
pl
l
1
1
si
n
1
l<
/p>
2
(
1
2
)
sin(
1
2
)
x
pl
l
1
1
cos
1
l
2
(
1
2<
/p>
)
cos(
1
2
)
y
其中
1
1
，
2
2
，将（
3
）
、
（
4
）两式联立整理成速度雅克
比矩阵形式
l
1
s
1
l
2
s
12
J
l
1
c
1
l
2
c
12
l
2
s
12
< br>
l
2
c
12
s
2
sin
2
，
c
12
=
cos(
1
2
)
。<
/p>
c
1
=
cos<
/p>
1
，
c
2
cos
2
，
s
12
=
sin(
1
2
)
，
其中
s
1
=
sin
1
，
1
在机器人基础坐标
系中的速度与各关节速度间的关系以及手部与外界接触力与对应各关节间的关系可
以利用
雅克比矩阵来建立。对机械臂末端速度方程（
3
）、方程（
4
）进行求导得到末端加速度方程如下
<
/p>
2
pl
(
l
1
s
1
l
2
s
12
)
1
l
2
s
12
2
(
l
1
c
1
l
2
c
12
)
1
2
l
2
c
12
2
x
< br>
2
l
2
1
2
c
12
2
2<
/p>
pl
(
l
1
s
1
l
2
s
12
)
1
l
2
s
12
2
y
l
2
s
12
)
<
/p>
1
l
2
s
12
2
2
l
2
1
2
< br>s
12
1
1
，
=
<
/p>
，上述推导的方程构成了进行动力学仿真的基础
,
它们表明了有效负荷的加速度与
其中
1
=
2
1
2
两节点处电动机的角速度和
角加速度之间的关系。
机械臂质心位置的加速度和关节处的变量之间关系方
程如下
2
A
c
1
< br>,
x
r
1
s
1
1
r
1
c
1
1
2
A
r
c
< br>
r
s
1
1
1
1
1
1
c
1
,
y
2
2
A
c
2
,
< br>x
(
l
1
s
1
r
2
s
12
)<
/p>
1
r
2
s
12
2
(
l
1
c
1
< br>l
2
c
12
)
1
l
2
c
12
2
2
l
2
1
2
c
12
2
2
A
(
l
c
r
c
)
r
s
<
/p>
(
l
s
l
s
)
l
c
2
l
s
1
1
2
12
1
2
12
2
1<
/p>
1
2
12
1
2
12
2
2
1
2
12
c
2
,
y
(
< br>l
s
2.2
构建拉格朗日模型
2.2.1
选定广义关节变量及广义力
选取笛卡尔坐标系。
θ
1
为关节
1
转角，
θ
2
为关节
2
转角，
关节
1
和关节
2
相应的力矩是
1
和
2
。
连杆
< br>1
和连杆
2
的质量分别为
m
1
和
m
2
，
l1
为杆
1
的长度，
l2
为杆<
/p>
2
的长度，质心分别为
k
1
和
k
2
< br>r1
为关
节
1
到杆
1
质心的距离，
r2
为关节
2
到杆
2
质心的距离。
因此，杆
1
质心
k
1
的位置坐标为
X
1
r
1
s
1
Y
< br>1
r
1
c
1
杆
1
质心
k<
/p>
1
速度的平方为
2
2
2
X
1
Y
1
(
r
1
1
)
杆
1
< br>质心
k
2
的位置坐标为
X
2
l
1
s
< br>1
r
2
s
12
Y
2
l
1<
/p>
c
1
r
2
c
12
杆
1
质心
k
1
速度的平方为
X
2
l
1
c
1
1
< br>
r
2
c
12
(
1
2<
/p>
)
Y
2
l
1
s
1
1
r
2
s
12
(
1
2
)
2
2
2
2
2
2
2
X
2
Y
< br>
2
l
1
1
r
2
(
1
2
)
2
l
1
r
< br>2
(
1
1
2
)
c
2
2.2.2
系统动能
E
k
E
k
1
1
2
1
2
E<
/p>
ki
i
1
,
2
m
1
r
1
1
2
< br>2
E
k
2
m
2
l
1
1
2
2
1
2
2
2
2
m
2
r
2
(
< br>
1
2
)
m
2
l
2
r
2
(
1
1
< br>2
)
c
2
2.2.3
系统势能
E
p
E
pi
i
1
,
2
E
p
1
m
1
gr
1
(
1
c
1
)
2
E
p
2
m
2
gl
1
(
1
c
1
)
m
2
gr
< br>2
(
1
c
12
)
2.2.4
拉格朗日函数
L
E
k
E
p
1
1
2
2
2
2
2<
/p>
2
(
m
1
r
1
m
2
l
1
)
1
m
2
l
1
r
2
(
1<
/p>
1
2
)
c
2
m
2
r
2
(
1
2
)<
/p>
2
2
(
m
1
r
1
m
2
l
1
)
g
(
1
c
1
)
m
2<
/p>
gr
2
(
1
c
12
)
2.2.5
系统动力学方程
根据拉格朗日方程式计算各关节上的力矩，得到系统动力学方程。
计算关节
1
上的力矩
1
L
L
1
2
2
2
(
m
1
< br>r
1
m
2
l
1
)
1
m
2
l
1
r
2
(
2
1
< br>2
)
c
2
m
2
r
2
(
1
2
)
1
(
m
< br>1
r
1
m
2
l
1
)
gs
1
<
/p>
m
2
gr
2
s
12
所以
1<
/p>
L
L
dt
1
1
d
< br>2
2
2
(
m
r
2
m
l
r
c
)
(
m
1
< br>r
1
m
2
r
2
m
2
l
1
2
m
2
l
1
r
2
c
2
)
< br>
1
2
2
2
1
2
2
2
2
(
2
m
2
l
1
r
2
s
2
)
< br>1
2
(
m
2
l
1
r
2
s
2
)
2
(
m
1
r
< br>1
m
2
l
1
)
gs
1
m
2<
/p>
gr
2
s
12<
/p>
计算关节
1
上的力矩
2
L
2
m
2
r
2
(
1
< br>
2
)
m
2
l
1
r
2
1
c
2
2
2
m
2
l
< br>1
r
2
(
1
1
2
)
s
2
m
2
gr
2
s
12
L
< br>
2
所以
2
L
L
2
<
/p>
m
r
2
(
m
2
r
2
m
2
l
1
r
2
c<
/p>
2
)
1
2
2
2
dt
2
2
< br>2
(
m
2
l
1
r
2
s
2
m
2
l
1
r
2
s
2
)
< br>1
2
(
m
2
l
1
r
2
s
2
)
1
m
2
gr
2
s
12
d
2.3
建立动力方程
将两个关节电机输入转矩作为输入，根据
2.1
中的角
度、角速度、角加速度的方程和建立的拉格朗日方
程，我们可以建立含
< br>14
维矩阵的动力方程如下：
3