python路线寻优_使用模型预测控制和PID实现自动驾驶的车道保持

本文为 AI 研习社编译的技术博客，原标题 ：

Lane keeping in autonomous driving with Model Predictive Control & PID

作者 |  Jonathan Hui

翻译 | 韧明

校对 | 酱番梨        整理 | 菠萝妹

原文链接：

https://medium.com/@jonathan_hui/lane-keeping-in-autonomous-driving-with-model-predictive-control-50f06e989bc9

照片来自 Campbell Boulanger

使用模型预测控制和PID实现自动驾驶的车道保持

(视频)黄色的路径是目标轨迹，绿色的路径是我们的汽车如何使用MPC移动。

自动驾驶的3大核心科技是定位(在哪里)，感知(周围是啥)以及控制(咋开车呢)。通过车道检测，我们可以对车的行进路线进行路径规划。本篇文章主要通过一个自行车的动力学模型讨论车辆的加速、刹车和转向的模型预测控制。目的不仅在于尽可能地控制车辆轨迹，同时也还要尽可能使速度平滑以避免晕车和频繁的刹车。

模型预测控制主要在约束条件下使损失函数最小。例如，我们想要以100ms的周期调整转向和速度，在转向角度不能超过25°的约束下，最小化以规划的路径和实际路径之间的误差。我们通过传感器获取车辆的状态，比如速度，而我们的动作基于传感器读数以一个短的周期执行(例如1s)。例如，我们顺时针转向20°，然后每100ms周期减小1°。加入这些动作可以1秒钟之后的损失函数最小，我们将会采用第一个动作：顺时针转动20°，但是却并不执行后续的动作，而是在100ms后，重复优化过程。100ms后，有了新的读数，我们就重新计算下一个最优动作。模型预测控制通过预测接下来一段较长时间(1s)的损失函数，来计算选择出下一个较短周期(100ms)的最优动作。相比于短视的贪心算法，模型预测控制更加健壮，因此能够控制得更好。

本篇是自动驾驶5篇系列之一，这一系列包括：自动驾驶感知：基于卡尔曼滤波得传感器数据融合

自动驾驶感知：扩展卡尔曼滤波和无损卡尔曼滤波

自动驾驶定位：基于粒子滤波的定位

自动驾驶控制：模型预测控制和PID控制

自动驾驶路径寻优双轮运动学模型

首先，我们定义一个模型来描述我们的车辆。

(图源：http://www.me.berkeley.edu/~frborrel/pdfpub/IV_KinematicMPC_jason.pdf)

(x, y)是车辆的质心，ψ是当前车身的角度，v是当前车辆的速度，lf是当前车辆质心到原点的距离， β是速度和车身的角度。在我们的例子中，我们假设β为零，也就是没有侧滑。

在我们的模型中，我们可以通过控制前轮的转角δf 以及车辆的加速度a来控制车辆轨迹。简单起见，我们只考虑前轮驱动的车辆，并且将δf记作δ。模型预测控制的细节

每个控制周期，我们都从传感器读取数据并得到车辆状态量：车辆的位置(x，y)

速度v

车身角度 ψ

转向角(舵角) δ

加速度a

轨迹模型：

我们的道路检测系统应该能够为我们规划好路线，比如，以接下来6个航点的坐标的形式。在我们的例子中，我们使用6个航点去逼近一个3阶多项式函数。我们用这个模型去计算y坐标和相对于x轴的车身角度ψ。

动态模型：

接下来，我们要创建动态模型利用t时刻的状态去预测在t+1拍时刻的车辆状态。利用动力学模型，我们可以轻易地从最新时刻地采样推导出下一时刻的位置，车身角度和速度。

我们可以在添加另外2个状态去衡量轨迹跟踪误差和车身角度误差ψ：

损失函数：

在模型预测控制中，我们需要定义损失函数来优化路径。如果模型不能保持目标速度，那么我们就要惩罚模型。如果可能的话，我们并不想要突然的加减速或者突然的转向。但是既然这些实际上是不可避免的，我们可以尽可能地抑制加减速和转向地变化率。这减轻了晕车同时更加省油(也省人民币)。模型的损失函数应当包含：雷锋网(公众号：雷锋网)跟踪误差

转向误差

速度损失函数项(尽量保持在100英里每小时)

转向损失函数项(尽量避免转向)

加速度损失函数项(尽量保持0加速度)

转向变化率(越小越好)

加速度变化率(越小越好)

因为这些目标也许会相互冲突，我们需要给这些损失项定义权重以体现优先级。损失函数如下：

总而言之：

我们需要用模型预测控制来寻找最优路径，那么就需要动力学模型来预测下一拍的状态，以下是动力学模型和系统约束：

优化模型预测控制

我们通过解决一个约束条件下优化损失函数的问题来解决了控制问题。这些约束条件包括油门和转向的控制。从道路中检测下6个航点，并且计算3次插值的来建立行驶轨迹

从传感器读取当前速度v,  方向ψ,  转向角 δ 以及加速度  a

使用传感器读取的数据和动力学模型计算出第一个车辆状态

根据1秒内的车辆状态响应优化控制动作，控制的周期为100ms，所以1s内有10个周期

模型预测控制的两个变量(也是控制量)：加速度(油门对应正加速度，刹车对应负加速度)和转向角

给出加速度和转向角的约束范围

我们将动态模型计算9次，得到未来9个时间拍的系统状态

给出每个采样计算周期的损失函数

用1个优化器解算出在约束定义下周期1到周期9的最小总损失(注意，在我们的定义中，时间周期并不从0开始，而是从1开始到10)

我们仅仅选择周期1给出的控制量

但是，我们延时100ms后再将控制量给模拟器。这样能够模拟现实世界，毕竟处理计算(读取传感器)和执行都需要时间。

从步骤1开始重复，寻找下一个最优控制量。

可调性：

在我们的例子中，我们计算了1秒中内的最优解，这个参数是可以调节的。长时间窗口的优化会给控制器的动作漂亮的曲线，但是也会积累过多的误差。实际上，如果这个优化时间窗口太大，汽车反而会脱离期望轨迹。

对于那些数学大神，以下有一个介绍自动驾驶会怎样出错的视频(原文并没有给出->_->)。雷锋网

PID

比例-积分-微分(PID)控制器是另一种在无人驾驶中应用的控制器，下方视频解释了它的工作原理。

在我们的例子中，控制函数由以下组成：交叉跟踪误差的比例部分

用于平滑运动过程的交叉跟踪误差微分部分

用于消除稳态干扰的交叉跟踪误差积分部分