8,MPC的简单matlab实现_matlab mpc

MPC概念简介

Medel predictive control(MPC)是一种预测控制，可以依据未来的信息来控制当下的输入，本文主要介绍一种线性MPC（系统和约束都是线性）的实现方法，大致思路是将控制问题进行数学建模，整理成二次规划（quadratic programming，QP）的形式，而后求解。总体MPC的实现包括以下三步：

1. 估计系统的初始状态
2. 通过优化，计算得到未来$N_p$个点的输入序列
3. 只输入第一个控制序列，而后将新状态视作初始状态，循环执行

MPC简单公式推导

系统方程推导

假设系统的离散状态方程为：$$\begin{gathered} x(k+1)=A_{m}x(k)+B{u}_m \\ y(k)=C_{m}x(k) \end{gathered}$$
则可以先将状态方程整理成增广形式（augmented form）,增广形式更加适用于轨迹跟踪，如下所示： [ Δ x m ( k + 1 ) y ( k + 1 ) ] ⏞ x ( k + 1 ) = [ A m o m T C m A m 1 ] ⏞ A [ Δ x m ( k ) y ( k ) ] ⏞ x ( k ) + [ B m C m B m ] ⏞ B Δ u ( k ) y ( k ) = [ o m 1 ] ⏞ C [ Δ x m ( k ) y ( k ) ] ,

$$\begin{array}{l} \overbrace{\left[\begin{array}{c} \Delta x_{m}(k+1) \\ y(k+1) \end{array}\right]}^{x(k+1)}=\overbrace{\left[\begin{array}{cc} A_{m} & o_{m}^{T} \\ C_{m} A_{m} & 1 \end{array}\right]}^{A} \overbrace{\left[\begin{array}{c} \Delta x_{m}(k) \\ y(k) \end{array}\right]}^{x(k)}+\overbrace{\left[\begin{array}{c} B_{m} \\ C_{m} B_{m} \end{array}\right]}^{B} \Delta u(k)\\ y(k)=\overbrace{\left[\begin{array}{ll} o_{m} & 1 \end{array}\right]}^{C}\left[\begin{array}{c} \Delta x_{m}(k) \\ y(k) \end{array}$$ \right], \end{array} [Δxm​(k+1)y(k+1)​] ​x(k+1)​=[Am​Cm​Am​​omT​1​] ​A​[Δxm​(k)y(k)​] ​x(k)​+[Bm​Cm​Bm​​] ​B​Δu(k)y(k)=[om​​1​] ​C​[Δxm​(k)y(k)​],​
随后可以根据初始状态$x(k_i)$和系统的状态方程得到在预测范围$N_p$的范围内所有的状态变量，如下所示：$$\begin{array}{rl}x\left(k_i+1\mid k_i\right)& =Ax\left(k_i\right)+B\Delta u\left(k_i\right)\\ x\left(k_i+2\mid k_i\right)& =Ax\left(k_i+1\mid k_i\right)+B\Delta u\left(k_i+1\right)\\ & =A^{2}x\left(k_i\right)+AB\Delta u\left(k_i\right)+B\Delta u\left(k_i+1\right)\\ & ⋮\\ x\left(k_i+N_p\mid k_i\right)& =A^{N_p}x\left(k_i\right)+A^{N_p-1}B\Delta u\left(k_i\right)+A^{N_p-2}B\Delta u\left(k_i+1\right)\\ & +\dots +A^{N_p-N_c}B\Delta u\left(k_i+N_c-1\right).\end{array}$$
而所有输出变量（观测变量）的表达如下所示：
$$\begin{array}{rl}y\left(k_i+1\mid k_i\right)& =CAx\left(k_i\right)+CB\Delta u\left(k_i\right)\\ y\left(k_i+2\mid k_i\right)& =C{A}^{2}x\left(k_i\right)+CAB\Delta u\left(k_i\right)+CB\Delta u\left(k_i+1\right)\\ y\left(k_i+3\mid k_i\right)& =C{A}^{3}x\left(k_i\right)+C{A}^{2}B\Delta u\left(k_i\right)+CAB\Delta u\left(k_i+1\right)\\ & +CB\Delta u\left(k_i+2\right)\\ & ⋮\\ y\left(k_i+N_p\mid k_i\right)& =C{A}^{N_p}x\left(k_i\right)+C{A}^{N_p-1}B\Delta u\left(k_i\right)+C{A}^{N_p-2}B\Delta u\left(k_i+1\right)\\ & +\dots +C{A}^{N_p-N_c}B\Delta u\left(k_i+N_c-1\right)\end{array}$$
根据上式，将复杂的线性方程写成矩阵形式：
$$Y=Fx(k_i)+\Phi \Delta U$$
矩阵表达如下所示：$$F=\left[\begin{array}{c}CA\\ C{A}^2\\ C{A}^3\\ ⋮\\ C{A}^{N_p}\end{array}\right];\Phi =\left[\begin{array}{ccccc}B& 0& 0& \dots & 0\\ AB& B& 0& \dots & 0\\ A^{2}B& AB& B& \dots & 0\\ ⋮& & & & \\ A^{N_p-1}B& A^{N_p-2}B& A^{N_p-3}B& \dots & A^{N_p-N_c}B\end{array}\right]$$

