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

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

摘要

我们提出了超级模测量，一种高精度的多模态传感器融合框架，提供了一种简单而有效的方法来融合多个传感器，如激光雷达、相机和IMU传感器，并在感知退化环境中实现稳健的状态估计。与传统的传感器融合方法不同，超级模测量法采用了以图像为中心的数据处理管道，它结合了松散耦合方法和紧密耦合方法的优点，以粗到细的方式恢复运动。

该框架由三部分组成：IMU测程法、视觉惯性测程法和激光雷达-惯性测程法。视觉惯性测程法和激光雷达-惯性测程法提供了在约束IMU偏差之前的姿态，并从IMU测程法中接收运动预测。

为了确保实时的高性能，我们应用了一个动态八叉树，与静态kd树相比，它只消耗了10%的运行时间。该拟议中的系统被部署在无人机和地面机器人上，作为探索者团队对DARPA地下挑战的努力的一部分，该团队分别在隧道和城市电路1中获得了第一名和第二名。

一、简介

多模态传感器融合对于自主机器人完成复杂和危险的任务至关重要，如在地下环境中的感知、工业检查和搜索和救援。在这些被gps拒绝的场景中，黑暗、空气中的遮蔽条件（灰尘、雾和烟雾）以及缺乏感知特征是目前阻碍我们使用机器人系统进行长期自主性的主要挑战。

为了在这样的环境中定位，基于激光雷达的测程仪[1]-[3]似乎是机器人的合适选择，因为激光雷达传感器可以提供高保真的3D测量。然而，在无结构的环境中，如长隧道或模糊物的存在（如雾、灰尘、烟雾），由于退化和异常值，基于激光雷达的方法无法提供可靠的运动估计。为了处理这些情况，还需要集成其他传感器，特别是摄像头。

尽管如此，视觉相机的用例仅限于照明良好的环境。因此，我们认为，基于激光雷达的[1]-[3]、基于视觉的[4]、[5]或基于激光雷达的[6]、[7]SLAM方法都不是在具有挑战性的环境中的最佳选择。由于这些传感器依赖于环境，因此如果将它们作为主要传感器来构建SLAM系统，就很难实现稳健的性能。 

图1：超级气味在DARPA地下挑战Alpha课程中的表现，这是一个具有挑战性的环境，包括黑暗(b)、重雾(c)和竖井(d).(a)表示多机器人(UGV1、UAV1、UAV2)重建的Alpha过程中超模测量的映射结果；绿色、橙色和红线分别为UGV1、UAV1和UAV2的估计轨迹

近十年来，多模态传感器融合得到了广泛的应用，可分为松散耦合方法和紧密耦合方法。如表I所示，松散耦合方法[1]、[3]、[8]因其简单性、可扩展性和计算费用低而更受青睐。相比之下，紧密耦合方法[2]在其精度和鲁棒性方面具有优势。然而，紧密耦合的方法通常难以扩展其他传感器，并实现在传感模式之间的切换方案，以应对各种环境。

此外，紧密耦合的方法可能容易处理潜在的传感器故障，因为其中大多数方法只使用单一的估计引擎[8]。相比之下，松散耦合的方法将传感器故障风险分配到多个估计引擎之间，并且应该具有更稳健的性能。然而，由于大多数松散耦合方法采用帧间估计，它们在精度和鲁棒性方面仍存在局限性。

基于上述讨论，我们提出了一个结合紧密耦合方法和松散耦合方法的优点的无全中心的SLAM系统。该系统设计遵循一个关键的见解：惯性测量单元(IMU)产生平滑的噪声测量，但很少异常值，只要偏移非常准确，偏移可以很好地约束。因此，超光学测量法是围绕IMU进行设计的，作为主传感器。

 

我们做的主要贡献如下：

• 我们提出了第一个以imu为中心的传感器融合管道，它能够在极端和具有感知挑战性的环境中实现准确的实时状态估计。

• 该管道结合了紧密耦合方法与松散耦合方法的优点，为融合多个传感器提供了一种简单而有效的方法。

• 我们提出使用一种有效的三维点组织（动态八叉树），这显著提高了扫描匹配的实时性能。

• 提出的方法被部署在多个物理系统上，包括无人机和地面机器人，并在各种具有挑战性的场景中进行了广泛的评估，包括侵略运动、弱光、长走廊，甚至在重灰尘环境中。

二、相关工作

