LOCUS 2.0：基于激光雷达的鲁棒且高效的3D实时建图

作者| dianyunPCL 编辑 | 点云PCL

点击下方卡片，关注“自动驾驶之心”公众号

ADAS巨卷干货，即可获取

点击进入→自动驾驶之心【多传感器融合】技术交流群

后台回复【多传感器融合综述】获取图像/激光雷达/毫米波雷达融合综述等干货资料！

摘要

激光雷达里程计作为在复杂的没有GNSS环境中实现鲁棒定位方法引起了广泛关注。然而，由于自主机器人所需的机载计算计和内存资源的限制，在大规模环境中的异构平台上实现可靠和高效的性能仍然是一个开放的挑战。在这项工作中，我们介绍了LOCUS 2.0，这是一种用于实时地下3D地图绘制的稳健且计算效率高的激光雷达里程计系统，LOCUS 2.0包括一种新的基于法线的广义迭代最近点（GICP）公式，该公式减少了点云对齐的计算时间，一种自适应体素网格滤波器，无论环境的几何结构如何，都能保持所需的计算负荷，以及一种滑动窗口建图方法，该方法限制了内存消耗。所提出的方法被证明适用于在严重的计算和内存限制下参与大规模探索的异构机器人平台。我们演示了LOCUS 2.0，这是CoSTAR团队参与DARPA地下挑战的关键要素，涵盖了各种地下场景，我们发布了LOCUS 2.0作为一个开放源代码库，还发布了一个基于激光雷达的里程计数据集，用于具有挑战性的大规模地下环境，该数据集的特点是在多个环境中，包括雾、灰尘、黑暗和几何退化环境中的平台，总共运行11小时，行驶16公里。

主要贡献

之前的发布了LOCUS 1.0，这是一种以多传感器激光雷达为中心的解决方案，用于实时高精度里程测量和3D建图，具有多级扫描匹配模块，配备了鲁棒的感知传感器集成，以松耦合的方案融合了其他传感模式。虽然在感知退化的环境中实现了显著的准确性和鲁棒性，但先前版本的LOCUS 1.0：

• 具有更大的计算负荷，

• 在存储器中保持全局地图，

• 对更广泛的传感器故障（例如，一个激光雷达传感器故障）的鲁棒性较差。

LOCUS 2.0提供了算法和系统级改进，以减少计算负载和内存需求，使系统能够在严重的计算和内存限制下，在大规模探索中，在具有挑战性的感知条件下实现准确和实时的自身运动估计。这项工作的新特点和贡献包括

• 来自法线的GICP：广义迭代最近点（GICP）的新公式，利用点云法线近似点协方差计算。

• 自适应体素网格滤波器，其独立于周围环境和激光雷达确保确定性和接近恒定的运行时间。

• 改进和评估两种滑动窗口地图存储数据结构：多线程八叉树、ikd树。

• 数据集发布，包括在具有挑战性的真实世界地下环境（城市、隧道、洞穴）中.

如图所示1，由异构机器人平台收集。所有这些特性都改进了计算和存储操作，同时保持了相同水平的准确性LOCUS 2.0的源代码已开源：https://github.com/NeBula-Autonomy/LOCUS.git。

图1：基于大型地下激光雷达的SLAM数据集的四个示例，包括超过16公里的路程和11小时在不同环境中的运行：（a）大型石灰石矿（肯塔基州地下），（b）具有大型开放空间和狭窄通道的三级城市环境（洛杉矶地铁），（c）垂直变化较大的熔岩管，（d）具有狭窄通道的熔岩管。LOCUS 2.0在计算受限的机器人上成功地在所有这些环境中运行。

主要内容

LOCUS 2.0提供了一种精确的基于广义迭代最近点（GICP）算法，实现多级扫描匹配单元和一个鲁棒感知传感器集成模块，用于在松耦合方案中稳健融合附加传感模态。

如图2 LOCUS 2.0架构，该架构包含三个主要组件：i）点云预处理器，ii）扫描匹配单元，iii）传感器集成模块。

