【MPC】模型预测控制 | 在移动机器人运动规划中的应用（一）算法基础与常用工具

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

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

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

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

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

---

---

前言

  本文不会花太多时间讲优化问题的本质和数学方法，而是会向大家介绍一些优化问题的算子以及一些软件工具包等。

---

一、线性MPC和非线性MPC的区别

  线性MPC所解决的优化问题通常为凸优化问题；
  非线性MPC解决的优化问题一般是一个非凸的优化问题。

我们之所以讲线性MPC，是希望它可以达到一个加强大家对 MPC 理解的一个作用，但是大部分工程问题，特别是运动规划问题中都需要使用到非线性 m p c。

  不管是线性 NPC 还是非线性NPC，它们的核心都是一个解优化的过程。

---

二、模型预测控制的三大核心要点

  从它名字中的模型、预测、控制这三个方面来介绍。

1、模型

a）系统模型

  首先来说什么是模型，对模型可能最简单的理解就是，打个比方，现有一个质点，如果有一个力，它有自己的质量，那么就可以算出它的加速度 $a$，它还有位置 $p$，那么对位置求导 $\dot{p}$ 就等于速度 $v$，对速度求导 $\dot{v}$ 就等于加速度 $a$。
  所以如果我们知道了对于这个质点输入的力以及它当前的速度和位置之后，我们就可以完整地描述这个质点在未来的轨迹。这也就是我们常在机器人学里面提到的所谓 车辆模型。

b）问题模型

  那么什么叫问题模型呢？
  打一个比方，如果我们现在这个质点，它有一个目标，就是到红色的这个叫 $p_d$ 的点那里。那么现在我们要最小化的就是 $p$，也就是说这个蓝色的点自己的位置到 $p_d$ 的距离。那么我们把这部分叫做问题模型(Problem model)，我们把之前描述问题本身的运动方式的叫做系统模型(System model)。
  需要注意的是，在模型预测控制中，这个模型往往是涵盖这两部分的。有些时候这个系统模型可能非常简单，以至于大家觉得可以用一些非常简单的线性方法来解决这个模型预测问题，但是这个问题模型可能非常复杂，也就是说它的优化目标可能非常复杂，于是我们还不得不用一些非线性的方法来解决这个问题。

2、预测

  我们需要知道给定一定的输入(input)，在这儿就是这个力 $F$ ，之后的其他状态，也就是说现在的 加速度 $a$ 、速度 $v$ 和 位置 $p$ 会怎么变化？这里特别需要注意的是 参数空间(Parameter space) 。
  参数空间的存在意义在于，因为现实世界是连续的，打一个比方，未来 $3$ 秒之内力的变化，可以有无限多种，这样的话很难做出一个优化问题。在现在的科学技术下，这种连续的优化问题很难解，因为它的本质是一个无限维度的问题，为了把这无限多个维度变成一个有限维度的一个问题，我们需要把力的变化放到一个参数空间里进行表达。
  打一个比方，对于未来 $3$ 秒之内力的变化，我们可以用一个有限阶的多项式来描述这个力的变化，那么在优化的时候我们就去优化这些多项式的参数，来得到一个想要的力。

3、控制

  最后一部分就是控制，其实本身就是在这个参数空间里面选取一个最好的参数的过程，也就是我们要怎么选取一个力的变化过程，使得这个质点到它想要去的位置 $P_d$ 之间的这个距离可以最小化。

---

三、模型预测控制的经典描述

1、目标函数

  这个描述适用于大部分的移动机器人。
  首先我们看它的问题模型，这个问题模型也就是它的优化目标分成两部分，第一部分叫 终端代价(final cost)，第二部分叫 运行代价(running cost)。
  我们先看这个运行代价，运行代价只关心 $t$ 从 $t_0$ 到 $t_f$ 之间这个时间段之内的运行代价。在每一时刻这个运行代价是关于这个系统状态的一个函数。
  同理，终端代价是当 $t$ 达到 $t_f$ 之后，也就是到最后那个时间点上的时候，有关于系统状态的一个函数。

2、四大约束条件

a）动态约束(Dynamic constraint)

  这一约束描述了系统的动态模型，像刚才提到的位置的导数等于速度，速度导数等于加速度等等，这个都归到这一类里面。

b）不等式约束(Inequality constraint)

  对于不等式条件，也就是说 $g(x,u)<0$ ，对于机器人来讲，它有最大速度限制、最大加速度限制、最大跃度(jerk) 限制等等，都归到这一类里面。

c）等式约束(Equality constraint)

  比如机器人当前的状态从一个状态点起始，终止于另一个状态点，就归到这个等式条件里面。

