Apollo规划模块：基于二次规划（QP）的车辆路径规划（论文研读）

一、背景概述

存在挑战：
对于自动驾驶车辆，路径规划不仅需要计算安全，而且要计算舒适路径，即几何特性发生合适变化的路径，也必须满足车辆的非完整约束。在城市驾驶环境中，车辆经常需要在狭窄的通道中穿梭。这些因素使得自动驾驶车辆的路径规划成为一个挑战性问题。

解决问题
道路被两侧的许多车辆/障碍物堵塞，为生成符合车辆运动学且平滑的路径，避免在复杂环境中发生碰撞，提出了一种新的二次规划方法，该方法生成完全避免碰撞能力的最优路径。

背景知识
差分约束的车辆的配置空间包括三维，二维（x，y）用于指定车辆某个参考点的坐标，通常为后轴中心或质心，一维θ用于指定地图框中车辆的前进方向，即航向角。自行车模型假设车辆因车轮转向而行驶一周。除了（x，y，θ）之外，还将一个维度κ（即转向产生的圆的曲率）合并到配置空间中，以隐式表示转向角。四维配置空间提供了更精确的位姿描述，并帮助控制器模块更好地设计反馈控制。

路径规划的目标是计算映射参数p的函数q∈ [0，1]到特定配置（x，y，θ，κ），并满足一系列要求：1）无碰撞：路径不能导致车辆与障碍物碰撞。轨迹规划总体策略将碰撞避免分为静态和动态障碍物避免，路径规划旨在实现与静态障碍物的碰撞避免。2）运动学上可行：路径必须在车辆的运动学限制范围内，以便车辆能够实际跟随。

软要求：舒适性，减少方向盘的摆动、减少不必要的急转弯、

二、算法思想

（1）规划框架从笛卡尔坐标系转换为Frenet框架。将路径规划问题解耦为两个阶段：第一阶段，只关注从地图数据中获得可平稳驾驶的参考线，这是精确的地图帧转换的前提条件；第二阶段，重点关注路径生成的Frenet框架中的优化。

（2）提出 piecewise-jerk路径公式。表示根据参考线的空间参数使用密集离散点序列的Frenet帧中的路径，每个点包含到参考线的横向距离及其一阶和二阶导数。在连续点之间，使用恒定的三阶项拼接它们并保持路径的二阶连续性。该公式由于全局离散化和全局二阶可微允许获得更大的灵活性和平滑的路径。

（3）第三种策略是在 piecewise-jerk路径公式中使用优化来计算路径。优化过程在Frenet框架中从安全驾驶走廊中搜索安全且运动学上可行的解，通过考虑障碍物占用、道路边界等进行计算。

ps： 工作空间： 机器人有形状和大小，难以进行运动规划；配置空间：将机器人看成一个点。将机器人半径 r 对障碍物各向进行膨胀。

三、算法流程

1、Frenet框架下的路径规划

关键思想是将运动规划问题从地图框架转换为Frenet框架，Frenet框架是根据假设的平滑定义的参考线。对于结构化环境中的车辆运动规划，平滑的参考线可以是车辆目标要遵循的道路的中心线。

给定一条平滑的引导线，车辆在地图框架中的运动被解耦为两个独立的一维运动，即在Frenet框架中沿引导线的纵向运动s和与引导线正交的横向运动l。

此外，与将横向运动l表示为时间t的函数不同，另一种方法是将横向运动表示为空间参数s的函数。一阶和二阶导数l‘=dl/ds，l’’=d2l/ds2表示l 的一阶和第二阶变化率。该表示基本上定义了Frenet帧中的几何形状，即路径。因此，路径规划问题现在是寻找满足上述约束的函数l（s）。(为社么不是横向运动s(l)函数，理解是横向运动是纵向运动引起的)

2、参考线的生成和平滑

在Frenet框架中规划的前提条件是一条参考线。参考线表示为地图上的位置坐标序列，即p0 =（x0，y0），p1=( x1, y1), . . . , pn−1 = (xn−1, yn−1)。没有额外的几何信息，例如方向、曲率等。

（为什么要进行参考线平滑）参考线是地图和Frenet框架之间的“桥梁”，其平滑度极大地影响了计算路径的质量。根据Map和Frenet转换，为了准确地将路径映射到曲率水平，参考线必须一阶连续，即参考线必须在曲率导数水平上连续。
该文提出了一种基于优化的平滑算法，并在优化公式中考虑以下方面：