注意：式中的A,B,C是增广矩阵中的系数，不是原方程的系数

而后进入优化部分，定义待追踪的向量是$R_s$，给输入的误差矩阵是$\bar{R}$，可以将代价函数写成以下格式（大多数书中对误差也有权重矩阵，但是通过调整$\bar{R}$也可以简介调整两项之间的权重差距，因此没有设置追踪误差权重）：
$$J={\left(R_s-Y\right)}^T\left(R_s-Y\right)+\Delta U^T\bar{R}\Delta U$$，带入前面得到的数学方程，可以将代价函数整理成以下形式：$$J={\left(R_s-Fx\left(k_i\right)\right)}^T\left(R_s-Fx\left(k_i\right)\right)-2\Delta U^T{\Phi }^T\left(R_s-Fx\left(k_i\right)\right)+\Delta U^T\left({\Phi }^T\Phi +\bar{R}\right)\Delta U.$$
至此可以使用二次规划的包，得到优化的$\Delta U$

约束推导

约束通常有三种形式，分别是约束控制变化速率，约束控制幅值和约束输出变量（观测变量），可以用以下矩阵统一表达：
[ M 1 M 2 M 3 ] Δ U ≤ [ N 1 N 2 N 3 ] \left[

$$\begin{array}{l} M_{1} \\ M_{2} \\ M_{3} \end{array}$$ \right] \Delta U \leq\left[ $$\begin{array}{l} N_{1} \\ N_{2} \\ N_{3} \end{array}$$ \right] ⎣ ⎡​M1​M2​M3​​⎦ ⎤​ΔU≤⎣ ⎡​N1​N2​N3​​⎦ ⎤​式中：
$$\begin{array}{c}{M}_1=\left[\begin{array}{c}-C_2\\ C_2\end{array}\right];N_1=\left[\begin{array}{c}-U^{\min }+C_{1}u\left(k_i-1\right)\\ U^{\max }-C_{1}u\left(k_i-1\right)\end{array}\right];M_2=\left[\begin{array}{c}-I\\ I\end{array}\right];\\ N_2=\left[\begin{array}{c}-\Delta U^{\min }\\ \Delta U^{\max }\end{array}\right];M_3=\left[\begin{array}{c}-\Phi \\ \Phi \end{array}\right];N_3=\left[\begin{array}{c}-Y^{\min }+Fx\left(k_i\right)\\ Y^{\max }-Fx\left(k_i\right)\end{array}\right].\end{array}$$
$M_1$对应约束控制幅值，$C_1,C_2$分别对应全为1矩阵和下三角矩阵$M_2$对应约束控制幅值$M_3$约束输出变量（观测变量）

MPC实例与Matlab代码

main

以下代码从连续系统出发，而后离散化，根据自定义函数计算mpcgain，而后使用自定义二次优化函数优化，本例中对变量无约束。

clc
clear
close all


dt = 0.1; % 采样时间

Ac = [1 1;0 1];
Bc = [0;1];
Cc = [1 0];
Dc = [0];
%% 离散方程
[Ap,Bp,Cp,Dp]=c2dm(Ac,Bc,Cc,Dc,dt);

Np=20;
Nc=4;