d）防撞约束(collision avoidance constraint)

  这是一类特殊的约束，主要作用是使机器人不撞到障碍物，其实这一类约束往往是可以归类于不等式条件里面的。但是由于它的性质比较特殊，它不像最大速度、最大加速度这类约束可以被写成一个凸优化的约束，而对于这一类防撞约束，因为障碍物的天然的特性，它往往都是非凸的，所以为了使解优化问题的实时性得到提升，我们往往需要对防撞约束使其不撞到障碍物的运动进行一些特殊的处理，所以它是最特殊的。

---

四、参数空间

1、引入参数空间的意义

  参数空间的重要性，它在预测这个轨迹的时候是一个非常重要的工具。那么像前面提到的，我们的最终目标其实是要每一步解决一个这样的优化问题，那么这个优化问题中，我们可以自由改变的变量是系统的输入 $u$ ，换句话说，我们就是要找到一组最佳的输入 $u$ 来使这个表达式最小化。
  但是在工程问题中，这个系统输入往往是一个随意变化的量，它是有无限维度的。但是广义上的无限维度优化问题，用现今的计算机是非常难解的，因为现在的计算机都只能解决有限维度的问题。那么我们就需要找到一个方法，把这个无限维度的系统输入变成用有限维度的参数来进行描述。

2、参数化方法

a）零阶保持法

  最传统的方法就是 零阶保持 (Zero order hold, 直接离散)，学过信号处理的同学应该非常熟悉，它其实就是把系统输入，按照一定的采样频率来进行直接的离散。如果给定一个固定的时间段，比如从 $t_0$ 到 $t_f$ 给定一个固定的采样频率，我们就能得到有限段的这样的离散出来的输入信号。
  比如像下面这幅图，共有 $13$ 个时间段，于是我们就需要调整这 $13$ 段输入来优化之前提到的这个目标函数。

b）多项式法

  第二种方式我们可以考虑使用一些 多项式(Polynomial) 或者 样条(B-spline)，这两种方法可以把未来一段时间内的输入表示成一个多项式，比如表示成一个三阶的多项式：
  上式共有 $a、b、c、d$ 这 $4$ 个参数来描述它未来的走向。在这种情况下，我们就需要调整 $a、b、c、d$ 这 $4$ 个参数来使相应的 $u(t)$，也就是输入的轨迹能够最小化我们的目标函数。
  在后续的文章中会介绍两种非线性的方法来建立这个参数空间，分别是基于一些数值解或者是机器学习的方式，比如用 神经网络 的方式来建立这个参数空间，目的主要都是进一步减小这个参数空间的维度，使得这个优化问题更容易解。

3、参数寻优方法

  参数空间一旦被建立好之后，我们就要考虑如何去找最优的参数，也就是优化问题本身。这里我们常用的方法有三个：

a）直接搜索法

  第一个当然是直接搜索，可以使用 图搜索(Graph search) 或者 随机采样(random sample sampling) 的搜索的方式来找这个最优的参数。

b）凸优化法

  第二种方法就是使用凸优化的工具，比如 二次规划(Quadratic programming)。

c）非凸优化法

  对于一些很难变成凸优化的问题，我们就不得不使用一些非凸优化的工具，比如 序列二次规划(Sequential Progratic Programming,SQP)，但在后面的文章里会主要讲到 粒子群优化(Particle Swam Optimization,PSO) 这个工具，它的最大的好处是不仅可以解决非凸的问题，还可以解决不连续的优化问题。

对于线性 MPC ，主要使用 二次规划(Quadratic programming,QP) 方法。
对于非线性 MPC ，主要使用 粒子群优化 (particle swam optimization,PSO) 方法

  这里需要大家知道：没有一种优化算法是万能的，我们需要根据不同的问题来找到最适合的优化算法。

---

五、模型预测控制算法流程

  有了以上基础知识后，我们先来看一下整个模型预测控制的框图，下面顺着这个信号流的顺序来解释。

Step 1: 优化目标设定

  我们会有一个 User，比如说人，或者一个更上层的程序来设置这个优化问题。我现在的优化目标是使得我的机器人去一个固定的点，那么就要告诉它这个点的位置到底在哪里。但是光靠这一部分信息，优化问题的设置还没有完成。

Step 2: 初始状态测量

  我们需要一个测量模块，告诉我们当前机器人的状态。我们把这个状态当作是优化问题的初始状态。这个初始条件会体现在等号约束条件里。

Step 3: 优化问题求解

  用适当的方法去解这个优化问题，会得到一系列的参数，这些参数最终会被翻译成系统的输入 $u$，我们管这一组最优的 $u$ 叫做 $u^{\ast }$。

Step 4: 模型滚动循环

  接着我们就会用这一组 $u^{\ast }$ 驱动我们的系统。但是由于模型不可能 100% 准确，有模型不确定性以及外界干扰的介入，我们的系统是不可能靠一组固定的 $u^{\ast }$ 来镇定的。为了解决这个问题，我们只使用 $u^{\ast }$ 很短一段的时间，比如只使用 $0.1$ 秒。然后利用当前观测到的状态重新设置优化问题，也就是回到 $Step2$，由 $Step4$ 回到 $Step2$，接着我们重新解这个优化问题，得到一组新的 $u^{\ast }$ 。然后同样我们也只使用这个 $u^{\ast }$ 作用很短的一段时间，也就是 $0.1$ 秒，然后周而往复。

  这个过程叫做 滚动时域控制(Receding Horizon Control,RHC) ，是大部分模型预测控制 MPC 的基础。