优化目标： 低且平滑的路径曲率不仅有助于减少控制器在路径跟踪中的不稳定性或过冲，而且还提高了驾驶体验的舒适性。在优化过程中，会对参考线曲率及其变化率进行惩罚。对于百度Aopollo平台针对的城市驾驶场景，始终鼓励车辆停留在车道上。因此，建议参考线靠近车道中心。

约束： 为了考虑可能的地图误差，允许输入点在一定程度上偏离其原始坐标，以实现可能更高的平滑度，同时保持线的原始形状。此外，为了确保安全，平滑的引导线必须保持在车道边界内。

过去的工作实现了一种用于参考线平滑的非线性优化算法。算法使用五次螺旋曲线序列对引导线进行建模，并计算一个使空间长度和几何特性波动最小的序列。该方法直接优化了直线的几何特性。

然而，螺旋曲线的使用在曲线形状及其位置属性之间形成了非线性关系。因此导致了一个困难且计算密集的非线性优化问题。

本文提出了一种基于快速二次规划（QP）的算法，获得了可接受的实证结果。在构造QP问题时，变量是n个输入点的位置变量。给定连续三个点pi−1，pi，pi+1，最小化从圆周率点到圆周率中点的欧几里德距离pi−1和pi+1。几何上，pi离中点越近，这三个点越直，因此曲率及其曲率变化率越小。允许这些点在一定程度上偏离其原始位置，以获得可能更直的直线。点的偏差也包括在目标函数中

参考线平滑的输出是具有优化位置的点序列。使用连续点位置之间的有限差分计算每个离散点上的几何信息。对于离散点之间的任何点，其几何信息使用线性插值近似。

3、路径边界决策与计算

路径优化是在Frenet框架中执行的。目标和约束是基于生成的参考线制定的。讨论如何为优化过程生成可行的搜索区域。路径的完整搜索区域可能包含一组几何同伦组，例如，从左侧或右侧通过一个静态障碍物形成两个组。选择最佳同伦群是一个非常重要的决策过程。文章提出了一种基于启发式搜索的决策策略，该策略考虑了道路数据（特定路段的可用车道）、ego车辆和静态障碍物的位置和几何信息，并建立了数学不等式，供后续优化过程使用。通过两个步骤实现：车道利用率决策和路径边界生成。

**A、车道利用决策：**计算出ego车辆要使用的道路车道。简单的方法是使用所有可用车道作为可驾驶区域。会带来如下问题：
（1）最终生成的路径可能不必要地跨越几个车道，会造成一定的风险。
（2）如果道路狭窄（例如在住宅区），且有一些其他障碍物阻塞了道路的一部分，我们需要临时借用相邻或反向车道通过。

利用基于规则的决策树，根据交通规则、ego车辆状态（速度、前进角度等）和障碍物信息（障碍物类型、位置等）确定要使用的车道。该模块输出一组可用车道，供ego车辆根据参考线的空间参数使用。

B、路径边界生成：
这一步是通过考虑车辆位置和周围障碍物，精细处理从上一步到特定边界的可用车道。将空间维度s离散为预定义的分辨率∆s、 沿着参考线。从第一个点s0开始，我们向前搜索。

如果si处没有障碍物，则可用车道的边界直接用作下限和上限横向边界，然后我们继续下一个点。

如果si处存在静态或低速障碍物，基于估计的自我车辆在si−1处的横向位置l和障碍物的可用间距，对可能的绕行方向进行排序。然后从最宽的可行值开始，搜索后面的sj。如果沿着这个选择的搜索在中间失败，回溯并尝试其他方向。

重复此过程，直到达到搜索范围（该文实现中为160m）。作为上述深度优先排序搜索的结果，路径边界可能是所有采样si的最合理边界。此处不考虑动态或高速障碍物，因为它们将由速度规划模块处理。

该步骤的输出是路径边界函数，该函数以空间参数s为输入，并输出ego车辆的安全边界（lmin，lmax）。

4、路径优化

这部分给定计算路径边界函数lB的Frenet框架中的详细优化公式。

A、最优性建模
在建模路径的最优性时，我们考虑以下因素：（1）无碰撞：路径不得与环境中的障碍物相交。（2）最小横向偏移：如果没有碰撞风险，车辆应尽可能靠近车道中心。（3）最大障碍物距离：最大化车辆与障碍物之间的距离，以便车辆安全通过。这个因素可以表示为车辆与其相应路径边界中心之间的距离。