近年来，人们对激光雷达-视觉惯性估计器做了大量的研究，它可以分为松散耦合方法和紧密耦合方法。

A松耦合Laser-Visual-Inertial里程计

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

B紧耦合Laser-Visual-Inertial里程计

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

C方法亮点

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

• 简单而有效：超级光学测量法避免了复杂的推导，因为IMU有一个简单而准确的概率模型。只要其他传感器能够在约束IMU预积分因子之前提供相对的姿态，这些传感器就能成功地融合到系统中。特别是，作为团队探险者对地下挑战的一部分，它已经在具有挑战性的环境中进行了广泛的评估。

• 低CPU使用率和高实时性能：由于超光学测量法没有将所有传感器数据合并成一个成熟的因子图，而是将大因子图划分为几个“子因子图”，每个“子因子图”接收来自IMU预积分因子的预测（见图2）。因此，每个测程因子的运动从粗恢复到细恢复，显著提高了实时性能。此外，超级光学测量法采用了动态八叉树来组织三维点，这使得扫描匹配非常有效。

三、系统综述

我们首先定义了我们在整个论文中使用的框架和符号。我们表示世界框架为W，IMU身体框架为B，激光雷达框架为L和相机框架为c。为简单起见，我们将把IMU框架作为我们的机器人身体框架和我们的目标是估计IMU身体框架的位置相对于一个固定的世界框架w。机器人状态xk可以写成：

 

其中，第k个机器人状态xk由位置pwbk、速度vbwk、IMU中心的方向qwbk和ba、bg分别为加速度计偏置和陀螺仪偏置组成。

该系统的概述如图2所示。它由三个模块组成：IMU测程法，视觉惯性法，测程法(VIO)和激光雷达惯性测程法(LIO)。我们的系统设计遵循了一个关键的见解：只要偏置漂移受到其他传感器的良好约束，IMU及其状态估计就可以非常精确。因此，超级光学测量法使用IMU作为主要传感器。它利用视觉惯性测程法(VIO)和激光雷达惯性测程法(LIO)提供的观测结果来约束加速度计偏置ba和陀螺仪偏置bg。作为回报，约束IMU测程法为VIO和LIO提供了预测，它们以从粗到细的方式恢复运动。此外，对IMU的关注使管道对状态估计中的环境相关问题更健壮，如几何或视觉退化。最后，该算法架构允许一种简单和灵活的方式来合并其他测速源，如GPS和车轮测速源。

图2：超光学分析算法概述。IMU测程法利用视觉测程法和激光雷达测程法提供的观测结果来约束IMU偏差。作为回报，约束IMU测程提供了视觉测程和激光雷达测程的预测。同时，可以根据计算资源来调整图节点的大小，而IMU光学测量法提供了最终的状态估计。

四、方法论

超级光学测量是使用因子图建模的。本文主要介绍了三种因数因子：(a)IMU；(b)激光雷达惯性；和(c)视觉惯性测程因子。

A IMU Odometry Factors

IMU测程因子的结构如图所示。 2.(a)，它类似于传统的姿态图结构。但是，由于IMU测程法的估计状态不仅包含位置pwb、速度vbw和方向qwb，还包含加速度计ba和陀螺仪偏置bg。姿态图中的每个节点都与一个状态相关联。节点的密度由最低频测程法决定。两个连续节点之间的边表示由IMU预积分法得到的相对体运动。其他边可以是依赖于传感器类型的局部约束或全局约束。

【以下内容包含大量公式，看原文】

1）IMU预整合因子:

2）相对姿势因子：

3）IMU Odometry Optimization：

B 激光雷达惯性测量因子

激光雷达惯性测速因子的结构如图所示。 2.(c).由于IMU测程法计算效率高，输出频率很高，因此它的IMU预积分因子可以自然地添加到激光雷达测程法的因子图中。IMU预积分因子将用于当前扫描图匹配的运动预测，并在因子图中连接连续的激光雷达帧。扫描图匹配过程的管道可分为四个步骤，即基于(a)pca的特征提取(b)多度量线性正方形ICP(c)激光惯性测量因子(d)动态八叉树。

1）基于PCA的特性提取：

2）多度量ICP因子：

3）惯性雷达里程计优化：

【以上内容包含大量公式，看原文】

 

图3：3DKD树与动态八叉树的对比。(a、b)显示了当添加新的点云（灰色圆圈）时，3DKD-tree的构建过程，这需要改变整棵树的结构。（c、d）表示添加新的点云（灰圆）时动态八叉树的构建过程，只需要改变子树的结构。

