>
Home

登龙（DLonng）

选择大于努力

自动驾驶规划算法 - MPC 基本原理

版权声明：本文为 DLonng 非原创文章，可以随意转载，但必须在明确位置注明出处！

本文非原创，整理自文末参考博客，用于自己的学习使用。

模型预测控制 MPC

基本原理

模型预测控制（Model Predictive Control）指一类算法，周期性基于当前帧测量信息在线求解一个有限时间开环优化问题，并将结果的前部分控制序列作用于被控对象。

模型预测控制（MPC）是一种致力于将更长时间跨度、甚至于无穷时间的最优化控制问题，分解为若干个更短时间跨度，或者有限时间跨度的最优化控制问题，并且在一定程度上仍然追求最优解。模型预测控制由如下三个要素组成：

  • 预测模型：预测模型能够在短时间内根据历史信息、当前输入很好地预测系统状态的变化
    • 需要一个模型能够基于历史信息和当前状态，来预测未来输出，这就涉及状态量的描述，非线性模型的线性化，从而确保预测输出最大限度接近期望值
  • 在线滚动优化：通过某种最优化算法（QP）来反复优化未来一段短时间的控制输入，使得在这种控制输入下预测模型的输出与参考值的差距最小
    • 由于外部干扰，模型系统误差等原因，预测输出与实际存在偏差，滚动优化要做的就是找到每个时刻下的局部最优解，一般会设计一个损失函数，转化为二次规划问题，找到最优解
  • 反馈校正：基于测量对模型预测进行修正，并在下一个时间点根据新的状态重新进行预测和优化

3 个框图类似，对比理解下：

预测模型

低速车辆使用运动学自行车模型作为预测模型：

各个状态量的更新公式如下（公式 3 和最后的贝塔怎么算？）：

基于以上的公式，在给定一个控制指令的情况下，我们的这个预测模型能够根据运动学的规律计算出 dt 时间以后车辆的状态 (x, y, ψ, v) ，这个预测模型（自行车模型）本身是建立在一定的假设前提的，所以计算出来的状态只是理论上车辆的可能状态。如下图是一条 S 路，图中的虚线是我们的参考线，我们控制的目标是让车辆尽量沿着参考线行驶：

我们选取 10 个 dt，假设 dt=0.05s ，那么根据预测模型，在已知一组控制输入的前提下，我们可以计算出车辆在未来 0.5s 的状态（本质上是一些离散的状态），如下图中的红点所示：

滚动优化

1 损失函数

为了得到最优的控制输出，可以把问题转化为最优化问题（二次规划），优化的目标是找出一组控制量（油门系数、刹车系数和方向盘转角），使得我们的 Loss function 最小化，首先定义损失函数（Loss Function）：

横向偏移误差：轨迹和参考线的差距

首先想到的一个损失函数就是我们的模型预测的轨迹和参考线之间的差距， 即 CTE（Cross Track Error）：

速度误差：当前速度和期望速度的差距

如果不仅仅想控制车辆按照参考线行驶，还想控制车辆在这个短时间范围内在每个点上的速度，那么就可以在损失函数添加一项速度的平方差：

刹车/油门调节量：油门刹车系数不突变

损失函数还可添加许多项使控制变得更加平滑，比如我们希望油门系数不要突变（即缓慢的才油门和刹车，这样乘客的体验会更加舒适），那么我们就可以将前后两个油门系数的差值的平方作为一项加入到损失函数中：

其他损失项：

  • 航向角变化率，相邻时间间隔的航向角变化量
  • 加速度变化量，描述相邻时间间隔，加速度的变化快慢

2 如何调整权重？

  • 我们想要输出满足什么要求，越重要的参数权重越大，反之依然，甚至不约束
    • 比如横向偏移误差权重最大，因为必须保证能够跟踪参考轨迹
  • 权重差异不能过于悬殊，本身数据间存在量纲差异，既然没有进行归一化处理，那就特别注意权重间的权衡

3 约束条件

设定了损失函数，车辆已经能够计算出可行的结果，但结果的可行性却还有待考究，这就需要约束条件，作用有 2 个：

  • 确保结果的实际可行性
    • 满足损失函数最小的最优解不一定模型能够执行，而加上约束，结果就一定是在车辆模型执行范围内的结果
  • 约束条件缩小了状态空间范围
    • 二次规划等最优解算法往往需要反复递归迭代得出结果，状态空间越小，计算时间也将缩减

MPC 中需要约束的是:

  • 车辆前轮的转角范围 $\delta$
    • $-25° <= \delta <= 25°$
  • 车辆油门系数 $\alpha$ 的取值范围(-1 表示慢刹车，1 表示满油门)：
    • $-1 <= \alpha <= 1$

4 求解器

  • CppAD::ipopt（内点法）
  • OSQP(ADMM，最快，Apollo 使用的)
  • OOQP(什么方法？)

反馈矫正

如图所示是MPC的简化示意图，可以看出：

  • MPC 本质上还是一种反馈控制
  • 我们通过最优化方法得到一组控制输出以后（比如未来 10 步的控制输出），车辆执行控制指令，并且继续以一定的频率反馈当前车辆的状态 $z_t$
  • 这个状态 $z_t$ 会被同时输入到路径规划模块以及 MPC 模块
    • 路径规划模块会依据新的车辆状态，结合感知模块的信息以及地图信息重新做出规划（新的参考路径）
    • MPC 则根据新的参考路径和车辆当前状态进行新一轮的预测控制

