Ch06 典型机电控制系统仿真实例

平面倒立摆是在可以做平面运动的运动模块上装有摆杆组件，平面运 动模块主要有两类：一类是XY 运动平台，另一类是两自由度SCARA 机械臂；摆体组件也有一级、二级、三级和四级很多种。如图 3 所示
图 3 平面倒立摆系列
7
4.倒立摆的特性
虽然倒立摆的形式和结构各异，但所有的倒立摆都具有 以下的特性： 非线性 不确定性 耦合性 开环不稳定性 约束限制
⎨ ⎩(I
+
ml2 )θ&&+ mgl sinθ
+ ml&x&cosθ
=
0
θ = π + φ 在φ=0附近微小变化，cosθ≈-1，sinθ≈-φ ， 线性化得：
⇒
⎧(m + M
⎨ ⎩(I
+
ml
)&x&+ bx& − mlφ&& = u 2 )φ&&− mglφ = ml&x&
14
6.1 倒立摆系统仿真实例
P
−
mg
=
m
d2 dt 2
(l
cosθ
)
即 ： P − mg = −mlθ&&sinθ − mlθ&2 cosθ
−Pl sinθ − Nl cosθ = Iθ&&
13
6.1 倒立摆系统仿真实例
一级倒立摆系统建模（3）
系统运动方程：
⎧(m + M )&x&+ bx& + ml cosθ ⋅θ&&− ml sinθ ⋅θ&2 = u
3g(M + m)
(4M + m)l
0⎤ 0⎥⎥ 1⎥⎥ 0⎥⎥⎦
⎡x⎤
⎢ ⎢
x& ⎥⎥
⎢φ ⎥
⎢⎢⎣φ&⎥⎥⎦
+
⎡ ⎢ ⎢ ⎢ ⎢ ⎢ ⎢⎣
0 4
(4M + 0 3
(4M +
m) m)l
⎤ ⎥ ⎥ ⎥u ⎥ ⎥ ⎥⎦
输出方程为：
⎡x⎤
y
=
⎡x⎤
⎢⎣φ ⎥⎦
=
⎡1 ⎢⎣0
0 0
0 1
0⎤ 0⎥⎦
⎢ ⎢
+ m3x& + 2m3l1 cosθ1θ&1,
∂L = 0 ∂x
d dt
∂L ∂x&
=
(M
+
m1
+
m2
+
m3 ) &x& +
(m1
+
2m2
+
2m3 )l1
cosθ1θ&&1
21 + m2l2 cosθ2θ&&2 − (m1 + 2m2 + 2m3 ) l1 sinθ1θ&12 − m2l2 sinθ2θ&22
0
−mlb
I (M + m) + Mml2
0
m2 gl 2
I (M + m) + Mml2
0
mgl (M + m) I (M + m) + Mml2
0⎤
⎡0
⎤
⎥ 0⎥⎥
⎥ 1⎥
⎥ 0⎥⎥⎦
⎡x⎤
⎢ ⎢
x&
⎥ ⎥
⎢φ ⎥
⎢⎢⎣φ&⎥⎥⎦
+
⎢ ⎢ ⎢I ⎢ ⎢0 ⎢ ⎢ ⎢⎣ I
(M (M
I + ml 2
+ 4l12θ&12 + l22θ&22 + 4l1l2 cos (θ2 −θ1 )θ&1θ&2 ⎤⎦
+
1 2
m3
⎡⎣ x& 2
+
4l1
cosθ1x&θ&1
+
4m3l12θ&12
⎤⎦
θ2
m2
θ1
l2
m1
m3
l1
M
x
20
6.1 倒立摆系统仿真实例
系统的势能 Ep = Ep1 + Ep2 + Ep3
耦合性
倒立摆的各级摆杆之间，以及和运动模块之间都有很强的 耦合关系，在倒立摆的控制中一般都在平衡点附近进行解 耦计算，忽略一些次要的耦合量。
9
开环不稳定性
倒立摆的平衡状态只有两个，即在垂直向上的状态和垂直 向下的状态，其中垂直向上为绝对不稳定的平衡点，垂直 向下为稳定的平衡点。
约束限制
由于机构的限制，如运动模块行程限制，电机力矩限制 等。为了制造方便和降低成本，倒立摆的结构尺寸和电机 功率都尽量要求最小，行程限制对倒立摆的摆起影响尤为 突出，容易出现小车的撞边现象。
d dt
(l1
cosθ1
)⎤⎥⎦2
⎫⎪ ⎬ ⎪⎭
Ek 2
=
1 2
m2
⎧⎪⎡ d ⎨⎪⎩⎢⎣ dt
(x
+
2l1 sinθ1
+
l2
sin θ 2
)⎤⎥⎦2
+
⎡d ⎢⎣ dt
( 2l1
cosθ1
+ l2
cos
θ
2
)⎤⎥⎦
2
⎫⎪ ⎬ ⎪⎭
Ek 3
=
1 2
m3
⎧⎪⎡ d ⎨⎪⎩⎢⎣ dt
(
x
+
2l1
sinθ1 )⎤⎥⎦2
+
m)mgl
s2
−
bmgl
Байду номын сангаас
s
q
q
q
其中 q = [(M + m)(I + ml 2 ) − (ml)2 ]
15
6.1 倒立摆系统仿真实例
一级倒立摆系统建模（4）
⎧x& = x&
求状态空间表达式：
⎪ ⎪&x& ⎪
=
− (4M
4b + m)
x&
+
3mg (4M +
m)
φ
+
4 (4M +
m)
u
⎪⎨φ& = φ&
d dt
⎛ ⎜ ⎝
∂L
∂θ&1
⎞ ⎟ ⎠
−
∂L
∂θ1
=
0
将L代入，得：
d dt
⎛ ∂L
⎜ ⎝
∂θ&2
⎞ ⎟ ⎠
−
∂L
∂θ2
=
0
(m1 + 2m2 + 2m3 )l1 cosθ1&x& + (m1 + 4m2 + 4m3 )l12θ&&1 +4m2l1l2 cos(θ2 −θ1)θ&&2 − (m1 + 2m2 + 2m3 )l1 sinθ1θ&1x& − 4m2l1l2 sin(θ2 −θ1)(θ&2 −θ&1)θ&2
z 其行为与火箭飞行以及两足机器人行走有很大的相似性,因 而对其研究具有重大的理论和实践意义。
3
6.1 倒立摆系统仿真实例
倒立摆系统分类
1直线倒立摆系统 2环形倒立摆系统 3平面倒立摆系统 4柔性连接倒立摆系统 5直线柔性连接两级倒立摆 6柔性倒立摆系统（柔性摆杆）
4
１. 直线倒立摆系列
10
6.1 倒立摆系统仿真实例
6.1.1系统建模 力学角度——牛顿-欧拉法
系统模型建立 能量角度——拉格朗日方程
力学角度：对每个拆分单元进行受力分析，应用牛顿 运动定律和动量矩定理得到系统的动态方程。条理明 晰，较为繁琐。
能量角度：从系统势能和动能两个角度对系统进行分 析，应用拉格朗日方程得到系统的动态方程。较为直 观，易行。
+ m) + Mml2
ml
+ m) + Mml2
⎥ ⎥ ⎥ ⎥u ⎥ ⎥ ⎥ ⎥⎦
16
6.1 倒立摆系统仿真实例
即状态方程为：
⎡x& ⎤ ⎢⎢&x&⎥⎥
⎢φ&⎥ ⎢⎢⎣φ&&⎥⎥⎦
=
⎡0 ⎢⎢0 ⎢⎢0 ⎢⎢⎣0
1 − 4b (4M + m)
0 − 3b (4M + m)l
0 3mg
(4M + m) 0
θ2 m2
θ1
l2
m1
m3
l1
M
x
18
6.1 倒立摆系统仿真实例
二级倒立摆系统建模（2）
拉格朗日函数为：
L(q, q&) = Ek (q, q&) − Ep (q, q&)
其中，L为拉格朗日算子，q为系统的广义坐标， Ek 为系统的动能， Ep为系统的势能。
拉格朗日方程为：
d ∂L − ∂L d t ∂ q& i ∂ q i
=
fi
在本系统中，设系统的三个广义坐标分别是 ：
x,θ1,θ2
19
6.1 倒立摆系统仿真实例
二级倒立摆系统建模（3）
系统的动能：Ek = Ek0 + Ek1 + Ek2 + Ek3
Ek 0
=
1 2
Mx& 2
F
Ek1
=