参考如下链接：

一个模型预测控制（MPC）的简单实现

建模例子详解

对于以下控制模型：

$$x(k+1)=Ax(k)+Bu(k)$$

• 注：需要注意对应的输入矩阵和状态矩阵每个矩阵的维度

  x矩阵：n*1

  A矩阵：n*n

  B矩阵：n*p

  u矩阵：p*1

写成矩阵格式：

$$\left[\begin{array}{l}{x}_1(k+1)\\ x_2(k+1)\end{array}\right]=\left[\begin{array}{cc}1& 0.1\\ 0& 2\end{array}\right]\left[\begin{array}{l}{x}_1(k)\\ x_2(k)\end{array}\right]+\left[\begin{array}{c}0\\ 0.5\end{array}\right]u(k)$$

此处的$x_1$和$x_2$可以理解为误差项

$X(k)=\left[\begin{array}{c}x(k\mid k)\\ x(k+1\mid k)\\ ⋮\\ x(k+j\mid k)\\ ⋮\\ x(k+N\mid k)\end{array}\right]$$U(k)=\left[\begin{array}{c}u(k\mid k)\\ u(k+1\mid k)\\ ⋮\\ u(k+i\mid k)\\ ⋮\\ u(k+N-1\mid k)\end{array}\right]$

• 矩阵的维度

  U(k) → NP1

  X(k) → (N+1)n*1

对于MPC矩阵：

$$x_k=M{x}_k+C{u}_k$$

$M=\left[\begin{array}{c}{I}_{n\times n}\\ A_{n\times n}\\ A_n^2\\ ⋮\\ A^N\end{array}\right]$$C=\left[\begin{array}{ccccc}0& 0& \dots & 0& \\ & ⋮& \dots & ⋮& \\ ⋮& 0& & 0& \\ 0& & 0& \dots & 0\\ B_{n\times p}& 0& \dots & 0& \\ A{B}_{n\times p}& B& ⋮& 0& \\ ⋮& ⋮& & B& \end{array}\right]$

• 矩阵的维度

  M → (N+1)nn

  X(k) → (N+1)nN*P

$X(k)=\left[\begin{array}{c}x(k\mid k)\\ x(k+1\mid k)\\ x(k+2\mid k)\\ x(k+3\mid k)\end{array}\right]=\left[\begin{array}{c}{x}_1(k\mid k)\\ x_2(k\mid k)\\ x_1(k+1\mid k)\\ x_2(k+1\mid k)\\ x_1(k+2\mid k)\\ x_2(k+2\mid k)\\ x_1(k+3\mid k)\\ x_2(k+3\mid k)\end{array}\right]$

$U(k)=\left[\begin{array}{c}u(k\mid k)\\ u(k+1\mid k)\\ u(k+2\mid k)\end{array}\right]=\left[\begin{array}{c}\frac{u(k\mid k)}{u(k+1\mid k)}\\ u(k+2\mid k)\end{array}\right]$

计算出M矩阵和C矩阵（此处省略计算过程）

之后构建二次规划模式（cost function）

$$J=\sum _{k=0}^{N-1}x(k+i\mid k{)}^{T}Qx(k+i\mid k)+u(k+i\mid k{)}^{T}Ru(k+i\mid k)+x(k+N\mid k{)}^{T}Fx(k+i\mid k)$$

Q、R、F表示对应输入输出的权重系数矩阵

• 矩阵的维度

  Q → n*n

  R → 1*1

  F → n*n

Q矩阵影响x变量的相关性，R矩阵跟输入相关，F矩阵跟终值相关

$x(k+N\mid k{)}^{T}Fx(k+i\mid k)$与系统对应的终值状态相关

简化上面的 cost function 矩阵

$$J=\mathbf{x}(k{)}^T\mathbf{G}\mathbf{x}(k)+\mathbf{U}(k{)}^T\mathbf{H}\mathbf{U}(k)+2\mathbf{x}(k{)}^T\mathbf{E}\mathbf{U}(k)$$

该矩阵只包含了输入项和已知的初始输入项x(k)

$$\begin{array}{rl}& G=M^T\bar{Q}M\\ & E=C^T\bar{Q}M\\ & H=C^T\bar{Q}C+\bar{R}\end{array}$$

$\bar{Q}$和$\bar{R}$表示上面式子中的Q和R矩阵的增广形式

G、H、E矩阵跟上面的M和C矩阵相关

$$\overline{\mathbf{Q}}=\left[\begin{array}{ccc}\mathbf{Q}& \cdots & \\ ⋮& \mathbf{Q}& ⋮\\ & \cdots & \mathbf{F}\end{array}\right]\overline{\mathbf{R}}=\left[\begin{array}{ccc}\mathbf{R}& \cdots & \\ ⋮& \ddots & ⋮\\ & \cdots & R\end{array}\right]$$

MATLAB代码详解

函数体

main.m

% 矩阵输入部分
A = input('Input Matrix (row n; col n) A=');
B = input('Input Matrix (row n; col p) B=');
N = input('Input Prediction Section N=');
x_k = input('Input Initial State Matrix (row n; col 1) x_k=');
Q = input('Input Matrix (row n; col n) Q=');
R = input('Input Matrix (row 1; col 1) R=');
F = input('Input Matrix (row n; col n) F=');
MPC_Zero_Ref(A,B,N,x_k,Q,R,F);

MPC_Zero_Ref.m

function [M,C,Q_bar,R_bar,G,E,H,U_k] = MPC_Zero_Ref(A,B,N,x_k,Q,R,F)
    n = size(A,1);              % A是n*n矩阵，得到n
    p = size(B,2);              % B是n*p矩阵，得到p
    % 初始化M矩阵，M矩阵是(N+1)*n*n的
    % M矩阵上面是一个n*n的I矩阵，这一步先把矩阵下半部分初始化为0
    M = [eye(n); zeros(N*n, n)];
    % 初始化C矩阵，这一步令它有(N+1)*N*P个0
    C = zeros((N+1)*n, N*p);
    tmp = eye(n);               % 定义一个n*n的I矩阵
    for i = 1:N                 % 循环N次
        rows = i*n+(1:n);       % 定义当前行数，从i*n开始，共n行
        C(rows, :) = [tmp*B, C(rows-n, 1:end-p)];   % 将C矩阵填满
        tmp = A*tmp;            % 每一次将tmp左乘一次
        M(rows, :) = tmp;       % 将M矩阵填满
    end
    % 定义Q_bar
    S_q = size(Q, 1);           % 输出Q的维度
    S_r = size(R, 1);           % 输出R的维度
    Q_bar = zeros((N+1)*S_q, (N+1)*S_r);    % 初始化Q_bar为全0矩阵
    for i = 0:N                 % 循环N+1次
        % 将Q写到Q_bar对角线上
        Q_bar(i*S_q+1:(i+1)*S_q, i*S_q+1:(i+1)*S_q) = Q;
    end
    Q_bar(N*S_q+1:(N+1)*S_q, N*S_q+1:(N+1)*S_q) = F;
    % 定义R_bar
    R_bar = zeros(N*S_r, N*S_r);
    for i = 0:N-1
        R_bar(i*S_r+1:(i+1)*S_r, i*S_r+1:(i+1)*S_r) = R;
    end
    % 求解G、E、H
    G = M'*Q_bar*M;
    E = C'*Q_bar*M;
    H = C'*Q_bar*C+R;
    % 最优化
    f = (x_k'*E')';             % 定义f矩阵
    U_k = quadprog(H, f);       % 求解最优化U_k
end

输入举例