【MPC】模型预测控制 | 在移动机器人运动规划中的应用（二）线性MPC

写在前面：
🌟 欢迎光临 清流君 的博客小天地，这里是我分享技术与心得的温馨角落。📝
个人主页：清流君_CSDN博客，期待与您一同探索 移动机器人 领域的无限可能。

🔍 本文系 清流君 原创之作，荣幸在CSDN首发🐒
若您觉得内容有价值，还请评论告知一声，以便更多人受益。
转载请注明出处，尊重原创，从我做起。

👍 点赞、评论、收藏，三连走一波，让我们一起养成好习惯😜
在这里，您将收获的不只是技术干货，还有思维的火花！

📚 系列专栏：【路径规划】系列，带您深入浅出，领略规划之美。🖊
愿我的分享能为您带来启迪，如有不足，敬请指正，让我们共同学习，交流进步！

🎭 人生如戏，我们并非能选择舞台和剧本，但我们可以选择如何演绎 🌟
感谢您的支持与关注，让我们一起在知识的海洋中砥砺前行~~~

---

一、基本模型

1、系统建模

  举一个最简单的例子，我们用一个三阶积分器模型作为系统模型：

  因为这个三阶积分器模型在机器人学中经常出现，从马达、电机到机械手的多自由度的末端执行器(n Effector)，再到多旋翼无人机等都可以套用这个模型。先看一下系统的连续模型，位置的导数等于速度，速度的导数等于加速度，加速度的导数等于 加加速度(Jerk)。
  考虑优化未来 4 秒时间的系统状态，这个 4 秒叫做 预测空间（prediction horizon）。
接下来第一步，我们先为我们的输入Jack，也就是这个 j 找一个参数空间，这里我们选取最常见的直接离散化的方式，把 4 秒的预测空间均匀地分成 20 段 $i=[0,2,...,19]$，每段是 $dt=0.2$ 秒。
  那么我们就可以得到这个离散的状态方程：

  一旦 $dt$ 给定后，这个离散方程其实是一个线性的，可以把它写成一个矩阵形式。

  首先第一步，把各自离散的 $p、v、a、j$ 在离散时间点上的值组成各自的向量 $P$、$V$、$A$ 、 $J$

  有了这四个向量之后，我们就可以把这个离散化的模型给变成一个矩阵的描述形式。

  这里的$B_p$、$B_v$ 和 $B_a$ 是关于 $p_0、v_0、a_0$ 的函数，我们把这个矩阵的表达形式叫做预测模型。
  然后我们的 $J$，也就是 $j_0$ 到 $j_{19}$ 是参数空间，每当选定一个参数空间中的一组值后，就可以通过预测模型来得到系统状态在未来的值 $P、V、A$。

示例程序

  具体的 $T_p、T_v、T_a、B_p$、$B_v$ 和 $B_a$这 6 个矩阵和向量通过如下 MATLAB 代码计算：

getPredictionMatrix.m

function [Tp, Tv, Ta, Bp, Bv, Ba] = getPredictionMatrix(K,dt,p_0,v_0,a_0)
Ta=zeros(K);
Tv=zeros(K);
Tp=zeros(K);

for i = 1:K
    Ta(i,1:i)= ones(1, i)*dt;
end

for i = 1:K
    for j= 1:i
        Tv(i,j)=(i-j+0.5)*dt^2;
    end
end

for i = 1:K
    for j=1:i
        Tp(i,j)=((i-j+1)*(i-j)/2+1/6)*dt^3;
    end
end

Ba = ones(K,1) * a_0;
Bv = ones(K,1) * v_0;
Bp = ones(K,1) * p_0;

for i=1:K
    Bv(i) = Bv(i)+ i*dt*a_0;
    Bp(i) = Bp(i)+ i*dt*v_0 + i^2/2*a_0*dt^2;
end

2、问题建模

  在有了预测模型之后， MPC 第二步就是根据要解决的问题来建立问题模型，在这个例子中，问题模型可直接写为优化目标函数。
  假设我们现在的问题有两个目标：

a）目标①：PVA终点都为0

  要把三阶积分器从任意一个初始位置带回到状态空间中的原点，也就是最终这个积分器要到 0 位置， 0 速度和 0 加速度的一个状态空间中的点上。
  对于目标①，我们可以写出这样的一个指标函数:  也就是尽量的减少位置的平方、速度的平方和加速的平方之和。

