导读：

本篇文章提出了一种局部地图表示方法（即速度场）来解决无法为所有场景设计通用规划规则的问题。此外，本文开发了一种高效的迭代轨迹优化器，其与速度场无缝兼容，实现了训练和推理过程。实验结果表明，本文方法为提高自动驾驶系统的规划性能和增强其模仿人类驾驶行为的能力提供了一种有前景的解决方案。©️【深蓝AI】编译

1. 摘要

轨迹规划涉及生成一系列在不久的将来车辆要跟随的空间点。然而，由于驾驶环境的复杂性和不确定性，自动驾驶汽车（AVs）不可能为优化未来轨迹而设计详尽的规划规则。为了解决这一问题，本文提出了一种局部地图表示方法，称为速度场。该方法为轨迹规划任务提供了航向和速度先验，简化了复杂城市驾驶场景中的规划过程。通过本文所提出的损失函数，可以从人类驾驶员的演示中学习航向和速度先验。此外，本文开发了一种基于迭代采样的规划器来训练和比较局部地图表示方法之间的差异。本文研究了局部地图表示形式在现实世界数据集上的规划性能。与学习的栅格代价地图相比，本文方法具有更高的可靠性和计算效率。

2. 介绍

轨迹规划是自动驾驶系统的重要组成部分，因为它使车辆沿着目标路径到达预期的目的地，同时保证了效率和安全性。现有的规划方法可以分为两类：基于规则的方法和基于学习的方法。

基于规则的方法依赖于人工设计的规则来避免与驾驶环境中的目标发生碰撞，例如距离测量和基于距离的速度策略，如图1（a）所示。然而，这些规则通常受限于特定场景，从而限制了它们的泛化能力。基于学习的方法（特别是那些基于模仿学习的方法）学习轨迹和驾驶环境之间的映射，从而提供了更广泛的应用。然而，直接将驾驶环境映射到规划轨迹通常被认为具有较差的可解释性。

神经运动规划器（NMP）引入了一种新的方法，它结合了基于规则的方法和基于学习的方法的优势，从而提高了规划模块的泛化能力和可解释性。具体而言，该方法采用深度神经网络将感知信息转换为场景中每个时空点的驾驶代价（图1（b）），并且选择总代价最低的采样轨迹。然而，由卷积神经网络生成的代价地图带来了巨大的计算负担并且无法满足实时要求。此外，在高速场景中的规划需要更大的地图范围，而过大的栅格尺寸会导致规划精度降低。同时，汽车后方或者不可达区域也有相当一部分大规模栅格被预测，这导致计算资源的浪费。因此，该方法面临着计算时间、地图范围和栅格尺寸之间的权衡。此外，纯代价值仅提供有关时空位置的信息，而没有考虑高阶运动信息。例如，车辆行驶到路边和沿着路边行驶的代价应该有所不同，代价地图无法传达有关道路限速的信息。

为了应对这些挑战，本文提出了速度场（VF），它包括两个新的关键设计。首先，为了克服计算权衡，本文提出了基于注意力机制实现隐式地图的概念。在该方法中，驾驶上下文信息被编码为键值对，同时轨迹采样位置被编码为查询，以获得该位置处的驾驶代价。该方法避免了将潜在变量显式解码为栅格地图并且输出不相关位置的代价值，从而提高了计算效率，并且不需要在栅格范围和尺寸之间进行权衡。其次，类似于占用地图和占用流之间的关系，本文使用速度矢量替换了原始的代价值（图1（c）），这引入了更高阶的运动信息，它通过计算轨迹速度和速度先验之差来估计行驶代价。本文在记录现实世界场景的数据集上验证了所提出的方法。

本文的贡献主要有如下三点：

1）本文探索了一种新的可解释的矢量化驾驶局部地图表示方法，称为速度场（VF），它以直接且高效的方式提升了规划性能；

2）本文开发了一种高效的迭代轨迹优化器，其与所提出的地图表示方法无缝兼容，实现了训练和推理过程；

3）本文以闭环形式在现实世界记录的场景中部署基于速度场和迭代优化的规划器，展现了所提出方法实现的人类相似性和安全性提升。

3. 方法

3.1 问题描述

