ScePT: 为规划的场景一致和基于策略轨迹预测

arXiv上传于2022年6月18日的论文“ScePT: Scene-consistent, Policy-based Trajectory Predictions for Planning“，作者来自Nvidia和斯坦福大学。

轨迹预测是自动驾驶系统的关键功能，其与无控制的智体共享一个环境，其中一个突出的例子是自动驾驶车辆。目前，大多数预测方法并不强制场景一致性，即场景中不同智体的预测轨迹之间存在大量自碰撞。此外，许多方法生成每个智体的单独轨迹预测，而不是整个场景的联合轨迹预测，这使得下游规划变得困难。

这项工作提出基于策略规划的轨迹预测模型ScePT，该模型可以生成适用于自主系统运动规划、精确、场景一致的轨迹预测。其明确地强制场景一致性，并学习可用于制约预测的智体交互策略。在多个真实行人和自主车辆数据集上的实验表明，ScePT匹配当前最先进的预测精度，并显著提高场景一致性。ScePT具备与下游意外规划器一起工作的能力。

代码是开源 https://github.com/nvr-avg/ScePT

预测非受控智体的未来运动对于与其交互的自动化系统安全至关重要。一个突出的例子是自动驾驶汽车，其中自车与其他道路使用者（如车辆、行人和骑自行车人）共享道路。由于人行为是出名的不确定和不一致，因此预测任务很困难。比如，众所周知，人类在驾驶环境中表现出多模态行为，能够同时保持当前车道、变换车道、让行或在未来超车。

因此，早期关于人类驾驶行为预测的工作不够准确，无法用于自动驾驶车辆的运动规划。为了解决这一问题，许多研究人员一直在开发现象学（phenomenological）方法，即从大量数据中学习智体行为，效果显著。

轨迹预测模型的典型特征包括高预测精度、快速推理速度和标定不确定性。随后当预测被下游规划器使用时，以下特征对整体系统性能也至关重要：

• 兼容性：场景中不同智体的轨迹预测应在单个联合预测中相互兼容。特别是，预测轨迹之间的碰撞应该是罕见的，因为碰撞本身在现实中是罕见的。

• 可跟踪的联合轨迹预测：如前所述，智体的未来运动可以是多模态的。在由多个智体组成的场景中，如果为单个节点生成多模式预测，则下游运动规划器需要考虑这些轨迹预测的所有组合。由于模态数随着智体数呈指数增长，规划器很快就会不知所措。或者运动规划器可以采取保守的方法，避开所有预测的轨迹，但往往以牺牲规划性能为代价（例如，如果所有规划似乎要发生冲突，则使自车处于静止状态）。因此，希望所有智体的多模态联合预测具有有限但完全代表性的模态数，以便下游规划器可以执行应急规划（contingency planning）。

• 时间一致性：对于下游规划器，运动规划在很大程度上取决于预测结果。为了确保平滑的运动规划，如果场景本身在此期间没有发生剧烈变化，则预测不应在后续时间发生显著变化。因此，应避免做数据采样，因为预测可能在时间步发生显著变化，导致生成的运动规划不连续，可能会影响规划性能和安全性。

• 制约（conditioning）：制约是固定一个或多个智体未来轨迹并预测其他智体未来轨迹的最终分布。制约预测有助于运动规划（以自车智体的运动规划为条件）和智体交互的理解。制约用在一些现有工作中，但需要显式建模。理想情况下，在不需要模型结构更改的情况下可生成制约分布。

ScePT是一种离散CVAE（Conditional Variational Auto Encoder) 模型，为场景中的多个智体输出联合轨迹预测，为确保其预测中的高度场景一致性，推理每个智体的运动策略及其周围智体的影响。

如图是ScePT输出的说明，包括每个智体的多模态轨迹预测：不同的线类型（实线、虚线、点线）表示场景一致性联合轨迹预测的不同模式；场景中的智体被划分为高度互动的小团，其中一个例子用黄色虚线显示。

