MPC模型预测控制原理
假设一个系统的离散状态方程描述为
x
(
k
+
1
)
=
A
x
(
k
)
+
B
u
(
k
)
y
(
k
)
=
C
x
(
k
)
+
D
u
(
k
)
\begin{aligned} x(k+1) & =A x(k)+Bu(k) \\ y(k) & =C x(k)+Du(k) \end{aligned}
x(k+1)y(k)=Ax(k)+Bu(k)=Cx(k)+Du(k)
那么对于某个时刻
k
k
k及其未来的
m
m
m个点,有
{
x
(
k
)
=
x
(
k
)
y
(
k
)
=
C
⋅
x
(
k
)
+
D
⋅
u
(
k
)
{
x
(
k
+
1
)
=
A
⋅
x
(
k
)
+
B
⋅
u
(
k
)
y
(
k
+
1
)
=
C
⋅
x
(
k
+
1
)
+
D
⋅
u
(
k
+
1
)
=
C
⋅
A
⋅
x
(
k
)
+
C
⋅
B
⋅
u
(
k
)
+
D
⋅
u
(
k
+
1
)
{
x
(
k
+
2
)
=
A
⋅
x
(
k
+
1
)
+
B
⋅
u
(
k
+
1
)
=
A
2
⋅
x
(
k
)
+
A
⋅
B
⋅
u
(
k
)
+
B
⋅
u
(
k
+
1
)
y
(
k
+
2
)
=
C
⋅
x
(
k
+
2
)
+
D
⋅
u
(
k
+
2
)
=
C
⋅
A
2
⋅
x
(
k
)
+
C
⋅
A
⋅
B
⋅
u
(
k
)
+
C
⋅
B
⋅
u
(
k
+
1
)
+
D
⋅
u
(
k
+
2
)
{
x
(
k
+
3
)
=
A
⋅
x
(
k
+
2
)
+
B
⋅
u
(
k
+
2
)
=
A
3
⋅
x
(
k
)
+
A
2
⋅
B
⋅
u
(
k
)
+
A
⋅
B
⋅
u
(
k
+
1
)
+
B
⋅
u
(
k
+
2
)
y
(
k
+
3
)
=
C
⋅
x
(
k
+
3
)
+
D
⋅
u
(
k
+
3
)
=
C
⋅
A
3
⋅
x
(
k
)
+
C
⋅
A
2
⋅
B
⋅
u
(
k
)
+
C
⋅
A
⋅
B
⋅
u
(
k
+
1
)
+
C
⋅
B
⋅
u
(
k
+
2
)
+
D
⋅
u
(
k
+
3
)
{
x
(
k
+
m
)
=
A
⋅
x
(
k
+
m
−
1
)
+
B
⋅
u
(
k
+
m
−
1
)
=
A
m
⋅
x
(
k
)
+
A
m
−
1
⋅
B
⋅
u
(
k
)
+
A
m
−
2
⋅
B
⋅
u
(
k
+
1
)
+
⋯
+
A
⋅
B
⋅
u
(
k
+
m
−
2
)
+
B
⋅
u
(
k
+
m
−
1
)
y
(
k
+
m
)
=
C
⋅
x
(
k
+
m
)
+
D
⋅
u
(
k
+
m
)
=
C
⋅
A
m
⋅
x
(
k
)
+
C
⋅
A
m
−
1
⋅
B
⋅
u
(
k
)
+
C
⋅
A
m
−
2
⋅
B
⋅
u
(
k
+
1
)
+
⋯
+
C
⋅
A
⋅
B
⋅
u
(
k
+
m
−
2
)
+
C
⋅
B
⋅
u
(
k
+
m
−
1
)
+
D
⋅
u
(
k
+
m
)
\begin{aligned} & \left\{\begin{array}{l} x(k)=x(k) \\ y(k)=C \cdot x(k)+D \cdot u(k) \end{array}\right. \\ & \left\{\begin{aligned} x(k+1) & =A \cdot x(k)+B \cdot u(k) \\ y(k+1) & =C \cdot x(k+1)+D \cdot u(k+1) \\ & =C \cdot A \cdot x(k)+C \cdot B \cdot u(k)+D \cdot u(k+1) \end{aligned}\right. \\ & \left\{\begin{aligned} x(k+2) & =A \cdot x(k+1)+B \cdot u(k+1) \\ & =A^2 \cdot x(k)+A \cdot B \cdot u(k)+B \cdot u(k+1) \\ y(k+2) & =C \cdot x(k+2)+D \cdot u(k+2) \\ & =C \cdot A^2 \cdot x(k)+C \cdot A \cdot B \cdot u(k)+C \cdot B \cdot u(k+1)+D \cdot u(k+2) \end{aligned}\right. \\ & \left\{\begin{aligned} x(k+3) & =A \cdot x(k+2)+B \cdot u(k+2) \\ & =A^3 \cdot x(k)+A^2 \cdot B \cdot u(k)+A \cdot B \cdot u(k+1)+B \cdot u(k+2) \\ y(k+3) & =C \cdot x(k+3)+D \cdot u(k+3) \\ & =C \cdot A^3 \cdot x(k)+C \cdot A^2 \cdot B \cdot u(k)+C \cdot A \cdot B \cdot u(k+1)+C \cdot B \cdot u(k+2)+D \cdot u(k+3) \end{aligned}\right. \\ & \left\{\begin{aligned} x(k+m) & =A \cdot x(k+m-1)+B \cdot u(k+m-1) \\ & =A^m \cdot x(k)+A^{m-1} \cdot B \cdot u(k)+A^{m-2} \cdot B \cdot u(k+1)+\cdots +A \cdot B \cdot u(k+m-2)+B \cdot u(k+m-1) \\ y(k+m) & =C \cdot x(k+m)+D \cdot u(k+m) \\ & =C \cdot A^m \cdot x(k)+C \cdot A^{m-1} \cdot B \cdot u(k)+C \cdot A^{m-2} \cdot B \cdot u(k+1)+\cdots +C \cdot A \cdot B \cdot u(k+m-2)+C \cdot B \cdot u(k+m-1)+D \cdot u(k+m) \end{aligned}\right. \\ & \end{aligned}
{x(k)=x(k)y(k)=C⋅x(k)+D⋅u(k)⎩
⎨
⎧x(k+1)y(k+1)=A⋅x(k)+B⋅u(k)=C⋅x(k+1)+D⋅u(k+1)=C⋅A⋅x(k)+C⋅B⋅u(k)+D⋅u(k+1)⎩
⎨
⎧x(k+2)y(k+2)=A⋅x(k+1)+B⋅u(k+1)=A2⋅x(k)+A⋅B⋅u(k)+B⋅u(k+1)=C⋅x(k+2)+D⋅u(k+2)=C⋅A2⋅x(k)+C⋅A⋅B⋅u(k)+C⋅B⋅u(k+1)+D⋅u(k+2)⎩
⎨
⎧x(k+3)y(k+3)=A⋅x(k+2)+B⋅u(k+2)=A3⋅x(k)+A2⋅B⋅u(k)+A⋅B⋅u(k+1)+B⋅u(k+2)=C⋅x(k+3)+D⋅u(k+3)=C⋅A3⋅x(k)+C⋅A2⋅B⋅u(k)+C⋅A⋅B⋅u(k+1)+C⋅B⋅u(k+2)+D⋅u(k+3)⎩
⎨
⎧x(k+m)y(k+m)=A⋅x(k+m−1)+B⋅u(k+m−1)=Am⋅x(k)+Am−1⋅B⋅u(k)+Am−2⋅B⋅u(k+1)+⋯+A⋅B⋅u(k+m−2)+B⋅u(k+m−1)=C⋅x(k+m)+D⋅u(k+m)=C⋅Am⋅x(k)+C⋅Am−1⋅B⋅u(k)+C⋅Am−2⋅B⋅u(k+1)+⋯+C⋅A⋅B⋅u(k+m−2)+C⋅B⋅u(k+m−1)+D⋅u(k+m)
写成矩阵的形式,有:
[
x
(
k
)
x
(
k
+
1
)
x
(
k
+
2
)
⋮
x
(
k
+
m
)
]
=
[
I
A
A
2
⋮
A
m
]
x
(
k
)
+
[
0
B
0
A
B
B
⋱
⋮
⋮
⋱
0
A
m
−
1
B
A
m
−
2
B
⋯
B
0
]
[
u
(
k
)
u
(
k
+
1
)
u
(
k
+
2
)
⋮
u
(
k
+
m
)
]
[
y
(
k
)
y
(
k
+
1
)
y
(
k
+
2
)
⋮
y
(
k
+
m
)
]
=
[
C
C
A
C
A
2
⋮
C
A
m
]
x
(
k
)
+
[
D
C
B
D
C
A
B
C
B
D
⋮
⋮
⋱
D
C
A
m
−
1
B
C
A
m
−
2
B
⋯
C
B
D
]
[
u
(
k
)
u
(
k
+
1
)
u
(
k
+
2
)
⋮
u
(
k
+
m
)
]
\begin{aligned} & {\left[\begin{array}{c} x(k) \\ x(k+1) \\ x(k+2) \\ \vdots \\ x(k+m) \end{array}\right]=\left[\begin{array}{c} I \\ A \\ A^2 \\ \vdots \\ A^m \end{array}\right] x(k)+\left[\begin{array}{ccccc} 0 & & & & \\ B & 0 & & & \\ A B & B & \ddots & & \\ \vdots & \vdots & \ddots & 0 & \\ A^{m-1} B & A^{m-2} B & \cdots & B & 0 \end{array}\right]\left[\begin{array}{c} u(k) \\ u(k+1) \\ u(k+2) \\ \vdots \\ u(k+m) \end{array}\right]} \\ & {\left[\begin{array}{c} y(k) \\ y(k+1) \\ y(k+2) \\ \vdots \\ y(k+m) \end{array}\right]=\left[\begin{array}{c} C \\ C A \\ C A^2 \\ \vdots \\ C A^m \end{array}\right] x(k)+\left[\begin{array}{ccccc} D & & & & \\ C B & D & & & \\ C A B & C B & D & & \\ \vdots & \vdots & \ddots & D & \\ C A^{m-1} B & C A^{m-2} B & \cdots & C B & D \end{array}\right]\left[\begin{array}{c} u(k) \\ u(k+1) \\ u(k+2) \\ \vdots \\ u(k+m) \end{array}\right]} \\ & \end{aligned}
x(k)x(k+1)x(k+2)⋮x(k+m)
=
IAA2⋮Am
x(k)+
0BAB⋮Am−1B0B⋮Am−2B⋱⋱⋯0B0
u(k)u(k+1)u(k+2)⋮u(k+m)
y(k)y(k+1)y(k+2)⋮y(k+m)
=
CCACA2⋮CAm
x(k)+
DCBCAB⋮CAm−1BDCB⋮CAm−2BD⋱⋯DCBD
u(k)u(k+1)u(k+2)⋮u(k+m)
将输出方程简写为
y
k
=
M
⋅
x
(
k
)
+
N
⋅
u
k
y_k = M \cdot x(k) + N \cdot u_k
yk=M⋅x(k)+N⋅uk
假设
ε
(
i
)
=
y
(
i
)
−
y
d
(
i
)
\varepsilon(i)=y(i)-y_d(i)
ε(i)=y(i)−yd(i),
y
d
y_d
yd是设置的跟踪值。定义 cost function
J
J
J:
min
J
=
(
∑
i
=
0
m
−
1
ε
(
k
+
i
)
⋅
Q
⋅
ε
(
k
+
i
)
)
+
ε
(
k
+
m
)
⋅
F
⋅
ε
(
k
+
m
)
+
u
k
T
⋅
R
⋅
u
k
\min J=\left(\sum_{i=0}^{m-1} \varepsilon(k+i) \cdot Q \cdot \varepsilon(k+i)\right)+\varepsilon(k+m) \cdot F \cdot \varepsilon(k+m)+u_k{ }^T \cdot R \cdot u_k
minJ=(i=0∑m−1ε(k+i)⋅Q⋅ε(k+i))+ε(k+m)⋅F⋅ε(k+m)+ukT⋅R⋅uk
Q
Q
Q是预测的除最后一个点的输出与设定值差异的权重,
F
F
F是预测的最后一个时刻的输出与输入差异的权重(最后一个点认为比较重要,单独一个权重),
R
R
R是能耗的权重。
Q
,
F
Q,F
Q,F越大表示要求输出与设定值更接近,就是要跟得好,
R
R
R越大表示要求系统的能耗越小。
写成矩阵形式如下:
J
=
[
ε
(
k
)
ε
(
k
+
1
)
⋮
ε
(
k
+
m
−
1
)
ε
(
k
+
m
)
]
[
Q
Q
⋱
Q
F
]
⏟
Q
‾
[
ε
(
k
)
ε
(
k
+
1
)
⋮
ε
(
k
+
m
−
1
)
ε
(
k
+
m
)
]
+
[
u
(
k
)
u
(
k
+
1
)
⋮
u
(
k
+
m
)
]
T
[
R
R
⋱
R
]
⏟
R
‾
[
u
(
k
)
u
(
k
+
1
)
⋮
u
(
k
+
m
)
]
J=\left[\begin{array}{c} \varepsilon(k) \\ \varepsilon(k+1) \\ \vdots \\ \varepsilon(k+m-1) \\ \varepsilon(k+m) \end{array}\right] \underbrace{ \left[\begin{array}{lllll} Q & & & & \\ & Q & & & \\ & & \ddots & & \\ & & & Q & \\ & & & & F \end{array}\right] }_{\overline{Q}} \left[\begin{array}{c} \varepsilon(k) \\ \varepsilon(k+1) \\ \vdots \\ \varepsilon(k+m-1) \\ \varepsilon(k+m) \end{array}\right]+\left[\begin{array}{c} u(k) \\ u(k+1) \\ \vdots \\ u(k+m) \end{array}\right]^T \underbrace{ \left[\begin{array}{cccc} R & & & \\ & R & & \\ & & \ddots & \\ & & & R \end{array}\right] }_{\overline{R}} \left[\begin{array}{c} u(k) \\ u(k+1) \\ \vdots \\ u(k+m) \end{array}\right]
J=
ε(k)ε(k+1)⋮ε(k+m−1)ε(k+m)
Q
QQ⋱QF
ε(k)ε(k+1)⋮ε(k+m−1)ε(k+m)
+
u(k)u(k+1)⋮u(k+m)
TR
RR⋱R
u(k)u(k+1)⋮u(k+m)
将
J
J
J化成能使用二次规划求解的通用形式:
min
J
=
(
M
⋅
x
(
k
)
+
N
⋅
u
k
−
y
d
k
)
T
⋅
Q
‾
⋅
(
M
⋅
x
(
k
)
+
N
⋅
u
k
−
y
d
k
)
+
u
k
T
⋅
R
‾
⋅
u
k
=
x
(
k
)
T
⋅
M
T
Q
‾
M
⋅
x
(
k
)
+
u
k
T
⋅
N
T
Q
‾
M
⋅
x
(
k
)
−
y
d
k
T
⋅
Q
‾
M
⋅
x
(
k
)
+
x
(
k
)
T
⋅
M
T
Q
‾
N
⋅
u
k
+
u
k
T
⋅
N
T
Q
‾
N
⋅
u
k
−
y
d
k
T
⋅
Q
‾
N
⋅
u
k
−
x
(
k
)
T
⋅
M
T
Q
‾
⋅
y
d
k
−
u
k
T
⋅
N
T
Q
‾
⋅
y
d
k
+
y
d
k
T
⋅
Q
‾
⋅
y
d
k
=
u
k
T
⋅
(
N
T
Q
‾
N
+
R
‾
)
⋅
u
k
+
2
(
x
(
k
)
T
⋅
M
T
Q
‾
N
−
y
d
k
T
⋅
Q
‾
N
)
⋅
u
k
+
x
(
k
)
T
⋅
M
T
Q
‾
M
⋅
x
(
k
)
+
y
d
k
T
⋅
Q
‾
⋅
y
d
k
−
2
(
x
(
k
)
T
⋅
M
T
Q
‾
⋅
y
d
k
)
\begin{aligned} \min \quad J & = (M \cdot x(k) + N \cdot u_k - y_{dk})^T \cdot \overline{Q} \cdot (M \cdot x(k) + N \cdot u_k - y_{dk}) + u_k{ }^T \cdot \overline{R} \cdot u_k\\ & = x(k)^T \cdot M^T\overline{Q}M \cdot x(k) + u_k{ }^T \cdot N^T\overline{Q}M \cdot x(k) - y_{dk}^T \cdot \overline{Q}M \cdot x(k)\\ & + x(k)^T \cdot M^T\overline{Q} N \cdot u_k + u_k{ }^T \cdot N^T\overline{Q}N \cdot u_k - y_{dk}^T \cdot \overline{Q}N \cdot u_k \\ & - x(k)^T \cdot M^T\overline{Q} \cdot y_{dk} - u_k{ }^T \cdot N^T\overline{Q} \cdot y_{dk} + y_{dk}^T \cdot \overline{Q} \cdot y_{dk} \\ & = u_k{ }^T \cdot \left(N^T\overline{Q}N + \overline{R}\right) \cdot u_k + 2\left(x(k)^T \cdot M^T\overline{Q} N - y_{dk}^T \cdot \overline{Q}N\right) \cdot u_k \\ & + x(k)^T \cdot M^T\overline{Q}M \cdot x(k) + y_{dk}^T \cdot \overline{Q} \cdot y_{dk} - 2\left(x(k)^T \cdot M^T\overline{Q} \cdot y_{dk}\right) \end{aligned}
minJ=(M⋅x(k)+N⋅uk−ydk)T⋅Q⋅(M⋅x(k)+N⋅uk−ydk)+ukT⋅R⋅uk=x(k)T⋅MTQM⋅x(k)+ukT⋅NTQM⋅x(k)−ydkT⋅QM⋅x(k)+x(k)T⋅MTQN⋅uk+ukT⋅NTQN⋅uk−ydkT⋅QN⋅uk−x(k)T⋅MTQ⋅ydk−ukT⋅NTQ⋅ydk+ydkT⋅Q⋅ydk=ukT⋅(NTQN+R)⋅uk+2(x(k)T⋅MTQN−ydkT⋅QN)⋅uk+x(k)T⋅MTQM⋅x(k)+ydkT⋅Q⋅ydk−2(x(k)T⋅MTQ⋅ydk)
求解使得
J
J
J最小的
u
k
u_k
uk序列,舍弃最后的常数部分,由此将
J
J
J化为了
1
2
u
k
T
⋅
H
⋅
u
k
+
C
T
⋅
u
k
\frac{1}{2}u_k{ }^T \cdot H \cdot u_k+C_T \cdot u_k
21ukT⋅H⋅uk+CT⋅uk的形式,其中:
H
=
2
(
N
T
Q
‾
N
+
R
‾
)
C
T
=
2
(
x
(
k
)
T
⋅
M
T
Q
‾
N
−
y
d
k
T
⋅
Q
‾
N
)
\begin{aligned} H &= 2 \left(N^T\overline{Q}N + \overline{R}\right) \\ C_T & = 2\left(x(k)^T \cdot M^T\overline{Q} N - y_{dk}^T \cdot \overline{Q}N\right) \end{aligned}
HCT=2(NTQN+R)=2(x(k)T⋅MTQN−ydkT⋅QN)
接下来可使用二次规划进行优化求解。得到最优输入后,仅施加序列第一个控制输入,再使用当前的状态值进行下一步预测,迭代优化。
仿真
假设控制对象
P
(
s
)
P(s)
P(s)为:
P
(
s
)
=
123456
s
2
+
23
s
+
456
P(s) = \frac{123456}{s^2 + 23s + 456}
P(s)=s2+23s+456123456
先将其写成状态方程的形式,手写了两种方法如下:
还可以使用 matlab 自带的 tf2ss 函数(其实就是我写的第一种方法)生成状态矩阵,将其离散化(如使用零阶保持法),设置需要预测序列的长度,如10,生成相应的
M
,
N
,
Q
‾
,
R
‾
M,N,\overline{Q},\overline{R}
M,N,Q,R矩阵。设置给定值,比如一个 5Hz 的 sin 信号。代码如下:
% Created by hyacinth on 2024/1/4
clc
clear
close all
%%
fs = 2e3;
dt = 1/fs;
P = tf(13456,[1 23 456]);
[num,den] = tfdata(P,"v");
dP = c2d(P,dt,"z");
[numz,denz] = tfdata(dP,"v");
A = [0, 1; -den(end),-den(end-1)];
B = [0; num(end)];
C = [1, 0];
D = 0;
% [A,B,C,D] = tf2ss(num,den);
[Ad,Bd,Cd,Dd] = c2dm(A,B,C,D,dt,"z");
t = 0:dt:2;
K = length(t);
gain = 30;
yd = gain*sin(2*pi*5*t);
m = 10;
q = 1; F = 1; r = 0.005;
threshold = 30;
M = Cd;
N = Dd;
for i = 1:m
M = [M; Cd*Ad^i];
if i == 1
last = [Cd*Bd, Dd];
else
last = [Cd*Ad^(i-1)*Bd, N(end,:)];
end
N = [N, zeros(i,1); last];
end
Q = diag([q*ones(1,m),F]);
R = diag(r*ones(1,m+1));
u0 = 0;
xk = [0; 0];
Uk = zeros(m+1,1);
%%
for k = 1:K
xz = Ad*xk + Bd*u0;
y(k) = Cd*xk + Dd*u0;
e(k) = yd(k) - y(k);
if k+m < K
tar = yd(k:k+m);
else
% fill with the last when the data is not enough
tar = [yd(k:end),yd(end)*ones(1,k+m-K)];
end
% use the pre Uk as the initial value, reduce solution time
Uk = MPCfun(M,N,Q,R,m,xk,tar,threshold,Uk);
u0 = Uk(1);
u(k) = u0;
xk = xz;
if mod(k,1000) == 0
disp(num2str(k)+"/"+num2str(K));
end
end
figure;
subplot(2,1,1);hold on
plot(t,yd,".-")
plot(t,y,".-")
plot(t,e,".-")
legend("tar","out","err")
subplot(2,1,2); plot(t,u,".-");
legend("u")
% Created by hyacinth on 2024/1/4
function Uk = MPCfun(M,N,Q,R,m,xk,tar,threshold,x0)
X = tar(:);
H = 2*(N'*Q*N + R); H = (H + H')/2;
CT = 2*(xk'*M'*Q*N - X'*Q*N);
% other = xk'*M'*Q*M*xk + X'*Q*X - 2*xk'*M'*Q*X; this item is not needed.
lb = -threshold*ones(m+1,1);
ub = threshold*ones(m+1,1);
options = optimset('Display','off');
Uk = quadprog(H,CT,[],[],[],[],lb,ub,x0,options);
结果如下