论文链接：https://arxiv.org/pdf/2401.06305.pdf

论文题目：用于自动驾驶最优间距选择和速度规划的多配置二次规划（MPQP）

1 摘要

本文介绍了用于自动驾驶最优间距选择和速度规划的多配置二次规划（MPQP）。平滑且安全的路径规划对于自动驾驶汽车的成功部署是至关重要的。本文提出了一种用于自动驾驶最优速度规划的数学表示，其在具有实际约束的高保真度的仿真和现实道路演示中得以验证。该算法使用广度优先搜索来探索时间和空间域中内部交通间距。对于每个间距，二次规划找到一个最佳速度曲线，将时间和空间对与动态障碍物同步。通过对Carla算法的定性和定量分析，讨论了该算法的光滑性和鲁棒性。最后，本文给出了城市驾驶的道路演示结果。

2 Introduction

背景：轨迹规划一直是自动驾驶领域研究的热点，虽然传统规划算法存在了很久[1][2]，要求在高度动态和不确定的场景中高效、精确、平稳地导航。在安全性至关重要的情况下，促使研究人员探索现有算法的局限性，优先考虑经常相互冲突的目标[3][4][5]。因为可解释性和理论保证，MPC和二次规划QP等形式优化方法仍然是自动驾驶轨迹规划和控制任务的热门选择[6][7]。

存在动态和不确定智能体的运动规划会导致非凸问题[8][9]。这些可能是（1）求解缓慢，限制了解决方案的最优性或时间范围 （2）使用陷入局部最优的近似，导致不稳定或其他不期望的轨迹[10]。因此，路径和速度分解方法已经被积极采用，以减轻规划子任务的复杂性[8][11][12][13]。通过将路径规划问题与速度规划问题解耦，我们可以限制优化空间，从而获得更快的高质量解决方案。

近年来，一直在努力改进分解方法[12][15]，特别是对于自动驾驶ST图[16][17]的应用。一直是一个热门的选择，细化下限和上限，从而减少搜索空间。ST图还有助于解决动态障碍物及其相关的排序问题[18]，见下图。

上图所示，车辆的动机：存在自车（绿色）沿着汽车123交叉时刻一起穿过交叉路口的多个时刻选择。精确同步时间和距离以及选择最佳间隙是安全机动的关键。
虽然ST图简化了问题，但时间和空间的同步仍然具有挑战性[19]。为了解决这个问题，作者在[13]中添加了同步时间作为决策变量，并使用QP迭代近似解决了非凸问题。然而，迭代过程中通常计算量很大，有可能近似导致局部最优[20]。类似地，作者在[16]中采用ST图并将非凸问题近似为QP，但在空间域中求解它便于基于位置的速度限制的积分（例如对于弯曲路径）。然而，空间域中的规划不能无缝地考虑0速度，因为他们导致时间评估无穷大[21]，这在走走停停地场景中是有问题的。在[22][17]中，作者应用动态规划DP直接优化ST图上的非凸问题。然而，DP通常受到维数灾难的影响，因此通常在状态和控制输入的数量上受到限制。自定义[22]和启发式解决方案[17]（例如，使用螺旋曲线插值[22]）对于实时实现是不可避免的，而状态和控制的数量仍然有限。总而言之，现有方法是有前途并且符合数学的，但还没有严格解决设计速度规划器的方法：（1）在时间空间中搜索解（2）在没有近似的情况下确保全局最优性（3）在没有并行计算的情况下具有高计算效率（4）解决沿着动态障碍空间与时间同步。重要的是，在各种场景中进行彻底的验证，在学术文献中经常缺失，在自动化车辆中部署方法之前是必不可少的。

所以，探讨一个严格的速度规划设计方法，通过在路径-速度分解方案下的最优速度规划中包含collective efforts。
我们采用了时空ST图的上下界进行细化，而细化是沿着时间的步骤进行的。允许将下限和上限直接且有效地集成到QP中。QP作为凸优化，产生确保舒适性和安全性的全局最优解。虽然鲁棒，但硬约束通常会导致过于保守的行为（如果不是不可行的解决方案），使实时系统无法采取行动。我们选择结合硬约束和软约束用来产生在动态适应当前场景的复杂性的同时保证安全的行为。

本文主要贡献

（1）本文探索了与动态障碍物相关的空间和时间场景的多种组合；
（2）优化探索场景的速度曲线。重要的是，构想仍在时间域，解决凸优化中的时空同步；
（3）本文还引入了将横向加速度限制与上一个给定路径上的曲率结合的技术；
（4）本文在仿真和道路测试中对所提出的框架进行大量验证。

3 时空ST图的构造与范围初始化

3.1 概述

