对MPC原理和公式进行通俗解释及MATLAB代码实现

笔者在翻阅了一天网上关于模型预测控制的讲解资料后，觉得绝大部分的讲解都没有讲解的很清楚，没有很清晰的展现模型预测这样设计的原理和目的到底是什么。于是决定自己理一理思路。

Dove_1234

7185人浏览 · 2023-01-03 13:45:31

Dove_1234 · 2023-01-03 13:45:31 发布

一、引言

根据MATLAB官方推出的讲解视频：

【Model Predictive Control】了解模型预测控制，第三部分：模型预测控制设计参数 - MATLAB&Simulink_哔哩哔哩_bilibili

我们可以先从驾驶汽车这个例子出发：

假设有一辆汽车正位于公路上行驶，我们希望它能沿着图中黄线做直线行驶（记住这是我们的目标），此时我们可以通过旋转方向盘来实现汽车位置的移动。开过车的人都知道，若方向盘打到底，会使得车身较快的进行偏转，而方向盘微微转动会使得车身的转向较缓。

接下来，我们将自己想象成机器人，想象如果自己只在固定间隔的时刻打方向盘，比如说开始时刻，第1秒，第2秒，第3秒......也就是每隔1秒调整一次车的方向，间隔之间的时间方向盘保持上一时刻点的状态。以下为示意图，其中箭头向右表示方向盘向右打，向左表示方向盘向左打，箭头与竖直方向偏离程度越大，则方向盘打的越狠。

然后，我们在头脑中想象两种的情景，第一个情景我们想玩漂移，于是将方向盘打的较狠，但最终还是需要达到我们的目标：汽车沿着黄线行驶。第二个情景我们吃了饭，因此车身不宜剧烈转动，于是我们轻打方向盘，最终达到目标。

两种情景下汽车的位置随着时间变化的图如下，这里仅仅为一种示意图：

其中黄线表示的我们的目标，我们只不过从汽车的侧面俯视汽车看而已。可以观察到情景一汽车先是很快的跨越黄线到达黄线的上面，再返回再次跨越黄线后逐渐达到目标。而情景二是逐渐靠近目标线。我们在脑海想象了这两种情况，也就是打方向盘对车身在未来一段时间内的位置的影响。

接下来我们可以根据自己的需求：追求刺激Or刚吃完饭来决定我们使用哪一种情景。比如说我们选择了情景一，于是我们在0时刻开始猛打方向盘，车立即开始猛转，接下来0到1时刻之间的时间，我们保持方向盘这个状态不动，静静的观察车的旋转.....当到达1时刻，如果幸运的话，车的位置将和我们预期的情况相同，但可能由于地面有水或者结冰，我们与预期的位置产生了一定的偏差。

此时，若使用我们最开始在头脑中设想的方向盘在0,1,2,3,4时刻转动的方案就很不合适了，因为你可能会与你的目标越差越大。因此，我们要在头脑中重新想象几种情景，也就是在1,2,3,4,5时刻方向盘的转动对车身方向的影响，然后根据需求：追求刺激Or刚吃完饭选择我们使用的情景，当然这些情景的设定前提一定是最终沿着黄线直线行驶。

于是，我们变得小心谨慎，每隔1个时刻，我们就根据车辆实际位置和预期位置之间的差异结合自己的需求和目标，重新规划多个情景：假设接下来5个时刻方向盘的转动程度，并想象最终的车辆行驶结果。图中情景一、二仅说明了车位置随着方向盘打的程度的不同而不同，没有画出方向盘具体的转动形式，读者可以自行想象。

选择最合适的一个情景中的第一个时刻方向盘的调整程度作为当前方向盘调整程度的准则。最终，我们经过多次的情景假设，结果想象，达到了最终的目标，同时也比较满足我们追求刺激的需求。

二、MPC是什么？

说完了驾驶汽车这个例子，我们再来说一说什么是MPC呢？

首先先来看几个专业术语：

预测模型：

根据被控对象的历史信息和未来的输入，可以预测系统未来的输出。预测模型可以是状态方程或传递函数，对于稳定的线性系统，可以是阶跃响应或脉冲响应。

在驾驶汽车的例子中，也就是指我们头脑中设想根据方向盘在下几个时刻的转向来想象出车辆的位置，方向盘的转动和车辆的位置一定满足某种映射关系，只不过这个是在我们人脑中根据经验想象的，没有明确的数学表达式。