b）目标②：轨迹光滑，减少输入幅值

  那么目标②是在希望在此过程中系统的轨迹可以尽量的平滑。
  对于第二个目标，可以通过减小输入幅值来达到，也就是优化 Jerk 的平方。
其中$w_1、w_2、w_3、w_4$ 都是我们可以调节的权重参数。

---

3、问题求解

  在问题模型建立好后，MPC 第三步就是使用优化算子来求解问题。
  以这个例子的目标函数为例，可以把这个目标改写为一个新的形式
  这要结合到我们之前得到的预测模型：
  我们可以把 $P、V、A$ 相应地转换成预测模型，通过输入 jerk 代入各自的 $P、V、A$ 的部分，我们可以把这个式子变化成如下形式：

  首先不难看出，这是一个二次优化的问题，上式中的第一部分是二次项，第二部分是一次项，第三部分是剩余的常数项，常数项在优化问题的时候我们可以把它忽略掉。
  其次可以看到，在这个二次规划问题的 $H$ 矩阵(Hashen) 这一部分，其中有一个正定的单位矩阵，剩下的几个部分也至少都是半正定的，所以这个二次规划问题的 $H$ 矩阵一定是正定的，于是我们可以肯定这是一个凸优化的问题。

示例程序

  对于这种凸优化的二次规划问题，有很多算子可以解，我们用 MATLAB 优化工具库中的 quadprog 方程进行求解：
MPC.m