4）动态八叉树：

大多数的LiDAR SLAM方法都采用KD-tree方法来组织三维点并实现数据关联。但是，我们认为传统的KD树组织3D点非常耗时，因为它只使用一棵树来组织所有的点，而且每次添加新点时都需要重新创建KD-树，如图3(b)所示。

为了解决这个问题，我们建议使用一个更有效的三维点组织称为动态八叉树，以促进数据关联。动态八叉树是基于Behley[19]的工作。它将映射存储为一个哈希表，如图3.(d).所示世界由体素表示，体素可以通过使用它们的XYZ索引从哈希表中访问。而不是只构建一棵树，每个体素将有它的八叉树来组织点，每个八叉树都可以通过一个哈希表访问。由于我们只需要更新特定的八叉树，而不是整个树，因此数据关联的实时性能将得到显著提高。

C 视觉惯性测量因子

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

1）视觉惯性里程计优化：

对于每一个新的关键帧，我们最小化了一个非线性优化问题，其中包括视觉重投影因子ereproj、IMU预积分因子eimu、边缘化因子Em和IMU测程E的姿态先验。视觉惯性测程因子的结构如图2(b).所示

 

obs(i)表示一个包含被其他帧跟踪的视觉特征i的集合。集合C包含由IMU因子连接的视觉节点(a、b)对。Wremej和Wimu是视觉重投影因子和IMU预积分因子的协方差矩阵。

与上一节的解释类似，当环境被视觉退化时，E proor_imuodom 将在优化问题中占主导地位，通过分析信息矩阵将拒绝不可靠的视觉因子。然而，当环境具有良好的光照条件时，视觉因子将在优化问题中占主导地位，而IMU预积分因子仅为视觉特征跟踪提供了初始猜测。

五、实验

在本节中，我们评估了超级里程计与其他最先进的算法对退化传感器输入的鲁棒性。然后，我们评估了算法的实时性能。我们的实验视频可以通过这个链接找到：https://sites.google.com/view/superodometry。

 

图4：（a-d）显示了我们收集数据集的具有挑战性的环境。(e)显示了我们的多传感器设置。

A 数据集

我们用团队探索者的DS无人机(图4(e))收集了我们的测试数据集，它部署在DARPA的地下挑战中。它有一个多传感器设置，包括一个动态动力VLP-16激光雷达，一个XsensIMU，一个带广角鱼眼镜头的uEye相机，和一个英特尔NUC板载电脑。数据序列被设计为包括视觉和几何退化的场景，这对于基于相机和激光雷达的状态估计尤其麻烦。

测试序列列表如下：

• 暗室：手动进出暗室，如图4(c).所示

• 白墙：手动行走，摄像头面向无特征的白色墙，如图4(b).所示

• 约束环境：在楼梯井中手动行走，如图4(b).所示

• 长走廊：手动步行穿过公寓楼内的长走廊，如图4(a).所示

• 灰尘：自动穿过布满灰尘的地下洞穴，如图4(d).所示

• 图一

用于定量分析的轨迹地面真实分析在暗室、长走廊、白墙和受限环境上，是由传感器套件顶部的全站仪(TS)和跟踪棱镜生成的。Dust和Urban Challenge Alpha Course没有真实的轨迹，用于地图质量和实时性能分析。

 

 

图6：长走廊数据序列中LIO-SAM、LOAM和超级优势法的图谱比较。上面的图像显示自上而下的视图，下面的图像显示结果地图的z漂移。我们可以看到，超级光学测量法是没有错位的，并给出了一个更高质量的地图。

B 鲁棒性和准确性评估

我们的数据集包含视觉上和几何上退化的场景。这种情况在地下环境中很常见，比如在DARPA地下挑战的每个阶段所遇到的情况。我们将我们的方法与其他基于激光雷达的测程法(LOAM[1]，LIOSAM[2])和基于视觉的测程法(VINS[4]，我们的深度增强VINS[20])进行了比较。我们采用EVO2软件包来计算每个估计轨迹对地面真实轨迹的平移绝对轨迹误差(ATE)。ATEs的最大值和RMSE值结果见表二。值得一提的是，在实验中没有使用循环关闭步骤。我们只关注于比较算法的测程部分。

