Super odometry:以IMU为核心的激光雷达视觉惯性融合框架（ICRA2021）

Super Odometry: IMU-centric LiDAR-Visual-Inertial Estimator for Challenging Environments

作者：Shibo Zhao，Hengrui Zhang, Peng Wang, Lucas Nogueira，Sebastian Scherer

论文地址：https://arxiv.org/pdf/2104.14938.pdf

视频：https://sites.google.com/view/superodometry.

该里程计不同于传统的方案，主线程为IMU线程；结合了松耦合，紧耦合的优点。由视觉-惯性、雷达-惯性提供先验信息解决bias的估计问题，并且为IMU 里程计提供运动信息；并且该方案建立了一种八叉树代替静态 KD-tree来管理空间3D点。

摘要： 我们提出了 Super Odometry，一种高精度多模态传感器融合框架，提供了一种简单而有效的方法来融合多个传感器，例如 LiDAR、camera和 IMU 传感器，并在感知退化的环境中实现稳健的状态估计。与传统的传感器融合方法不同，Super Odometry 采用以 IMU 为中心的数据处理方案，它结合了松耦合和紧耦合的优点，并以粗到细的方式恢复运动。所提出的框架由三部分组成：IMU 里程计、视觉惯性里程计和激光惯性里程计。视觉惯性里程计和激光惯性里程计在约束 IMU 偏差之前提供位姿，并从 IMU 里程计接收运动预测。为了确保实时的高性能，我们应用了一个动态八叉树，与静态 KD-tree相比，它只消耗了 10% 的运行时间。提出的系统部署在无人机和地面机器人上，作为探索者团队为 DARPA 地下挑战所做努力的一部分，该团队分别在隧道和城市赛道 1 中获得了第一和第二名。

1 引言

多模态传感器融合对于自主机器人完成复杂和危险的任务至关重要，例如在地下环境中感知、工业检查和搜救。在这些 无GPS 的场景中，黑暗、空气中的模糊条件（灰尘、雾和烟雾）和缺乏感知特征是目前阻碍我们采用机器人系统实现长期自主的主要挑战。为了在这种环境中实现定位，基于激光雷达的里程计 [1]-[3] 似乎是机器人的合适选择，因为激光雷达传感器可以提供高保真 3D 测量。然而，在无结构的环境中，例如长隧道或存在遮挡物（例如雾、灰尘、烟雾），由于退化和异常值，基于激光雷达的方法难以提供可靠的运动估计。为了处理这些情况，还需要集成额外的传感器，尤其是camera。尽管如此，视觉相机的用例很大程度上仅限于光线不足的条件。因此，我们认为基于激光雷达的 [1]-[3]、基于视觉的 [4]、[5] 或基于激光雷达的视觉的 [6]、[7] SLAM 方法都不是具有挑战性的环境中的最佳选择。由于这些传感器是环境相关的，如果将它们用作主要传感器来构建 SLAM 系统，则很难实现稳健的性能。

在过去的十年中，多模态传感器融合非常流行，可以分为松耦合或紧耦合方法。如表 I 所示，松耦合方法 [1]、[3]、[8] 因其简单性、可扩展性和低计算成本而更受青睐。相比之下，紧耦合的方法 [2] 已证明其准确性和稳健的性能具有优势。然而，紧耦合的方法通常难以扩展其他传感器并实现传感模式之间的切换方案以应对各种环境。此外，紧密耦合的方法可能容易应对潜在的传感器故障，因为它们中的大多数只使用单个估计引擎 [8]。相比之下，松耦合的方法利用多个估计引擎之间分配传感器故障风险，并且应该具有更稳健的性能。然而，由于大多数松耦合方法采用帧到帧估计，它们在准确性和鲁棒性上仍然存在局限性。

受上述讨论的启发，我们提出了一个以 IMU 为中心的 SLAM 系统，它结合了紧耦合和松耦合的优点。系统设计遵循一个关键理解：IMU产生带有噪声的平滑测量，但很少有异常值，只要其他传感器可以很好地约束偏置漂移，就可以使其估计非常准确。因此，Super Odometry 是围绕 IMU 作为主要传感器而设计的。本文主要贡献如下：

1）我们提出了第一个以 IMU 为中心的传感器融合方案，可在极端和感知挑战的环境中实现准确的实时状态估计。

2）提议的方案结合了紧耦合和松耦合的优点，提供了一种简单但有效的方法来融合多个传感器。

3）我们建议使用有效的 3D 点组织（动态八叉树），显着提高扫描匹配的实时性能。

4）提议的方法已部署在包括无人机和地面机器人在内的多个物理系统上，并已在各种具有挑战性的场景中进行了广泛评估，这些场景包括剧烈运动、低光、长走廊，甚至在重尘环境中。