点云预处理器负责多个输入激光雷达流的管理，以产生可由扫描匹配单元有效处理的统一3D数据，预处理器模块由点云的运动失真校正（MDC）组成。该模块使用IMU测量值校正扫描期间由于机器人移动引起的传感器旋转引起的点云失真。点云合并通过使用已知的外部变换将来自机器人机身中不同激光雷达传感器的点云组合在一起，扩大了机器人的FOV。为了实现多个激光雷达源的弹性合并，我们引入了一个基于外部超时的监测器，该监测器可动态更新点云合并中应组合哪些激光雷达（即，如果激光雷达的消息延迟太久则忽略）。监测使子模块对单个激光雷达的滞后和故障具有鲁棒性，从而始终向下游管道中提供输出数据。然后，滤波器移除属于机器人自身的三维点。接下来，自适应体素网格过滤器保持固定数量的体素化点，以管理CPU负载并确保确定性行为。它允许机器人具有一致的计算负载，而不管环境的大小或激光雷达的数量（或潜在的激光雷达故障）。与LOCUS 1.0相比，自适应体素网格滤波器将点云缩减策略从具有固定叶子大小和随机滤波器的体素化策略更改为自适应系统。

法线计算模块从体素化点云计算法线。扫描匹配单元执行GICP扫描以扫描并扫描到子地图配准以估计机器人的6自由度运动，与前一代相比，LOCUS 2.0没有重新计算协方差，而是利用了一种新的GICP公式来使用法线，法线只需要计算一次并存储在地图中。在具有多模态传感的机器人中，如果可用，LOCUS 2.0使用来自非激光雷达源（来自传感器集成模块）的初始估计，通过使用接近最优的种子初始化优化，来简化扫描到扫描匹配阶段的GICP收敛，从而提高精度并减少计算，增强实时性能。

LOCUS 2.0还包括更有效的地图存储技术，该系统使用滑动窗口方法，因为在内存中维护大规模区域是不可行的。例如，在这里展示的一个洞穴数据集中，1厘米分辨率全局地图需要50 GB的内存，远远超过了小型移动机器人通常可用的内存，这种方法需要插入、删除和搜索的高效计算解决方案。

A、 基于法线的GICP

LOCUS 2.0使用GICP进行扫描数据和扫描数据到子地图匹配，GICP通过使用配准问题的概率模型概括了点对点和点对面ICP配准，为了做到这一点，GICP要求点云中每个点的协方差可用性，协方差通常基于给定点周围相邻点的分布来计算，Segal等人提出了平面对平面应用，假设真实世界表面至少是局部平面的，在该公式中，曲面上的点由协方差矩阵局部表示，其中已知该点属于具有高置信度的平面，但其在平面中的精确位置具有较高的不确定性。这里展示了平面到平面的协方差计算如何等同于从预先计算的法线计算协方差，仅需要法线这一事实对于扫描数据到子地图的对齐尤其重要，因为否则地图将需要重新计算点协方差，这是一项昂贵的操作，涉及创建kd树和最近邻居搜索，相反，通过使用法线，协方差计算只执行一次（因为它不受附加点的影响），其结果可以存储和重用。

B、 自适应体素网格滤波器

为了管理激光雷达里程计的计算负荷，无论环境和激光雷达配置如何（根据激光雷达的数量和类型），我们提出了一种自适应体素网格滤波器，在这种方法中，目标是将体素化的点的数量保持在固定水平，而不是指定体素叶子大小并使系统暴露于输入点云的变化，这些变化源于不同的传感器配置和环境的横截面几何形状。这个设计目标来自这样一个事实，即配准阶段中的几乎所有计算都依赖于点云的数量。因此，想法是保持3D点的体素化数量固定，以便每次扫描具有近似固定的计算时间。这种简单的技术将点数保持在固定的水平上，同时避免点数的任何大的跳跃、点数太少（例如扫描错误）或点数太多，结果是提高了效率并减少了系统的计算负荷。

C、 滑动窗口地图