首先，我们考虑视觉退化的环境，其中基于相机的算法预计会给出较差的结果。接下来，我们考虑几何退化的环境，其中基于激光雷达的算法通常会受到影响。最后，我们同时考虑这两种类型的退化。我们证明了超模法在所有情况下都具有更好的精度。

1)在视觉退化环境中的鲁棒性比较:这里我们展示了暗室序列的结果。在这个数据序列中，我们手持无人机，经过一个光线昏暗的房间，并伴有激进的动作。图4.(c)所示图像大多为暗色，视觉里程计前端特征提取具有一定的挑战性。

图5显示了算法的输出轨迹与全站的地面真实轨迹的比较。考虑到表二中所示的绝对轨迹误差(ATE)，我们可以看到纯基于视觉的方法在这种情况下的性能最差，正如预期的那样。我们的以图像为中心的超模测量法优于上述方法，最低的ATE为0.174m。我们还在数据集部分中提到的白墙数据序列上评估了我们的方法。每种方法的ATE值见表二。欲了解更多细节，请参考读者阅读所附的视频。

2)几何退化环境下的鲁棒性比较：在这个实验中，我们在长走廊数据序列图上运行了算法。 4.(a).这是一个几何上退化的情况。它在公寓建筑、医院和工厂等人造环境中很常见。由于缺乏几何特征和重复的结构，可能会给基于激光雷达的算法带来问题。

图6显示了该数据序列中来自LOAM、LIO-SAM和超级模测法的结果图。一方面，我们观察到在LIO-SAM和LOAM图中的黄色圆圈中所示的错位和显著的漂移。另一方面，超测程法优于前两种方法，并给出了准确的贴图和测程法结果。我们还在类似的几何退化情况下评估了我们的方法，包括数据集部分中提到的长走廊和约束-环境数据序列。每种方法的ATE值见表二。可以看出，Super Odometry的表现更为出色,其他方法的最低ATE为0.055m。要了解更多细节，请参阅附带的视频。

3)在视觉和几何退化环境下的鲁棒性比较：在这里，我们在尘埃数据集上运行了算法。这个实验显示了该算法克服空气中的模糊物的能力。尘埃减少图像中有用特征的数量，给视觉测程带来了挑战。这也是激光雷达测程法的一个问题，因为它阻塞了具有尘埃粒子的传感器视场，因此我们获得了更少的几何特征。图4(d)为本实验中无人机推力产生的大量尘埃。这两种基于视觉的方法(VINS-Mono和深度增强的VINS)在这个实验中都失败了，因为照相机为尘埃中的状态估计提供的信息有限。

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

C 实时性能评估

在本节中，我们将展示超级光学测量器的实时性能，以及它在轻量级机载系统上有效运行的能力。我们在一个8核的英特尔酷睿i7-4790KCPU上对这些算法进行了基准测试。首先我们评估用动态八叉树代替传统KD树。然后，我们计算每个子模块的处理时间

1)动态八叉树性能：为了讨论我们的动态八叉树与传统KDtree的性能，我们比较了动态八叉树和kd树在户外环境中的插入和查询时间。

 

图8：动态八叉树的运行时性能vs。户外环境中的KD树。

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

2)子模块运行时：我们评估了超级光学测量法的每个子模块处理时间，并在各种数据集中将我们的激光雷达-惯性光学测量法与流行的LIO-SAM算法进行了比较，如表三所示。总的来说，超级模测量可以实现非常高频的状态估计，因为所有子模块都是并行运行的。需要注意的是，除了顺序处理数据外，超测程法可以异步实现传感器融合。每个子模块的优化器将缓存约束条件，并以自己的频率小批量处理它们。因此，总的运行时间将由最大的子模块处理时间来决定，这显著提高了实时性能。

表二：对LOAM、LIO-SAM、VINS、VINS-深度和我们在具有挑战性的环境下运行的方法的准确性评估

 

TABLE III: The average running time of Super Odometry in various dataset on desktop PC (with Intel Core i7-4790K CPU (ms))

                

 

IMU测深法可以直接输出1000Hz的状态估计。超光学的激光雷达过程模块比LIO-SAM花费的时间更短。在Dust数据集中，差异更为显著，其中LIO-SAM平均102ms，而超级光学测量法平均21ms。这是因为在尘土飞扬的环境中，激光雷达扫描匹配收敛速度慢，超模测量可以拒绝一些不可靠的约束，更好地利用动态八叉树的快速插入和查询。

六、总结

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