一、状态空间模型离散化
某模型为：
x
˙
=
A
x
+
B
u
y
=
C
x
+
D
u
\dot x=Ax+Bu\\y=Cx+Du
x˙=Ax+Buy=Cx+Du
取采样时间0.005s，状态空间模型可离散化为：
x
(
k
+
1
)
=
A
m
x
(
k
)
+
B
m
u
(
k
)
y
(
k
)
=
C
m
x
(
k
)
+
D
m
u
(
k
)
x(k+1)=A_mx(k)+B_mu(k)\\y(k)=C_mx(k)+D_mu(k)
x(k+1)=Amx(k)+Bmu(k)y(k)=Cmx(k)+Dmu(k)
由于有：
u
(
k
+
1
)
=
u
(
k
)
+
Δ
u
(
k
+
1
)
u(k+1)=u(k)+\Delta u(k+1)
u(k+1)=u(k)+Δu(k+1)
将控制量增广到状态量里得到：
[
x
(
k
+
1
)
u
(
k
)
]
=
[
A
m
B
m
0
I
]
[
x
(
k
)
u
(
k
−
1
)
]
+
[
B
m
I
]
Δ
u
(
k
)
y
(
k
)
=
[
C
m
D
m
]
[
x
(
k
)
u
(
k
−
1
)
]
+
D
m
Δ
u
(
k
)
\begin{bmatrix} x(k+1)\\u(k) \end{bmatrix}=\begin{bmatrix} A_m&B_m\\ 0&I \end{bmatrix}\begin{bmatrix} x(k)\\u(k-1) \end{bmatrix}+\begin{bmatrix} B_m\\I \end{bmatrix}\Delta u(k)\\ y(k)=\begin{bmatrix} C_m&D_m \end{bmatrix}\begin{bmatrix} x(k)\\u(k-1) \end{bmatrix}+D_m\Delta u(k)
[x(k+1)u(k)]=[Am0BmI][x(k)u(k−1)]+[BmI]Δu(k)y(k)=[CmDm][x(k)u(k−1)]+DmΔu(k)
表示为：
x
a
(
k
+
1
)
=
A
a
x
a
(
k
)
+
B
a
Δ
u
(
k
)
y
(
k
)
=
C
a
x
a
(
k
)
+
D
a
Δ
u
(
k
)
x_a(k+1)=A_ax_a(k)+B_a\Delta u(k)\\y(k)=C_ax_a(k)+D_a\Delta u(k)
xa(k+1)=Aaxa(k)+BaΔu(k)y(k)=Caxa(k)+DaΔu(k)
二、递推
1、状态递推
由于
x
a
(
k
+
1
)
=
A
a
x
a
(
k
)
+
B
a
Δ
u
(
k
)
x
a
(
k
+
2
)
=
A
a
2
x
a
(
k
)
+
A
a
B
a
Δ
u
(
k
)
+
B
a
Δ
u
(
k
+
1
)
x
a
(
k
+
3
)
=
A
a
3
x
a
(
k
)
+
A
a
2
B
a
Δ
u
(
k
)
+
A
a
B
a
Δ
u
(
k
+
1
)
+
B
a
Δ
u
(
k
+
2
)
.
.
.
x
a
(
k
+
n
y
)
=
A
a
n
y
x
a
(
k
)
+
A
a
n
y
−
1
B
a
Δ
u
(
k
)
+
.
.
.
+
A
a
B
a
Δ
u
(
k
+
n
y
−
1
)
+
B
a
Δ
u
(
k
+
n
y
−
1
)
x_a(k+1)=A_ax_a(k)+B_a\Delta u(k)\\ x_a(k+2)=A_a^2x_a(k)+A_aB_a\Delta u(k)+B_a\Delta u(k+1)\\ x_a(k+3)=A_a^3x_a(k)+A_a^2B_a\Delta u(k)+A_aB_a\Delta u(k+1)+B_a\Delta u(k+2)\\ ...\\ x_a(k+n_y)=A_a^{n_y}x_a(k)+A_a^{n_y-1}B_a\Delta u(k)+...+A_aB_a\Delta u(k+n_y-1)+B_a\Delta u(k+n_y-1)
xa(k+1)=Aaxa(k)+BaΔu(k)xa(k+2)=Aa2xa(k)+AaBaΔu(k)+BaΔu(k+1)xa(k+3)=Aa3xa(k)+Aa2BaΔu(k)+AaBaΔu(k+1)+BaΔu(k+2)...xa(k+ny)=Aanyxa(k)+Aany−1BaΔu(k)+...+AaBaΔu(k+ny−1)+BaΔu(k+ny−1)
写成矩阵形式为：
[
x
a
(
k
+
1
)
x
a
(
k
+
2
)
.
.
.
x
a
(
k
+
n
y
)
]
=
[
A
a
A
a
2
.
.
.
A
a
n
y
]
x
a
(
k
)
+
[
B
a
0
.
.
.
0
A
a
B
a
B
a
.
.
.
0
.
.
.
.
.
.
.
.
.
.
.
.
A
a
n
y
−
1
B
a
A
a
n
y
−
2
B
a
.
.
.
B
a
]
[
Δ
u
(
k
)
Δ
u
(
k
+
1
)
.
.
.
Δ
u
(
k
+
n
y
−
1
)
]
\begin{bmatrix} x_a(k+1)\\x_a(k+2)\\...\\x_a(k+n_y) \end{bmatrix}= \begin{bmatrix} A_a\\A_a^2\\...\\A_a^{n_y} \end{bmatrix}x_a(k)+ \begin{bmatrix} B_a & 0 & ... & 0\\ A_aB_a & B_a & ... & 0\\ ... & ... & ... & ...\\ A_a^{n_y-1}B_a & A_a^{n_y-2}B_a & ... & B_a \end{bmatrix} \begin{bmatrix} \Delta u(k)\\ \Delta u(k+1)\\...\\ \Delta u(k+n_y-1) \end{bmatrix}
⎣⎢⎢⎡xa(k+1)xa(k+2)...xa(k+ny)⎦⎥⎥⎤=⎣⎢⎢⎡AaAa2...Aany⎦⎥⎥⎤xa(k)+⎣⎢⎢⎡BaAaBa...Aany−1Ba0Ba...Aany−2Ba............00...Ba⎦⎥⎥⎤⎣⎢⎢⎡Δu(k)Δu(k+1)...Δu(k+ny−1)⎦⎥⎥⎤
可简写为：
X
=
F
x
a
(
k
)
+
Q
Δ
U
X=Fx_a(k)+Q\Delta U
X=Fxa(k)+QΔU
2、预测输出递推
（1） n u = n y n_u=n_y nu=ny
由于
y
(
k
+
1
)
=
C
a
A
a
x
a
(
k
)
+
C
a
B
a
Δ
u
(
k
)
+
D
a
Δ
u
(
k
+
1
)
y
(
k
+
2
)
=
C
a
A
a
2
x
a
(
k
)
+
C
a
A
a
B
a
Δ
u
(
k
)
+
C
a
B
a
Δ
u
(
k
+
1
)
+
D
a
Δ
u
(
k
+
2
)
.
.
.
y
(
k
+
n
y
)
=
C
a
A
a
n
y
x
a
(
k
)
+
C
a
A
a
n
y
−
1
Δ
u
(
k
)
+
.
.
.
+
C
a
B
a
Δ
u
(
k
+
n
y
−
1
)
+
D
a
Δ
u
(
k
+
n
y
)
y(k+1)=C_aA_ax_a(k)+C_aB_a\Delta u(k)+D_a\Delta u(k+1)\\ y(k+2)=C_aA_a^2x_a(k)+C_aA_aB_a\Delta u(k)+C_aB_a\Delta u(k+1)+D_a\Delta u(k+2)\\ ...\\ y(k+n_y)=C_aA_a^{n_y}x_a(k)+C_aA_a^{n_y-1}\Delta u(k)+...+C_aB_a\Delta u(k+n_y-1)+D_a\Delta u(k+n_y)
y(k+1)=CaAaxa(k)+CaBaΔu(k)+DaΔu(k+1)y(k+2)=CaAa2xa(k)+CaAaBaΔu(k)+CaBaΔu(k+1)+DaΔu(k+2)...y(k+ny)=CaAanyxa(k)+CaAany−1Δu(k)+...+CaBaΔu(k+ny−1)+DaΔu(k+ny)
写成矩阵形式为：
[
y
(
k
+
1
)
y
(
k
+
2
)
.
.
.
y
(
k
+
n
y
)
]
=
[
C
a
A
a
C
a
A
a
2
.
.
.
C
a
A
a
n
y
]
x
a
(
k
)
+
[
C
a
B
a
D
a
.
.
.
0
C
a
A
a
B
a
C
a
B
a
.
.
.
0
.
.
.
.
.
.
.
.
.
.
.
.
C
a
A
a
n
y
−
1
B
a
C
a
A
a
n
y
−
2
B
a
.
.
.
D
a
]
[
Δ
u
(
k
)
Δ
u
(
k
+
1
)
.
.
.
Δ
u
(
k
+
n
y
)
]
\begin{bmatrix} y(k+1)\\y(k+2)\\...\\y(k+n_y) \end{bmatrix}= \begin{bmatrix} C_aA_a\\C_aA_a^2\\...\\C_aA_a^{n_y} \end{bmatrix}x_a(k)+ \begin{bmatrix} C_aB_a & D_a & ... & 0\\ C_aA_aB_a & C_aB_a & ... & 0\\ ... & ... & ... & ...\\ C_aA_a^{n_y-1}B_a & C_aA_a^{n_y-2}B_a & ... & D_a \end{bmatrix} \begin{bmatrix} \Delta u(k)\\ \Delta u(k+1)\\...\\ \Delta u(k+n_y) \end{bmatrix}
⎣⎢⎢⎡y(k+1)y(k+2)...y(k+ny)⎦⎥⎥⎤=⎣⎢⎢⎡CaAaCaAa2...CaAany⎦⎥⎥⎤xa(k)+⎣⎢⎢⎡CaBaCaAaBa...CaAany−1BaDaCaBa...CaAany−2Ba............00...Da⎦⎥⎥⎤⎣⎢⎢⎡Δu(k)Δu(k+1)...Δu(k+ny)⎦⎥⎥⎤
由于这里超出控制时域
n
u
n_u
nu的
Δ
u
(
k
+
n
y
)
=
0
\Delta u(k+n_y)=0
Δu(k+ny)=0，因此可简写为：
[
y
(
k
+
1
)
y
(
k
+
2
)
.
.
.
y
(
k
+
n
y
)
]
=
[
C
a
A
a
C
a
A
a
2
.
.
.
C
a
A
a
n
y
]
x
a
(
k
)
+
[
C
a
B
a
D
a
.
.
.
0
C
a
A
a
B
a
C
a
B
a
.
.
.
0
.
.
.
.
.
.
.
.
.
.
.
.
C
a
A
a
n
y
−
1
B
a
C
a
A
a
n
y
−
2
B
a
.
.
.
C
a
B
a
]
[
Δ
u
(
k
)
Δ
u
(
k
+
1
)
.
.
.
Δ
u
(
k
+
n
y
−
1
)
]
\begin{bmatrix} y(k+1)\\y(k+2)\\...\\y(k+n_y) \end{bmatrix}= \begin{bmatrix} C_aA_a\\C_aA_a^2\\...\\C_aA_a^{n_y} \end{bmatrix}x_a(k)+ \begin{bmatrix} C_aB_a & D_a & ... & 0\\ C_aA_aB_a & C_aB_a & ... & 0\\ ... & ... & ... & ...\\ C_aA_a^{n_y-1}B_a & C_aA_a^{n_y-2}B_a & ... & C_aB_a \end{bmatrix} \begin{bmatrix} \Delta u(k)\\ \Delta u(k+1)\\...\\ \Delta u(k+n_y-1) \end{bmatrix}
⎣⎢⎢⎡y(k+1)y(k+2)...y(k+ny)⎦⎥⎥⎤=⎣⎢⎢⎡CaAaCaAa2...CaAany⎦⎥⎥⎤xa(k)+⎣⎢⎢⎡CaBaCaAaBa...CaAany−1BaDaCaBa...CaAany−2Ba............00...CaBa⎦⎥⎥⎤⎣⎢⎢⎡Δu(k)Δu(k+1)...Δu(k+ny−1)⎦⎥⎥⎤
即：
Y
=
P
x
a
(
k
)
+
H
Δ
U
Y=Px_a(k)+H\Delta U
Y=Pxa(k)+HΔU
（2） n u ≤ n y n_u\le n_y nu≤ny
由于：
y
(
k
+
1
)
=
C
a
A
a
x
a
(
k
)
+
C
a
B
a
Δ
u
(
k
)
+
D
a
Δ
u
(
k
+
1
)
y
(
k
+
2
)
=
C
a
A
a
2
x
a
(
k
)
+
C
a
A
a
B
a
Δ
u
(
k
)
+
C
a
B
a
Δ
u
(
k
+
1
)
+
D
a
Δ
u
(
k
+
2
)
.
.
.
y
(
k
+
n
u
−
1
)
=
C
a
A
a
n
u
−
1
x
a
(
k
)
+
C
a
A
a
n
u
−
2
Δ
u
(
k
)
+
.
.
.
+
C
a
B
a
Δ
u
(
k
+
n
u
−
2
)
+
D
a
Δ
u
(
k
+
n
u
−
1
)
y
(
k
+
n
u
)
=
C
a
A
a
n
u
x
a
(
k
)
+
C
a
A
a
n
u
−
1
Δ
u
(
k
)
+
.
.
.
+
C
a
B
a
Δ
u
(
k
+
n
u
−
1
)
+
D
a
∗
0
.
.
.
y
(
k
+
n
y
)
=
C
a
A
a
n
y
x
a
(
k
)
+
C
a
A
a
n
y
−
1
Δ
u
(
k
)
+
.
.
.
+
C
a
A
a
n
y
−
n
u
B
a
Δ
u
(
k
+
n
u
−
1
)
+
.
.
.
+
C
a
B
a
∗
0
y(k+1)=C_aA_ax_a(k)+C_aB_a\Delta u(k)+D_a\Delta u(k+1)\\ y(k+2)=C_aA_a^2x_a(k)+C_aA_aB_a\Delta u(k)+C_aB_a\Delta u(k+1)+D_a\Delta u(k+2)\\ ...\\ y(k+n_u-1)=C_aA_a^{n_u-1}x_a(k)+C_aA_a^{n_u-2}\Delta u(k)+...+C_aB_a\Delta u(k+n_u-2)+D_a\Delta u(k+n_u-1)\\ y(k+n_u)=C_aA_a^{n_u}x_a(k)+C_aA_a^{n_u-1}\Delta u(k)+...+C_aB_a\Delta u(k+n_u-1)+D_a*0\\ ...\\ y(k+n_y)=C_aA_a^{n_y}x_a(k)+C_aA_a^{n_y-1}\Delta u(k)+...+C_aA_a^{n_y-n_u}B_a\Delta u(k+n_u-1)+...+C_aB_a*0
y(k+1)=CaAaxa(k)+CaBaΔu(k)+DaΔu(k+1)y(k+2)=CaAa2xa(k)+CaAaBaΔu(k)+CaBaΔu(k+1)+DaΔu(k+2)...y(k+nu−1)=CaAanu−1xa(k)+CaAanu−2Δu(k)+...+CaBaΔu(k+nu−2)+DaΔu(k+nu−1)y(k+nu)=CaAanuxa(k)+CaAanu−1Δu(k)+...+CaBaΔu(k+nu−1)+Da∗0...y(k+ny)=CaAanyxa(k)+CaAany−1Δu(k)+...+CaAany−nuBaΔu(k+nu−1)+...+CaBa∗0
转化为矩阵形式：
[
y
(
k
+
1
)
y
(
k
+
2
)
.
.
.
y
(
k
+
n
y
)
]
=
[
C
a
A
a
C
a
A
a
2
.
.
.
C
a
A
a
n
y
]
x
a
(
k
)
+
[
C
a
B
a
D
a
.
.
.
0
C
a
A
a
B
a
C
a
B
a
.
.
.
0
.
.
.
.
.
.
.
.
.
.
.
.
C
a
A
a
n
y
−
1
B
a
C
a
A
a
n
y
−
2
B
a
.
.
.
C
a
A
a
n
y
−
n
u
B
a
]
[
Δ
u
(
k
)
Δ
u
(
k
+
1
)
.
.
.
Δ
u
(
k
+
n
u
−
1
)
]
\begin{bmatrix} y(k+1)\\y(k+2)\\...\\y(k+n_y) \end{bmatrix}= \begin{bmatrix} C_aA_a\\C_aA_a^2\\...\\C_aA_a^{n_y} \end{bmatrix}x_a(k)+ \begin{bmatrix} C_aB_a & D_a & ... & 0\\ C_aA_aB_a & C_aB_a & ... & 0\\ ... & ... & ... & ...\\ C_aA_a^{n_y-1}B_a & C_aA_a^{n_y-2}B_a & ... & C_aA_a^{n_y-n_u}B_a \end{bmatrix} \begin{bmatrix} \Delta u(k)\\ \Delta u(k+1)\\...\\ \Delta u(k+n_u-1) \end{bmatrix}
⎣⎢⎢⎡y(k+1)y(k+2)...y(k+ny)⎦⎥⎥⎤=⎣⎢⎢⎡CaAaCaAa2...CaAany⎦⎥⎥⎤xa(k)+⎣⎢⎢⎡CaBaCaAaBa...CaAany−1BaDaCaBa...CaAany−2Ba............00...CaAany−nuBa⎦⎥⎥⎤⎣⎢⎢⎡Δu(k)Δu(k+1)...Δu(k+nu−1)⎦⎥⎥⎤
即：
Y
=
P
x
a
(
k
)
+
H
Δ
U
Y=Px_a(k)+H\Delta U
Y=Pxa(k)+HΔU
这里
H
H
H和
Δ
U
\Delta U
ΔU的维度进行了削减。
三、反馈校正
预测模型的输出和实际输出之间不一定完全匹配，此时可通过引入反馈环节对预测输出进行修正。即利用当前时刻获得的实际输出与预测输出之差，在下一时刻加入到预测输出之中，再进行非线性优化。
跟踪误差：
e
(
k
)
=
y
(
k
)
−
y
^
(
k
)
e(k)=y(k)-\hat y(k)
e(k)=y(k)−y^(k)
修正后的预测输出为：
Y
^
(
k
)
=
Y
^
(
k
)
+
K
e
(
k
)
\hat Y(k)=\hat Y(k)+Ke(k)
Y^(k)=Y^(k)+Ke(k)
四、性能指标滚动优化
1、求解控制量序列思路
为了解决跟踪问题，一般性能指标设置为：
m
i
n
J
=
(
Y
−
Y
r
e
f
)
T
Q
(
Y
−
Y
r
e
f
)
+
Δ
U
T
R
Δ
U
min J=(Y-Y_{ref})^TQ(Y-Y_{ref})+\Delta U^TR\Delta U
minJ=(Y−Yref)TQ(Y−Yref)+ΔUTRΔU
由于：
J
=
(
P
x
a
(
k
)
−
Y
r
e
f
+
H
Δ
U
)
T
Q
(
P
x
a
(
k
)
−
Y
r
e
f
+
H
Δ
U
)
+
U
T
R
U
J=(Px_a(k)-Y_{ref}+H\Delta U)^TQ(Px_a(k)-Y_{ref}+H\Delta U)+U^TRU
J=(Pxa(k)−Yref+HΔU)TQ(Pxa(k)−Yref+HΔU)+UTRU
令
E
=
P
x
a
(
k
)
−
Y
r
e
f
E=Px_a(k)-Y_{ref}
E=Pxa(k)−Yref,得到：
J
=
(
E
+
H
Δ
U
)
T
Q
(
E
+
H
Δ
U
)
+
Δ
U
T
R
Δ
U
=
E
T
Q
E
+
E
T
Q
(
H
Δ
U
)
+
(
H
Δ
U
)
T
Q
E
+
(
H
Δ
U
)
T
Q
(
H
Δ
U
)
+
Δ
U
T
R
Δ
U
=
E
T
Q
E
+
2
(
H
Δ
U
)
T
Q
E
+
(
H
Δ
U
)
T
Q
(
H
Δ
U
)
+
Δ
U
T
R
Δ
U
=
E
T
Q
E
+
2
(
H
Δ
U
)
T
Q
E
+
Δ
U
T
H
T
Q
H
Δ
U
+
Δ
U
T
R
Δ
U
=
Δ
U
T
(
H
T
Q
H
+
R
)
Δ
U
+
2
Δ
U
T
H
T
Q
E
+
E
T
Q
E
J=(E+H\Delta U)^TQ(E+H\Delta U)+\Delta U^TR\Delta U\\ =E^TQE+E^TQ(H\Delta U)+(H\Delta U)^TQE+(H\Delta U)^TQ(H\Delta U)+\Delta U^TR\Delta U\\ =E^TQE+2(H\Delta U)^TQE+(H\Delta U)^TQ(H\Delta U)+\Delta U^TR\Delta U\\ =E^TQE+2(H\Delta U)^TQE+\Delta U^TH^TQH\Delta U+\Delta U^TR\Delta U\\ =\Delta U^T(H^TQH+R)\Delta U+2\Delta U^TH^TQE+E^TQE
J=(E+HΔU)TQ(E+HΔU)+ΔUTRΔU=ETQE+ETQ(HΔU)+(HΔU)TQE+(HΔU)TQ(HΔU)+ΔUTRΔU=ETQE+2(HΔU)TQE+(HΔU)TQ(HΔU)+ΔUTRΔU=ETQE+2(HΔU)TQE+ΔUTHTQHΔU+ΔUTRΔU=ΔUT(HTQH+R)ΔU+2ΔUTHTQE+ETQE
2、常规求解
为了求得使得性能指标最小时的控制量序列，可通过
∂
J
∂
Δ
U
=
0
\frac{\partial J}{\partial \Delta U}=0
∂ΔU∂J=0求解。
∂
J
∂
Δ
U
=
2
(
H
T
Q
H
+
R
)
Δ
U
−
2
(
H
T
Q
E
)
=
0
\frac{\partial J}{\partial \Delta U}=2(H^TQH+R)\Delta U-2(H^TQE)=0
∂ΔU∂J=2(HTQH+R)ΔU−2(HTQE)=0
得到：
Δ
U
=
−
(
H
T
Q
H
+
R
)
−
1
(
H
T
Q
E
)
\Delta U=-(H^TQH+R)^{-1}(H^TQE)
ΔU=−(HTQH+R)−1(HTQE)
3、转化为二次规划问题求解
二次型优化函数quadprog可以求解这样的问题：
J
(
U
)
=
0.5
U
T
H
U
+
f
T
U
J(U)=0.5U^THU+f^TU
J(U)=0.5UTHU+fTU
而由于
E
T
Q
E
E^TQE
ETQE为与
U
U
U无关，因此可舍去，并令：
H
=
2
(
H
T
Q
H
+
R
)
f
T
=
2
E
T
Q
H
H=2(H^TQH+R)\\f^T=2E^TQH
H=2(HTQH+R)fT=2ETQH
即可求解使得性能指标最小的控制量序列。
五、约束处理
1、二次规划问题
形如
J
(
x
)
=
0.5
x
T
H
x
+
f
T
x
s
.
t
.
A
x
≤
b
A
e
q
x
=
b
e
q
l
b
≤
x
≤
u
b
J(x)=0.5x^THx+f^Tx\\ s.t. ~~~~Ax\le b\\ ~~A_{eq}x=b_{eq}\\ ~~lb\le x\le ub
J(x)=0.5xTHx+fTxs.t. Ax≤b Aeqx=beq lb≤x≤ub
可通过MATLAB函数quadprog求解，因此需要将约束条件转化为以上形式。
调用格式为x=quadprog(H,f,A,b,Aeq,Beq,lb,ub);
2、约束处理
(1)控制量增量约束
控制量增量约束为：
Δ
u
m
i
n
≤
Δ
u
(
k
)
≤
Δ
u
m
a
x
\Delta u_{min}\le \Delta u(k)\le \Delta u_{max}
Δumin≤Δu(k)≤Δumax
可根据
l
b
≤
x
≤
u
b
lb\le x\le ub
lb≤x≤ub的形式直接写为：
Δ
U
m
i
n
≤
Δ
U
≤
Δ
U
m
a
x
\Delta U_{min}\le \Delta U\le \Delta U_{max}
ΔUmin≤ΔU≤ΔUmax
(2)控制量约束
控制量受到约束为：
u
m
i
n
≤
u
(
k
)
≤
u
m
a
x
u_{min}\le u(k)\le u_{max}
umin≤u(k)≤umax
需要经过一定的转化：
u
(
k
+
1
)
=
u
(
k
)
+
Δ
u
(
k
)
u
(
k
+
2
)
=
u
(
k
)
+
Δ
u
(
k
)
+
Δ
u
(
k
+
1
)
.
.
.
u
(
k
+
n
u
)
=
u
(
k
)
+
Δ
u
(
k
)
+
.
.
.
+
Δ
u
(
k
+
n
u
−
1
)
u(k+1)=u(k)+\Delta u(k)\\ u(k+2)=u(k)+\Delta u(k)+\Delta u(k+1)\\ ...\\ u(k+n_u)=u(k)+\Delta u(k)+...+\Delta u(k+n_u-1)
u(k+1)=u(k)+Δu(k)u(k+2)=u(k)+Δu(k)+Δu(k+1)...u(k+nu)=u(k)+Δu(k)+...+Δu(k+nu−1)
得到：
[
u
(
k
+
1
)
−
u
(
k
)
u
(
k
+
2
)
−
u
(
k
)
.
.
.
u
(
k
+
n
u
)
−
u
(
k
)
]
=
[
I
m
∗
m
0
m
∗
m
.
.
.
0
m
∗
m
I
m
∗
m
I
m
∗
m
.
.
.
0
m
∗
m
.
.
.
I
m
∗
m
I
m
∗
m
.
.
.
I
m
∗
m
]
[
Δ
u
(
k
)
Δ
u
(
k
+
1
)
.
.
.
Δ
u
(
k
+
n
u
−
1
)
]
\begin{bmatrix} u(k+1)-u(k)\\ u(k+2)-u(k)\\ ...\\ u(k+n_u)-u(k) \end{bmatrix}= \begin{bmatrix} I_{m*m} & 0_{m*m} & ... & 0_{m*m}\\ I_{m*m} & I_{m*m} & ... & 0_{m*m}\\ ...\\ I_{m*m} & I_{m*m} & ... & I_{m*m} \end{bmatrix} \begin{bmatrix} \Delta u(k)\\ \Delta u(k+1)\\ ...\\ \Delta u(k+n_u-1) \end{bmatrix}
⎣⎢⎢⎡u(k+1)−u(k)u(k+2)−u(k)...u(k+nu)−u(k)⎦⎥⎥⎤=⎣⎢⎢⎡Im∗mIm∗m...Im∗m0m∗mIm∗mIm∗m.........0m∗m0m∗mIm∗m⎦⎥⎥⎤⎣⎢⎢⎡Δu(k)Δu(k+1)...Δu(k+nu−1)⎦⎥⎥⎤
由此推出：
[
I
m
∗
m
0
m
∗
m
.
.
.
0
m
∗
m
I
m
∗
m
I
m
∗
m
.
.
.
0
m
∗
m
.
.
.
I
m
∗
m
I
m
∗
m
.
.
.
I
m
∗
m
]
[
Δ
u
(
k
)
Δ
u
(
k
+
1
)
.
.
.
Δ
u
(
k
+
n
u
−
1
)
]
≤
[
u
m
a
x
−
u
(
k
)
u
m
a
x
−
u
(
k
)
.
.
.
u
m
a
x
−
u
(
k
)
]
\begin{bmatrix} I_{m*m} & 0_{m*m} & ... & 0_{m*m}\\ I_{m*m} & I_{m*m} & ... & 0_{m*m}\\ ...\\ I_{m*m} & I_{m*m} & ... & I_{m*m} \end{bmatrix} \begin{bmatrix} \Delta u(k)\\ \Delta u(k+1)\\ ...\\ \Delta u(k+n_u-1) \end{bmatrix}\le \begin{bmatrix} u_{max}-u(k)\\ u_{max}-u(k)\\ ...\\ u_{max}-u(k) \end{bmatrix}
⎣⎢⎢⎡Im∗mIm∗m...Im∗m0m∗mIm∗mIm∗m.........0m∗m0m∗mIm∗m⎦⎥⎥⎤⎣⎢⎢⎡Δu(k)Δu(k+1)...Δu(k+nu−1)⎦⎥⎥⎤≤⎣⎢⎢⎡umax−u(k)umax−u(k)...umax−u(k)⎦⎥⎥⎤
−
[
I
m
∗
m
0
m
∗
m
.
.
.
0
m
∗
m
I
m
∗
m
I
m
∗
m
.
.
.
0
m
∗
m
.
.
.
I
m
∗
m
I
m
∗
m
.
.
.
I
m
∗
m
]
[
Δ
u
(
k
)
Δ
u
(
k
+
1
)
.
.
.
Δ
u
(
k
+
n
u
−
1
)
]
≤
[
−
u
m
i
n
+
u
(
k
)
−
u
m
i
n
+
u
(
k
)
.
.
.
−
u
m
i
n
+
u
(
k
)
]
-\begin{bmatrix} I_{m*m} & 0_{m*m} & ... & 0_{m*m}\\ I_{m*m} & I_{m*m} & ... & 0_{m*m}\\ ...\\ I_{m*m} & I_{m*m} & ... & I_{m*m} \end{bmatrix} \begin{bmatrix} \Delta u(k)\\ \Delta u(k+1)\\ ...\\ \Delta u(k+n_u-1) \end{bmatrix}\le \begin{bmatrix} -u_{min}+u(k)\\ -u_{min}+u(k)\\ ...\\ -u_{min}+u(k) \end{bmatrix}
−⎣⎢⎢⎡Im∗mIm∗m...Im∗m0m∗mIm∗mIm∗m.........0m∗m0m∗mIm∗m⎦⎥⎥⎤⎣⎢⎢⎡Δu(k)Δu(k+1)...Δu(k+nu−1)⎦⎥⎥⎤≤⎣⎢⎢⎡−umin+u(k)−umin+u(k)...−umin+u(k)⎦⎥⎥⎤
上述下三角矩阵设为W，合并为：
[
W
−
W
]
Δ
U
≤
[
U
m
a
x
−
U
−
U
m
i
n
+
U
]
\begin{bmatrix} W\\ -W \end{bmatrix} \Delta U\le \begin{bmatrix} U_{max}-U\\ -U_{min}+U\\ \end{bmatrix}
[W−W]ΔU≤[Umax−U−Umin+U]
(3) 被控量约束
同理，被控量的约束为：
Y
m
i
n
≤
Y
≤
Y
m
a
x
Y
m
i
n
≤
P
x
a
(
k
)
+
H
Δ
U
≤
Y
m
a
x
Y
m
i
n
−
P
x
a
(
k
)
≤
H
Δ
U
≤
Y
m
a
x
−
P
x
a
(
k
)
Y_{min}\le Y\le Y_{max}\\ Y_{min}\le Px_a(k)+H\Delta U\le Y_{max}\\ Y_{min}-Px_a(k)\le H\Delta U\le Y_{max}-Px_a(k)
Ymin≤Y≤YmaxYmin≤Pxa(k)+HΔU≤YmaxYmin−Pxa(k)≤HΔU≤Ymax−Pxa(k)
可推出：
H
Δ
U
≤
Y
m
a
x
−
P
x
a
(
k
)
−
H
Δ
U
≤
−
Y
m
i
n
+
P
x
a
(
k
)
H\Delta U\le Y_{max}-Px_a(k)\\ -H\Delta U\le -Y_{min}+Px_a(k)
HΔU≤Ymax−Pxa(k)−HΔU≤−Ymin+Pxa(k)
即得到：
[
H
−
H
]
Δ
U
≤
[
Y
m
a
x
−
P
x
a
(
k
)
−
Y
m
i
n
+
P
x
a
(
k
)
]
\begin{bmatrix} H\\ -H \end{bmatrix} \Delta U\le \begin{bmatrix} Y_{max}-Px_a(k)\\ -Y_{min}+Px_a(k)\\ \end{bmatrix}
[H−H]ΔU≤[Ymax−Pxa(k)−Ymin+Pxa(k)]
六、仿真代码
clc;clear all
%% 维度定义
n=2; % 状态量个数
m=2; % 控制量个数
r=2; % 输出量个数
%% 连续状态空间模型
A=[-2.022,1.057;0.211,-2.406]; % n*n
B=[0.558,1.24;0.559,0.444]; % n*m
C=[1,0;0,1]; % r*n
D=[0,0;0,0]; % r*m
%% 离散化
Ts=0.005; % 采样周期
[Am,Bm]=c2d(A,B,Ts);
Cm=C;Dm=D;
%% 将控制量增广到状态量，得到新的状态空间方程
Aa=[Am,Bm;zeros(m,n),eye(m)]; % (m+n)*(m+n)
Ba=[Bm;eye(m)]; % (n+m)*m
Ca=[Cm,Dm]; % r*(n+m)
Da=Dm; % r*m
%% 参数定义
ny=30; % 预测时域
nu=30; % 控制时域
Q=eye(r*ny); % 误差权重 (r*ny)*(r*ny)
R=eye(m*nu)*0.01; % 控制量权重 (m*ny)*(m*ny)
umax=1.5; % 控制量上界
umin=-1.5; % 控制量下界
dumax=1*ones(m,1); % 控制量增量上界
dumin=-1*ones(m,1); % 控制量增量下界
Ymax=ones(r*ny,1)*1; % 输出量上界
Ymin=ones(r*ny,1)*0; % 输出量下界
%% 控制量约束转化为二次规划Ax<=b形式
W=[];
for i=1:nu
W_row=[];
for j=1:nu
if j<=i
W_row=[W_row,eye(m)];
else
W_row=[W_row,zeros(m,m)];
end
end
W=[W;W_row];
end
AA=[W;-W;];
%% 系统初始状态
xa(:,1)=zeros(n+m,1);
%% 具体控制过程
for k=1:1:100
yr(:,k)=[0.1;0.05]; % 指令信号
%% 预测时域内P、H、Yref矩阵计算
P=[];H=[];Yref=[];K=[];
for i=1:ny
H_row=[];
for j=1:i
H_row=[H_row,Ca*Aa^(i-j)*Ba];
end
if i<=ny-1
H_row=[H_row,Da];
end
if i<=ny-2
for j=i+2:ny
H_row=[H_row,zeros(r,m);];
end
end
P=[P;Ca*Aa^i]; % P矩阵维度为(r*ny)*(n+m)
H=[H;H_row]; % H矩阵维度为(r*ny)*(m*ny)
Yref=[Yref;yr(:,k)]; % Yref矩阵维度为(r*ny)*1
K=[K;diag([1 1])]; % 预测误差修正系数
end
H=H(:,1:nu*m); % 若控制时域小于预测时域 (r*ny)*(m*nu)
%% 求解二次规划问题
if k==1
E=P*xa(:,k)-Yref; % E矩阵维度为(r*ny)*1
else
E=P*xa(:,k)-Yref+K*e(:,k-1); % E矩阵维度为(r*ny)*1
end % 对预测输出进行修正
HH=2*(H'*Q*H+R); % HH矩阵维度为(m*nu)*(m*nu)
f=(2*E'*Q*H)'; % f矩阵维度为(m*ny)*1
AA=[W;-W;H;-H];
bb1=[];bb2=[];bb3=[];bb4=[];
for i=1:1:nu
if k==1
bb1=[bb1;[umax;umax]-[0;0]];
bb2=[bb2;[-umin;-umin]+[0;0]];
else
bb1=[bb1;[umax;umax]-u(:,k-1)];
bb2=[bb2;[-umin;-umin]+u(:,k-1)];
end
end
bb3=Ymax-P*xa(:,k);
bb4=-Ymin+P*xa(:,k);
bb=[bb1;bb2;bb3;bb4]; % 控制量约束与输出量约束转化为Ax<=b形式
Uk=quadprog(HH,f,AA,bb,[],[],dumin,dumax);
du(:,k)=Uk(1:m,1);
%% 直接使用DMC算法的结论
% Uk=(H'*Q*H+R)^(-1)*H'*Q*(Yref-P*xa(:,k));
% du(:,k)=Uk(1:m,1);
%% 控制作用于系统
xa(:,k+1)=Aa*xa(:,k)+Ba*du(:,k); % 增广系统的状态x(k+1)
y(:,k)=Ca*xa(:,k)+Da*du(:,k)+[0;0]; % 增广系统的输出y(k+1) 增加了模型不确定性
Y=P*xa(:,k)+H*Uk; % 预测输出
y_hat(:,k)=Ca*xa(:,k)+Da*du(:,k); % 当前时刻预测输出
e(:,k)=y(:,k)-y_hat(:,k); % 当前时刻预测误差
if k==1
u(:,k)=zeros(m,1)+du(:,k);
else
u(:,k)=u(:,k-1)+du(:,k); % 原始系统的输入u(k+1)
end
end
%% 绘图
figure(1)
plot(yr(1,:),'r');
hold on
plot(y(1,:),'b');
title('y1');
figure(2)
plot(yr(2,:),'r');
hold on
plot(y(2,:),'b');
title('y2');
figure(3)
plot(u(1,:),'r');
hold on
plot(u(2,:),'b');
title('u');
figure(4)
plot(du(1,:),'r');
hold on
plot(du(2,:),'b');
title('du');
figure(5)
plot(e(1,:),'r');
hold on
plot(e(2,:),'b');
title('e');
335
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
