参考如下链接：
一个模型预测控制（MPC）的简单实现
建模例子详解
对于以下控制模型：
x(k+1)=Ax(k)+Bu(k)x(k+1)=A x(k)+B u(k) x(k+1)=Ax(k)+Bu(k)
注：需要注意对应的输入矩阵和状态矩阵每个矩阵的维度
x矩阵：n*1
A矩阵：n*n
B矩阵：n*p
u矩阵：p*1
写成矩阵格式：
[x1(k+1)x2(k+1)]=[10.102][x1(k)x2(k)]+[00.5]u(k)\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) [x1(k+1)x2(k+1)]=[100.12][x1(k)x2(k)]+[00.5]u(k)
此处的x1x_{1}x1和x2x_{2}x2可以理解为误差项
X(k)=[x(k∣k)x(k+1∣k)⋮x(k+j∣k)⋮x(k+N∣k)]X(k)=\left[\begin{array}{c}x(k \mid k) \\x(k+1 \mid k) \\\vdots\\ x(k+j \mid k)\\\vdots \\x(k+N \mid k)\end{array}\right]X(k)=⎣⎢⎢⎢⎢⎢⎢⎢⎢⎡x(k∣k)x(k+1∣k)⋮x(k+j∣k)⋮x(k+N∣k)⎦⎥⎥⎥⎥⎥⎥⎥⎥⎤U(k)=[u(k∣k)u(k+1∣k)⋮u(k+i∣k)⋮u(k+N−1∣k)]U(k)=\left[\begin{array}{c}u(k \mid k) \\u(k+1 \mid k) \\\vdots\\ u(k+i \mid k)\\\vdots \\u(k+N-1 \mid k)\end{array}\right]U(k)=⎣⎢⎢⎢⎢⎢⎢⎢⎢⎡u(k∣k)u(k+1∣k)⋮u(k+i∣k)⋮u(k+N−1∣k)⎦⎥⎥⎥⎥⎥⎥⎥⎥⎤
矩阵的维度
U(k) → NP1
X(k) → (N+1)n*1
对于MPC矩阵：
xk=Mxk+Cukx_{k}=M x_{k}+C u_{k} xk=Mxk+Cuk
M=[In×nAn×nAn2⋮AN]M=\left[\begin{array}{c}I_{n \times n} \\A_{n \times n} \\A_{n}^{2} \\\vdots \\A^{N}\end{array}\right]M=⎣⎢⎢⎢⎢⎢⎡In×nAn×nAn2⋮AN⎦⎥⎥⎥⎥⎥⎤C=[00…0⋮…⋮⋮0000…0Bn×p0…0ABn×pB⋮0⋮⋮B]C=\left[\begin{array}{cccc} 0 & 0 & \ldots & 0 \\ & \vdots & \ldots & \vdots \\ \vdots & 0 & & 0 \\ 0 & & 0 & \ldots & 0 \\ B_{n \times p} & 0 & \ldots & 0 \\ A B_{n \times p} & B & \vdots & 0 \\ \vdots & \vdots & & B \end{array}\right]C=⎣⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎡0⋮0Bn×pABn×p⋮0⋮00B⋮……0…⋮0⋮0…00B0⎦⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎤
矩阵的维度
M → (N+1)nn
X(k) → (N+1)nN*P
X(k)=[x(k∣k)x(k+1∣k)x(k+2∣k)x(k+3∣k)]=[x1(k∣k)x2(k∣k)x1(k+1∣k)x2(k+1∣k)x1(k+2∣k)x2(k+2∣k)x1(k+3∣k)x2(k+3∣k)]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]X(k)=⎣⎢⎢⎡x(k∣k)x(k+1∣k)x(k+2∣k)x(k+3∣k)⎦⎥⎥⎤=⎣⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎡x1(k∣k)x2(k∣k)x1(k+1∣k)x2(k+1∣k)x1(k+2∣k)x2(k+2∣k)x1(k+3∣k)x2(k+3∣k)⎦⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎤
U(k)=[u(k∣k)u(k+1∣k)u(k+2∣k)]=[u(k∣k)u(k+1∣k)u(k+2∣k)]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]U(k)=⎣⎡u(k∣k)u(k+1∣k)u(k+2∣k)⎦⎤=[u(k+1∣k)u(k∣k)u(k+2∣k)]
计算出M矩阵和C矩阵（此处省略计算过程）
之后构建二次规划模式（cost function）
J=∑k=0N−1x(k+i∣k)TQx(k+i∣k)+u(k+i∣k)TRu(k+i∣k)+x(k+N∣k)TFx(k+i∣k)J=\sum_{k=0}^{N-1} x(k+i \mid k)^{T} Q x(k+i \mid k)+u(k+i \mid k)^{T} R u(k+i \mid k)+x(k+N \mid k)^{T} F x(k+i \mid k) J=k=0∑N−1x(k+i∣k)TQx(k+i∣k)+u(k+i∣k)TRu(k+i∣k)+x(k+N∣k)TFx(k+i∣k)
Q、R、F表示对应输入输出的权重系数矩阵
矩阵的维度
Q → n*n
R → 1*1
F → n*n
Q矩阵影响x变量的相关性，R矩阵跟输入相关，F矩阵跟终值相关
x(k+N∣k)TFx(k+i∣k)x(k+N \mid k)^{T} F x(k+i \mid k)x(k+N∣k)TFx(k+i∣k)与系统对应的终值状态相关
简化上面的 cost function 矩阵
J=x(k)TGx(k)+U(k)THU(k)+2x(k)TEU(k)J=\boldsymbol{x}(k)^{T} \boldsymbol{G} \boldsymbol{x}(k)+\boldsymbol{U}(k)^{T} \boldsymbol{H} \boldsymbol{U}(k)+2 \boldsymbol{x}(k)^{T} \boldsymbol{E} \boldsymbol{U}(k) J=x(k)TGx(k)+U(k)THU(k)+2x(k)TEU(k)
该矩阵只包含了输入项和已知的初始输入项x(k)
G=MTQˉME=CTQˉMH=CTQˉC+Rˉ\begin{aligned}&G=M^{T} \bar{Q} M \\&E=C^{T} \bar{Q} M \\&H=C^{T} \bar{Q} C+\bar{R}\end{aligned} G=MTQˉME=CTQˉMH=CTQˉC+Rˉ
Qˉ\bar{Q}Qˉ和Rˉ\bar{R}Rˉ表示上面式子中的Q和R矩阵的增广形式
G、H、E矩阵跟上面的M和C矩阵相关
Q‾=[Q⋯⋮Q⋮⋯F]R‾=[R⋯⋮⋱⋮⋯R]\overline{\boldsymbol{Q}}=\left[\begin{array}{ccc}\boldsymbol{Q} & \cdots & \\\vdots & \boldsymbol{Q} & \vdots \\& \cdots & \boldsymbol{F}\end{array}\right] \quad \overline{\boldsymbol{R}}=\left[\begin{array}{ccc}\boldsymbol{R} & \cdots & \\\vdots & \ddots & \vdots \\& \cdots & R\end{array}\right] Q=⎣⎢⎡Q⋮⋯Q⋯⋮F⎦⎥⎤R=⎣⎢⎡R⋮⋯⋱⋯⋮R⎦⎥⎤
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矩阵，得到np = size(B,2); % B是n*p矩阵，得到p% 初始化M矩阵，M矩阵是(N+1)*n*n的% M矩阵上面是一个n*n的I矩阵，这一步先把矩阵下半部分初始化为0M = [eye(n); zeros(N*n, n)];% 初始化C矩阵，这一步令它有(N+1)*N*P个0C = 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_barS_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;endQ_bar(N*S_q+1:(N+1)*S_q, N*S_q+1:(N+1)*S_q) = F;% 定义R_barR_bar = zeros(N*S_r, N*S_r);for i = 0:N-1R_bar(i*S_r+1:(i+1)*S_r, i*S_r+1:(i+1)*S_r) = R;end% 求解G、E、HG = 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_kend