本节详细介绍了该算法，该算法依次（1）生成ST图（2）找到交通代理之间相对于自车的通行顺序（我们称之为“配置文件”）（3）获得与每个配置文件相关联的上下限。我们假设知道当前的状态、要遵循的路径以及对其他流量的预测。
为了说明该算法，参见下图。

T形连接场景（左上）、相应的ST图（中上部）和ST单元规划器（右上）。ST Cell Planner还显示了使用广度优先搜索算法（BFS）找到的可能路径。下图显示了ST图中找到的可行路径和不可行路径。

给定自车的路径（黄色），对其他交通的预测（具有恒定速度预测）表明自车与三辆车（1号车、2号车和3号车）相交。车辆的形状是一个多边形，我们画一个（时间t，距离s）的轨迹。因此，ST图。作为ST图的一个实例，顶部中心的图传达了：车辆1当前与路径相交，并将沿着路径行驶;车辆2以较低的速度接近，并将在稍后的时间并入路径;车辆3最接近本车辆，并将以非常低的速度穿过路径。ST Cell Planner然后分割连续ST图并找到“活细胞”（表示离散化的可行空间）。在分割内，宽度优先搜索（BFS）找到关于自车的通行顺序的可能情况（即，profile）。给定轮廓，考虑运动学限制（左下图中的绿色空间）生成相关的下限和上限。
当存在多个轮廓时，相对于QP的最小成本选择最佳的曲线。注意，配置文件是相互独立的，因此适用于并行计算。

3.2 ST图的构造

与[22]，[16]中的先前努力一致，ST图的构造是MPQP的重要步骤，因为它是在后续步骤中确定二次规划的可行空间的基础。
对于每个代理，我们根据其尺寸和预测位置计算多边形。该多边形可以通过参数化的边缘或不确定性度量（例如，噪声、风险或预测噪声）。如果多边形与本车的路径相交，则在ST图上标记相交区域（时间和距离）。注意ST图在Frenet框架[23]中，因此我们将相交区域转换为Frenet框架1。毕竟，每个代理都表示为（时间，距离）坐标中的两条线段。请注意，这些线可以移动以考虑安全裕度。

3.3 活细胞生成

“活细胞”表示ST图中的可行集，不包括由其他代理占用的空间（由“占用的细胞”表示）。在每个时间段，单元由距离s的边界定义，即，cell = {$s_{min}$，$s_{max}$}。为了简洁起见，将分割和细胞生成的算法称为“Cell Planner”。Cell Planner首先识别在每个时间段与每个代理相关联的占用小区。可能存在多个被占用的小区。如果被占用的单元格重叠，则将它们合并。然后发现活细胞作为被占据细胞的互补组。例如，在t = 2的时间段，假设存在三个车辆（即三个被占用的单元格）：

3.4 搜索轮廓

采用BFS [24]来发现ST图中可能的轮廓。搜索过程如下：
（1）从原点出发，时间向前迈进一步。
（2）如果细胞与上一步骤中的任何活细胞重叠，则扩展每个活细胞的图谱。
（3）否则，终止并丢弃配置文件。
（4）如果时间步到达终点（即，计划范围），返回所有有效的配置文件。

步骤2表明在每个时间步中可以连接多个活细胞（参见图2中的右上图）。如果在原点不存在活细胞，则ST图不可遍历，因此不存在曲线。

3.5 整合到QP

构建ST图最终是为了促进底层的速度优化。三种技术：
（1）分割的ST图的等效时间步长（0.1秒）的QP。这是直接的，同时在同步路径和速度方面非常有效。
（2）通过连接每个时间步活细胞的下端来生成每个曲线的下限。类似地，通过连接活细胞的上端来生成上限。
（3）后过滤所有有效的配置文件与运动学限制（在速度，加速度和加加速度）。这有助于减少轮廓的数量，并确保（纵向）运动限制。

根据步长大小，整个过程可能在计算上是昂贵的。然而，处理第II-D节和第II-E节中的每个轮廓是完全可分离的，因此并行计算解决了这个问题。所得到的轮廓由具有同步时间的一对边界表示。可以无缝集成到QP中。

4 优化问题

给定从ST图获得的分段线性边界，我们用公式表示二次规划[20]，优化速度，保证安全。
（1）系统动力学：状态包括由[p，v，a]表示的[距离，速度，加速度]，并且控制是加加速度j。目标是沿着纵向方向沿着找到最佳速度曲线，因此我们利用简化的纵向动力学：

对于增广变量x = [p，v，a，j]，我们将动力学写成紧凑形式：

其中f(.)是线性映射

4.1 目标函数

