二阶倒立摆的控制
指导老师：屈桢深
问题描述
小车质量0.8kg ，摆杆1质量0.3kg, 摆杆长度1.0m ；摆杆2质量0.1kg, 摆杆长度0.5m 。
要求：设计NN 控制器，满足指标要求：0.2Hz 正弦信号幅值裕度2阶倒立摆。
一阶倒立摆建模
小车由电机通过同步带驱动在滑杆上来回运动，保持摆杆平衡。电机编码器和角编码器向运动卡反馈小车和摆杆位置（线位移和角位移）。小车在轨道上可以自由滑动。
单级倒立摆系统数学模型
N 和P 分别为小车与摆杆相互作用力的水平和垂直方向的分量。分析小车水平方向所受的合力，可得到方程为：
=F -b x -N M x
由摆杆水平方向的受力进行分析可以得到下面等式：
d 2
N =m 2(x +l sin θ)
dt
cos θ-ml θ 2sin θ +ml θN =m x
把这个等式代入式中，得到系统的第一个运动方程：
2sin θ=F +b x +ml θcos θ-ml θ(M +m ) x
为了推出系统的第二个运动方程，对摆杆垂直方向的合力进行分析，得到下面的方程：
d 2
P -mg =m 2(l cos θ)
dt
sin θ-ml θ 2cos θ P -mg =-ml θ
力矩平衡方程如下：
-Pl sin θ-Nl cos =I θ
方程中力矩的方向，cos φ
=-cos θ, sin φ=-sin θ，故等
式前面有负号。合并这两个方程，约去P 和N ，得到第二个运动方程：
+mgl sin θ=-ml cos θ (I +ml )θx
2
假设φ与1（单位是弧度）相比很小，即φ〈〈1，则可进行近似处理：
⎛d θ⎫
cos θ=-1, sin θ=-φ, ⎪=0
⎝dt ⎭
用u 代表被控对象的输入力，线性化后两个运动方程如下：
2 ⎧ -mgl φ=ml x ⎪I +ml φ
⎨ ⎪ +b x -ml φ=u x ⎩(M +m )
2
()
对方程（7）进行拉普拉斯变换，得到：
222
⎧I +ml φ(s ) s -mgl φ(s ) =mlX (s ) s ⎪
⎨22
⎪⎩(M +m )X (s ) s +bX (s ) s -ml φ(s ) s =U (s )
()
推导时假设初始条件为0则摆杆角度和小车位移的传递函数为：
mls 2
=22
X (s ) (I +ml ) s -mgl
φ(s )
摆杆角度和小车加速度之间的传递函数为：
φ(s ) ml
=
A (s ) I +ml 2s 2-mgl
摆杆角度和小车所受外界作用力的传递函数：
ml 2
s
φ(s ) q
=
b (I +ml 2) 3(M +m ) mgl 2bmgl F (s ) 4
s +s -s -s
q q q
222
q =⎡(M +m )(I +ml ) -m l ⎤⎣⎦
以外界作用力作为输入的系统状态空间表达式为：
⎡0 ⎤⎢⎡x ⎢0⎢ ⎥⎢⎢x ⎥=⎢ ⎥0⎢φ⎢⎢ ⎥⎢⎣φ⎦0
⎢⎣
1
-(I +ml 2) b I (M +m ) +Mml 2
0-mlb
I (M +m ) +Mml 2
m 2gl 2
I (M +m ) +Mml 2
0mgl (M +m ) I (M +m ) +Mml 2
0⎤⎥x ⎡⎤0⎥⎢⎥
⎥x ⎢⎥⎥1⎥⎢φ⎥
⎥⎥⎢φ0⎥⎣⎦⎦
0⎡⎤
⎢⎥2
I +ml ⎢⎥
2
⎢I (M +m ) +Mml ⎥+⎢⎥u
0⎢⎥
⎢⎥ml ⎢2⎥I (M +m ) +Mml ⎣⎦
⎡x ⎤
⎢ ⎥0
⎡x ⎤⎡1000⎤⎢x ⎥+⎡⎤u y =⎢⎥=⎢⎥⎢⎥⎣φ⎦⎣0010⎦⎢φ⎥⎣0⎦
⎢ ⎥⎣φ⎦
以小车加速度作为输入的系统系统状态空间表达式：
0 ⎤⎡⎡x ⎢0⎢ ⎥ ⎢⎢x ⎥=⎢0⎢φ⎥⎢⎢ ⎥⎢⎣φ⎦⎢0
⎣
1000
0003g 4l
0⎤⎡0⎤
x ⎡⎤⎢1⎥0⎥⎢⎥⎥x ⎢⎥' ⎢1⎥⎥+⎢0⎥u ⎥⎢φ⎥⎢⎥⎢ ⎥⎢3⎥0⎥⎣φ⎥⎦⎣⎢4l ⎦⎥⎦
⎡x ⎤⎢ ⎥0
⎡x ⎤⎡1000⎤⎢x ⎥+⎡⎤u ' y =⎢⎥=⎢⎥⎢⎥⎣φ⎦⎣0010⎦⎢φ⎥⎣0⎦
⎢ ⎥⎣φ⎦
2系统的可控性、可观测性分析 对于连续时间系统：
=AX +Bu X
y =CX +Du
系统状态完全可控的条件为：当且仅当向量组B , AB ,..., A n -1B 是线性无关的，或n ×n 维矩阵[B AB A n -1B ]的秩为n 。
系统的输出可控条件为：当且仅当矩阵
[CB CAB CA
2
B CA n -1B D
]的秩等于输出向量y 的维数。
应用以上原理对输入为加速度输出为摆杆与竖直方向的角度的夹角时的系统进行可控性分析即可。
二阶倒立摆建模
在忽略了空气流动，各种摩擦之后，可将倒立摆系统抽象成小车、匀质杆的系统，如图所示。
图1 直线两级倒立摆物理模型
下面利用拉格朗日方程推导运动学方程。 拉格朗日方程为：
)=T (q , q )-V (q , q ) L (q , q
d δL δL
-=f i dt δq δq
T =T M +T m 1+T m 2+T m 3
'1+T m '1 T m 1=T m
'2+T m ''2 T m 2=T m
T M =
1
2
Mx 2
22
⎛⎫d x -l sin θd l sin θ⎛⎫⎛⎫((111)11)'1=m 1 T m ⎪+ ⎪⎪2 ⎝dt dt ⎭⎝⎭⎪⎝⎭ 1 cos θ+1m l 2θ 2 2-m 1l 1x θ=m 1x 1111122
1112⎫2122''1=J p ω12=⎛T m m 1l 1⎪θ1=m 1l 1θ1
22⎝36⎭
' ''
T m 1=T m 1+T m 1=
1 cos θ+2m l 2θ 2 2-m 1l 1x θm 1x 11111
23
同样可以求出
T
'
m 2
1⎛d (x -2l 1sin θ1-l 2sin θ2⎫1⎛d (2l 1cos θ1+l 2cos θ2) ⎫=m 2 ⎪+m 2 ⎪2⎝dt dt ⎭2⎝⎭
2211 =m 2x -2l 1θ1cos θ1-l 2θ2cos θ2+m 22l 1θ1sin θ1+l 2θ2sin θ2
22
22
()()
11⎛12⎫212 2'' 2
T m =J ω=222 m 2l 2⎪ω2=m 2l 2θ2
22⎝36⎭
1 cos θ+l θ 2-2x 2l 1θm 2x 1122cos θ2
2
1⎛⎫ 2+4l 2θ 2 +m 2 4l 12θ122+4l 1l 2θ1θ2cos (θ2-θ1)⎪2⎝3⎭
' '' T m 2=T m 2+T m 2=
(
(
))
因此，可以得到系统的总动能为：
T =T M +T m 1+T m 2
11 cos θ+2m l 2θ 2 2+m 1x 2-m 1l 1x θMx 11111223
12 -2x 2l 1θ1cos θ1+l 2θ2cos θ2+m 2x
21⎛⎫ 2+4l 2θ 2 +m 2 4l 12θ122+4l 1l 2θ1θ2cos (θ2-θ1)⎪2⎝3⎭=
(
(
))
系统的总势能为：
V =V m 1+V m 2+V m 3
=m 1gl 1cos θ1+m 2g (2l 1cos θ1+l 2cos θ2)
从而拉格朗日算子：
L =T -V
11 cos θ+2m l 2θ 2 2+m 1x 2-m 1l 1x θMx 111112231 cos θ+l θ 2-2x 2l 1θ+m 2x 1122cos θ221⎛⎫ 2+4l 2θ 2 +m 2 4l 12θ122+4l 1l 2θ1θ2cos (θ2-θ1)⎪2⎝3⎭=
(
(
))
-m 1gl 1cos θ1-m 2g (2l 1cos θ1+l 2cos θ2)
由于因为在广义坐标θ1, θ2上均无外力作用，有以下等式成立：
d ⎛∂L ⎫∂L
=0 ⎪-
dt ⎝∂θ∂θ1⎭1
d ⎛∂L ⎫∂L
=0 ⎪-
dt ⎝∂θ2⎭∂θ2
, θ 对θ12求解代数方程，得到以下两式
=(3(-2gm sin θ-4gm sin θ-4m g sin θ+3m g cos(θ-θ)sin θθ[1**********]
2+4m l sin(θ-θ) θ 2-2m +6m 2l 1cos(θ1-θ2)sin(θ1-θ2) θ1221221x cos θ1
-4m 2 x cos θ1-4m 3 x cos θ1+3m 2 x cos(θ1-θ2)cos θ2)) /(2l 1(-4m 1-12m 2-12m 3+9m 2cos 2(θ1-θ2)))
=-(-m (m +3(m +m )) l 2l (-3g sin θ-6l θ 2sin(θ-θ) -3 θx cos θ2) [1**********]2
2 2sin(θ-θ) +m 2l 12l 2cos(θ1-θ2)(6m 2l 2θ2123
-3(m 1+2(m 2+m 3))(g sin θ1+ x cos θ1))) /(-
表示成以下形式：
4
9
162222
m 2(m 1+3(m 2+m 3)) l 12l 2+4m 2l 1l 2cos 2(θ1-θ2)) 9
=f (x , θ, θ, x θ1112, θ1, θ2, x ) =f (x , θ, θ, x θ2212, θ1, θ2, x )
取平衡位置时各变量的初值为零，
, θ , θA =(x , θ1, θ2, x 12, x ) =(0,0,0,0,0,0,0)=0
将（23）式在平衡位置进行泰勒级数展开，并线性化，令:
K 11=
∂f 1
∂x
A =0
=0
K 12=
∂f 1∂θ1
A =0
=
3(-2gm 1-4gm 2-4gm 3)
2(-4m 1-3m 2-12m 3) l 1
K 13=
∂f 1∂θ2
A =0
=
9m 2g
2(-4m 1-3m 2-12m 3) l 1
∂f 1 ∂x
A =0
K 14=
=0
K 15=
∂f 1∂θ1∂f 1∂θ2
A =0
=0
K 16=∂f 1
∂ x
A =0
=0
K 17=
A =0
=
3(-2m 1-m 2-4m 3)
2(-4m 1-3m 2) l 1
带入式，得到线性化之后的公式
=K θ+K θ+K θ112113217x
将式在平衡位置进行泰勒级数展开，并线性化，令
K 22=
∂f 2∂θ1
K 21=
∂f 2∂x
=0
A =0
A =0
=
2g (m 1+2m 2) 4m 2l 2-(m 1+3m 2) l 2
94g (m 1+3m 2) 3(4m 2l 2-(m 1+3m 2) l 2)
9
∂f 2 ∂x
A =0
K 23=
∂f 2
∂θ2
A =0
=-
K 24=
=0
K 25=
∂f 2∂θ1∂f 2∂θ2
A =0
=0
K 26=
A =0
=0
K 27=
∂f 2
∂ x
A =0
4
2(m 1+2m 2) -(m 1+3m 2)
=
4m 2l 2-(m 1+3m 2) l 2
9
带入（22）式，得到 即：
=K θ+K θ+K θ222123227x
=K θ+K θ+K θ112113217x =K θ+K θ+K θ222123227x
现在得到了两个线性微分方程，由于我们采用加速度作为输入，因此还需加上一个方程
取状态变量如下：
⎧x 1=x
⎪x =θ
1⎪2
⎪⎪x 3=θ2
⎨
⎪x 4=x ⎪x 5=θ1
⎪ ⎪⎩x 6=θ2
u = x
由（33），（41），（42）式得到状态空间方程如下：
1⎤⎡00⎡x
⎢x 2⎥⎢00⎢⎥⎢⎢x 3⎥⎢00⎢⎥=⎢
4⎥⎢00⎢x ⎢x 5⎥⎢0K 12⎢⎥⎢ 6⎦⎢x ⎥⎣⎢0K 22⎣
000K 13K 23
100⎤⎡x 1⎤⎡0⎤
⎢x ⎥⎢0⎥010⎥⎥⎢2⎥⎢⎥
001⎥⎢x 3⎥⎢0⎥
⎥⎢⎥+⎢⎥u
000⎥⎢x 4⎥⎢1⎥000⎥⎢x 5⎥⎢K 17⎥
⎥⎢⎥⎢⎥
000⎦x K ⎥⎣⎢6⎦⎥⎣⎢27⎦⎥
其中直线两级倒立摆系统参数为：
M
小车质量 2.32kg
m 1=0.3kg；m 2=0.2kg；θ1为摆杆1与垂直向上方向的夹角θ2
为摆杆2与垂直向上方向的夹角；l 1=1m；l 2=0.5m；F 为作用在系统上的外力
由以上方程，将以下参数代入即可。
⎧M =0.8⎪m =0.3⎪1⎪⎪m 2=0.2 ⎨
⎪g =9.8⎪l 1=1⎪⎪⎩l 2=0.5
神经网络建模
本文采用的神经网络采用4-5-3结构的三层前馈网。输入变量为
x =[x 1
x 2
x 3]
⎧x 1=e (t )⎰⎪
⎪x 2=e (t ) ⎪⎨de (t )⎪x 3=
dt ⎪
⎪e (t )=x in (t )-y out (t )⎩
(0-1)
网络隐含层的局部诱导域和输出分别为
v (n )=∑ω1ji (n )O i (n )1j
i =0M
(0-2)
O i 0(n )=ϕ(v 1j (n ))
j =1, 2,3,..., Q
其中，w 为隐含神经元的突触权值，w 0表示神经元的偏置，Q 为隐含神经元的节点数，隐含神经元的激活函数取双曲正切函数
e x -e -x
ϕ(x )=tanh (x )=x -x
e +e
(0-3)
网络输出层的诱导局部域和输出分别为
2
v (n )=∑w kj (n )O 1j (n ) 2k
j =0Q
(0-4)
2
O k 2(n )=f (v k (n ))k =1, 2,3
(0-5)
⎧k p =O 12(n )
⎪⎪2
⎨k i =O 2(n ) ⎪2k =O (n )d 3⎪⎩
(0-6)
考虑到输出参数不能为负值，所以激活函数采用非负函数
控制率为
1e x
f (x )=(1+tanh (x ))=x -x
2e +e
(0-7)
de (t )
M c (t )=k p ⋅e (t )+k i ⋅⎰e (t )+k d ⋅
dt
(0-8)
采用BP 学习算法，对网络的突触权值进行迭代修正，并附加一个使搜索快速收敛的全局极小的动量项。定义系统的代价函数为。
ε(t )=
12
e (t ) 2
(0-9)
2dw kj ∂ε(t )(t )
w (t )=-η2+α
w kj t dt 2
kj
(0-10)
=ηδ
2k
(t )O (n )+α
1j
2dw kj (t )
dt
其中，yita 是学习率，alpha 是动量因子，根据微分链式规则，局部梯度可计算如下
∂ε(t )
δ(t )=-2
∂v k t 2k
∂ε(t )∂y out (t )∂∆u (t )∂O k 2(t )=-
∂y out t ∂∆u t ∂O k 2t ∂v k 2t (0-11)
由于输出对控制量的偏导未知，所以用符号函数近似表示，由此带来的计算不精确的影响尽量由调整学习率来补偿。由控制方程不难得到
de (t )
M c (t )=K pid ⋅X ' =k p ⋅e (t )+k i ⋅⎰e (t )dt +k d ⋅ (0-12)
dt
∂M c (t )
=x k (t ) 2
∂O k t (0-13)
将所有公式整合，不难得到神经元k 的局部梯度为
⎛∂y out (t )⎫2
δk 2(t )=e (t )sgn x t f ' v ()(⎪k (t )) ∂M t ⎪k
c ⎝⎭
(0-14)
由此可得，网络输出层神经元的突触权值调整的修正公式为
22
w kj ' (t )=ηδk 2(t )O 1j (t )+α⎰w kj '' dt
(0-15)
同理，可得隐含层神经元的突触权值学习算法。
01w 1ji ' (t )=ηδ1j (t )O i (t )+α⎰w ji '' (t )dt
(0-16)
其中，神经元j 的局域梯度为
2δ(t )=ϕ' (v (t ))⋅∑δk 2(t )w kj (t ) 1j
1j
k =13
(0-17)
至此，本文采用的神经网络原理已介绍完成，考虑到本次仿真过程采用的变时间步长仿真方式类似于连续仿真，故在上文公式中将神经网络中所有离散部分连续化。现将网络工作过程归纳如下。
① 初始化确定网络结构，确定输入层节点数M 和隐含层节
点数Q ，并给出各层突触权值的初值，选择学习率和动量因子；
② 采样得到xin 和yout ，并计算此时误差e ；
③ 计算神经网络各层神经元的输入和输出，输出即为PID 的
三个可调参数Kp 、Ki 、Kd ; ④ 计算控制器的输出u ；
⑤ 进行神经网络的学习，在线调整突触权值矩阵，实现PID
参数的自适应调整；
⑥ 进行下一步迭代运算直至仿真完成。
仿真结果
PID 仿真曲线与神经网络PID 实验仿真曲线为：
-3
欧拉角/r a d
时间/s
-5
欧拉角/r a d
时间/s
从仿真曲线来看两种控制方式都达到了预期目标。两实验的仿真曲线都约在60s 左右达到平衡位置，而后期结果来看，第一个仿真在120S 左右不如第二个仿真结果。