滚动优化：

通过对某一性能指标的在线反复优化来确定最优控制动作。

而用驾驶汽车例子来理解，其实优化的任务就是调整方向盘在保证最终目标（沿着黄线走）的前提下尽量满足我们那个需求：追求刺激or刚吃完饭。

这也是与经典控制理论有较大区别的地方，因为传统控制只关心输入和输出，而MPC不仅关心输入和输出，也关心中间的每个状态。只不过我们的需求是个定性，而这个是个定量的，因此我们想象一下我们假设将需求进行定量，假设方向盘转动速率v越快，追求刺激的需求量x越高，假设它们满足线性关系，即

$x=av$

其中a为比例系数。此时，若我们选择追求刺激，则需要保证在设想的情景能够完成最终目标的前提下，选择情景中方向盘转动速率更高的。若选择刚吃完饭，则选择情景中方向盘转动速率更低的。

反馈矫正：

被控对象的实际输出用于修正预测结果，防止模型不匹配或环境干扰预测结果。

在此例中，其实就是我们根据车身预测位置和实际位置不相同，决定不沿用上一次选择的情景中方向盘的操控方式，而是重新进行情景假设和方向盘转动程度选择。

约束：

在实际的应用过程中，被控对象得状态往往受限于环境，即状态受约束，例如避障问题中被控对象得位置；或由于执行机构得性能，控制量受约束，即控制约束。

在此例中，指的就是车的方向盘有转角限制，毕竟你不可能把方向盘转个360°嘛，此外，车的位置也有限制，毕竟你不能开到这条路两边的人行道上。

控制时域：

控制量的预测范围，也就是控制序列的长度，即给出执行器在未来多长时间的输出。

在此例中，指的是你情景假设中控制方向盘的次数，比如0,1,2,3,4秒分别控制一次方向盘，那你的控制时域就是5。

预测时域：

预测被控对象在未来多长时间内的状态，预测时域可以大于控制时域，也可以等于，但是不能小于。

在此例中，指的就是情景假设中你预测出来的未来多长时间内车的位置，重点在于这个时间。比如你可以操控4次方向盘，而预测0-6秒之间的车的状态，此时控制时域是4，而预测时域是6。

模型预测控制：

在每个采用点处，根据被控对象的状态和预测模型，预测系统在未来一段时间内的状态，依据某一性能指标（成本函数）来求解最优的一组控制序列，并将这组控制序列的第一个控制作用作为输出给执行机构，在下一个采样点继续执行优化算法。

此处采用了博主背景lmc的图，这是我看到现在决定表示含义最清晰的一张图。

这张图显示了模型预测控制的过程：上图为被控对象状态，红色实线为实际状态，星线为预测状态，一共画出了四步;下图为控制序列，相应颜色的线对应上图的预测序列。

从第一步看，初始状态为-1，控制器预测一组状态序列和控制序列（蓝线），然后将控制序列第一个作为输出，被控对象根据这个控制作用调节自身状态，这个状态和预期状态（蓝线的第二个点）有误差，所以控制器又依据当前的实际状态对系统未来状态进行又一次预测，并给出了新的控制序列（橙色线），再把第一个控制序列输入到被控对象，如此循环，直到系统稳定。

三、MPC数学公式推导

首先，我们大致理一下MPC模型预测的思路：

1.估计/测量系统在k时刻（初始时刻）的状态值

2.基于 $u_{k},u_{k+1}...u_{k+N-1}$ 做最优化，其中u代表的是系统的输入量，下标说明的是时刻，比如 $u_{k}$ 就是在k时刻系统的输入量

3.只对该系统施加 $u_{k}$ ，即每一次设想情景并优化选择后只取第一个时刻的输入量

之后，我们再考虑一下二次规划问题，二次规划问题的一般形式通常是求解使得目标函数取得极值时的输入量

$min$ $Z^{T}QZ+C^{T}Z$

$Z = \begin{bmatrix} z_{1}\\z_{2} \\... \\z_{n} \end{bmatrix}$

当Q为正定矩阵，即Q = $\begin{bmatrix} q_{1} & 0 & 0 &0 \\ 0 & q_{2} & 0 &0 \\... &... &... &... \\ 0 &0 &0 & q_{n} \end{bmatrix}$ 时，该目标函数可以转变为求解 $q_{1}z_{1}^{2}+q_{2}z_{2}^{2}+...+q_{n}z_{n}^{2}$ 的最小值，此时我们可以通过MATLAB或者python等软件进行求解。