需要注意的是：

  • 车辆真实状态的反馈并不是一个预测时间段的控制执行完以后才反馈的
  • 反馈的时间间隔往往小于一个预测时间段
    • 在本文的例子中，预测时间段长度为 0.05×10=0.5s
    • 状态的反馈时间小于 0.5s

MPC 参数选择

选择一个好的参数不仅影响 MPC 控制的性能，而且还会影响到 MPC 每一个 timestep 内进行在线优化的计算复杂度。这里将会给出关于控制器采样周期、预测及控制范围(prediction and control)的参数的影响和选择。

采样周期的选择

  • 采样周期过大，则系统反应过慢导致难以及时进行修正控制
  • 采样周期过小，则会导致系统产生大量的在线优化计算，给系统带来较大的开销
  • 建议采样周期设计采用开环响应时间(10~90% 上升时间)的十分之一或二十分之一

预测范围(prediction horizon)的选择

  • 预测范围指的是一次优化后预测未来输出的时间步的个数（轨迹点的个数？）。
  • 建议范围：在开环响应时间内采样 20-30 个样本的范围

控制范围(control horizon)的选择

  • 过小的控制范围，可能无法做到较好的控制
  • 较大的控制范围，比如与预测范围相等，则会导致只有前一部分的控制范围才会有较好的效果，而后一部分的控制范围则收效甚微，并带来大量的计算开销
  • 建议控制范围应该在预测范围的 10~20% 之间，最小值为 2~3 个 timestep 时间步

这个参数不是太理解，可能是对优化后的控制量的选择执行范围，比如把优化后的前 2 个控制实践步的状态发送给车辆执行。

PID 控制的问题

高延迟

  • 在实际的车辆控制中，车辆往往不能立刻执行 PID 的指令，这其中存在一定的延迟，也就是说当前状态发出的控制指令会在未来的车辆状态被执行
  • 比如在当前状态（很急的弯道）去执行刹车指令就会有一定的危险性（由于延迟可能导致车辆刹车太晚，发生事故）

多输入多输出系统使得 PID 参数整定困难

如下图所示的多输入多输出系统，虽然也可以采用多个 PID 控制器进行控制，但是往往由于各个控制回路之间存在耦合关系，其 PID 参数的整定变得非常困难

MPC 的优点

可以处理延迟

  • MPC 采用了一个折中的策略，不是只考虑当前状态，而是考虑未来的有限时间域，一定程度上牺牲了最优性
  • 在进入很急的弯道之前（考虑未来几个时间步）就开始发出控制指令，如果等到车辆行驶到弯道再做动作的话（PID）势必会晚了一些

可以处理 MIMO 系统

MPC 可以非常方便考虑多输入多输出系统，用一个 MPC 控制器就可以解决

可以处理约束

  • 现实世界中往往存在各种各样的约束条件
    • 对车速有约束（因为有交通法规限速）
    • 对加速度也有约束（因为发动机的负载是有限的）
    • 跟踪参考路径约束
    • 油门系数不突变（缓慢踩油门和刹车），将前后 2 个油门系数的差值平方作为一项损失
  • 由于模型预测控制是通过构建优化问题来求解控制器的动作的，所以可以非常自然的将这些约束建立在优化问题中以此来保证这些约束的满足

MPC 的缺点

  • 计算量：需要较好的处理器和较大的内存，因为需要大量的在线优化，存储大量的变量
  • 实时性：模型预测控制需要在每一个时间步迭代求解优化问题，而优化问题的求解往往比较耗费时间，但是控制器又对输出动作的实时性的要求比较强，这样就会产生一个矛盾，这也是目前模型预测控制的一个主要缺点。

MPC 基本步骤总结

通过使用不同的预测模型和损失函数，可以构造出各种模型预测控制器，总的来说，模型预测控制往往可以分解成如下 4 步：

  • 从 t 时刻开始，预测未来 a 步系统的输出信号（采样多组还是一组?）
  • 构造并最优化损失函数
  • 将最优控制信号输入到系统
  • 等到下一个时间点，在新的状态重复步骤 1

一些思考

  • 模型预测控制实际上是以优化方法来求解控制问题，或者说是以优化问题的求解来给出控制器的输出动作
    • 所以说模型预测控制是外边套着一层控制，内部包含着优化
  • 模型预测控制需要在每一个时间步通过反复的预测 + 优化来求解优化问题
    • 当得到优化问题最优解后再将这个解作为真正的控制器的输出作用给被控对象

不懂的问题

  • 运动学模型部分公式

    • 非线性模型如何线性化
    • 学习车辆的自行车运动学和动力学模型

  • MPC 第一步与 DWA 的区别，是采样一组还是多组控制输入？

    • DWA 采样多组可能的状态，然后利用 cost 选择最优
    • MPC 只对当前状态进行 QP 优化

  • 为什么用 QP 优化 MPC？

    • 即为何把 MPC 建模为 QP 问题？
    • 为啥不用其他的？？？

  • 如何在代码里面构造 QP 问题并利用 QSQP 求解？

    • 学习 QSQP 求解器使用方法（C++ 封装接口，osqp-eigen，osqp-cpp）

      • 定义二次规划问题，损失函数

      • 必须转为标准 QP 问题，即从原始问题得到以下 5 个标准 QP 问题的矩阵

        • Hessian 矩阵 P
        • Gradient 向量 q
        • 线性约束矩阵 A
        • 下界向量 l
        • 上界向量 u

  • 初始化求解器 solver，设置上面的 5 个矩阵为求解的参数 - 迭代求解 QPSolution = solver.getSolution()

参考博客

本文原创首发于微信公号「登龙」，分享机器学习、算法编程、Python、机器人技术等原创文章，扫码即可关注！

DLonng at 04/16/21

‹ ›