该任务的目标是生成一组自车的未来轨迹点，表示为$\mathbf{P}=[p^1,...,p^T]$。在短时间$T$内，给定其他智能体$h_a$和自车$h_e$的历史跟踪数据，以及当前时间的地图信息$\mathcal{M}$，包括参考车道$m_{ref}$、交通灯状态$mtl$和静态障碍物$m_{ob}$。驾驶上下文由$\mathcal{S}=(h_a,h_e,\mathcal{M})$表示，并且本文模型旨在建立从驾驶上下文到轨迹点的映射$\mathbf{P}=\mathcal{F}(\mathcal{S})$。整体方法如图2所示。

3.2 驾驶上下文编码

与Vectornet类似，本文实现了一个图神经网络，其为预测模块编码矢量化上下文信息。$T$时间步长自车历史数据$h_e\in {\mathbb{R}}^{T\times [x,y,yaw,\dot{x},\dot{y}]}$和$N_a$辆周围汽车$h_a\in {\mathbb{R}}^{N_a\times T\times [x,y,yaw,\dot{x},\dot{y},type]}$特征被连接到一起，并且由门控循环单元（GRU）嵌入来编码时间相关信息。随后，在256维空间中提取的特征由多层感知器（MLP）处理。分段车道和人行横道多边形等地图元素最初由MLP直接编码。所有256维编码特征在第一维中连接起来，并且由全局多头注意力网络（GAT）聚合，以获得车辆到车辆和车辆到上下文的关系。整体场景上下文嵌入用符号$\mathcal{S}$表示。

该模块生成相邻智能体的预测未来轨迹以及自车的初始规划猜测。通过计算平滑的$L1$损失，数据集的轨迹回放直接作为监督信号，从而引导轨迹优化过程。

3.3 迭代高斯采样轨迹优化器

在训练和推理过程中，采用一种基于迭代高斯采样的规划器来提高采样效率和采样精度。在本工作中，采样器对包括加速度和转向$(a,s)$在内的控制变量进行高斯采样，这是通过对初始猜测进行微分来获得的。它选择最佳的k个样本作为新的均值，并且在判断出迭代限制后进行重采样。第一次循环中的样本由三类轨迹组成：控制变量概率栅格采样器、lattice状态采样器和基于vanilla模仿学习的初始猜测。加速度和转向的采样方差由两个独立的超参数$({\sigma }_a,{\sigma }_s)$控制，并且它们独立采样。

值得注意的是，第一次迭代中的扰动是时间常量，而在随后的迭代中，它是随时间变化的，其方差逐渐减小。第一次循环旨在确保探索最可行的轨迹，而随后的迭代优化最佳选择。

3.4 隐式速度场

隐式速度场是一个将位置$(x,y)$和时间戳$t$查询映射到关于场景上下文嵌入的推荐速度矢量$v=f(x,y,t|\mathcal{S})$。注意力机制计算查询（Q）和键（K）之间的距离作为权重，并且将其乘以值（V），如图3所示。该方法有效地捕获了驾驶上下文中各种元素之间的关系和交互。

本文将查询框架集成到所提出方法中。将具有时间嵌入的位置$(x,y,t,sin(t),cos(t))$作为正交方向$(\dot{x},\dot{y})$上速度尺度的查询。通过以下等式生成关于轨迹样本${\mathcal{T}}_{sample}=[{\mathcal{T}}_1,...,{\mathcal{T}}_N]$的速度矢量：

$${\mathcal{V}}_{p,M}^{t,n}=\mathcal{M}(p_{\mathcal{T}}^{t,n}|\mathcal{S})$$

其中，$\mathcal{M}$是速度查询函数，$p_{\mathcal{T}}^{t,n}$是$n$个轨迹点在时间$t$的位置，${\mathcal{V}}_{p,M}^{t,n}$是从隐式速度场模块中获得的速度矢量。最近的$n$个样本的速度被认为是可接受的速度值。每个速度模仿损失乘以演示轨迹的$[x,y,yaw]$和采样位置之间的$L2$距离的指数映射：

$${\mathcal{L}}_{imit}=\frac{1}{Z}\sum _{n=0}^N\sum _{t=0}^T{e}^{-\frac{\Vert p_{\mathcal{T},yaw}^{t,n}-p_{{\mathcal{T}}_e,yaw}^t{\Vert }_2}{2}}({\mathcal{V}}_{p,M}^{t,n}-{\mathcal{V}}_{p,\mathcal{T}}^{t,n})$$

本文引入了速度修正损失，为每个位置分配适当的速度。假设当前时刻的速度应该能够引导自车沿着演示轨迹点行驶。这种设计是为了确保规划与预期轨迹保持一致，同时维持平滑性和安全性。修正损失定义如下：