说二次规划问题有何作用呢？那是因为如果我们能将上面模型预测的思路中最优化问题转化为二次规划的标准形式，就可以通过软件求解输入量啦，这也就是达成了我们的目标。这样说可能听的有点迷糊，没事，接着往下看。

公式推导部分

$x_{k+1} = Ax_{k}+Bu_{k}$ （1）

含义：在k+1时刻的系统状态 = A矩阵*在k时刻的系统状态 + B矩阵*在k时刻系统的输入

令

$X_{k} = \begin{bmatrix} x(k|k)\\x(k+1|k) \\x(k+2|k) \\... \\x(k+N|k) \end{bmatrix}$ $U_k = \begin{bmatrix} u(k|k)\\u(k+1|k) \\u(k+2|k) \\... \\ u(k+N-1|k) \end{bmatrix}$ （2）

其中x(k+i|k)意思是在k时刻预测的第k+i时刻的状态量，u(k+i|k)意思是在k时刻预测的第k+i时刻的输入量。

之后，我们假设此时系统的输出y = x,参考R = 0

而误差E = 输出-参考 = y -R = y = x

而代价函数（最优化中的目标函数）J我们将其定义为

$J = \sum_{i=0}^{N-1}(E(k+i|k)^{T}QE(k+i|k)+u(k+i|k)^{T}Ru(k+i|k))+E(k+N)^{T}FE(k+N)$ (3)

其中 $E(k+i|k)^{T}QE(k+i|k)$ 为误差的加权和， $u(k+i|k)^{T}Ru(k+i|k)$ 为输入的加权和， $E(k+N)^{T}FE(k+N)$ 为终端误差。我们的目的是使代价函数取得最小值。

考虑到E = x ，因此我们将（3）式中的E用替代得到了代价函数J

$J = \sum_{i=0}^{N-1}(x(k+i|k)^{T}Qx(k+i|k)+u(k+i|k)^{T}Ru(k+i|k))+x(k+N)^{T}Fx(k+N)$ (4)

其中Q,R均为对角矩阵

我们将（4）式中的加权和项展开

关于x的展开：

$x(k|k)^{k}Qx(k|k)+x(k+1|k)^{T}Qx(k+1|k)+...+x(k+N-1|k)^{T}Qx(k+N-1|k)+x(k+N)^{T}Fx(k+N)$ (5)

该式写成矩阵形式为：

$\begin{bmatrix} x(k|k)\\x(k+1|k) \\... \\x(k+N|k) \end{bmatrix}^{T}$ $\begin{bmatrix} Q & & & \\ & Q & & \\ & &... & \\ & & &F \end{bmatrix}\begin{bmatrix} x(k|k)\\x(k+1|k) \\... \\x(k+N|k) \end{bmatrix}$ (6)

令 $\begin{bmatrix} Q & & & \\ & Q & & \\ & &... & \\ & & &F \end{bmatrix}$ $=\bar{Q}$ ， $\begin{bmatrix} x(k|k)\\x(k+1|k) \\... \\x(k+N|k) \end{bmatrix}$ $=X_{k}$

因此（6）式可以转变为

$X_{k}^{T} \bar{Q}X_{k}$ （7）

关于u的展开：

$u(k|k)^{k}Qu(k|k)+u(k+1|k)^{T}Qu(k+1|k)+...+u(k+N-1|k)^{T}Qu(k+N-1|k)$ (7)

同理可以将（7）式转变为

$U_{k}^{T}\bar{R}U_{k}$ （8）

因此，代价函数J可以写成

$J =X_{k}^{T} \bar{Q}X_{k}+U_{k}^{T}\bar{R}U_{k}$ （9）

仔细看这个式子，是不是跟二次规划的标准型 $Z^{T}QZ+C^{T}Z$ 很相似了呢？

但有一个不同点就是，在标准型中，只有一个变量Z，而代价函数中有两个X、U，因此我们需要将其中一个变量消去。

其实， $x_{k+1} = Ax_{k}+Bu_{k}$ 这个式子在之前还没有派上用场呢，于是我们使用它来试试看

令初始条件= $x_{k}$

$x(k|k)=x_{k}$

$x(k+1|k)=Ax(k|k)+Bu(k|k)=Ax_{k}+Bu(k|k)$

