Control-模型预测控制(Model Predict Control,MPC)

模型预测控制(Model Predict Control)利用一个已有的模型、系统当前的状态和未来的控制量去预测系统未来的输出；这个输出的长度是控制周期的整数倍；由于未来的控制量是未知的，需要根据一定的条件进行求解，以得到未来的控制量序列，并在每个控制周期结束后，系统根据当前实际状态重新预测系统未来的输出。因此模型预测控制有三个关键步骤，分别是：预测模型、滚动优化和反馈校正。

• 预测模型：预测模型是控制的基础，根据对象的历史信息和未来输入，预测系统未来的输出。预测模型有：状态空间方程、传递函数、阶跃响应、脉冲响应、神经网络模型等等。

• 滚动优化：模型预测控制通过控制某一性能指标的最优来确定控制序列，但优化不是一次离线进行，而是反复在线进行的。

• 反馈校正：为了防止模型失配或者干扰等引起控制对理想状态的偏差，在新的采样时刻，首先检测对象的实际输出，并利用这一实时信息对基于模型的预测结果进行修正，然后再进行新的优化。

在此，选择状态空间方程作为预测模型。

1 状态空间方程

状态空间方程为：
$$\{\begin{array}{l}\dot{x}=Ax+Bu+D_{dis}\\ y=Cx+Du\end{array}\text{\hspace{1em}(1)}$$
连续状态空间方程需要离散化，常用的离散化方法有：欧拉公式离散化，后向差分和双线性变换离散化

Matrix	Euler	Backward Rect	Bilinear Transform
$A_d$	$I+A\ast \Delta t$	$(I-A\ast \Delta t{)}^{-1}$	$(I-\frac{A\ast \Delta t}{2}{)}^{-1}(I+\frac{A\ast \Delta t}{2})$
$B_d$	$B\ast \Delta t$	$(I-A\ast \Delta t{)}^{-1}B\ast \Delta t$	$(I-\frac{A\ast \Delta t}{2}{)}^{-1}B\ast \Delta t$
$D_{d,dis}$	$D_{dis}\ast \Delta t$	$(I-A\ast \Delta t{)}^{-1}{D}_{dis}\ast \Delta t$	$(I-\frac{A\ast \Delta t}{2}{)}^{-1}{D}_{dis}\ast \Delta t$
$C_d$	$C$	$C(I-A\ast \Delta t{)}^{-1}$	$C(I-\frac{A\ast \Delta t}{2}{)}^{-1}$
$D_d$	$D$	$D+C(I-A\ast \Delta t{)}^{-1}B\ast \Delta t$	$D+C(I-\frac{A\ast \Delta t}{2}{)}^{-1}\frac{B\ast \Delta t}{2}$

离散的状态空间方程为：
$$\{\begin{array}{l}{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{array}\text{\hspace{1em}(2)}$$

2 预测模型

根据经验模型(状态空间方程)和当前状态、未来控制量，可以预测未来的输出量。
$$\{\begin{array}{ll}{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_d^2{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_d^2{x}_{k+1}+(A_d{B}_d{u}_{k+1}+B_d{u}_{k+2})+A_d{D}_{d,dis}+D_{d,dis}\\ & =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,dis}+A_d{D}_{d,dis}+D_{d,dis}\\ ⋮& \\ x_{k+N_p}& =A_d^{N_p}{x}_k+(A_d^{N_p-1}{B}_d{u}_k+\cdots +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,dis}+\cdots +A_d{D}_{d,dis}+D_{d,dis})\end{array}\text{\hspace{1em}(3)}$$
其中，$N_p$是预测时域，$N_c$是控制时域，并且$N_p\ge N_c$，在$Nc<k\le N_p$时域内，$u_k=0$。

将公式(3)整理可得：
$$X=F{X}_0+\Phi U+E\text{\hspace{1em}(4)}$$
其中：
$$\{\begin{array}{l}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_d^2,\cdots ,A_d^{N_p}{]}^T;\\ \Phi =\left[\begin{array}{cccc}{B}_d& 0& \cdots & 0\\ A_d{B}_d& B_d& \cdots & 0\\ ⋮& ⋮& \ddots & ⋮\\ A^{N_p-1}d{B}_d& A^{N_p-2}d{B}_d& \cdots & A_d^{N_p-N_c}{B}_d\end{array}\right]\\ E=[D_{d,dis},A_d{D}_{d,dis}+D_{d,dis},\cdots ,{\sum }_{i=0}^{N_p-1}{A}_k^i{D}_{d,dis}{]}^T\end{array}\text{\hspace{1em}(5)}$$
假设$D_d=0$，由公(4)可以得到系统的预测输出量：
$$Y=C_{d}F{X}_0+C_d\Phi U+C_{d}E=F_y{X}_0+{\Phi }_{y}U+E_y=[y_{k+1},y_{k+2},\cdots ,y_{k+N_p}{]}^T\text{\hspace{1em}(6)}$$

3 代价函数

以系统的期望输出与预测输出的误差最小作为代价函数
$$\begin{array}{rl}J& =(Y-Y_{ref}{)}^T{Q}_e(Y-Y_{ref})+U^T{R}_{e}U\\ & =(F_y{X}_0+{\Phi }_{y}U+E_y-Y_{ref}{)}^T{Q}_e(F_y{X}_0+{\Phi }_{y}U+E_y-Y_{ref})+U^T{R}_{e}U\\ & =U^T({\Phi }_y^T{Q}_e{\Phi }_y+R_e)U+U^T[2{\Phi }_y^T{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{array}\text{\hspace{1em}(7)}$$
则代价函数可以简写为：
$$J=U^{T}HU+2U^{T}G+P\text{\hspace{1em}(8)}$$
其中，$Q$是状态权重矩阵，$R$是控制输入权重矩阵，$P$是常量，显然代价函数是一个$QP$问题的求解。
$$\begin{array}{c}H={\Phi }_y^T{Q}_e{\Phi }_y+R_e;\\ G={\Phi }_y^T{Q}_{e}M;\\ P=M^T{Q}_{e}M;\\ M=F_y{X}_0+E_y-Y_{ref}\\ Q_e={\left[\begin{array}{cccc}Q& 0& \cdots & 0\\ 0& Q& \cdots & 0\\ ⋮& ⋮& \ddots & ⋮\\ 0& 0& \cdots & Q\end{array}\right]}_{N_p\times N_p}\\ R_e={\left[\begin{array}{cccc}R& 0& \cdots & 0\\ 0& R& \cdots & 0\\ ⋮& ⋮& \ddots & ⋮\\ 0& 0& \cdots & R\end{array}\right]}_{N_c\times N_c}\end{array}\text{\hspace{1em}(9)}$$

4 约束条件

假设只考虑控制变量的上下界约束，则矩阵$U$的约束为：
$$U_{min}\le U\le U_{max}\text{\hspace{1em}(10)}$$
或者写成以下形式：
$$\left[\begin{array}{c}I\\ -I\end{array}\right]U\ge \left[\begin{array}{c}{U}_{min}\\ -U_{max}\end{array}\right]\text{\hspace{1em}(11)}$$

5 $QP$问题

由上可知，$MPC$问题的求解最终转化为$QP$问题的求解，对于$QP$问题工程上可以求解的，有多种方法及开源库可以进行求解。
$$\begin{array}{c}min:J=\frac{1}{2}{U}^{T}HU+U^T\\ s.t.U_{min}\le U\le U_{max}\end{array}\text{\hspace{1em}(12)}$$

将求解的控制量系列的第一个值作为控制量。

6 增量约束问题

将状态空间方程(2)改写为增量模式，以$\Delta u$为控制量：
$$\{\begin{array}{l}{\xi }_{k+1}=A_m{\xi }_k+B_m\Delta u_k+D_{m,dis}\\ {\theta }_k=C_m{\xi }_k\end{array}\text{\hspace{1em}(13)}$$
其中：
$$\begin{array}{c}\xi =[x_k,u_k{]}^T;\\ {\theta }_k=[y_k,u_k{]}^T;\\ A_m=\left[\begin{array}{cc}{A}_d& B_d\\ 0& I\end{array}\right];\\ B_m={\left[\begin{array}{cc}0& I\end{array}\right]}^T;\\ D_{m,dis}={\left[\begin{array}{cc}{D}_{d,dis}& 0\end{array}\right]}^T;\\ C_m=\left[\begin{array}{cc}{C}_d& 0\\ 0& I\end{array}\right];\end{array}\text{\hspace{1em}(14)}$$
与上述相同的推到可以得到增量模型的$QP$形式：
$$\begin{array}{c}min:J=\frac{1}{2}\Delta U^{T}H\Delta U+\Delta U^T\\ s.t.U_{min}\le U\le U_{max}\\ \Delta U_{min}\le \Delta U\le \Delta U_{max}\end{array}\text{\hspace{1em}(15)}$$
其中，$R_m$是$\Delta U$的权重矩阵：
$$\begin{array}{c}{Q}_e={\left[\begin{array}{cccccccc}Q& 0& \cdots & 0& 0& 0& \cdots & 0\\ 0& Q& \cdots & 0& 0& 0& \cdots & 0\\ ⋮& ⋮& \ddots & ⋮& ⋮& ⋮& \ddots & ⋮\\ 0& 0& \cdots & Q& 0& 0& \cdots & 0\\ 0& 0& \cdots & 0& R& 0& \cdots & 0\\ 0& 0& \cdots & 0& 0& R& \cdots & 0\\ ⋮& ⋮& \ddots & ⋮& ⋮& ⋮& \ddots & ⋮\\ 0& 0& \cdots & 0& 0& 0& \cdots & R\end{array}\right]}_{(N_p+N_C)\times (N_p+N_C)}\\ R_e={\left[\begin{array}{cccc}{R}_m& 0& \cdots & 0\\ 0& R_m& \cdots & 0\\ ⋮& ⋮& \ddots & ⋮\\ 0& 0& \cdots & R_m\end{array}\right]}_{N_c\times N_c}\end{array}\text{\hspace{1em}(16)}$$

根据$\Delta U$来求解$U$，使$U$满足其约束：
$$\begin{array}{cccc}{u}_k& =u_{k-1}+\Delta u_k& =u_{k-1}+[10\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}+[11\cdots 0]\Delta U\\ ⋮& & & \\ 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}+[11\cdots 1]\Delta U\end{array}\text{\hspace{1em}(17)}$$
即：
$$\left[\begin{array}{c}{u}_k\\ u_{k+1}\\ u_{k+2}\\ ⋮\\ u_{k+N_c}\end{array}\right]=\left[\begin{array}{c}I\\ I\\ I\\ ⋮\\ I\end{array}\right]u_{k-1}+\left[\begin{array}{ccccc}I& 0& 0& \cdots & 0\\ I& I& 0& \cdots & 0\\ I& I& I& \cdots & 0\\ ⋮& ⋮& ⋮& \ddots & ⋮\\ I& I& I& \cdots & I\end{array}\right]\left[\begin{array}{c}\Delta u_k\\ \Delta u_{k+1}\\ \Delta u_{k+2}\\ ⋮\\ \Delta u_{k+N_c}\end{array}\right]\text{\hspace{1em}(18)}$$
简化可得：
$$\begin{gathered} \left[\begin{array}{c}{C}_1\\ -C_1\end{array}\right]\Delta U\ge \left[\begin{array}{c}{U}_{min}-C_2{u}_{k-1}\\ -U_{max}+C_2{u}_{k-1}\end{array}\right] \\ \text{\hspace{1em}(19)} \end{gathered}$$
其中：
$$C_1=\left[\begin{array}{ccccc}I& 0& 0& \cdots & 0\\ I& I& 0& \cdots & 0\\ I& I& I& \cdots & 0\\ ⋮& ⋮& ⋮& \ddots & ⋮\\ I& I& I& \cdots & I\end{array}\right];C_2=\left[\begin{array}{c}I\\ I\\ I\\ ⋮\\ I\end{array}\right]\text{\hspace{1em}(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