LOCUS 1.0通过八叉树数据结构将全局地图存储在存储器中，原始八叉树实现没有一种有效的方法来修剪数据。虽然一种可能的解决方法是过滤机器人周围的点云，并相应地重建八叉树，但这可能在计算上很昂贵，并导致长时间刷新。为了解决这些挑战，并在内存限制下实现大规模探索，LOCUS 2.0提供了两种地图滑动窗口方法：i）多线程八叉树，ii）增量k-dtree。

图3：我们的滑动地图方法的图解，保持所有点云，直到机器人到达原始窗口的边界。然后，将设置一个新窗口，并删除该窗口之外的点云

多线程八叉树方法只在内存中维护环境的以机器人为中心的子映射。两个分别处理专用数据结构（mapa/octreea和mapb/octreeb）的并行线程（threada和threadb）负责通过盒过滤器动态过滤当前机器人位置周围的点云图，并根据更新后的图重建八叉树，同时考虑并行工作进程之间的机器人运动。Ikd树是一种通过合并新扫描动态存储3D点的二叉搜索树。Ikd树不仅在叶节点中维护3D点：它们在内部节点中也有点。这种结构允许动态插入和删除功能，并依赖于整个数据结构中的惰性标签存储。ikd树的初始构建类似于kd树，其中空间在最长维度的中点处递归拆分。

实验

A、 数据集

在过去的3年中，CoSTAR团队在洞穴、隧道和废弃工厂等真实环境中对我们的激光雷达里程表系统进行了深入测试，选择每个数据集（表一），以包含激光雷达里程表具有挑战性的组件，该数据集提供激光雷达扫描、IMU和轮式惯性里程表（WIO）测量以及相机流。所有数据集都记录在不同的机器人平台上，例如Husky和Spot（图4），具有振动和大加速度，这是滑动转向轮式机器人穿越崎岖地形和在崎岖地面上滑动并动态动作的腿式机器人的特点。该机器人配备了3个机载VLP16激光雷达传感器，进行了外部校准。Spot机器人配备了一个外部校准的机载激光雷达传感器，指出开箱即用的工具（运动惯性里程计）KIO和（视觉惯性里程计）V IO，因此数据也记录了这些读数，激光雷达扫描以10 Hz记录。WIO和IMU以50 Hz记录。为了确定机器人在环境中的地面实况，使用了勘测级3D地图。地面实况轨迹是通过对照测量坡度图运行LOCUS 1.0生成的（即扫描到地图即扫描到测量地图）。在这种模式下，LOCUS 1.0以计算效率为代价进行调整，以达到最大精度，因为它不需要实时运行。

图4：DARPA地下挑战星云框架中异构机器人系统的机器人类型

B、 指标

对于CPU和内存配置，使用跨平台库来检索有关运行进程和系统利用率的信息，该库用于系统监视和分析，CPU表示当前系统范围CPU利用率的百分比值，其中100%表示使用1个内核。内存通过根据平台对不同的内存值求和来表示统计信息。里程计延迟测量里程表消息创建与系统当前时间戳之间的差异。该系统在机器人操作系统（ROS）框架中实现。这项工作考虑了最大延迟和平均延迟，因为这两个度量更直接地影响使用里程测量结果的模块的性能，例如控制器和路径规划器。

C、 计算时间

1）来自法线的GICP：给出的实验旨在显示来自法线GICP的GICP优于GICP的优势，并支持这样的说法，即这种重新计算在不牺牲精度的情况下产生更好的计算性能。图5.a-e显示了法线GICP和数据集GICP之间的比较结果，而图5.f显示了关于GICP方法的每个度量的所有数据集的平均百分比变化，来自法线的GICP减少了LOCUS 2.0中的所有计算指标：平均和最大CPU使用率、平均和最大里程表延迟、稀疏扫描、扫描到子地图、激光雷达回调持续时间及其最大时间。

图5:LOCUS 2.0中正常和GICP比较的GICP结果。

2） 自适应体素网格滤波器：实验显示了LOCUS 2.0自适应行为，实验在所有数据集上进行，GICP来自法线，使用ikd树数据结构进行地图维护，框大小为50 m，Ndesired范围为1000至10000，尽管如此，数据集的计算时间仍有一些变化。