节点和智体，可以是车辆、行人、自行车手或其他类型的道路使用者。用s表示智体的状态，e表示两个节点之间边。由于模型是CVAE，遵循CVAE的标准术语，即x表示制约变量，y表示观测变量，z表示隐藏的潜变量。

为了保持场景一致性，ScePT是一种以场景为中心的模型，即其输出预测是场景中多个节点的联合轨迹。给定具有多个节点的场景，生成时空场景图，其中节点表示智体，边缘表示智体之间的交互。用智体的最近未来距离作为交互的一个代替，根据恒速模型向前传播每个节点。两个智体之间最近未来距离定义为

然后定义场景图邻接矩阵

通过邻接矩阵确定场景图，不同于将所有节点保持在单图模型，而是将场景图划分为具有最大规模（固定为参数）的小团（cliques）。这样做是为了减少乘积潜空间（product latent space）的维度，该空间随图的大小呈指数级扩展，如果太大，会导致预测精度下降。

虽然加权图划分是NP-hard问题，但有许多现成的算法，由于其，我们使用了著名有强大性能的Louvain算法。在划分之后，一个小团的每一对节点都被连接（尽管距离阈值不同）。然后收集节点历史并将其提供给ScePT。如果可用，还利用地图信息和与最近车道的相对位置。

有了小团，智体的状态和边（智体之间的相对状态）历史通过LSTM编码到特征向量中。编码器对联合潜分布进行建模，而不是将每个节点与独立于其邻居的潜变量分布相关联。具体来说，每个智体都配备基数为N的离散潜变量zi，使得小团的联合潜变量仅为z=[z1，z2，…zn]。这意味着联合潜空间的基数随小团中节点数呈指数增长，这也是限制小团规模的原因。

ScePT将联合潜变量的分布表示为一个Gibbs分布，由节点因子和边缘因子组成，

其中，xi是节点i的状态历史，fi是节点i的节点因子，一个前馈神经网络将xi和zi映射为一个实数。fij是节点对（i，j）的边因子，也是前馈网络，E是边缘集。

对数似然可以通过构造因子图（factor graph）来计算，因子图是一个具有可变节点和因子节点的bipartite graph。一个因子图例子如图所示：

归一化是通过汇总z的所有可能估值来完成的（因为z是离散的）。虽然联合潜空间的基数与小团大小呈指数变化，但概率分布通常只集中在少数（<10）个模态上。

解码器设计受运动规划过程的启发，即将每个智体视为运动规划器，并仿真其规划过程以输出轨迹预测。典型的运动规划器采用参考轨迹，即所需运动，并进行调整以满足约束（例如，避撞）并最小化指定的成本函数。受此过程的启发，策略网络（policy network）的结构如图所示：

策略网络的输入是小团节点的当前状态、参考轨迹Sdes和小团潜变量z。参考轨迹以状态历史编码、地图编码和潜变量z为输入的GRU网络生成。然后将当前节点状态与参考轨迹进行比较，获得跟踪误差∆和局部坐标系的下一个航路点∆s+。

为了建模边，将其两个节点状态配对在一起，并将状态对馈送到预编码网络（全连接），然后输入到LSTM单元。对于每个节点，根据图结构，可能存在不同数量的邻居。为了编码可变数的邻居，通过注意网络（attention network）将节点的所有边压缩为单个观测编码。然后将观测编码、潜变量和跟踪误差连接并馈送给全连接的动作网络（action network），获得节点的控制动作预测a。

在此，假设节点的动力学是输入状态和控制的可微分函数，这适用于常见的智体类型，例如车辆（例如Dubin的汽车模型）和行人（单积分器或双积分器）。然后将状态预测反馈到状态向量，并重复该过程。

ScePT的整体结构如图所示：编码器采用LSTM编码的状态和边历史以及CNN编码的局部图，并在小团潜变量生成离散Gibbs分布；潜变量与状态历史和图编码一起，用于GRU为每个节点生成所需轨迹；然后将所需轨迹和潜变量传递到策略网络，获得闭环轨迹预测。

