MPC模型预测控制，理论+仿真

MPC模型预测控制原理

假设一个系统的离散状态方程描述为
$$\begin{array}{rl}x(k+1)& =Ax(k)+Bu(k)\\ y(k)& =Cx(k)+Du(k)\end{array}$$
那么对于某个时刻$k$及其未来的$m$个点，有
$$\begin{array}{rl}& \{\begin{array}{l}x(k)=x(k)\\ y(k)=C\cdot x(k)+D\cdot u(k)\end{array}\\ & \{\begin{array}{rl}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{array}\\ & \{\begin{array}{rl}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{array}\\ & \{\begin{array}{rl}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{array}\\ & \{\begin{array}{rl}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{array}\\ & \end{array}$$
写成矩阵的形式，有：
$$\begin{array}{rl}& \left[\begin{array}{c}x(k)\\ x(k+1)\\ x(k+2)\\ ⋮\\ x(k+m)\end{array}\right]=\left[\begin{array}{c}I\\ A\\ A^2\\ ⋮\\ A^m\end{array}\right]x(k)+\left[\begin{array}{ccccc}0& & & & \\ B& 0& & & \\ AB& B& \ddots & & \\ ⋮& ⋮& \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)\\ ⋮\\ u(k+m)\end{array}\right]\\ & \left[\begin{array}{c}y(k)\\ y(k+1)\\ y(k+2)\\ ⋮\\ y(k+m)\end{array}\right]=\left[\begin{array}{c}C\\ CA\\ C{A}^2\\ ⋮\\ C{A}^m\end{array}\right]x(k)+\left[\begin{array}{ccccc}D& & & & \\ CB& D& & & \\ CAB& CB& D& & \\ ⋮& ⋮& \ddots & D& \\ C{A}^{m-1}B& C{A}^{m-2}B& \cdots & CB& D\end{array}\right]\left[\begin{array}{c}u(k)\\ u(k+1)\\ u(k+2)\\ ⋮\\ u(k+m)\end{array}\right]\\ & \end{array}$$
将输出方程简写为
$$y_k=M\cdot x(k)+N\cdot u_k$$
假设$\epsilon (i)=y(i)-y_d(i)$，$y_d$是设置的跟踪值。定义 cost function $J$：
$$\min J=\left(\sum _{i=0}^{m-1}\epsilon (k+i)\cdot Q\cdot \epsilon (k+i)\right)+\epsilon (k+m)\cdot F\cdot \epsilon (k+m)+u_k{}^T\cdot R\cdot u_k$$
$Q$是预测的除最后一个点的输出与设定值差异的权重， $F$是预测的最后一个时刻的输出与输入差异的权重（最后一个点认为比较重要，单独一个权重）,$R$是能耗的权重。$Q，F$越大表示要求输出与设定值更接近，就是要跟得好， $R$越大表示要求系统的能耗越小。
写成矩阵形式如下：
$$J=\left[\begin{array}{c}\epsilon (k)\\ \epsilon (k+1)\\ ⋮\\ \epsilon (k+m-1)\\ \epsilon (k+m)\end{array}\right]\underset{\overline{Q}}{\underbrace{\left[\begin{array}{lllll}Q& & & & \\ & Q& & & \\ & & \ddots & & \\ & & & Q& \\ & & & & F\end{array}\right]}}\left[\begin{array}{c}\epsilon (k)\\ \epsilon (k+1)\\ ⋮\\ \epsilon (k+m-1)\\ \epsilon (k+m)\end{array}\right]+{\left[\begin{array}{c}u(k)\\ u(k+1)\\ ⋮\\ u(k+m)\end{array}\right]}^T\underset{\overline{R}}{\underbrace{\left[\begin{array}{cccc}R& & & \\ & R& & \\ & & \ddots & \\ & & & R\end{array}\right]}}\left[\begin{array}{c}u(k)\\ u(k+1)\\ ⋮\\ u(k+m)\end{array}\right]$$

将$J$化成能使用二次规划求解的通用形式：
$$\begin{array}{rl}\min 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{array}$$
求解使得$J$最小的$u_k$序列，舍弃最后的常数部分，由此将$J$化为了$\frac{1}{2}{u}_k{}^T\cdot H\cdot u_k+C_T\cdot u_k$的形式，其中：
$$\begin{array}{rl}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{array}$$
接下来可使用二次规划进行优化求解。得到最优输入后，仅施加序列第一个控制输入，再使用当前的状态值进行下一步预测，迭代优化。

仿真

假设控制对象$P(s)$为：
$$P(s)=\frac{123456}{s^2+23s+456}$$
先将其写成状态方程的形式，手写了两种方法如下：

还可以使用 matlab 自带的 tf2ss 函数（其实就是我写的第一种方法）生成状态矩阵，将其离散化（如使用零阶保持法），设置需要预测序列的长度，如10，生成相应的$M,N,\overline{Q},\overline{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);

结果如下