[Phi,F,Phi_Phi,Phi_F,Phi_R,A_e, B_e,C_e] = mpcgain(Ap,Bp,Cp,Nc,Np);
[n,n_in]=size(B_e);
xm=[0;0];
Xf=zeros(n,1);
N_sim=100;
r=ones(N_sim,1);
u=0; % u(k-1) =0
y=0;
R_bar = 0.1*eye(Nc,Nc);
H = 2*(Phi_Phi + R_bar);
R_ref = ones(Np,1);
A_cons = 0; b = 0;

for kk=1:N_sim
%     DeltaU=inv(Phi_Phi+R_bar)*(Phi_R*r(kk)-Phi_F*Xf);
    f = -2*Phi'*(R_ref-F*Xf);
    DeltaU=QPhild(H,f,A_cons,b);


    deltau=DeltaU(1,1);


    u=u+deltau;
    u1(kk)=u;
    y1(kk)=y;
 
    xm_old=xm;
    xm=Ap*xm+Bp*u;
    y=Cp*xm;
    Xf=[xm-xm_old;y];
end
% 将系统输出、控制量曲线显示出来：
k=0:(N_sim-1);
figure
subplot(211)
plot(k,y1)
xlabel('Sampling Instant')
legend('Output')
subplot(212)
plot(k,u1)
xlabel('Sampling Instant')
legend('Control')
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59

mpcgain

说明，本函数和优化函数都来自参考文献的代码，我只进行了小修改，本来书中是使用求导的方法进行优化，所以本函数返回的矩阵并不都有用。

function [Phi,F,Phi_Phi,Phi_F,Phi_R,A_e, B_e,C_e]=mpcgain(Ap,Bp,Cp,Nc,Np)
[m1,n1]=size(Cp);
[n1,n_in]=size(Bp);
A_e=eye(n1+m1,n1+m1);
A_e(1:n1,1:n1)=Ap;
A_e(n1+1:n1+m1,1:n1)=Cp*Ap;
B_e=zeros(n1+m1,n_in);
B_e(1:n1,:)=Bp;
B_e(n1+1:n1+m1,:)=Cp*Bp;
C_e=zeros(m1,n1+m1);
C_e(:,n1+1:n1+m1)=eye(m1,m1);
 
n=n1+m1;
h(1,:)=C_e;
F(1,:)=C_e*A_e;
for kk=2:Np
h(kk,:)=h(kk-1,:)*A_e;
F(kk,:)= F(kk-1,:)*A_e;
end
v=h*B_e;
Phi=zeros(Np,Nc); %declare the dimension of Phi
Phi(:,1)=v; % first column of Phi
for i=2:Nc
Phi(:,i)=[zeros(i-1,1);v(1:Np-i+1,1)]; %Toeplitz matrix
end
BarRs=ones(Np,1);
Phi_Phi= Phi'*Phi;
Phi_F= Phi'*F;
Phi_R=Phi'*BarRs;
end
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30

QPhild

自定义优化函数，也可以使用matlab自带的优化函数：quadprog

function eta=QPhild(H,f,A_cons,b)
% E=H;
% F=f;
% M=A_cons;
% gamma=b;
% eta =x
[n1,m1]=size(A_cons);
eta=-H\f;
kk=0;
for i=1:n1
if (A_cons(i,:)*eta>b(i))
    kk=kk+1;
else
kk=kk+0;
end
end
if (kk==0) 
    return; 
end
P=A_cons*(H\A_cons');
d=(A_cons*(H\f)+b);
[n,m]=size(d);
x_ini=zeros(n,m);
lambda=x_ini;
al=10;
for km=1:38
%find the elements in the solution vector one by one
% km could be larger if the Lagranger multiplier has a slow
% convergence rate.
lambda_p=lambda;
for i=1:n
w= P(i,:)*lambda-P(i,i)*lambda(i,1);
w=w+d(i,1);
la=-w/P(i,i);
lambda(i,1)=max(0,la);
end
al=(lambda-lambda_p)'*(lambda-lambda_p);
if (al<10e-8); break; 
end
end
eta=-H\f -H\A_cons'*lambda;
end
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42

输出

参考文献

Wang L. Model predictive control system design and implementation using MATLAB®[M]. Springer Science & Business Media, 2009.