图8：自适应体素化的计算时间与3000点和恒定叶子大小0.25的一致性比较。a） 使用2台激光雷达的城市数据集。b） 使用3个激光雷达的洞穴数据集。

如图8所示，该方法在不同的环境和传感器配置中产生了一致的平均计算时间，而没有任何大的计算时间峰值。这种性能提供了更可预测的计算负载，无论是机器人还是环境。

如图7所示，在相同的自适应体素化设置下，所有数据集的平均回调时间和CPU负载都相似。

D、 内存

1） 地图维护：第三个实验展示了与经典的静态八叉树结构相比，滑动窗口地图在实时系统中的优势。在这些实验中，LOCUS 2.0使用：ikd树和多线程八叉树（mto）。

图9显示了F和I数据集的最大内存使用情况以及内存占用随时间的变化。最大的内存占用是八叉树和mto版本的0.001m叶大小。ikd树在内存和CPU使用方面的性能与叶大小为0.01m的mto相似。

2） 地图大小：这些实验表明，在权衡计算、内存负载和结果准确性的同时，滑动地图的大小是一个需要考虑的重要参数。根据实时操作系统要求的范例，滑动窗口映射允许系统被机器人分配的最大内存限制。

图10显示了ikd树和mto在映射大小方面的最大APE、CPU和内存度量。

E、 与最先进技术的比较

表III显示了针对不同环境领域（城市、隧道、洞穴（A、C、F、H、I、J））的LOCUS 2.0与最先进方法FAST-LIO和LINS的比较研究。该表显示，LOCUS 2.0在最大和平均APE误差度量方面表现出最先进的性能，并在6个数据集中的5个数据集中实现了最小的误差。此外，LOCUS 2.0是唯一一种不会在发生激光雷达滑动的隧道型环境（数据集F）中失败的方法。在计算方面，LOCUS 2.0的性能与FAST-LIO相当，LOCUS 2.0的内存使用量稍大，但这可能与所有系统中默认选择的地图分辨率有关。

表II显示了与LOCUS 1.0中的参考方法相比，滑动窗口映射方法如何减少内存使用，同时增加CPU使用。

总结

这项工作介绍了LOCUS 2.0，一个强大且计算效率高的激光雷达里程计系统，用于在严重的计算和内存限制下进行实时、大规模探测，适合部署在异构机器人平台上。这项工作从预计算的法线重新计算GICP协方差计算，提高了GICP的计算性能，LOCUS 2.0使用自适应体素网格滤波器，使计算负荷独立于环境和传感器配置，自适应行为使激光雷达的点数保持一致，同时保持环境的体素化结构，这稳定并提高了计算负载。评估了两种减少内存使用的滑动建图策略：多线程八叉树和ikd树，并显示了它们的计算成本和内存使用的改进，开源了LOCUS 2.0和数据集，用于具有挑战性和大规模的地下环境，这些环境具有各种现实条件，如雾、灰尘、黑暗和限制流动性的几何退化环境

往期回顾

视觉和Lidar里程计SOTA方法一览!（Camera/激光雷达/多模态）

【自动驾驶之心】全栈技术交流群

自动驾驶之心是首个自动驾驶开发者社区，聚焦目标检测、语义分割、全景分割、实例分割、关键点检测、车道线、目标跟踪、3D目标检测、BEV感知、多传感器融合、SLAM、光流估计、深度估计、轨迹预测、高精地图、规划控制、模型部署落地、自动驾驶仿真测试、硬件配置、AI求职交流等方向；

添加汽车人助理微信邀请入群

备注：学校/公司+方向+昵称

自动驾驶之心【知识星球】

想要了解更多自动驾驶感知（分类、检测、分割、关键点、车道线、3D目标检测、多传感器融合、目标跟踪、光流估计、轨迹预测）、自动驾驶定位建图（SLAM、高精地图）、自动驾驶规划控制、领域技术方案、AI模型部署落地实战、行业动态、岗位发布，欢迎扫描下方二维码，加入自动驾驶之心知识星球（三天内无条件退款），日常分享论文+代码，这里汇聚行业和学术界大佬，前沿技术方向尽在掌握中，期待交流！