2 相关文献

近年来，人们对laser-visual-inertial估计器进行了许多努力，可以将其分为松耦合方法或紧耦合方法。

A. Loosely Coupled Laser-Visual-Inertial Odometry

Zhang 和 Singh [9] 提出了 V-LOAM 算法，该算法采用顺序数据处理方案，并使用视觉-惯性里程计为激光扫描匹配提供运动预测。然而，由于该方案仍执行逐帧运动估计，并且当前估计完全基于上次估计，因此如果上次估计出错，则难以实现恢复机制。为了解决这个问题，Super Odometry 采用因子图优化，当前估计是基于滑动窗口内的历史帧。因此，Super Odometry对于单个故障是安全的。为了提高鲁棒性，一些工作将其他约束纳入laser-visual-inertial估计器并获得非常有希望的结果，例如添加thermal-inertial先验 [10]、leg odometry先验 [11] 或闭环 [12]。然而，由于这些方法仍然执行帧到帧的运动估计，它们与 V-LOAM 有类似的局限性。

B. Tightly Coupled Laser-Visual-Inertial Odometry

为了执行联合状态优化，Shao 提出了 VILSLAM [13]，它在图优化中有效地融合了视觉-惯性里程计和激光雷达扫描匹配。Zuo[14] 介绍了 LIC-Fusion，它通过使用 MSCKF 框架 [15] 将激光雷达边缘特征紧耦合，将视觉特征和平面特征保留在一起。David [16] 提出了一种统一的多传感器里程计，它联合优化了 3D 基元，例如线和平面。然而，由于这些方法的测量高度紧耦合，它们很难扩展到其他传感器。此外，由于这些方法通常使用单个状态估计引擎，因此当传感器损坏时它们可能具有更高的故障率。相比之下，Super Odometry是一个以 IMU 为中心的传感器融合方案，它只接收来自其他里程计的位姿约束。因此，很容易融合其他传感器。此外，由于它具有多个状态估计引擎，因此能够克服潜在的传感器故障。

C. Method Highlights

与之前的作品相比，Super Odometry 的优势在于：

稳健性和可扩展性：我们以 IMU 为中心的传感器融合架构（见图 2）使我们能够实现高精度并以低故障率运行，因为 IMU 传感器与环境无关。该系统包括故障安全机制，并提供一种更简单灵活的方式来融合多个传感器，如 GPS、车轮里程计等。

简单但有效：Super Odometry避免了融合多个传感器的复杂推导，因为 IMU 具有简单而准确的概率模型。只要其他传感器能够在约束 IMU 预积分因子之前提供相对位姿，这些传感器就会成功地融合到系统中。特别是，作为 Team Explorer 为 DARPA 地下挑战所做的努力的一部分，它在具有挑战性的环境中得到了广泛的评估。

低 CPU 使用率和高实时性能：由于 Super Odometry 没有将所有传感器数据组合成一个完整的因子图，而是将大因子图划分为多个“子因子图”和每个“子因子图”接收来自 IMU 预积分因子的预测（见图 2）。因此，每个里程计因子的运动从粗到细并行恢复，显着提高了实时性能。此外，Super Odometry 采用动态八叉树来组织 3D 点，这使得扫描匹配非常高效。

图2. Super Odometry算法概述。IMU 里程计利用视觉-惯性里程计和 LiDAR -惯性里程计提供的观测来限制 IMU 偏差。作为回报，受约束的 IMU 里程计为视觉里程计和激光里程计提供了预测.

3 系统概述

我们首先定义我们在整篇论文中使用的框架和符号，它们在图 4（e）中指定。我们将世界坐标系表示为 W，IMU坐标系为 B，激光雷达坐标系为 L，相机坐标系为 C。为简单起见，我们将 IMU 坐标系视为我们的机器人载体坐标系，我们的目标是估计 IMU 载体的位置相对于固定世界坐标系 W 的坐标系。机器人状态 xk 可以写为：

提出的系统的概述如图2（b）所示。它由三个模块组成：IMU 里程计、视觉-惯性里程计 (VIO) 和 LiDAR -惯性里程计 (LIO)。我们的系统设计遵循一个关键见解：只要偏置漂移受到其他传感器的良好约束，IMU 及其状态估计就可以非常准确。因此，Super Odometry 使用 IMU 作为主要传感器。它使用视觉-惯性里程计 (VIO) 和 LiDAR -惯性里程计 (LIO) 提供的观测值来约束加速度计偏差 ba 和陀螺仪偏差 bg。作为回报，受约束的 IMU 里程计为 VIO 和 LIO 提供预测，它们以粗到细的方式恢复运动。此外，对 IMU 的关注使方案对状态估计中与环境相关的问题（例如几何或视觉退化）更加稳健。最后，算法架构允许以简单灵活的方式结合其他里程计源，例如 GPS 和车轮里程计。

