带硬件辅助存储的自动驾驶路径规划加速方法

arXiv上2022年5月5日上传的论文“Accelerating Path Planning for Autonomous Driving with Hardware-assisted Memorization“，作者来自Cornell大学。

动态障碍物的自动驾驶路径规划是一个挑战，因为需要在满足实时约束的同时执行更高维搜索（包括时间）。作者提出了一种算法-硬件协同优化方法，加速基于高维搜索空间的路径规划。

首先，将节点和障碍物映射到低维空间并存储最近的搜索结果，减少最近邻搜索和碰撞检测的时间。然后，提出一种用于高效存储的硬件扩展方法。在一个现代处理器和一个循环级（cycle- level）模拟器的实验结果表明，该算法的执行时间显著缩短。

路径规划的及时执行对车辆的安全和效率至关重要。修剪（pruning）是被用来在软件中加速的方法。针对RRT、PRM和A*等算法，人们用不同的硬件技术，包括FPGA或ASIC，还提出了特定于算法的加速器。虽然这些路径规划加速器确实解决了特定场景中的个人需求，但不是针对自动驾驶，也不考虑动态障碍。

此外，随着法规（regulation）和算法的不断发展，选择和修改加速路径规划算法的灵活性也非常重要。然而，许多现有路径规划加速器一次设计只考虑一个特定的算法，大多数超参在设计/编程时固化到加速器硬件中。虽然这些硬件化算法在发布时表现良好，并在目标场景提供良好的性能，但不够灵活，无法适应新的或修改算法。

• 动态障碍物的对付

自动驾驶车辆在2D平面执行路径规划。在不同的时间，障碍物将位于2D平面的不同位置。2D平面的动态障碍物成为一组障碍物，在三维空间的坐标可以用（x，y，t）来描述。

可以有效地将带有动态障碍物的2D规划问题视为3D规划问题。然而，时间维度不同于普通的空间维度，这需要对其进行额外约束。由于时间只沿一个方向移动，对于路径P上i<j的任意两个节点（xi，yi，ti）和（xj，yj，tj），必须保证ti<t j。在RRT选择最近邻时，将强制执行附加约束。

• 基于软件的存储RRT算法

用存储（memorization）来减少路径规划的总执行时间。在传统基于采样的规划（比如RRT）中，大部分执行时间都花在寻找最近邻（NN）以及检测会不会碰撞。为了加速这些过程，存储最近访问的节点和碰撞状态，以便跳过一些类似的查询。

如图显示了整个方法的流程：基准（蓝色箭头）规划流程和改进（橙色箭头）规划流程。

在基准RRT，所有迭代执行耗时的最近邻搜索和碰撞检测。然而用一个名为Morton store的小型数据存储器（以Morton空间归档曲线命名）记住基准最近邻搜索和碰撞检测的关键信息。对每个迭代，首先检查Morton store，以便有机会跳过耗时的基准NN搜索和碰撞检测。

规模有限的Morton store的运作如下。

先计算树节点坐标 Xn =（Xn，yn，tn）以及障碍物 Xo =（x，y，t）的Morton codes Mn 和 Mo。Morton code将点（x，y，t）从3D空间ooo投影到1D曲线，同时保持空间位置（32位整数）。Morton codes Mn 和 Mo 是64位数，用作相应节点和障碍物的标记。为调整投影粒度，屏蔽Morton codes的k个最低有效位

其RRT算法伪代码如下：

这个解决方案必须对每段（segment）路径执行精确的碰撞检测，以确保安全（无碰撞）。与使用Morton store进行精确碰撞检测节省的时间相比，该解决方案检查开销可以忽略不计。算法时间复杂度为O（N *（α * logN + β * logL）），其中N是树中的节点数，L是障碍物数，α、β是第8行和第12行的概率（0<α，β≤ 1） 。对于不用Morton store的正常RRT算法，α，β = 1。

• 硬件辅助的存储RRT算法

基于软件的Morton store算法跳过耗时的基准NN搜索和碰撞检测，显著减少执行时间，并且可以通过硬件辅助存储进一步加速。由于Morton codes的计算和匹配按顺序在软件中迭代所有Morton codes，因此可能需要多条指令。为了估计Morton store需要多少动态指令，用Valgrind来计算Morton store查找和更新的动态指令数。

如图显示了不同地图边长度、时间步长和动态障碍物数量的不同测试用例结果。