制约预测是一种重要的能力。之前的工作，在编码器中显式编码自车未来轨迹来执行制约。然而，假设只有一个智体可以被制约，使得如驾驶模拟之类的用例变得困难，因为需要为每对智体训练显式制约模型。相比之下，PRECOG只需设置车的潜变量，即可生成未来的制约预测。

同样，ScePT不需要任何结构变化来产生制约预测，因为它直接学习智体的交互策略。简单地固定制约智体的轨迹并输出小团中其他智体的轨迹预测，就生成制约预测。由于固定的未来轨迹不会落入任何潜模态，可以从Gibbs分布因子图中删除与制约节点有关的任何因子。

证据下界损失作为目标函数（标准CAVE训练程序）：

对于似然损失，假设每个模态的未来轨迹噪声是高斯，则得到L2-norm损失如下：

添加碰撞惩罚项，惩罚不匹配预测的正则化项，还可以添加其他类型的正则化，例如乘坐舒适性，因为节点动力学是明确包括在策略网络中。

虽然离散潜空间可枚举，但Z的基数随着小团规模呈指数增长。因此，有时无法对所有模态进行解码。为解决这个问题，采用多样性采样（diversity sampling）。具体地说，采用Ng个最高概率模太，并从其余模态中随机抽样Nr个模态。当Z总基数小于Ng+Nr时，选择所有模态。然后，对样本概率进行归一化，以便预期损失不会崩溃为0。

用于轨迹预测的离散CVAE容易发生模态崩溃（mode collapse），即解码器倾向于在不同模态下预测相似的轨迹，因为似然成本是L2误差加权和，平均预测可能是局部最小值。模态崩溃通过多种方法解决，如多轨迹预测（MTP）损失、先验知识、以及通过真值类别分配模态。这里的方法保持预期损失函数，但引入CVaR作为一种新方法来避免模态崩溃。

风险中制约价值（Conditional Value at Risk，CVaR）是金融和优化中常用的风险度量，定义为

其中P是X的概率分布，α调整风险-反对的度。CVaR是分布P下状态x最低α百分位的平均。在α的极限→ 0产生X的本质下确界，α=1产生E[X]。

受对偶形式的启发，将公式（4）的期望损失修改为CVaR是分布P下状态x的最低α百分位值的平均值。在α的极限处→ 0产生X的必要下确界（essential infimum），α=1产生E[X]。

在公式（5）中的第二行是CVaR的对偶形式，可以理解为在P′必须是适当分布的约束下，将分布P转移到P′，对于所有x，P′（x）≤ α1 P（x）。受对偶形式的启发，将公式（4）中的期望损失修改为

这种CVaR损失不会迫使所有而只有已经接近的模态，与真值相匹配，这样直接防止模式崩溃。与通常关注最坏结果的风险测度常见用法相比，CVaR关注最佳预测，保持输出多样性。在训练过程中，α用于权衡模型对编码器精度和多样性的关注。除了合并CVaR外，这里用贪婪算法对乘积潜空间进行不同采样。

为证明ScePT与下游规划器集成的性能，将其预测反馈给基于下游MPC的规划器。MPC规划器考虑了多模态轨迹预测，并通过分支执行应急规划（contingency planning）。给定M个联合轨迹预测，MPC规划M个对应的自车轨迹，并附加约束，即所有M个自车轨迹的第一个控制输入必须相同。

公式（7）是一个非线性优化问题，用IPOPT求解。作为预测和规划一起运行示例，M=3，未优化Pytorch预测代码在不到240ms的时间内执行，MPC规划运行时间不到60ms，所有这些都在CPU上跑。

实验结果如下：

如图显示将ScePT预测与下游MPC规划器相结合的结果，可视化预测模态及其产生的自运动规划：黑车是自车；蓝车是自小团之外的相邻车辆；青色轨迹是规划轨迹（3种模态）；绿车和棕车是自小团内的相邻车辆；绿色和棕色轨迹是预测轨迹（前3种模态）；品红色圆是行人；红线是自小团内的连接节点。