目的是提高乘客的舒适度，减少旅行时间。形式上，目标函数F为：
$$F=\frac{1}{2}{x}^{T}Hx+x^{T}h$$
其中，$H\in R^{dim(x)\ast dim(x)}$是对角矩阵，权重$w_a$施加在加速度上(对于所有$t\in [0，...，N-2])$，$w_j$施加在jerk上（对于所有t ∈ [0，…，N-3]），在其他地方它都是零。矢量h对于除了在t = N处的最终位移之外的所有元素具有零值（即$p(N)$），其被设置为$-w_f$（$w_f>0$）以促进相对于初始位置的大位移（即，更短的运行时间）。

4.2 约束

有两种类型的约束：
（1）初始条件（2）下限和上限。
最初的条件是：

其中$p_0$和$v_0$表示在当前时间测量的位移和速度。在紧凑的形式中，可以用增广变量x重写它，即$x(0)=x_0$
回想一下，下界和上界是从第II-B节的ST图中得到的，因此它们是已知的。相应的不等式约束如下：

注意，下限和上限可以通过自定义余量选择（例如参数）进行设计，这可能会导致不可行性问题。详情见第III-D节。

4.3 完整的问题

完整的优化问题如下：

注意，这是没有任何（迭代）近似的QP的通用形式，保留了良好的凸优化属性[20]，例如，最优性和收敛速度。

4.4 松弛问题的软约束

不等式约束（11）可以根据所构造的ST图而被违反，从而导致不可行解决的问题。示例包括（1）两个预测的代理轨迹重叠，导致规划范围内不存在可行的区域（2）自车初始位置在可行的区域外。

不可行的解决方案可能会导致不稳定和不舒服的操纵（取决于不可行解决方案的预定义操作）。

一个广受欢迎的解决方案是通过放松约束来重新表述问题，使问题变成软约束。我们为每个边界添加一个松弛变量（即，$S_{lb}$和$S_{ub}$），并惩罚目标函数中的非零松弛变量。现在的问题是：

其中，$w_b$表示边界松弛变量上的惩罚权重。注意，松弛变量对于所有t都是时态的，只有非零值才会被惩罚。此外，松弛变量是有界的（通过$s_{maxlb}$和$s_{maxub}$），以确保安全性。下图展示了一个简单的T型连接场景的软边界。

图三.T型交叉口场景，2个agents（1966年的汽车为灰色，1967年的汽车为暗红色）。硬约束可以在纯色填充中看到，软约束可以在半透明颜色填充中看到。

4.5 施加横向加速度限制的漏斗技术

纵向速度规划的一个警告是，它忽略了与给定路径相关的横向加速度和相关运动学限制。然而，直接评估横向加速度会导致问题非凸，从而导致解决更高的复杂性问题[16]，[13]。我们介绍了一种容易实现但有效的方法：漏斗技术。
给定路径，我们计算每个位置步长s处的曲率K。对于参数横向加速度限制a，相关的速度限制为：

利用参数预瞄距离$L$，$v_l$被设置为$v_s$的最小值，即$v\in min(v_s,v_{max})$，$v_l$ ∈ {0，…，L}，其中$v_{max}$是交通速度限制。然而，速度限制$v_l$不能立即用于上界等式11中的QP中的速度。因为如果当前速度高于$v_l$，则QP变得不可行。相反，通过以固定速率线性下降，上限从交通速度限制下降到v。请注意，高的递减率可能会过度收紧上界（这反过来又限制了可行集）。我们发现，最小加速度的一半，即$-\frac{a_{min}}{2}$，是一个舒适的速度。

5 验证

5.1 仿真概述

我们将CARLA [25]用于高保真仿真环境，该环境已被广泛用于验证自动驾驶研究。使用CARLA内置的模拟生成平台“scenario runner”生成场景。
用四种不同的场景来测试MPQP：
（1）高速公路合并

自车在坡道上启动并并入车道，目标车道上的其他车辆快速行驶，其对自车让步行为具有随机性

（2）T型交叉口

自车在不受保护的情况下左转，同时保持行人的安全。

（3）四路交叉口

自车左转。增加的复杂性是由于从相反方向接近的额外车辆。

（4）密集交通中的车道变换

目标是变道进入密集的交通，以避免紧急车辆停在前面。不存在用于本车辆在不与其它汽车相互作用的情况下合并的安全空间（即，没有其他车辆的配合）。

如下图所示，每个场景都有不同的道路结构、目标车道和交通参与者。

四种不同场景的模拟结果。每个场景的结果是50次运行。身份系统用于所有四种场景，以及位置和驾驶行为（例如，目标速度）在每次运行中随机初始化。如果发生碰撞或达到时间限制（80秒），则运行停止。