4 数学模型

Super Odometry是使用因子图建模的。这里我们主要介绍三种用于因子图构建的里程计因子，即：（a）IMU；(b) LiDAR-惯性；(c) 视觉-惯性里程计因子。

A. IMU Odometry Factors

IMU里程计因子的结构如图2(a)所示，类似于传统的位姿图结构。位姿图中的每个节点都与一个状态相关联。节点的密度由最低频率里程计确定。两个连续节点之间的边代表通过 IMU 预积分方法获得的相对载体运动。其他边缘可以是局部约束或全局约束，这取决于传感器的类型。由于相机和 LiDAR 不是全局参考的，它们的里程计是基于机器人的第一个位姿，我们只使用它们的相对状态估计作为局部约束来纠正 IMU 预积分的偏差。

1) IMU Preintegration Factor:

2) IMU Odometry Optimization

请注意，在不失一般性的情况下，IMU 里程计还可以结合来自其他传感器（例如 GPS 和车轮里程计）的测量值。

B. Laser-Inertial Odometry Factors

激光-惯性测距因子的结构如图2(c)所示。由于IMU里程计计算效率高，输出频率非常高，所以它的IMU预积分因子可以很自然地加入到Laser-Inertial里程计的因子图中。IMU 重新集成因子将用作当前扫描图匹配的运动预测，并连接因子图中的连续激光雷达帧。

扫描地图匹配过程的流程可以分为三个步骤，即：(a) 基于 PCA 的特征提取 (b) 多度量线性平方 ICP (c) 激光惯性测距因子 (d) 动态八叉树。

1) PCA Based Feature Extraction：当系统接收到新的激光雷达扫描时，将进行特征提取。使用主成分分析（PCA）方法提取点、线和平面特征。

2) Multi-metric ICP Factors：在提取出可靠的几何特征后，我们使用 IMU 里程计提供的运动预测，将 Fi+1 从 B 转换为 W。然后，我们使用所提出的动态八叉树在地图中找到它的点、线和平面对应关系。为简洁起见，详细过程将在下一节中描述。

为了提高Super Odometry的鲁棒性，我们期望估计联合最小化点对点、点对线和点对平面距离误差度量的最佳变换，可以使用以下等式计算：

3) Laser-inertial Odometry Optimization: 然后使用 levenberg-marquardt 方法通过最小化来求解最优变换：

4) Dynamic Octree: 扫描到地图匹配完成后，需要在地图上配准新的扫描。因此，地图将在每个 LiDAR 配准步骤后通过添加或删除点来更新。通常，大多数激光 SLAM 方法采用 KD-tree 方法来组织 3D 点并完成数据关联。然而，我们认为传统的 KD-tree不是组织 3D 点的最佳选择，尤其是在地图频繁更新时。

Motivation：图3（a）显示了在全局地图（紫色块）中添加新点的过程。地图上的点保持在体素中，黄色块表示添加新点时更新的体素。由于传统的KD-tree只用一棵树来组织所有的点，我们需要重新创建KD-tree，每次添加点时KD-tree的结构都会改变，如图3（b）所示。我们发现这个过程非常耗时，而且计算效率不高。

图 3：3D KD 树和动态八叉树的比较。(a,b) 显示了添加新点云（灰色圆圈）时 3D KD-tree 的构建过程，需要改变整个树的结构。(c,d) 展示了动态八叉树添加新点云（灰色圆圈）时的构建过程，只需要改变子树的结构即可。

Key Insight：我们没有改进三维点集中的邻居搜索，而是在如何重用现有树的结构并避免在地图更新时重新创建新树上做出更多努力。为了实现这一点，我们提出了一种更有效的 3D 点组织，称为动态八叉树，以促进数据关联。

Method Description:动态八叉树基于 Behley [19] 的工作。它将映射存储为哈希表，如图 3（d）所示。世界由体素表示，体素可以使用体素世界坐标的散列函数从哈希表中访问。3D 点根据它们的位置坐标存储在不同的体素中。不是只构建一棵树，每个体素都有自己的八叉树来组织点，每个八叉树都可以通过哈希表访问。由于我们只需要更新特定的八叉树而不是整棵树，因此数据关联的实时性能得到了显着提高。更详细的说明可以在算法 1 中找到。

C. Visual-Inertial Odometry Factors