---

六、模型预测控制算法的改进

1、传统MPC的问题

  传统 MPC 的主要问题在于，如果你的系统要求控制频率非常高才可以镇定，比如说是一个四旋翼的内环，那么它要求 $200-1000Hz$ 的控制频率，这要求优化问题在非常短的时间内得到求解，但这对于四旋翼上面可用的计算资源来说往往都是不现实的。

2、Tube-based MPC

  那么为了解决这个问题，有人发明了一种新的框架，叫做 Tube-based MPC，这个框架就和我们熟悉的 运动规划轨迹生成(Motion Planning Trajectory Generation) 的框架非常类似。在这个框架里面多了两部分内容，第一个是叫 标称系统模型(Nominal System) 的东西，第二个是叫 关联控制器(Associate Controller) 的东西。

a）标称系统模型(Nominal System)

  这个标称系统模型其实就是优化问题中的系统模型 $\dot{x}=f(x,u)$。那么在这个新框架下面第一部分，我们是只对标称系统模型进行控制，因为这个标称系统本身只是一个数学模型，不涉及任何的模型不确定性和外界的干扰，这样我们就可以一个非常低的频率对这个标称系统进行控制。
  这个标称系统的输出 $x^{\ast }$ ，我们把它叫做 标称状态(Nominal State)。我们的期望就是系统的真实的状态能和标称状态保持一致，于是系统的第二部分就引入了一个关联控制器去跟踪这个标称状态。在这里我们也可以把标称状态叫做参考信号。

b）关联控制器(Associate Controller)

  这个关联控制器往往可以化作一个线性控制器，或者简单的非线性控制器，或者 MPC 控制器皆可。它的作用是处理模型的不确定性以及外界的干扰来镇定系统，使它可以跟踪这个标称状态，也就是参考信号。

  这个系统的主要优势在于它可以以极低的频率来解这个优化问题。从而节省计算量。但它又可以以比较高的频率来对真实的系统进行控制，使真实的系统得到镇定。目前在大量的无人机、无人车、机器人、机械臂等方面都有大量的应用。它之所以叫做 Tube-based MPC，主要原因是这个关联控制器可以使系统真实状态镇定在参考信号周围一定的区间之内，这个参考轨迹会在状态空间形成一个管道，只要系统的真实状态待在这个管道之内，我们的控制就算成功了。

具体关于 tube based MPC 的理论问题，可以看这篇参考文献：
Tube-Based MPC:a Contraction Theory Approach

---

七、有关MPC的资料与工具

1、Matlab MPC toolbox

Matlab MPC toolbox
  只要有 MATLAB 就可以下载并使用，这个 Toolbox 内置了大量的 MPC 教学视频，从 线性 MPC 到 自适应MPC(Adaptive MPC) 到 非线性 (Non-linear MPC) 等等。

2、$\mu$AO-MPC

$\mu$AO-MPC
  它的性质和 Matlab MPC toolbox 类似,，这两个 Toolbox 都可以做代码生成，其实第三个工具 Acado toolkit 也可以做代码生成，也就是说只要有一个用 Matlab 或者 Acado toolkit 或者 $\mu$AO 的语言写好的 MPC 设计，就可以把它一键生成为 C/C + +，或者其他单片机代码的 MPC 软件，这些软件就可以移植在自己的代码库中进行二次开发。这些软件在一键生成的时候都非常高效，不需要再链接其他第三方的软件。

3、Acado toolkit

Acado toolkit
  Acado toolkit 和 YANE 主要解决的是非线性 MPC 问题。 Acado 中最好的功能是它实现了实时迭代，这个功能把复杂的非线性优化问题分布在多次 MPC 迭代中解决，并且关于它的镇定稳定性等等都有非常全面深入的分析。

4、YANE

YANE
  YANE这个工具包集成了大量的非线性 MPC 算子，可以通过它来测试哪一个非线性 MPC 算子对于你的问题效果最佳。

5、Multi-Parametric Toolbox 3

Multi-Parametric Toolbox 3
  这个工具包主要实现的是显示 MPC 的方法，不同于前面讲到的四个Toolbox ，都是基于数值优化来解决 MPC 问题的，这个 mpt3 Toolbox 可以给常见的二次规划找到一个基于查表的解析解，所以它非常迅速、非常高效。

---

参考资料

  移动机器人运动规划

---

后记：

🌟 感谢您耐心阅读这篇关于 算法基础与常用工具 的技术博客。 📚

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

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

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

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