MPC和PID实现自动驾驶车道保持原理
MPC和PID实现自动驾驶车道保持原理
前言
本文主要通过一个自行车的动力学模型讨论车辆的加速、刹车和转向的模型预测控制。目的不仅在于尽可能地控制车辆轨迹，同时也还要尽可能使速度平滑以避免晕车和频繁的刹车。
**模型预测控制主要在约束条件下使损失函数最小。**例如，我们想要以100ms的周期调整转向和速度，在转向角度不能超过25°的约束下，最小化以规划的路径和实际路径之间的误差。我们通过传感器获取车辆的状态，比如速度，而我们的动作基于传感器读数以一个短的周期执行（例如1s）。例如，我们顺时针转向20°，然后每100ms周期减小1°。加入这些动作可以1秒钟之后的损失函数最小，我们将会采用第一个动作：顺时针转动20°，但是却并不执行后续的动作，而是在100ms后，重复优化过程。100ms后，有了新的读数，我们就重新计算下一个最优动作。
模型预测控制通过预测接下来一段较长时间（1s）的损失函数，来计算选择出下一个较短周期（100ms）的最优动作。相比于短视的贪心算法，模型预测控制更加健壮，因此能够控制得更好。
MPC控制方法
自行车动力学模型
（x, y）是车辆的质心，ψ是当前车身的角度，v是当前车辆的速度，lf是当前车辆质心到原点的距离， β是速度和车身的角度。在我们的例子中，我们假设β为零，也就是没有侧滑。
在该模型中，我们通过控制前轮的转角δf 以及车辆的加速度a来控制车辆轨迹。为了简单起见，只考虑前轮驱动，并且将δf记作δ。
在每个控制周期内，也就是一次循环内，我们都能够通过传感器获取到当前的车身状态：
- 车身位置（x，y）
- 自车速度v
- 车身横摆角ψ
- 转向角δ
- 车辆加速度a
轨迹模型预测
我们按照事先规划好的roadpoint 前行，以接下来的5个roadpoint为例，我们使用6个点来去逼近一个3阶多项式，去计算y的坐标值以及相对于x轴的车身角度ψ。
动态模型方程
然后我们创建动态模型，来利用t时刻的状态去预测t+1状态时的车辆状态量是什么，通过动力学模型，我们可以算出t+1时刻的位置（x，y）、横摆角、自车速度等。
然后添加2个状态去量化轨迹跟踪误差和自车角度误差ψ：
损失函数计算
在控制中，我们需要明确的损失函数来计算当前状态误差，以方便跟随既定轨迹，减少误差，优化控制。如果模型无法保持目标状态，我们就需要对模型做一些惩罚性修改。例如我们不想车辆突然加减速，突然大幅度转向等等。
但是其实在生活中和实际中，这些情况有时候不可避免，所以我们应该尽可能的抑制或者控制加减速和转向角变化率的出现，这样既可以大大提高车辆乘坐人员的舒适性，也可以保证车辆的平稳运转。
一个损失函数，主要包含以下几点参数：
- 跟踪误差（位置误差）
- 转向误差（航向角）
- 速度损失函数项
- 转向损失函数项（尽量避免转向，或者大幅度）
- 加速度损失函数项（尽量保持0加速度）
- 转向变化率（越小越好）
- 加速度变化率（越小越好）
然后我们需要将这些参数结合起来，按照不同的权重将其融合为一个综合损失输出。
我们需要用模型预测控制来寻找最优路径，那么就需要动力学模型来预测下一拍的状态，以下是动力学模型和系统约束：
优化模型预测控制
我们通过解决一个约束条件下优化损失函数的问题来解决了控制问题。这些约束条件包括油门和转向的控制。具体的操步骤：
从道路中检测下6个航点，并且计算3次插值的来建立行驶轨迹
1.从传感器读取当前速度v, 方向ψ, 转向角 δ 以及加速度 a；
2.使用传感器读取的数据和动力学模型计算出第一个车辆状态；
3.根据1秒内的车辆状态响应优化控制动作，控制的周期为100ms，所以1s内有10个周期；
4.模型预测控制的两个变量（也是控制量）：加速度（油门对应正加速度，刹车对应负加速度）和转向角；
5.给出加速度和转向角的约束范围
6.我们将动态模型计算9次，得到未来9个时间拍的系统状态
7.给出每个采样计算周期的损失函数
8.用1个优化器解算出在约束定义下周期1到周期9的最小总损失（注意，在我们的定义中，时间周期并不从0开始，而是从1开始到10）
9.我们仅仅选择周期1给出的控制量，然后延时100ms后再将控制量给模拟器。<这样能够模拟现实世界，毕竟处理计算（读取传感器）和执行都需要时间>。
10.从步骤1开始重复，寻找下一个最优控制量。
PID
比例积分微分控制同样可以用来控制无人车，它有以下几个部分组成，分别是：
- 交叉跟踪误差的比例部分
- 用于平滑运动过程的交叉跟踪误差微分部分
- 用于消除稳态干扰的交叉跟踪误差积分部分
