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

+
⎡d ⎢⎣ dt
( 2l1
cosθ1 )⎤⎥⎦2
⎫⎪ ⎬ ⎪⎭
Ek = Ek 0 + Ek1 + Ek 2 + Ek3
=
1 2
Mx& 2
+
1 2
m1
⎡⎣
x& 2
+
2l1
cosθ1x&θ&1
+
l12θ&12
⎤⎦
( ) +
1 2
m2
⎡⎣
x& 2
+
2x&
2l1θ&1 cosθ1 + l2θ&2 cosθ2
= m1gl1 cosθ1 + m2 g (2l1 cosθ1 + l2 cosθ2 ) + 2m3gl1 cosθ1
Lagrange 函数为 L = Ek − Ep
d dt
∂L ∂x&
−
∂L ∂x
=
F
( ) 其中
∂L ∂x&
=
Mx&
+
m1x&
+
m1l1
cosθ1θ&1
+
m2 x&
+
m2
2l1θ&1 cosθ1 + l2θ&2 cosθ2
求传递函数
对上面方程组进行拉普拉斯变换，得到
⎧4
⎪ ⎨
3
lΦ(s)s 2
−
gΦ(s)
=
X
(s)s 2
⎪⎩(M + m) X (s)s 2 + bX (s)s − mlΦ(s)s 2 = U (s)
I
=
1 3
ml 2
整理后得到传递函数：
ml s 2
Φ(s) =
q
U (s)
s4
+
4 3
bml 2
s3
−
(M
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
6.1 倒立摆系统仿真实例
二级倒立摆系统建模（5）
所以小车的动力学方程为：
(M + m1 + m2 + m3 )&x& + (m1 + 2m2 + 2m3 )l1 cosθ1θ&&1
+ m2l2 cosθ2θ&&2 − (m1 + 2m2 + 2m3 ) l1 sinθ1θ&12 − m2l2 sinθ2θ&22 = F
非线性
倒立摆是一个典型的非线性复杂系统，实际中可以通过线 性化得到系统的近似模型，然后再进行控制。也可以利用 非线性控制理论对其进行控制。倒立摆的非线性控制正成 为一个研究的热点。
8
不确定性
主要是模型误差以及机械传动间隙，各种阻力等，实际控制 中一般通过减少各种误差来降低不确定性，如通过施加预紧 力减少皮带或齿轮的传动误差，利用滚珠轴承减少摩擦阻力 等不确定因素。
θ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
耦合性
倒立摆的各级摆杆之间，以及和运动模块之间都有很强的 耦合关系，在倒立摆的控制中一般都在平衡点附近进行解 耦计算，忽略一些次要的耦合量。
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
23
6.1 倒立摆系统仿真实例
二级倒立摆系统建模（6）
将平衡位置各变量的初值均取为零，进行泰勒级数展开，并 线性化处理 :
(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
10
6.1 倒立摆系统仿真实例
6.1.1系统建模 力学角度——牛顿-欧拉法
系统模型建立 能量角度——拉格朗日方程
力学角度：对每个拆分单元进行受力分析，应用牛顿 运动定律和动量矩定理得到系统的动态方程。条理明 晰，较为繁琐。
能量角度：从系统势能和动能两个角度对系统进行分 析，应用拉格朗日方程得到系统的动态方程。较为直 观，易行。
直线倒立摆是在直线运动模块上装有摆体组件，直线运动模块有一个自由度， 小车可以沿导轨水平运动，在小车上装载不同的摆体组件，可以组成很多类别 的倒立摆。直线柔性倒立摆和一般直线倒立摆的不同之处在于，柔性倒立摆有 两个可以沿导轨滑动的小车，并且在主动小车和从动小车之间增加了一个弹 簧，作为柔性关节。直线倒立摆系列产品如图 1 所示。
系统状态方程
⎪⎪φ&& ⎩
=
− (4M
3b + m)l
x&
+
3g(M + m) (4M + m)l
φ
+
(4M
3 +
m)l
u
I
=
1 3
ml
2
⎡0
⎡x& ⎤ ⎢⎢ &x&⎥⎥
⎢φ& ⎥ ⎢⎢⎣φ&&⎥⎥⎦
=
⎢ ⎢⎢0 ⎢ ⎢0 ⎢ ⎢⎢⎣0
1
− ( I + ml2 )b
I (M + m) + Mml2
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
+ (m1 + 2m2 + 2m3 ) gl1 sinθ1 = 0
m2l2 cosθ2&x& + m2l22θ&&2 + 2m2l1l2 cos(θ2 −θ1)θ&&1 − m2l2 sinθ2θ&2 x& − 2m2l1l2 sin(θ2 −θ1)(θ&2 −θ&1)θ&1
+m2 gl2 sinθ2 = 0
z 其行为与火箭飞行以及两足机器人行走有很大的相似性,因 而对其研究具有重大的理论和实践意义。
3
6.1 倒立摆系统仿真实例
倒立摆系统分类
1直线倒立摆系统 2环形倒立摆系统 3平面倒立摆系统 4柔性连接倒立摆系统 5直线柔性连接两级倒立摆 6柔性倒立摆系统（柔性摆杆）
4
１. 直线倒立摆系列
在 θ1,θ2 都很小的条件下（平衡位置处），线性化处理后得：
(M + m1 + m2 + m3 )&x& + (m1 + 2m2 + 2m3 )l1θ&&1 + m2l2θ&&2 = F
22
6.1.1系统建模
二级倒立摆系统建模（6）
在广义坐标θ1,θ2 上均没有外力作用，由拉格朗日方程可
知：
5
图 1 直线倒立摆系列
２.环形倒立摆系列
环形倒立摆是在圆周运动模块上装有摆体组件，圆周运动模块有一个自 由度，可以围绕齿轮中心做圆周运动，在运动手臂末端装有摆体组件， 根据摆体组件的级数和串连或并联的方式，可以组成很多形式的倒立 摆。如图 2所示。
图 2 环形倒立摆系列
6
３、平面倒立摆系列
直线一级倒立摆——牛顿-欧拉法建模， 直线二级倒立摆——拉格朗日方法建模。
11
6.1.1系统建模
一级倒立摆系统建模（1）
任一时刻，系统的状态由4个变 量描述：
小车位置x,
小车平移速度 x& 摆偏离向上垂直方向的角度φ,
以及摆的角速度 φ&
假设条件： 摆杆及小车都是刚体， 小车运
+ 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
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
动时的阻尼系数为b。将控制力F 记为控制输入u
12
6.1.1系统建模
一级倒立摆系统建模（2）
小车和摆杆受力分析
bx&
x& M&x& = F − bx& − N
N = m d 2 (x + l sinθ )
dt 2
(M + m)&x& + bx& + mlθ&&cosθ − mlθ&2 sinθ = F
建模仿真与相似原理
第6章 典型机电控制系统仿真实例
1
6.典型机电控制系统仿真实例
本章主要教学内容
6.1 倒立摆系统仿真实例 6.2 X-Y工作台系统仿真实例
2
6.1 倒立摆系统仿真实例 Back
z 倒立摆是进行控制理论研究的典型实验平台，是典型的高 阶次、多变量、严重不稳定和强耦合的非线性系统。
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⎥⎦
Fra Baidu bibliotek
⎢ ⎢
x& ⎥⎥
⎢φ ⎥
⎢⎢⎣φ&⎥⎥⎦
+
⎡0⎤ ⎢⎣0⎥⎦u
17
6.1 倒立摆系统仿真实例
二级倒立摆系统建模（1）
由于通常角位移传感器 的质量与摆杆质量相比 不可忽略，因此将下摆 杆角位移传感器与小车 视为同一部分，而将上 摆杆角位移传感器视为 一质量块 m3来处理。 将系统的水平方向加速 F 度记为控制输入u
=
fi
在本系统中，设系统的三个广义坐标分别是 ：
x,θ1,θ2
19
6.1 倒立摆系统仿真实例
二级倒立摆系统建模（3）
系统的动能：Ek = Ek0 + Ek1 + Ek2 + Ek3
Ek 0
=
1 2
Mx& 2
F
Ek1
=
1 2
m1
⎧⎪⎡ ⎨⎪⎩⎢⎣
d dt
(
x
+
l1
sin θ1
)⎤⎥⎦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
+
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
⎪⎨φ& = φ&
+ 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
平面倒立摆是在可以做平面运动的运动模块上装有摆杆组件，平面运 动模块主要有两类：一类是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 倒立摆系统仿真实例