这组场景及其固有的挑战评估所提出的算法的一般性能。注意，我们对所有场景使用相同的算法，而位置和驾驶行为（例如，目标速度）在每次运行中随机初始化。其他车辆被建模为智能驾驶员模型（IDM）[26]，并进行了一些修改以检测相邻车道中的自我车辆。让道行为根据预设的伯努利参数触发。有兴趣的读者可以参考[5]了解详情。总的来说，这些车辆被设计成具有攻击性，以挑战我们的算法。智能体的行为模型是车道跟随（除了随机改变方向）和交通不改变车道。
MPQP的处理时间平均为20毫秒，配备英特尔®酷睿™ i912900 H和32 GB RAM的笔记本电脑上的最大时间周期为84毫秒。此处理时间包括所有上述步骤：（1）ST图构建（使用ClipperLib [27]）（2）ST单元规划（3）二次编程（使用qpOASES [28]）。当规划的轨迹被发送到底层控制器时，MPQP找到新的解决方案。计划范围设定为10秒（低速情况）和15秒（高速情况）。这表明MPQP适用于实时系统的强大潜力，即使有一个相对较长的规划范围。

5.2 仿真结果

我们首先研究了MPQP在四路交叉口场景中的表现，与图1中的激励示例一致。与激励性的例子不同，自车左转与意外的行人交叉互动。MPQP的一般行为被观察为：（1）自车接近交叉口（2）自车保持停止，直到迎面而来的车辆穿过。下图所示自我车辆缓慢巡航，直到迎面而来的汽车穿过交叉口。（3）如果合并车道上的汽车（图5中的汽车264）不减速，则自我车辆减速并等待。否则，它继续。（4）在完成转弯时，一个行人突然穿过道路，MPQP等待。根据流量的初始位置和攻击性，行为可能会有所不同，但是，MPQP始终确保安全和成功的合并。
现在，我们将我们的分析扩展到上述四个场景：高速公路合并，T型交叉口，四路交叉口和车道变换。表I列出了随机初始化重复50次的每个场景的总体性能
总的来说，MPQP成功地完成了所有场景，没有冲突或超时。注意，MPQP并不事先知道将运行什么场景以及流量基于什么行为模型。也就是说，MPQP展示了一种能力的概括性没有情境知识和存在的不确定性。
结果中的一些观察结果：（1）T型交叉口场景在制动器上的平均值高于高速公路（高速公路具有更高的速度吞吐量），由交通的不确定的行为变化的T型路口的情况下解释。由于车辆在合流点处突然加速，本车不得不强力减速以保证安全。事实上，在四路场景中观察到相同的行为，然而，由于密度导致较不积极的制动和急动，平均速度要低得多。（2）自我车辆倾向于在密集交通中更平稳和更慢地行驶以进行车道变换，等待它们的合作（即，为自我车辆腾出空间）（3）自车往往是更保守的T型路口和四路的情况下，存在优先级之间的交通参与者。行人和迎面而来的汽车（特别是在T形路口的情况下）优先。因此，自车倾向于防御性的，并且只有在汽车明显地给自车让路或者如果安全性得到保证（例如，无车辆），整体行为与经验丰富的人类驾驶员保持一致，他们在防御性和不太保守之间保持平衡。

5.3 道路测试

虽然仿真有效地评估了MPQP的一般性能，但仍然存在模拟到真实的差距。也就是说，其他车辆的行为模型通常不能正确地表示其他驾驶员的决策过程。仿真环境不能精确地表示定位和感知噪声。使用不同的控制器，并且在物理和系统定时方面存在变化。因此，我们于2023年7月在日本Joso市的现实世界城市课程中扩展了我们的验证。

课程包括几个驾驶场景，包括4路交叉口（上图中的快照），T型交叉口和车道变换。公众参与者的反馈普遍是积极的。参与者提到乘坐非常平稳，用户舒适度良好。一些用户表示，拥挤的十字路口即使对人类司机来说也感觉很复杂。一些参与者表示，一些操作，比如变道和绕过停着的汽车，感觉有点危险，但表示他们对自动驾驶系统有信心。

6 总结

受到路径-速度分解方法有效性的启发，本文提供了一个数学框架来最优地规划自动驾驶汽车的速度配置，即MPQP。给定一条路径，当存在动态目标时，我们利用广度优先搜索算法来寻找跟随路径的时序组合。然后，将每个配置作为下限和上限集成到二次规划中。该表示和实现在计算方面是高效的，能够为10s以上的规划范围在平均20ms内提供一个新的解决方案。CARLA中的仿真结果证明了MPQP的强大潜力，随后进行了实车演示。一个注意点是，这项工作依赖于对交通的确定性预测，这阻碍了产生交互行为的能力。此外，处理多模态预测中的不确定性将是未来的研究工作。