$${\hat{\mathcal{V}}}_{p,\mathcal{T}}^{t,n}=(p_{{\mathcal{T}}_e}^{t+1,n}-p_{\mathcal{T}}^{t,n})/dt$$
$${\mathcal{L}}_{correct}=\frac{1}{Z}\sum _{n=0}^N\sum _{t=0}^{T-1}{e}^{-\frac{\Vert p_{\mathcal{T}}^{t,n}-p_{{\mathcal{T}}_e}^t{\Vert }_2}{2}}\Vert {\hat{\mathcal{V}}}_{p,\mathcal{T}}^{t,n}-{\mathcal{V}}_{p,M}^{t,n}{\Vert }_2$$

速度场构建损失定义为：

$${\mathcal{L}}_{VF}={\mathcal{L}}_{imit}+{\mathcal{L}}_{correct}$$

3.5 行驶代价估计

行驶代价估计函数由拉格朗日项构建，包括加速度、加加速度、转向、转向变化以及采样速度和地图建议速度之间的速度差${\mathcal{V}}_{diff}$。这些分量使用可学习的系数$w_c$的加权和进行组合，使优化过程能够高效地适应和响应各种驾驶场景。原始测量矢量为：

KaTeX parse error: Undefined control sequence: \dddot at position 42: …}[\ddot x^2_t, \̲d̲d̲d̲o̲t̲ ̲x^2_t, \dot H^2…

目标函数在数学上可以表示为：

$${\mathcal{C}}_{\mathcal{T}}=\mathcal{D}{w}_c^T$$

NMP将专家轨迹和负样本之间的平均距离误差（ADE）直接映射到行驶代价，以平滑负样本的代价值。与NMP不同，本文将平均距离仿射到概率空间。选择损失通过下式来计算采样轨迹集${\mathcal{T}}_s$的代价分布和距离分布的交叉熵（CE）：

$$\mathcal{P}(ADE)=Softmax(1-\eta (\Vert {\mathcal{T}}_s-{\mathcal{T}}_e{\Vert }_2))$$
$$\mathcal{P}({\mathcal{C}}_{{\mathcal{T}}_{samp}})=Softmax(1-\eta ({\mathcal{C}}_{{\mathcal{T}}_{samp}}))$$
$${\mathcal{L}}_{sele}=CE(\mathcal{P}({\mathcal{C}}_{{\mathcal{T}}_s}),\mathcal{P}(ADE))$$

其中，$\eta$是沿采样候选轨迹的归一化函数。请注意，并非所有样本都进行损失计算，而只考虑具有较小代价的20个样本。

除了${\mathcal{L}}_{VF}$和${\mathcal{L}}_{sele}$以外，本文引入了多模态模仿学习规划和多智能体预测头来分别生成初始猜测和提供多模态信息。对于初始猜测和预测头，损失定义如下：

$${\mathcal{L}}_{IL}={\mathcal{L}}_{ADE}^{plan}+{\mathcal{L}}_{FDE}^{plan}+{\mathcal{L}}_{ADE}^{pre}+{\mathcal{L}}_{modal}$$

其中，${\mathcal{L}}_{modal}$是模型选择的交叉熵损失。规划和预测误差之和最小的模态预计具有最高的概率。最后，整体训练损失为：

$$\mathcal{L}={\mathcal{L}}_{VF}+{\mathcal{L}}_{sele}+{\mathcal{L}}_{IL}$$

4. 实验

4.1 实现细节

1）数据集

本文在Waymo open motion数据集上进行实验，该数据集包含在复杂城市路线上采集的现实世界驾驶样本。高精地图和交通信号/参与者的标注以10Hz的频率提供，并且提供了1000个包，每个包中包含100个场景。这些场景被处理为7秒的目标轨迹。

为了缓解模仿学习中的领域漂移问题，本文提出了一种新的方法，该方法利用时变高斯分布来增强驾驶历史的控制变量。具体而言，本文应用公式1和公式2来引入控制输入的可变性，同时保持未来轨迹不变。在所有实验中，数据增强都是在没有特殊说明的情况下实现的。

$${\mathcal{U}}_{his}^{aug}={\mathcal{U}}_{his}+\mathcal{N}(0,ϵ\cdot {\mathbf{u}}_{lim})(1)$$

$${\mathcal{X}}_{his}^{aug}=BicycleModel({\mathcal{U}}_{his}^{aug},{\mathcal{X}}_{his}^{t=0})(2)$$

2）训练

在配备4×RTX3090 GPU和2×48线程Intel（R）Xeon（R）Gold 5318S CPU @2.10GHz的服务器上，使用AdamW优化器和学习率1e-4进行20个周期的训练。迭代采样优化器在训练过程中进行3次迭代，在验证过程中进行10次迭代。在迭代采样过程中，选择最佳的10个样本作为父样本。本实验进行两阶段的监督学习过程。首先从所有车辆中训练预测模型，然后通过模仿自车行为演示来进行地图参数和代价函数训练过程。

3）评估

本实验将模型和数据集上的其它基线与通用指标进行比较。此外，引入了平均距离误差（ADE）和终点距离误差（FDE）来说明模仿专家行为的能力。碰撞率（CR）、偏离道路率（ORR）和交通灯违规率（TLV）是安全性指标。加速度、加加速度用于衡量舒适性。

4.2 基线

模仿学习（IL）的输入是矢量化上下文，并且直接输出多模态未来自规划控制和其它智能体的轨迹。

微分集成的运动预测和规划（DIPP）是一种基于逆最优控制的规划方法。该方法使用IL模型来提供其它智能体的初始猜测和轨迹预测。对与道路上目标的欧式距离和轨迹平滑性进行加权求和，以估计驾驶代价。请注意，数据增强不适用于此方法。

基于采样的DIPP（EULA）也使用与DIPP相同的代价函数来测量欧式距离，但是在训练和评估过程中都使用所提出的迭代采样规划器来实现。

代价场（CF）使用与所提出的速度场相同的框架，但是矢量输出被求和以直接表示查询点的驾驶代价。

4.3 开环比较

表1展示了上述方法的开环结果。

标有“Human”的第一行表示，尽管仿真系统中的数据大致准确，但是存在一些噪声和不准确之处。总体而言，所有基于深度神经网络的方法（包括IL）都能够准确模仿人类驾驶员的行为，证明了深度学习在模式识别中的有效性。比较DIPP和EULA，观察到模仿指标的降低，这表明本文的迭代采样规划器可以找到与专家演示非常相似的低代价轨迹。通过比较CF和VF指标，发现加入先验速度矢量可以提高代价计算的准确性，并且获得更精确的结果。总体而言，这些开环结果表明，本文所提出的方法在模仿人类驾驶行为、安全性和舒适性方面可以达到与最先进的基线相当或者更好的性能。

4.4 闭环比较

表2展示了闭环结果，对模型处理领域漂移和泛化到现实世界驾驶场景的能力进行更真实的评估。

尽管使用了数据增强技术，但是深度神经网络（DNN）规划器难以应对复杂的动态驾驶环境。相比之下，DIPP利用人工设计的规则来优化模型的规划策略，从而显著提升规划安全性。通过比较DIPP和EULA，发现EULA中使用的规划模块在寻找最优轨迹方面不如高斯-牛顿方法有效，但是在安全性和与人类驾驶员相似性方面仍然优于基于DNN的方法，并且取得了与DIPP相当的结果。对于CF方法，在碰撞率、相似性方面不如VF方法，这证明了CF方法的能力有限。VF方法有效地利用专家数据来形成信息丰富的局部地图，避免了人工设计和各种交通信息交互模式造成的偏差。因此，VF方法在安全性和模仿能力方面取得了更好的效果。

4.5 运行时间

运行时间评估是在配备Intel 19200K和NVIDIA RTX3080Ti的PC上进行的。生成规划结果的平均运行时间为0.048s（最快0.017s，最慢0.111s）。运行时间满足实时性要求。

5. 总结

在本项工作中，证明了速度场是一种为局部路径规划提供行驶代价的信息方式，并且通过实验证明了迭代采样规划器生成安全轨迹的有效性。与基线DIPP方法相比，本文方法显著提高了规划性能。具体而言，它将碰撞概率降低了33.3%，闯红灯的概率降低了80%，并且与人类驾驶员的相似性提高了43.81%。总体而言，本文方法为提高自动驾驶系统的规划性能和增强其模仿人类驾驶行为的能力提供了一种有前景的解决方案。
©️【深蓝AI】

Ref：
Velocity Field: An Informative Traveling Cost Representation for Trajectory Planning
编译｜auto_driver
审核｜Los

本文首发于微信公众号【深蓝AI】，移步公众号【深蓝AI】，第一时间获取自动驾驶、人工智能与机器人行业最新最前沿论文和科技动态👇
深蓝AI·赋能自动驾驶+机器人+AI