B、公式
公式表示为加权和，如下：

该方法根据空间参数s将路径函数 l（s）离散化到一定的分辨率∆s，并使用这些离散化的点来控制路径的形状，并近似评估约束满意度。

思想是将一维函数离散到二阶导数水平，并使用恒定的三阶导数项来“连接”连续离散点。位置变量的三阶导数通常被称为加加速度。该公式被称piecewise-jerk。

优化变量是ｓ，ｌ，ｌ‘，ｌ‘’，这些空间参数控制路径的形状。在连续点之间，piecewise-jerk假设一个恒定的三阶项来连接它们。该项的值通过有限差分二阶项获得。
三阶项仅在两个连续点内恒定；在不同的连续点中，其他值是可能的。为了保持路径的连续性，在点i和i+1之间引入了额外的等式约束：

路径优化的完整分段加加速度是最小化的ｌ，ｌ‘，ｌ‘’。

C、运动可行性的可变边界估计
除了满足几何连续性和安全边界约束外，计算出的路径必须遵循车辆的运动学约束，才能在物理上行驶。由于运动学约束是在地图框架中定义的，因此由于复杂的地图-法语框架转换，很难在Frenet框架中直接实施该约束。该方法对Frenet框架中变量的数值边界进行了估计，以隐式地实施约束。

运动学可行性的最重要因素是路径的曲率。路径中一个点的曲率定义为以下方程：

为了简化，做出假设：（１）车辆几乎平行于行驶引导线，即假设车辆的航向角与相应点的引导线方向相同，因此∆θ=0。（2） 横向加速度ｌ’‘在数值上很小，假设为0。曲率近似公式如下：

给定车辆的运动学模型（见图2）和车辆的最大转向角αmax，可以计算车辆的最大曲率：
将l的线性约束添加到运动学可行性的优化程序中，如下所示：

四、应用场景

随机规划器（RRT等）：通用的方法，难以利用道路结构化信息，用于特殊场景如停车等。

离散搜索方法（State lattice search等），通过串联一系列预先计算的路径段或策略来计算路径。通过检查一个段的结束配置是否足够接近目标段的开始配置来完成连接。路径的形状严重依赖于网格的离散化，这限制了路径的灵活性，用于简单场景。

基于优化的方法，灵活；直接在全局/地图框架中运行二次规划，以直接计算轨迹（同时进行路径和速度规划）。轨迹在地图框架中被精细地离散化，这些离散化的位置属性被用作优化变量。优化过程迭代以找到最小化目标函数的轨迹，该目标函数结合了安全措施和舒适因素。优点是提供了优化建模的直接实施。此外，由于路径/轨迹被密集离散化以用作优化变量，这些方法实现了对路径/轨迹的最大控制，以处理复杂场景。

该文方法是将规划问题转换到不同的空间以降低规划复杂性。在Frenet框架中执行轨迹规划，该框架由平滑的驾驶参考线定义。车辆的运动被解耦为两个一维运动，即相对于引导线的纵向和横向运动。对于每个一维规划问题，以多项式的形式生成一组候选运动。

具体地，提出了一种新的基于优化的自主车辆路径规划方法，可用于一般道路驾驶场景（如道路被两侧的许多车辆/障碍物堵塞）。该方法将路径规划分解为两个主要阶段：第一阶段，驱动参考线平滑过程生成平滑线，这是在Frenet框架中规划的前提条件；在第二阶段，路径优化器使用分段jerk公式找到最优且运动可行的路径。利用了道路结构并实现了更清晰的场景理解。

在普通PC上计算效率高，平均总计算时间为40ms（用于引导线平滑的20ms，用于路径查找的15ms）。该方法在百度Apollo开放平台上发布，并已部署在硬件上，用于多种场景中的道路测试。

整理不易，欢迎点赞订阅，一起学习交流！

参考与推荐学习链接：
论文： Optimal V ehicle Path Planning Using Quadratic Optimization for Baidu Apollo Open Platform
作者： Yajia Zhang* | Hongyi Sun | Jinyun Zhou | Jiacheng Pan | Jiangtao Hu | Jinghao Miao

学习链接： 二次规划（QP）算法：https://zhuanlan.zhihu.com/p/325645742
代码：https://github.com/ApolloAuto/apollo/tree/master/modules/planning

图片来源于网络和参考链接，侵权联系删