p_0 = 10;
v_0 = 0;
a_0 = 0;
K = 20;
dt = 0.2;
log = [0 p_0 v_0 a_0];
w1 = 1;
w2 = 1 ;
w3 = 1;
w4 = 1;
for t=0.2:0.2:10
    %% Construct the prediction matrix
	[Tp, Tv, Ta, Bp, Bv, Ba] = getPredictionMatrix(K,dt,p_0,v_0,a_0)
    
    %%  Construct the optimization problem
    H = w4*eye(K)+w1*(Tp'*Tp)+w2*(Tv'*Tv)+w3*(Ta'*Ta);
    F = w1*Bp'*Tp+w2*Bv'*Tv+w3*Ba'*Ta;
    
    %% Solve the optimization problem
    J= quadprog(H,F,[],[]);
    
    %% Apply the control
    j = J(1);
    p_0 = p_0 + v_0*dt + 0.5*a_0*dt^2 + 1/6*j*dt^3;
    v_0 = v_0 + a_0*dt + 0.5*j*dt^2;
    a_0 = a_0 +j*dt;

	%% Log the states
    log = [log; t p_0 v_0 a_0];
end

  在求解优化问题后，MPC的第四步是用得到的最优控制信号驱动实际系统，也就是代码的 %% Apply the control 部分所做的内容。每一次驱动只使用得到的最优信号 $0.2$ 秒，所以我们也只需要 $20$ 个 $j$ ，就是最优控制信号分段中的第一段。

---

  现在我们借助这个代码来整体回顾一下 MPC 控制过程。

a）建立预测模型

• 第1到第3行设置了系统的初始状态，比如说位置为10，速度为0，加速度为0。
• 第4到第5行是把未来的 4 秒均匀的离散成 20 个时间段，每一段是 0.2 秒。
• 第6行是用来记录系统的轨迹。
• 第7到第8、第 10 行是我们优化目标函数中的各自针对位置、速度、加速度和 Jerk 的平方的权重。

  作为 MPC 过程的第一步——构建预测模型的矩阵，其中的矩阵有$T_p、T_v、T_a、B_p$、$B_v$ 、$B_a$ 这六个，其中前三个只基于 $k$ 和 $dt$ 就可以构建，后三个基于 $p_0、v_0、a_0$ 。

b）建立问题模型

• 第 16 和 17 行，构建问题模型，写成二次规划形式。

c）解决优化问题

• 在第 20 行用 MATLAB 的quadprog来解二次规划，然后得到最优输入序列 $J$，这里 $J$ 是一个 $20\times 1$ 的向量。

d）用求解得到的优化控制去驱动系统

• 我们只取它的第一段 j = J(1) 来驱动系统，只驱动 0.2 秒，然后就立刻进入下一个循环。

---

  很多更复杂的 MPC 也都符合这个框架，只不过在建立问题模型和预测模型的时候，过程会更加复杂，而复杂的预测或者问题模型，必须要用到一些特殊的优化算法。
下面看一下 MPC 控制结果：

  通过调节 $w_1、w_2、w_3、w_4$ 四个权重值，比如把 $w_1$ 从 $w_1=1$ 增加到 $w_1=5$ 再增加到 $w_1=10$ 再到 $w_1=100$，可以明显看到速度和加速度的轨迹会变动地更激烈。

  另外还需要注意一点，在刚才这个例子里，这个 MPC 是有解析解的，可以变成一个线性控制器，线性的控制率完全可以达到一样的效果，而且用这个线性的控制率解 MPC 时，为了找到最优的控制信号 $J$，我们是解的这个优化方程：

  如果令它的这一部分为 $H$ 矩阵：

  令这一部分为 $F$ 矩阵的话：

  可以发现其实它是一个这样的形式：

  为了要找到对这个方程的最小的 $J$ ，因为它不带任何的约束条件，可以知道 $J$ 的解析解是这样：

  在 MATLAB 中可以用反除号 J = -H\F'来得到 $J$.
  这样比直接对 $H$ 求逆效率要高，然后我们最后通过控制发现最后当 $w_1、w_2、w_3、w_4$ 全部等于1时，可以对比一下这个结果：

  这是用解析解得到的控制结果，和之前解二次规划的控制结果是一样的。

---

二、模型约束

1、模型硬约束(Hard constraint)

a）加入速度和加速度约束

那要如何加入速度和加速度约束呢？

  假设约束是希望速度和加速度保持在 $\pm 1$ 之内：
  可以把这两组不等式（注意他们是两组，因为他们有一共有 20 个不等式）改写成这个矩阵的形式：

  这个 $-1$ 是 $20\times 1$的向量。
  把这两个改写成矩阵形式后，就可以带入之前已得到的预测模型：

  代入后就可以把有关 $V$ 和 $A$ 的不等式变成只有关 $J$ 的不等式：

  这里第一个关于 $V$ 的不等式直接变形出来，可以把 $B_v$ 移到两个不等号的左边和右边去：

  对于 $A$ 的约束条件，也可以得到这样的不等式：
  由于很多像 Matlab 的优化工具，只能处理小于等于式型的不等式条件，所以把这个不等式首先拆开成两部分，

  上面第二个不等式是加一个负号，把它从大于等于条件变成小于等于的条件。对 $a$ 的约束条件同理，也有两个不等式。这样一共我们有 4 组约束条件。
  可以清楚的看到这四组约束条件全部都是线性的，所以依旧可以用凸优化、二次凸优化的工具来解决这个问题。

b）示例程序

  接下来我们就可以把这四个约束条件写成代码的形式：

A =[Tv;-Tv;Ta;-Ta];
b = [ones(20,1)-Bv;ones(20,1)+Bv;ones(20,1)-Ba;ones(20,1)+Ba];

J = quadprog(H,F,A,b);

  在 MATLAB 中，默认是 $A\times J<b$ 。也就是说这个 quadprog 它只能写 $A、b$ 这两个不等式的参数条件，于是就需要把这四个不等式用来构建相应的 $A、b$ 矩阵。

  用 quadprog 解这个带不等号条件的优化问题后做模型预测控制，结果如下：

  这三幅图里面分别给出了在不同的参数权重下面的模型预测控制的效果。

---

c）硬约束的不足

  模型预测控制和很多运动规划问题不同，模型预测控制受限于预测区间，在这个问题里面我们只能往前预测 4 秒时间，但是 4 秒时间是无论如何也不够从 10 米瞬间就回到 0 米的，因为限制了它的速度，最大只有$\pm 1m/s$，加速度只有 $1m/s^2$。
  如果我们硬要规定终止条件一定是在 4 秒之内回到 0 米的话，这时优化算子就会告诉我们这个问题是没有解的，因为它解不出来，找不到一个满足速度和加速度约束条件的解。所以大部分 MPC 问题都愿意把这个目标函数放在优化函数里，而不是把它作为约束条件。这样的话，无论如何，优化问题至少是有一个解的，它会尽量去靠近你的最终目标。
  在 MPC 框架中，除了调整目标函数的权重外，还可以调整预测空间，可以延长它，但是延长预测区间就需要更大的算力去解优化问题，而太短的预测区间会导致解出来的 MPC 控制器不能对系统起到镇定的作用，有时候会发散。

2、模型软约束(Soft constraint)

下面来看一下软约束主要用在什么时候，比如四旋翼无人机，现在的约束条件是速度和加速度都在$\pm 1m/s$内，那么当初始速度超过这个值时，该怎么办呢？

比如有一阵狂风刮来，把速度吹到很大的值，这时传统的数值优化算子（像 MATLAB 中的 quadprog 算子），会直接告诉你说这个问题是没有解的。然后这个 MPC 控制器它就会告诉你，我根本就不知道应该怎么办，因为我的优化算子告诉我说我没有解。

  这时就要用到软约束条件，软约束可以使这个优化算子依旧给一个最接近正确的解出来。

a）软约束的基本思想

  把约束条件变成一个惩罚函数(penalty function)放到目标函数中。

b）惩罚函数

  如果要加一个对速度 $v$ 的软约束，那么就可以列出这样一个惩罚函数：

  其中，$M$ 是一个很大的正数。
  加入惩罚函数后，只要 $v$ 超出了$\pm 1$ 区间，惩罚项的数值就会很大；如果 $v$ 在$\pm 1$ 区间内，惩罚项就是 0 。
  这个形式的惩罚函数是比较理想化的，在正常使用时，会用一些带指数函数的惩罚函数，比如下面这种情况：

  普通的惩罚函数在上图转角处不是可导的，如果使用连续的指数函数形式的惩罚项，在转角处是可导的，一旦可以求导后，就可以用类似序列二次规划法（Sequential Quadratic Programming, SQP） 等解非线性优化问题的算子去解。但是无论如何，如果放这个惩罚函数，都不可以把它再变成一个二次规划的问题了。
  然而大部分的 MPC 算子都是基于二次规划的，这样会导致很多 MPC 算子都不可以使用。

c）软约束的求解

那么有没有办法依旧使用二次规划问题的算子来解决这个软约束？
解决软约束问题的方法其实是有的，但是比直接加一个惩罚函数 $S$ 会稍微复杂一点。

  先看一下之前推导的四个不等式约束条件：

  对于 $-T_{v}J\le 1_{20\times 1}+B_v$ ，这个不等式的约束条件说的是使速度 $v\ge -1m/s$ ，我们只把这一个不等式条件加上软约束进行一下推导，剩下的三个大家有兴趣的话可以自己加一下软约束，然后推导一下。

  首先我们是加一个叫 松驰变量(slack variable) $L$ 的东西。这是一个 20 乘以1的矩阵向量，必须每一个 $L_1，L_2，L_3$ 直到 $L_{20}$ 都必须是大于等于 0 了。然后把它加在刚才那个不等式的约束条件后面。

  只要当这个 $L$ 足够的大时，这个不等式的约束就始终是成立的。那么现在我们的目标就是对于这个 $L$， 一边要它足够大，使这个约束一定成立；另一边又希望这个$L$ 的值不要太大，如果太大的话证明超过这个约束条件太多了。

  为了使 $L$ 的值不要太大，我们在优化的目标函数里加上 $L^{T}L$ 这部分，它的权重系数为 $w_5$，并且给一个非常大的权重。
  现在我们的编程变量就有 $J$、$L$ 两部分 ， $J$ 是最优控制的部分，$L$ 是加入的松弛变量，把 $J$ 和 $L$ 接在一起，形成一个大的向量 $\bar{J}=\left[\begin{array}{c}J\\ L\end{array}\right]$，这就是最终我们的编程变量，接在一起后，之前相应的 $H、F、A、b$ 矩阵都要进行相应的调节。

d）示例程序

  下面是怎么得到 $H、F、A、b$ 和松弛变量的代码：

%% Construct the prediction matrix
[Tp, Tv, Ta, Bp, Bv, Ba] = getPredictionMatrix(K,dt,p_0,v_0,a_0)

%%  Construct the optimization problem
H = blkdiag(w4*eye(K)+w1*(Tp'*Tp)+w2*(Tv'*Tv)+w3*(Ta'*Ta), w5*eye(K));
F = [w1*Bp'*Tp+w2*Bv'*Tv+w3*Ba'*Ta zeros(1,K)];

A = [Tv zeros(K);-Tv -eye(K);Ta zeros(K);-Ta zeros(k); zeros(size(Ta)) -eye(K)];
b = [ones(20,1)-Bv;ones(20,1)+Bv;ones(20,1)-Ba;ones(20,1)+Ba; zeros(k,1)];
%% Solve the optimization problem
J = quadprog(H,F,A,b);

  如果需要加更多的松弛变量的话，可以试着自己推导一下要如何得到相应的 $H、F、A、b$ 矩阵。
有了这 4 个矩阵之后，就可以依旧用二次规划来求解，下面是带软约束 MPC 的实际效果。

  这两组参数的权重都是一样的，唯一不同的就是速度的初始条件：

• 第一个初始速度 $v=0$，不违反任何约束条件。
• 第二个初始速度 $v=-3m/s$，违反约束条件：速度必须要在 $\pm 1m/s$ 之内。

  如果初始状态不违反任何约束条件时，软约束的 MPC 和硬约束的 MPC 解出来的结果是一样的。

  当初始状态违反约束条件时，由于对违反约束条件的权重给的非常高，软约束的 MPC 会先把违反约束条件的状态控制到约束条件范围内，然后才开始把三阶积分器的所有状态控制到状态空间的目标位置。

e）软约束的使用场景

那么在设计 MPC 时，哪些约束条件要考虑成软约束，哪些约束条件要考虑成硬约束呢？

• 一般的来讲，所有涉及状态的约束，都设计为软约束。
  这是因为状态往往会受到测量噪声以及外界干扰的影响，使得它的初始值一来就已经违反了我们的约束条件，所以我们需要软约束，以避免我们的优化问题没有解。

• 输入的不等式，设计为硬约束
  控制器的输入是不会受到测量噪声和外界干扰的影响的，可以随意变动，所以一般把它考虑为硬约束。
  而且如果输入的信号超过了它的约束范围，会对机械系统带来损伤。

---

三、线性MPC的缺陷

  讲线性 MPC 主要是希望大家有一个对 MPC 基本了解，但是线性 MPC 在实际使用的时候往往也是有一些缺陷的。

1、要求系统模型是线性或可被线性化的

  对于无人车模型，它本身不是线性的，需要进行线性化，在每一个工作点线性化得到的 MPC，叫做 自适应MPC(Adaptive MPC)，对自适应MPC感兴趣的同学可以看 MATLAB MAC toolbox 里相关的教程。

2、对障碍物的处理不理想

  我们看下面这张图：

  蓝色区域是一个由五条直线拼起来的五边形，每条直线把这个平面分成两部分。
  第一条直线所带的约束条件 $A_{1}x<b_1$ 指的是这条直线右下部分, $A_{2}x<b_2$ 指的是这条直线的右上部分，类似的，对于 $A_{3}x<b_3$ 指的是这条直线的上半部分，$A_{4}x<b_4$，指的是直线左上部分，$A_{5}x<b_5$指的是直线左下部分。
  当我们把这五个约束条件合在一起，形成一个大的约束条件，叫做 $Ax<b$ 时，所指的空间就是这个多边形的内部。

但是如果这个多边形本身是一个障碍物，我们希望车辆的轨迹在这个多边形的外部，此时的约束条件是要求
车辆在 $A_{1}x>b_1$ 这条直线以外
或者在 $A_{2}x>b_2$ 这条直线以外
或者在 $A_{3}x>b_3$ 这条直线以外
或者在 $A_{4}x>b_4$ 这条直线以外
或者在 $A_{5}x>b_5$ 这条直线以外

  需要注意到，当这些条件组合起来时，我们用了很多的 或，或操作并不像与操作可以形成一个 $Ax<b$ 这样凸空间的约束条件，而当涉及到“或”这个条件时，它所代指的约束条件往往就是非凸的，这样就不再能用凸优化的方法来解这个优化问题了，我们需要用到非线性MPC才能解一个非凸的优化问题。

  现在很多非凸的优化算子，比如 SQP(sequential cragatic programming) 算子，在解非凸优化问题时，它对 初始猜测(initial guess) 非常敏感，如果初始猜测不好，有可能就解不出来，或者解出一条非常不符合逻辑、不优的曲线。

  在后续的文章中会介绍非线性MPC及其相关的非凸优化算子。

---

参考资料

  移动机器人运动规划

---

后记：

🌟 感谢您耐心阅读这篇关于 线性MPC 的技术博客。 📚

🎯 如果您觉得这篇博客对您有所帮助，请不要吝啬您的点赞和评论 📢

🌟您的支持是我继续创作的动力。同时，别忘了收藏本篇博客，以便日后随时查阅。🚀

🚗 让我们一起期待更多的技术分享，共同探索移动机器人的无限可能！💡

🎭感谢您的支持与关注，让我们一起在知识的海洋中砥砺前行 🚀