平均而言，与Morton store相关的动态指令计数占动态指令总数的20%，最坏的情况下可能占动态指令总数的45%以上。然而，通过硬件辅助存储，动态指令计数可以显著减少。可以直接在硬件使用CAM（content-addressable memory ）进行Morton store查找和更新，减少动态指令计数和相应的执行时间。

基于硬件的Morton store由一个全关联的CAM实现，memoryline大小为64字节，可以维护RRT节点的8个8字节地址。由于这些节点是在堆栈上分配的，因此设置中MSB始终为0，并且地址的最高有效8位指示是否存在碰撞。

如图显示在CAM中实现硬件Morton store的示例。

其中tagstore保存Morton codes，每行包含状态（碰撞/无碰撞）信息以及节点地址。对于碰撞状态，在一个memoryline的所有状态中，只要有一个状态指示碰撞，则输出是碰撞。

如图所示是Morton store的架构：CAM直接连接到CPU。

一旦命中（hit）后，将处理提取碰撞状态或节点地址相应的memoryline。如果未读取（read miss），在Morton store中不会修改任何内容。当未写入（write miss）时，最旧的参考memoryline被逐出。

为了在软件中访问此CAM并为不同的规划算法提供灵活性，定义了以下指令集架构（ISA）扩展。morton-update、morton-col和morton-nn，如下表。

• 指令morton-update获取节点的坐标及其碰撞状态，并在morton store中更新此信息。

• 指令morton-col获取节点的坐标，在CAM中查找，并确定是否存在碰撞。

• 指令morton-nn通过查找具有相同morton codes的记录，在morton store中查找近似最近邻的内存地址。

注：这项工作设定这些指令加速RRT。然而，也可以用于其他基于采样规划算法，如PRM。

• 合成测试用例

对于合成测试用例，采取一个正方形地图，其中动态障碍物运行。综合测试用例由三个参数表征，即动态障碍物的数量、方形地图的边长度和时间步数。求解路径应从（0,0）开始，目标是（l，l），其中l是方形地图的边长度。

在这个模拟周期的开始和结束，随机生成障碍物的开始和结束位置。对障碍物的中间位置进行线性插值。如图显示：是带5个动态障碍物、边长度为100和20个时间步的测试用例，从（0,0）到（100100）的解决方案路径以蓝色标记。

模拟12种配置，地图边长为{100，200}，时间步长为{10，100}，障碍物数量为{5，10，20}。比较基准RRT和Morton store软件RRT（sw-Morton）的总执行时间。对于每个配置，随机生成10个不同的测试用例。由于该算法是随机的，对于每个测试用例，重复实验10次，并显示平均执行时间。

如图显示执行时间的对比：

与基准RRT算法相比，基于软件Morton store的RRT路径规划平均减少了51%的执行时间。

• 硬件评估

提出的硬件辅助方法使用CAM快速查找和更新最近访问的树节点，进行最近邻搜索和碰撞检测。用GEM5模拟器（开源系统和处理器模拟器）评估这种基于硬件的Morton store的RRT算法性能。模拟器配置如下表所示：

实现了HW Morton store和一个CPU与HW Morton store store之间的自定义端口，以便在GEM5中进行查找和更新。

与软件评估中使用的12种配置相同，如图显示基准RRT、基于软件Morton store的RRT（sw-Morton）和基于硬件Morton store的RRT（hw-Morton）的执行时间。

与基准RRT相比，基于软件Morton store的RRT性能平均提高了2.28倍，基于硬件Morton store的RRT性能平均提高了8倍。对于最佳配置，软件和硬件Morton Store的性能分别提高6.74倍和56.3倍。

如图显示基准RRT、基于软件Morton store的RRT和基于硬件Morton store的 RRT解决方案路径长度。

软件Morton store和硬件Morton store的平均RRT解决方案长度分别是基准RRT解决方案的1.42倍和1.65倍。这是因为SW或HW Morton store只能找到近似的最近邻，而不是精确的最近邻。因此，解决方案路径可能比基准波动更大。

如下右图所示，在地图位置（80,80）附近，基于硬件的Morton store给出的路径在到达（100，100）之前来回移动。与下左图所示的基线解决方案相比，这增加了解决方案的长度。这样的路径可以使用细化算法（refinement）来减少总长度。

• 在Commonroad基准数据集的评估

为了证明所提方法在现实场景中的有效性，用一个示例（USA\U US101-20\U 1\U T\U 1），如下图所示：其中包含Commonroad基准（“Commonroad: Composable benchmarks for motion planning on roads”. IEEE IV‘2017）的动态障碍物，其场景记录部分来自真实交通。

执行时间和解决方案路径长度如图所示，这证实了减少执行时间的有效性。