$x(k+2|k)=Ax(k+1|k)+Bu(k+1|k)=A^{2}x_{k}+ABu(k|k)+Bu(k+1|k)$

....

$x(k+N|k)=A^{N}x_{k}+A^{N-1}Bu(k|k)+...+Bu(k+N-1|k)$

将上面这四个式子化成矩阵的形式：

$X_{k}=\begin{bmatrix} I\\A \\A^{2} \\... \\ A^{N} \end{bmatrix}x_{k}+\begin{bmatrix} 0 &0 &... &0 \\B & 0 &... &0 \\AB &B &... &0 \\... & ...&... &0 \\ A^{N-1} & A^{N-2}B & ...& B \end{bmatrix}\begin{bmatrix} u(k|k)\\u(k+1|k) \\... \\u(k+N-1|k) \end{bmatrix}$ （10）

令 $\begin{bmatrix} I\\A \\A^{2} \\... \\ A^{N} \end{bmatrix}=M$ ， $\begin{bmatrix} 0 &0 &... &0 \\B & 0 &... &0 \\AB &B &... &0 \\... & ...&... &0 \\ A^{N-1} & A^{N-2}B & ...& B \end{bmatrix}=C$

则（10）式可以转化为

$X_{k}=Mx_{k}+CU_{k}$ （11）

现在，我们已经可以用初始条件和输入量U来表示状态X了，于是我们将（11）式代入（9）式

$J=X_{k}^T\bar{Q}X_{k}+U_{k}^{T}\bar{R}U_{k}=(Mx_{k}+CU_{k})^{T}\bar{Q}(Mx_{k}+CU_{k})+U_{k}^{T}\bar{R}U_{k}$

将等式右边展开得到，

$J = x_{k}^{T}M^{T}\bar{Q}Mx_{k}+x_{k}^{T}M^{T}\bar{Q}CU_{k}+U_{k}^{T}C^{T}\bar{Q}Mx_{k}+U_{k}^{T}C^{T}\bar{Q}CU_{k}+U_{k}^{T}\bar{R}U_{k}$ （12）

其中 $x_{k}^{T}M^{T}\bar{Q}CU_{k}$ 和 $U_{k}^{T}C^{T}\bar{Q}Mx_{k}$ 两项相等，合并为一项 $2x_{k}^{T}M^{T}\bar{Q}CU_{k}$ ，令 $M^{T}\bar{Q}C=E$ ，则两项之和为 $2x_{k}^{T}EU_{k}$

另外三项中我们令 $x_{k}^{T}M^{T}\bar{Q}Mx_{k}$ 中的 $M^{T}\bar{Q}M=G$ ， $U_{k}^{T}C^{T}\bar{Q}CU_{k}+U_{k}^{T}\bar{R}U_{k}$ 合并得到 $U_{k}^{T}(C^{T}\bar{Q}C+R)U_{k}$ ,令 $C^{T}\bar{Q}C+R=H$

最终我们将（12）式写成

$J = x_{k}^{T}Gx_{k}+2x_{k}^{T}EU_{k}+U_{k}^{T}HU_{k}$ （13）

大功告成！

我们终于把代价函数转化成只跟初始条件和输入量有关的函数了，这就满足了二次规划的标准形式，因此我们可以使用软件对其进行求解啦。

四、MPC代码实现

代码从DR_CAN教授那里转载，具体含义大家可以看下面这个链接

【MPC模型预测控制器】4_完整案例讲解 - Octave代码_哔哩哔哩_bilibili

MPC_test.m

%% 清屏
clear ;
close all;
clc;
%% 第一步，定义状态空间矩阵
%% 定义状态矩阵 A, n x n 矩阵
A = [1 0.1; -1 2];
n= size (A,1);
%% 定义输入矩阵 B, n x p 矩阵
B = [ 0.2 1; 0.5 2];
p = size(B,2);
%% 定义Q矩阵,n x n 矩阵
Q=[1 0;0 100];
%% 定义F矩阵，n x n 矩阵
F=[100 0;0 1];
%% 定义R矩阵，p x p 矩阵
R=[0.1 0 ;0 .1];
%% 定义step数量k
k_steps=100;
%% 定义矩阵 X_K， n x k 矩 阵
X_K = zeros(n,k_steps);
%% 初始状态变量值， n x 1 向量
X_K(:,1) =[20;-20];
%% 定义输入矩阵 U_K， p x k 矩阵
U_K=zeros(p,k_steps);
%% 定义预测区间K
N=5;
%% Call MPC_Matrices 函数 求得 E,H矩阵 
[E,H]=MPC_Matrices(A,B,Q,R,F,N);
%% 计算每一步的状态变量的值
for k = 1 : k_steps
%% 求得U_K(:,k)
U_K(:,k) = Prediction(X_K(:,k),E,H,N,p);
%% 计算第k+1步时状态变量的值
X_K(:,k+1)=(A*X_K(:,k)+B*U_K(:,k));
end
 
