模型预测控制(Model Predict Control)利用一个已有的模型、系统当前的状态和未来的控制量去预测系统未来的输出;这个输出的长度是控制周期的整数倍;由于未来的控制量是未知的,需要根据一定的条件进行求解,以得到未来的控制量序列,并在每个控制周期结束后,系统根据当前实际状态重新预测系统未来的输出。因此模型预测控制有三个关键步骤,分别是:预测模型、滚动优化和反馈校正。
-
预测模型:预测模型是控制的基础,根据对象的历史信息和未来输入,预测系统未来的输出。预测模型有:状态空间方程、传递函数、阶跃响应、脉冲响应、神经网络模型等等。
-
滚动优化:模型预测控制通过控制某一性能指标的最优来确定控制序列,但优化不是一次离线进行,而是反复在线进行的。
-
反馈校正:为了防止模型失配或者干扰等引起控制对理想状态的偏差,在新的采样时刻,首先检测对象的实际输出,并利用这一实时信息对基于模型的预测结果进行修正,然后再进行新的优化。
在此,选择状态空间方程作为预测模型。
1 状态空间方程
状态空间方程为:
{
x
˙
=
A
x
+
B
u
+
D
d
i
s
y
=
C
x
+
D
u
(1)
\begin{cases} \dot{x} = A x + B u + D_{dis} \\ y = C x + D u \end{cases} \tag{1}
{x˙=Ax+Bu+Ddisy=Cx+Du(1)
连续状态空间方程需要离散化,常用的离散化方法有:欧拉公式离散化,后向差分和双线性变换离散化
Matrix | Euler | Backward Rect | Bilinear Transform |
---|---|---|---|
A d A_d Ad | I + A ∗ Δ t I + A*\Delta t I+A∗Δt | ( I − A ∗ Δ t ) − 1 (I - A*\Delta t)^{-1} (I−A∗Δt)−1 | ( I − A ∗ Δ t 2 ) − 1 ( I + A ∗ Δ t 2 ) (I - \frac{A*\Delta t}{2})^{-1} (I + \frac{A*\Delta t}{2}) (I−2A∗Δt)−1(I+2A∗Δt) |
B d B_d Bd | B ∗ Δ t B* \Delta t B∗Δt | ( I − A ∗ Δ t ) − 1 B ∗ Δ t (I - A*\Delta t)^{-1} B * \Delta t (I−A∗Δt)−1B∗Δt | ( I − A ∗ Δ t 2 ) − 1 B ∗ Δ t (I - \frac{A*\Delta t}{2})^{-1} B * \Delta t (I−2A∗Δt)−1B∗Δt |
D d , d i s D_{d,dis} Dd,dis | D d i s ∗ Δ t D_{dis} * \Delta t Ddis∗Δt | ( I − A ∗ Δ t ) − 1 D d i s ∗ Δ t (I - A*\Delta t)^{-1} D_{dis} * \Delta t (I−A∗Δt)−1Ddis∗Δt | ( I − A ∗ Δ t 2 ) − 1 D d i s ∗ Δ t (I - \frac{A*\Delta t}{2})^{-1} D_{dis} * \Delta t (I−2A∗Δt)−1Ddis∗Δt |
C d C_d Cd | C C C | C ( I − A ∗ Δ t ) − 1 C (I - A*\Delta t)^{-1} C(I−A∗Δt)−1 | C ( I − A ∗ Δ t 2 ) − 1 C (I - \frac{A*\Delta t}{2})^{-1} C(I−2A∗Δt)−1 |
D d D_d Dd | D D D | D + C ( I − A ∗ Δ t ) − 1 B ∗ Δ t D + C (I - A*\Delta t)^{-1} B * \Delta t D+C(I−A∗Δt)−1B∗Δt | D + C ( I − A ∗ Δ t 2 ) − 1 B ∗ Δ t 2 D + C (I - \frac{A*\Delta t}{2})^{-1} \frac{B * \Delta t}{2} D+C(I−2A∗Δt)−12B∗Δt |
离散的状态空间方程为:
{
x
k
+
1
=
A
d
x
k
+
B
d
u
k
+
D
d
,
d
i
s
y
k
=
C
d
x
k
+
D
d
u
k
(2)
\begin{cases} x_{k+1} = A_d x_k + B_d u_k + D_{d,{dis}} \\ y_k = C_d x_k + D_d u_k \end{cases} \tag{2}
{xk+1=Adxk+Bduk+Dd,disyk=Cdxk+Dduk(2)
2 预测模型
根据经验模型(状态空间方程)和当前状态、未来控制量,可以预测未来的输出量。
{
x
k
+
1
=
A
d
x
k
+
B
d
u
k
+
D
d
,
d
i
s
x
k
+
2
=
A
d
x
k
+
1
+
B
d
u
k
+
1
+
D
d
,
d
i
s
=
A
d
(
A
d
x
k
+
B
d
u
k
+
D
d
,
d
i
s
)
+
B
d
u
k
+
1
+
D
d
,
d
i
s
=
A
d
2
x
k
+
(
A
d
B
d
u
k
+
B
d
u
k
+
1
)
+
A
d
D
d
,
d
i
s
+
D
d
,
d
i
s
x
k
+
3
=
A
d
x
k
+
2
+
B
d
u
k
+
2
+
D
d
,
d
i
s
=
A
d
2
x
k
+
1
+
(
A
d
B
d
u
k
+
1
+
B
d
u
k
+
2
)
+
A
d
D
d
,
d
i
s
+
D
d
,
d
i
s
=
A
d
3
x
k
+
(
A
d
2
B
d
u
k
+
A
d
B
d
u
k
+
1
+
B
d
u
k
+
2
)
+
A
d
2
D
d
,
d
i
s
+
A
d
D
d
,
d
i
s
+
D
d
,
d
i
s
⋮
x
k
+
N
p
=
A
d
N
p
x
k
+
(
A
d
N
p
−
1
B
d
u
k
+
⋯
+
A
d
N
p
−
N
c
+
1
B
d
u
k
+
1
+
A
d
N
p
−
N
c
B
d
u
k
+
N
c
−
1
)
+
(
A
d
N
p
−
1
D
d
,
d
i
s
+
⋯
+
A
d
D
d
,
d
i
s
+
D
d
,
d
i
s
)
(3)
\begin{cases} x_{k+1} &= A_d x_k + B_d u_k + D_{d,{dis}} \\ x_{k+2} &= A_d x_{k+1} + B_d u_{k+1} + D_{d,{dis}} \\ &= A_d(A_d x_k + B_d u_k + D_{d,{dis}}) + B_d u_{k+1} + D_{d,{dis}} \\ &= A^2_{d} x_k + (A_d B_d u_k + B_d u_{k+1}) + A_d D_{d,{dis}} + D_{d,{dis}} \\ x_{k+3} &= A_d x_{k+2} + B_d u_{k+2} + D_{d,{dis}} \\ &= A^2_{d} x_{k+1} + (A_dB_du_{k+1} + B_d u_{k+2}) + A_dD_{d,{dis}} + D_{d,{dis}} \\ &= A^3_d x_k + (A^2_dB_du_{k} + A_dB_du_{k+1} + B_d u_{k+2}) + A^2_d D_{d,{dis}} + A_dD_{d,{dis}} + D_{d,{dis}} \\ \vdots \\ x_{k+N_p} &= A^{N_p}_d x_{k} + (A^{N_p -1}_dB_du_{k} + \cdots + A^{N_p-N_c+1}_dB_du_{k+1} + A^{N_p-N_c}_d B_d u_{k+N_c-1}) \\ &+(A^{N_p -1}_d D_{d,{dis}} + \cdots + A_d D_{d,{dis}} + D_{d,{dis}}) \end{cases} \tag{3}
⎩
⎨
⎧xk+1xk+2xk+3⋮xk+Np=Adxk+Bduk+Dd,dis=Adxk+1+Bduk+1+Dd,dis=Ad(Adxk+Bduk+Dd,dis)+Bduk+1+Dd,dis=Ad2xk+(AdBduk+Bduk+1)+AdDd,dis+Dd,dis=Adxk+2+Bduk+2+Dd,dis=Ad2xk+1+(AdBduk+1+Bduk+2)+AdDd,dis+Dd,dis=Ad3xk+(Ad2Bduk+AdBduk+1+Bduk+2)+Ad2Dd,dis+AdDd,dis+Dd,dis=AdNpxk+(AdNp−1Bduk+⋯+AdNp−Nc+1Bduk+1+AdNp−NcBduk+Nc−1)+(AdNp−1Dd,dis+⋯+AdDd,dis+Dd,dis)(3)
其中,
N
p
N_p
Np是预测时域,
N
c
N_c
Nc是控制时域,并且
N
p
≥
N
c
N_p \geq N_c
Np≥Nc,在
N
c
<
k
≤
N
p
Nc < k \leq N_p
Nc<k≤Np时域内,
u
k
=
0
u_k = 0
uk=0。
将公式(3)整理可得:
X
=
F
X
0
+
Φ
U
+
E
(4)
X = F X_{0} + \Phi U + E \tag{4}
X=FX0+ΦU+E(4)
其中:
{
X
=
[
x
k
+
1
,
x
k
+
2
,
⋯
,
x
k
+
N
P
]
T
;
X
0
=
x
k
;
U
=
[
u
k
,
u
k
+
2
,
⋯
,
u
k
+
N
c
−
1
]
T
;
F
=
[
A
d
,
A
d
2
,
⋯
,
A
d
N
p
]
T
;
Φ
=
[
B
d
0
⋯
0
A
d
B
d
B
d
⋯
0
⋮
⋮
⋱
⋮
A
N
p
−
1
d
B
d
A
N
p
−
2
d
B
d
⋯
A
d
N
p
−
N
c
B
d
]
E
=
[
D
d
,
d
i
s
,
A
d
D
d
,
d
i
s
+
D
d
,
d
i
s
,
⋯
,
∑
i
=
0
N
p
−
1
A
k
i
D
d
,
d
i
s
]
T
(5)
\begin{cases} X = [x_{k+1},x_{k+2},\cdots,x_{k+N_P}]^T; \\ X_0 = x_k; \\ U = [u_{k},u_{k+2},\cdots,u_{k+N_c-1}]^T; \\ F = [A_d, A^{2}_d,\cdots, A^{N_p}_d]^T; \\ \Phi = \left[\begin{matrix} B_d &0 & \cdots & 0 \\ A_d B_d & B_d & \cdots & 0 \\ \vdots & \vdots & \ddots & \vdots \\ A^{N_p -1}d B_d & A^{N_p -2}d B_d & \cdots & A^{N_p-N_c}_d B_d \\ \end{matrix} \right] \\ E = [D_{d,dis}, A_d D_{d,dis} + D_{d,dis},\cdots,\sum_{i=0}^{N_p-1}A^i_kD_{d,dis}]^T \\ \end{cases} \tag{5}
⎩
⎨
⎧X=[xk+1,xk+2,⋯,xk+NP]T;X0=xk;U=[uk,uk+2,⋯,uk+Nc−1]T;F=[Ad,Ad2,⋯,AdNp]T;Φ=
BdAdBd⋮ANp−1dBd0Bd⋮ANp−2dBd⋯⋯⋱⋯00⋮AdNp−NcBd
E=[Dd,dis,AdDd,dis+Dd,dis,⋯,∑i=0Np−1AkiDd,dis]T(5)
假设
D
d
=
0
D_d=0
Dd=0,由公(4)可以得到系统的预测输出量:
Y
=
C
d
F
X
0
+
C
d
Φ
U
+
C
d
E
=
F
y
X
0
+
Φ
y
U
+
E
y
=
[
y
k
+
1
,
y
k
+
2
,
⋯
,
y
k
+
N
p
]
T
(6)
Y = C_d FX_{0} + C_d \Phi U + C_d E = F_yX_0 +\Phi _{y} U + E_y = [y_{k+1},y_{k+2},\cdots,y_{k+N_p}]^T \tag{6}
Y=CdFX0+CdΦU+CdE=FyX0+ΦyU+Ey=[yk+1,yk+2,⋯,yk+Np]T(6)
3 代价函数
以系统的期望输出与预测输出的误差最小作为代价函数
J
=
(
Y
−
Y
r
e
f
)
T
Q
e
(
Y
−
Y
r
e
f
)
+
U
T
R
e
U
=
(
F
y
X
0
+
Φ
y
U
+
E
y
−
Y
r
e
f
)
T
Q
e
(
F
y
X
0
+
Φ
y
U
+
E
y
−
Y
r
e
f
)
+
U
T
R
e
U
=
U
T
(
Φ
y
T
Q
e
Φ
y
+
R
e
)
U
+
U
T
[
2
Φ
y
T
Q
e
(
F
y
X
0
+
E
y
−
Y
r
e
f
)
]
+
(
F
y
X
0
+
E
y
−
Y
r
e
f
)
T
Q
e
(
F
y
X
0
+
E
y
−
Y
r
e
f
)
(7)
\begin{aligned} J &= (Y - Y_{ref})^T Q_e (Y - Y_{ref}) + U^T R_e U \\ &= (F_yX_0 +\Phi _{y} U + E_y - Y_{ref})^T Q_e (F_yX_0 +\Phi _{y} U + E_y - Y_{ref}) + U^T R_e U \\ &= U^T(\Phi ^T_{y} Q_e \Phi _{y} + R_e)U + U^T[2\Phi ^T_{y} Q_e (F_y X_0 + E_y -Y_{ref})] + (F_y X_0 + E_y -Y_{ref})^T Q_e (F_y X_0 + E_y -Y_{ref}) \end{aligned} \tag{7}
J=(Y−Yref)TQe(Y−Yref)+UTReU=(FyX0+ΦyU+Ey−Yref)TQe(FyX0+ΦyU+Ey−Yref)+UTReU=UT(ΦyTQeΦy+Re)U+UT[2ΦyTQe(FyX0+Ey−Yref)]+(FyX0+Ey−Yref)TQe(FyX0+Ey−Yref)(7)
则代价函数可以简写为:
J
=
U
T
H
U
+
2
U
T
G
+
P
(8)
J = U^T H U + 2U^TG+P \tag{8}
J=UTHU+2UTG+P(8)
其中,
Q
Q
Q是状态权重矩阵,
R
R
R是控制输入权重矩阵,
P
P
P是常量,显然代价函数是一个
Q
P
QP
QP问题的求解。
H
=
Φ
y
T
Q
e
Φ
y
+
R
e
;
G
=
Φ
y
T
Q
e
M
;
P
=
M
T
Q
e
M
;
M
=
F
y
X
0
+
E
y
−
Y
r
e
f
Q
e
=
[
Q
0
⋯
0
0
Q
⋯
0
⋮
⋮
⋱
⋮
0
0
⋯
Q
]
N
p
×
N
p
R
e
=
[
R
0
⋯
0
0
R
⋯
0
⋮
⋮
⋱
⋮
0
0
⋯
R
]
N
c
×
N
c
(9)
\begin{array}{cc} H = \Phi ^T_{y} Q_e \Phi _{y} + R_e; \\ G = \Phi ^T_{y} Q_e M; \\ P =M^T Q_e M; \\ M = F_y X_0 + E_y -Y_{ref} \\ Q_e=\left[\begin{matrix} Q &0 &\cdots &0 \\ 0 &Q &\cdots &0 \\ \vdots &\vdots &\ddots &\vdots \\ 0 &0 &\cdots &Q \\ \end{matrix} \right]_{N_p \times N_p} \\ R_e=\left[\begin{matrix} R &0 &\cdots &0 \\ 0 &R &\cdots &0 \\ \vdots &\vdots &\ddots &\vdots \\ 0 &0 &\cdots &R \\ \end{matrix} \right]_{N_c \times N_c} \end{array} \tag{9}
H=ΦyTQeΦy+Re;G=ΦyTQeM;P=MTQeM;M=FyX0+Ey−YrefQe=
Q0⋮00Q⋮0⋯⋯⋱⋯00⋮Q
Np×NpRe=
R0⋮00R⋮0⋯⋯⋱⋯00⋮R
Nc×Nc(9)
4 约束条件
假设只考虑控制变量的上下界约束,则矩阵
U
U
U的约束为:
U
m
i
n
≤
U
≤
U
m
a
x
(10)
U_{min} \leq U \leq U_{max} \tag{10}
Umin≤U≤Umax(10)
或者写成以下形式:
[
I
−
I
]
U
≥
[
U
m
i
n
−
U
m
a
x
]
(11)
\left[ \begin{matrix} I \\ -I \end{matrix}\right]U \geq \left[ \begin{matrix} U_{min} \\ -U_{max} \end{matrix}\right] \tag{11}
[I−I]U≥[Umin−Umax](11)
5 Q P QP QP问题
由上可知,
M
P
C
MPC
MPC问题的求解最终转化为
Q
P
QP
QP问题的求解,对于
Q
P
QP
QP问题工程上可以求解的,有多种方法及开源库可以进行求解。
m
i
n
:
J
=
1
2
U
T
H
U
+
U
T
s
.
t
.
U
m
i
n
≤
U
≤
U
m
a
x
(12)
\begin{matrix} min: \; \; J = \frac{1}{2}U^T H U + U^T \\ s.t.\; \; U_{min} \leq U \leq U_{max} \end{matrix} \tag{12}
min:J=21UTHU+UTs.t.Umin≤U≤Umax(12)
将求解的控制量系列的第一个值作为控制量。
6 增量约束问题
将状态空间方程(2)改写为增量模式,以
Δ
u
\Delta u
Δu为控制量:
{
ξ
k
+
1
=
A
m
ξ
k
+
B
m
Δ
u
k
+
D
m
,
d
i
s
θ
k
=
C
m
ξ
k
(13)
\begin{cases} \xi_{k+1} = A_m \xi_k + B_m \Delta u_k + D_{m,{dis}} \\ \theta_k = C_m \xi_k \end{cases} \tag{13}
{ξk+1=Amξk+BmΔuk+Dm,disθk=Cmξk(13)
其中:
ξ
=
[
x
k
,
u
k
]
T
;
θ
k
=
[
y
k
,
u
k
]
T
;
A
m
=
[
A
d
B
d
0
I
]
;
B
m
=
[
0
I
]
T
;
D
m
,
d
i
s
=
[
D
d
,
d
i
s
0
]
T
;
C
m
=
[
C
d
0
0
I
]
;
(14)
\begin{array}{cc} {}\xi = [x_k,u_k]^T; \\ \theta _k = [y_k, u_k]^T; \\ A_m = \left[\begin{matrix} A_d &B_d \\ 0 &I \end{matrix}\right]; \\ B_m = \left[\begin{matrix} 0 &I \end{matrix}\right]^T; \\ D_{m,dis} = \left[\begin{matrix} D_{d,dis} &0 \end{matrix}\right]^T; \\ C_m = \left[\begin{matrix} C_d &0 \\ 0 &I \end{matrix}\right]; \\ \end{array} \tag{14}
ξ=[xk,uk]T;θk=[yk,uk]T;Am=[Ad0BdI];Bm=[0I]T;Dm,dis=[Dd,dis0]T;Cm=[Cd00I];(14)
与上述相同的推到可以得到增量模型的
Q
P
QP
QP形式:
m
i
n
:
J
=
1
2
Δ
U
T
H
Δ
U
+
Δ
U
T
s
.
t
.
U
m
i
n
≤
U
≤
U
m
a
x
Δ
U
m
i
n
≤
Δ
U
≤
Δ
U
m
a
x
(15)
\begin{matrix} min: \; \; J = \frac{1}{2} \Delta U^T H \Delta U + \Delta U^T \\ s.t.\; \; \; \; \; \; \; \; \; \; U_{min} \leq U \leq U_{max} \\ \; \; \; \; \; \; \; \; \; \; \; \; \; \; \; \; \; \; \; \; \Delta U_{min} \leq \Delta U \leq \Delta U_{max} \end{matrix} \tag{15}
min:J=21ΔUTHΔU+ΔUTs.t.Umin≤U≤UmaxΔUmin≤ΔU≤ΔUmax(15)
其中,
R
m
R_m
Rm是
Δ
U
\Delta U
ΔU的权重矩阵:
Q
e
=
[
Q
0
⋯
0
0
0
⋯
0
0
Q
⋯
0
0
0
⋯
0
⋮
⋮
⋱
⋮
⋮
⋮
⋱
⋮
0
0
⋯
Q
0
0
⋯
0
0
0
⋯
0
R
0
⋯
0
0
0
⋯
0
0
R
⋯
0
⋮
⋮
⋱
⋮
⋮
⋮
⋱
⋮
0
0
⋯
0
0
0
⋯
R
]
(
N
p
+
N
C
)
×
(
N
p
+
N
C
)
R
e
=
[
R
m
0
⋯
0
0
R
m
⋯
0
⋮
⋮
⋱
⋮
0
0
⋯
R
m
]
N
c
×
N
c
(16)
\begin{array}{cc} Q_e=\left[\begin{matrix} Q &0 &\cdots &0 &0 &0 &\cdots &0\\ 0 &Q &\cdots &0 &0 &0 &\cdots &0\\ \vdots &\vdots &\ddots &\vdots &\vdots &\vdots &\ddots &\vdots\\ 0 &0 &\cdots &Q &0 &0 &\cdots &0\\ 0 &0 &\cdots &0 &R &0 &\cdots &0\\ 0 &0 &\cdots &0 &0 &R &\cdots &0\\ \vdots &\vdots &\ddots &\vdots &\vdots &\vdots &\ddots &\vdots\\ 0 &0 &\cdots &0 &0 &0 &\cdots &R\\ \end{matrix} \right]_{(N_p + N_C) \times (N_p + N_C)} \\ R_e=\left[\begin{matrix} R_m &0 &\cdots &0 \\ 0 &R_m &\cdots &0 \\ \vdots &\vdots &\ddots &\vdots \\ 0 &0 &\cdots &R_m \\ \end{matrix} \right]_{N_c \times N_c} \end{array} \tag{16}
Qe=
Q0⋮000⋮00Q⋮000⋮0⋯⋯⋱⋯⋯⋯⋱⋯00⋮Q00⋮000⋮0R0⋮000⋮00R⋮0⋯⋯⋱⋯⋯⋯⋱⋯00⋮000⋮R
(Np+NC)×(Np+NC)Re=
Rm0⋮00Rm⋮0⋯⋯⋱⋯00⋮Rm
Nc×Nc(16)
根据
Δ
U
\Delta U
ΔU来求解
U
U
U,使
U
U
U满足其约束:
u
k
=
u
k
−
1
+
Δ
u
k
=
u
k
−
1
+
[
1
0
⋯
0
]
Δ
U
u
k
+
1
=
u
k
+
Δ
u
k
+
1
=
u
k
−
1
+
Δ
u
k
+
Δ
u
k
+
1
=
u
k
−
1
+
[
1
1
⋯
0
]
Δ
U
⋮
u
N
c
=
u
N
c
−
1
+
Δ
u
N
c
=
u
N
c
−
2
+
Δ
u
N
c
−
1
+
Δ
u
N
c
=
u
k
−
1
+
[
1
1
⋯
1
]
Δ
U
(17)
\begin{array}{cc} u_{k} &= u_{k-1} + \Delta u_{k} &= u_{k-1} + [1\;0\;\cdots\;0] \Delta U \\ u_{k+1} &= u_{k} + \Delta u_{k+1} &= u_{k-1} + \Delta u_{k} + \Delta u_{k+1} &= u_{k-1} + [1\;1\;\cdots\;0] \Delta U \\ \vdots \\ u_{N_c} &= u_{N_c -1} + \Delta u_{N_c} &= u_{N_c -2} + \Delta u_{N_c - 1} + \Delta u_{N_c} &= u_{k-1} + [1\;1\;\cdots\;1] \Delta U \\ \end{array} \tag{17}
ukuk+1⋮uNc=uk−1+Δuk=uk+Δuk+1=uNc−1+ΔuNc=uk−1+[10⋯0]ΔU=uk−1+Δuk+Δuk+1=uNc−2+ΔuNc−1+ΔuNc=uk−1+[11⋯0]ΔU=uk−1+[11⋯1]ΔU(17)
即:
[
u
k
u
k
+
1
u
k
+
2
⋮
u
k
+
N
c
]
=
[
I
I
I
⋮
I
]
u
k
−
1
+
[
I
0
0
⋯
0
I
I
0
⋯
0
I
I
I
⋯
0
⋮
⋮
⋮
⋱
⋮
I
I
I
⋯
I
]
[
Δ
u
k
Δ
u
k
+
1
Δ
u
k
+
2
⋮
Δ
u
k
+
N
c
]
(18)
\left[ \begin{matrix} u_k \\u_{k+1} \\u_{k+2} \\ \vdots \\u_{k+N_c} \end{matrix}\right] = \left[ \begin{matrix} I \\I \\I \\ \vdots \\I \end{matrix}\right]u_{k-1} + \left[ \begin{matrix} I &0 &0 & \cdots &0 \\ I &I &0 & \cdots &0 \\ I &I &I & \cdots &0 \\ \vdots & \vdots & \vdots & \ddots & \vdots\\ I &I &I & \cdots &I \end{matrix}\right] \left[ \begin{matrix} \Delta u_k \\ \Delta u_{k+1} \\ \Delta u_{k+2} \\ \vdots \\ \Delta u_{k+N_c} \end{matrix}\right] \tag{18}
ukuk+1uk+2⋮uk+Nc
=
III⋮I
uk−1+
III⋮I0II⋮I00I⋮I⋯⋯⋯⋱⋯000⋮I
ΔukΔuk+1Δuk+2⋮Δuk+Nc
(18)
简化可得:
[
C
1
−
C
1
]
Δ
U
≥
[
U
m
i
n
−
C
2
u
k
−
1
−
U
m
a
x
+
C
2
u
k
−
1
]
(19)
\left[ \begin{matrix} C_1 \\ -C_1 \end{matrix}\right] \Delta U \geq \left[ \begin{matrix} U_{min} - C_2 u_{k-1} \\ -U_{max} + C_2 u_{k-1}\end{matrix}\right] \\ \tag{19}
[C1−C1]ΔU≥[Umin−C2uk−1−Umax+C2uk−1](19)
其中:
C
1
=
[
I
0
0
⋯
0
I
I
0
⋯
0
I
I
I
⋯
0
⋮
⋮
⋮
⋱
⋮
I
I
I
⋯
I
]
;
C
2
=
[
I
I
I
⋮
I
]
(20)
C_1 = \left[ \begin{matrix} I &0 &0 & \cdots &0 \\ I &I &0 & \cdots &0 \\ I &I &I & \cdots &0 \\ \vdots & \vdots & \vdots & \ddots & \vdots\\ I &I &I & \cdots &I \end{matrix}\right]; C_2 =\left[ \begin{matrix} I \\I \\I \\ \vdots \\I \end{matrix}\right] \tag{20}
C1=
III⋮I0II⋮I00I⋮I⋯⋯⋯⋱⋯000⋮I
;C2=
III⋮I
(20)
7 matlab求解
function [flag, control_out] = SolveLinearMpc(A, B, C, D, Q, R, sample_period, upper, lower, state_k, reference, Np, Nc)
if (size(A,1) ~= size(A,2) || ...
size(B,1) ~= size(A,1) || ...
size(D,1) ~= size(A,1) || ...
size(Q,1) ~= size(Q,2) || ...
size(R,1) ~= size(R,2) || ...
size(Q,1) ~= size(C,1) || ...
size(R,1) ~= size(B,2) || ...
size(C,2) ~= size(A,1) || ...
size(B,2) ~= size(lower,1) || ...
size(lower,1) ~= size(upper,1) || ...
size(state_k,1) ~= size(A,1))
flag = false;
return;
end
%% ÀëÉ¢»¯
matrix_i = eye(size(A,1));
% Ad = inv(matrix_i - sample_period * 0.5 * A) * (matrix_i + sample_period * 0.5 * A);
Ad = matrix_i + A * sample_period;
Bd = B * sample_period;
Dd = D * sample_period;
Cd = C;
%% Ô€²âŸØÕó
F = zeros(Np * size(Cd,1), size(Ad,2));
Phi = zeros(Np * size(Cd,1), Nc * size(Bd,2));
E = zeros(Np * size(Cd,1), size(Dd,2));
matrix_f = zeros(Np * size(C,1), size(A,2));
F(1:size(Cd,1), 1:size(Ad,2)) = Cd * Ad;
matrix_f(1:size(Cd,1), 1:size(Ad,2)) = Cd;
%% update F
for i = 2:1:Np
F((i-1) * size(Cd,1) + 1 : i * size(Cd,1), :) = ...
F((i-2) * size(Cd,1) + 1 : (i-1) * size(Cd,1), :) * Ad;
matrix_f((i-1) * size(Cd,1) + 1 : i * size(Cd,1), :) = ...
matrix_f((i-2) * size(Cd,1) + 1 : (i-1) * size(Cd,1), :) * Ad;
end
%% update Phi
% for i = 1:1:Np
% for j = 1:1:i
% Phi((i-1) * size(Cd,1) + 1 : i * size(Cd,1), (j-1) * size(Bd,2) + 1 : j * size(Bd, 2)) = ...
% matrix_f((i-j) * size(Cd,1) + 1 : (i-j+1) * size(Cd,1), 1 : size(matrix_f,2)) * Bd;
% if j == Nc
% break;
% end
% end
% end
matrix_phi = matrix_f * Bd;
Phi(: , 1 : size(Bd,2)) = matrix_phi;
for i = 1 : Nc - 1
Phi(:, (i * size(Bd,2) + 1) : (i + 1) * size(Bd,2)) = [zeros(i * size(Cd,1), size(Bd,2)); ...
matrix_phi(1 : (Np - i) * size(Cd,1), :)];
end
%% update E
E(1 : size(Cd,1), 1 : size(Dd,2)) = Cd * Dd;
for i = 2:1:Np
E((i-1) * size(Cd,1) + 1 : i * size(Cd,1), :) = ...
matrix_f((i-1) * size(Cd,1) + 1 : i * size(Cd,1), :) * Dd +...
E((i-2) * size(Cd,1) + 1 : (i-1) * size(Cd,1), :);
end
%% update Cost Function: min J = (Y_p - Ref)^T * Qe * (Y_p - Ref) + U^T * Re * U
%% s.t. matrix_inequality_constraint * U ¡Ü matrix_inequality_boundary
%% convert to QP problem: min J = 0.5 * U^T * H * U + U^T * G + P
%% where: H = Phi^T * Qe * Phi + Re
%% G = Phi^T * Qe * M
%% P = M^T Qe * M
%% M = F * X_k + E - Ref
Qe = zeros(Np * size(Q,1), Np * size(Q,2));
Re = zeros(Nc * size(R,1), Nc * size(R,2));
for i = 1:1:Np
Qe((i-1) * size(Q,1) + 1 : i * size(Q,1), (i-1) * size(Q,2) + 1 : i * size(Q,2)) = Q;
end
for i = 1:1:Nc
Re((i-1) * size(R,1) + 1 : i * size(R,1), (i-1) * size(R,2) + 1 : i * size(R,2)) = R;
end
M = F * state_k + E - reference;
H = Phi' * Qe * Phi + Re;
G = Phi' * Qe * M;
P = M' * Qe * M;
%% update constraint
matrix_ctrl_k = zeros(size(Bd,2), Nc * size(Bd,2));
matrix_ctrl_k(1 : size(Bd,2), 1 : size(Bd,2)) = eye(size(Bd,2)); %% use to get the fisrt contol value
Upper_boundary = zeros(Nc * size(Bd,2), 1);
Lower_boundary = zeros(Nc * size(Bd,2), 1);
for i = 1:1:Nc
Upper_boundary((i-1) * size(Bd,2) + 1 : i * size(Bd,2), :) = upper;
Lower_boundary((i-1) * size(Bd,2) + 1 : i * size(Bd,2), :) = lower;
end
inequality_boundary = [Upper_boundary; - Lower_boundary];
matrix_inequality_constraint_upper = eye(Nc * size(Bd,2), Nc * size(Bd,2));
matrix_inequality_constraint_lower = eye(Nc * size(Bd,2), Nc * size(Bd,2));
matrix_inequality_constraint = [matrix_inequality_constraint_upper; ...
-matrix_inequality_constraint_lower];
%% solve QP
solve = - H \ G; %% optimal slove with no constraint
is_satisfy_constraint = true;
for i=1:1:size(matrix_inequality_constraint,1)
if matrix_inequality_constraint(i, :) * solve > inequality_boundary(i)
is_satisfy_constraint = false;
end
end
if is_satisfy_constraint
control_out = matrix_ctrl_k * solve;
flag = true;
else
ppp = matrix_inequality_constraint * (H \ matrix_inequality_constraint');
ddd = (matrix_inequality_constraint * (H \ G) + inequality_boundary);
lambda = zeros(size(ddd,1), size(ddd,2));
tolerance = 10;
for i = 1:38
lambda_p = lambda;
for j = 1:size(ddd,1)
www = ppp(i,:) * lambda - ppp(i,i) * lambda(i,1);
www = www + ddd(i,1);
la = - www / ppp(i,i);
lambda(i,1) = max(0, la);
end
tolerance = (lambda - lambda_p)' * (lambda - lambda_p);
if tolerance < 10e-8
break;
end
end
solve = - H \ G - H \ matrix_inequality_constraint' * lambda;
control_out = matrix_ctrl_k * solve;
flag = true;
end