为了充分利用视觉和激光雷达传感模式的融合，我们在相机视野内跟踪单目视觉特征和激光点，并使用它们为视觉特征提供深度信息。

1) Visual-inertial Odometry Optimization。

5 实验分析

我们使用 Team Explorer 的 DS 无人机（图 4（e））收集了我们的测试数据集，部署在 DARPA 地下挑战中。它具有多传感器设置，包括 Velodyne VLP-16 LiDAR、Xsens IMU、带有广角鱼眼镜头的 uEye 摄像头和英特尔 NUC 板载计算机。数据序列旨在包括视觉和几何退化的场景，这对于基于相机和激光雷达的状态估计尤其麻烦。

图 5 显示了算法输出轨迹与来自全站仪的真值轨迹的比较。虑到表 II 中显示的绝对轨迹误差 (ATE)，我们看到纯基于视觉的方法在这种情况下的性能最差，正如预期的那样。我们的Super Odometry 以 0.174m 的最低 ATE 优于上述方法。我们还在数据集部分提到的白墙数据序列上评估了我们的方法。每种方法的 ATE 可以在表 II 中找到。

结构可能会导致基于激光雷达的算法出现问题。图 6 显示了该数据序列中 LOAM、LIO-SAM 和 Super Odometry 的结果图。一方面，我们观察到错位和显着漂移，如 LIO-SAM 和 LOAM 图中的黄色圆圈所示。另一方面，超级里程计优于前两种方法，并提供准确的地图和里程计结果。我们还在类似的几何退化情况下评估我们的方法，包括数据集部分中提到的长走廊和受限环境数据序列。每种方法的 ATE 可以在表 II 中找到。

从图 7 中，我们可以看到 LOAM 和 LIO-SAM 都无法创建准确的场景地图。在图 7（a）中，我们可以观察到 LOAM 生成的地图有明显的错位，如黄色圆圈所示。这主要是因为 LOAM 严重依赖于 LiDAR 测量，而 IMU 仅用于初始化激光扫描匹配。在灰尘较多的情况下，由于噪声返回，扫描匹配很可能会失败。LIO-SAM 的性能优于 LOAM，这得益于其紧耦合的 LiDAR-IMU 架构和减少对 LiDAR 测量的依赖，尽管我们仍然可以观察到图 7(b) 中黄色圆圈中的地图未对准。在这个实验中，Super Odometry 优于 LOAM 和 LIO-SAM。它给出了一个准确且对齐良好的地图，如图 7(c) 所示。取得优异结果的主要原因是 Super Odometry 采用基于主成分分析的特征提取方法，并对每个特征的质量进行评估。因此，如果一个特征质量低，它将被视为噪声，而不用于状态估计

实时评估

我们展示了 Super Odometry 的实时性能及其在轻量级车载系统上高效运行的能力。我们在 8 核 Intel Core i7-4790K CPU 上对算法进行了基准测试。首先，我们评估使用动态八叉树代替传统 KD -tree的影响。然后，我们评估每个子模块的处理时间。

正如图 8 中看到的，虽然动态八叉树和 KD-tree的查询时间保持相似，但建树时间明显不同，KD-tree的运行时间随着帧数呈指数增长，而动态八叉树的运行时间几乎保持不变。这是因为动态八叉树可以避免重建整棵树，只通过哈希映射查找更新相关的子树。

我们评估了 Super Odometry 的每个子模块处理时间，并将我们的 Lidar-inertial Odometry 与流行的 LIO-SAM 算法在各种数据集中进行了比较，如表 III 所示。总的来说，Super Odometry可以实现非常高频的状态估计，因为所有子模块并行运行。IMU Odometry 可以直接输出 1000 Hz 的状态估计。Super Odometry 的 LiDAR 过程模块比 LIO-SAM 花费的时间更短。差异在 Dust 数据集中更为显着，其中 LIO-SAM 平均为 102 ms，而 Super Odometry 平均为 21 ms。这是因为在尘土飞扬的环境中，LiDAR 扫描匹配收敛速度较慢，而 Super Odometry 可以拒绝一些不可靠的约束，更好地利用动态八叉树的更快插入和查询。

6 总结

由于缺乏可靠的测量，在感知退化的环境中进行稳健的状态估计非常具有挑战性。为了解决这个问题，我们提出了 Super Odometry，一种以 IMU 为中心的数据处理方案，结合了松耦合和紧耦合的优点。为了提高实时性能，我们建议使用显着提高实时性能的动态八叉树。所提出的方法已经在视觉和几何退化的环境中进行了测试。结果表明，我们的框架对单个传感器故障具有鲁棒性，并且能够在具有挑战性的现实世界场景中实现高精度和弹性的运动估计。