%% 绘制状态变量和输入的变化
subplot  (2, 1, 1);
hold;
for i =1 :size (X_K,1)
    plot (X_K(i,:));
end
legend("x1","x2")
hold off;
subplot (2, 1, 2);
hold;
for i =1 : size (U_K,1)
plot (U_K(i,:));
end
legend("u1","u2")

Prediction.m

function u_k= Prediction(x_k,E,H,N,p)
    U_k = zeros(N*p,1); % NP x 1
    U_k = quadprog(H,E*x_k);
    u_k = U_k(1:p,1); % 取第一个结果
end

MPC_Matrices.m

function  [E , H]=MPC_Matrices(A,B,Q,R,F,N)
    n=size(A,1);   % A 是 n x n 矩阵, 得到 n
    p=size(B,2);   % B 是 n x p 矩阵, 得到 p
    %%%%%%%%%%%%
    M=[eye(n);zeros(N*n,n)]; % 初始化 M 矩阵. M 矩阵是 (N+1)n x n的， 
                             % 它上面是 n x n 个 "I", 这一步先把下半部
                             % 分写成 0 
    C=zeros((N+1)*n,N*p); % 初始化 C 矩阵, 这一步令它有 (N+1)n x NP 个 0
    % 定义M 和 C 
    tmp=eye(n);  %定义一个n x n 的 I 矩阵
    %　更新Ｍ和C
    for i=1:N % 循环，i 从 1到 N
        rows =i*n+(1:n); %定义当前行数，从i x n开始，共n行 
        C(rows,:)=[tmp*B,C(rows-n, 1:end-p)]; %将c矩阵填满
        tmp= A*tmp; %每一次将tmp左乘一次A
        M(rows,:)=tmp; %将M矩阵写满
    end
    % 定义Q_bar和R_bar
    Q_bar = kron(eye(N),Q);
    Q_bar = blkdiag(Q_bar,F);
    R_bar = kron(eye(N),R);
    % 计算G, E, H
    G=M'*Q_bar*M; % G: n x n
    E=C'*Q_bar*M; % E: NP x n
    H=C'*Q_bar*C+R_bar; % NP x NP
end

一些碎碎念：

之前搞四足机器人的时候看到当前主流算法就是vmc和mpc等等，然后大致看了一下，果断选择研究了vmc,毕竟原理相对简单一些。本以为能躲掉mpc的毒打，毕竟也不是自动化专业的人，但却在做电气的项目的时候发现，相关论文也用到了mpc，比如说一篇叫Model Predictive Control—A Simple and Powerful
Method to Control Power Converters，害，终究还是没逃掉，只能慢慢学啦哈哈哈。

————————————————
版权声明：本文为CSDN博主「某只旺仔」的原创文章，遵循CC 4.0 BY-SA版权协议，转载请附上原文出处链接及本声明。
原文链接：https://blog.csdn.net/qq_54543084/article/details/128437786

开放原子开发者工作坊

开放原子开发者工作坊旨在鼓励更多人参与开源活动，与志同道合的开发者们相互交流开发经验、分享开发心得、获取前沿技术趋势。工作坊有多种形式的开发者活动，如meetup、训练营等，主打技术交流，干货满满，真诚地邀请各位开发者共同参与！

更多推荐

第二届openEuler生态大会（中国·湖南）成功举办

10月30日，第二届openEuler生态大会（中国·湖南）成功举办。

开放原子开发者工作坊

“小满”安全车控操作系统正式在AtomGit开源

10月24日，由中国汽车工业协会指导，普华基础软件股份有限公司主办的“小满”安全车控操作系统开源发布会暨共建计划说明会成功举行。普华基础软件宣布将安全车控操作系统“小满”（简称“小满”）V24.10源代码正式在开放原子开源基金会（简称“基金会”）旗下AtomGit开源协作平台开源，并在AtomGit平