本文主要是对Super Odometry论文的一些学习总结，具体的参考文献请参考原文，如有不对之处，希望大家提出宝贵的意见，共同进步~

摘要

我们提出了一种高精度多模态传感器融合框架-----Super Odometry，提供了一种简单而有效的方法来实现多个传感器的融合，如激光雷达、相机和IMU传感器等，并在感知退化环境中实现鲁棒状态估计。与传统的传感器融合方法不同，Super Odometry 采用以 IMU-centric为中心的数据处理方案，结合了松散耦合方法和紧密耦合方法的优点，可以从粗到细的方式恢复运动。该框架由三部分组成:IMU里程计、视觉惯性里程计和激光惯性里程计视觉惯性里程计和激光惯性里程计之前提供先验位姿约束 IMU bias，并从 IMU 里程计接收运动预测。为了确保实时的高性能，我们应用一个动态八叉树，与静态KD树相比，它只消耗10%的运行时间。该系统被部署在无人机和地面机器人上，作为美国国防部高级研究计划局(DARPA)地下挑战赛探险团队努力的一部分，该团队在隧道和城市赛道中分别获得了第一名和第二名。

I. 介绍

多模态传感器融合是自主机器人完成复杂和危险任务的关键，如地下环境感知、工业检测和搜救。在这些没有gps的情况下，黑暗、空气中模糊的条件(灰尘、雾和烟雾)，以及缺乏感知特征是目前阻碍我们使用机器人系统实现长期自主的主要挑战。为了在这样的环境中进行定位，基于激光雷达的里程计[1]-[3]似乎是机器人的合适选择，因为激光雷达传感器可以提供高保真的3D测量。然而，在无结构环境中，如长隧道或存在遮蔽物(如雾、灰尘、烟雾)，基于激光雷达的方法因为退化和离群值而难以提供可靠的运动估计。为了处理这些情况，还需要集成额外的传感器，特别是摄像头。尽管如此，视觉摄像机的用例很大程度上局限于光照条件差的情况。因此，我们认为基于激光雷达的[1]-[3]、基于视觉的[4]、[5]或基于激光视觉的[6]、[7]SLAM方法都不是具有挑战性环境下的最佳选择。由于这些传感器依赖于环境，如果将它们用作构建SLAM系统的主要传感器，则很难实现鲁棒的性能。

近十年来，多模态传感器融合得到了广泛的应用，可分为松散耦合和紧密耦合两种方法。如表I所示，松散耦合方法[1]、[3]、[8]由于其简单性、可扩展性和较低的计算费用而更受青睐。相比之下，紧密耦合方法[2]在准确性和鲁棒性方面具有优势。然而，紧密耦合方法通常难以扩展其他传感器，实现不同传感模式之间的切换方案，从而应对各种环境。此外，紧密耦合方法可能很容易出现潜在的传感器故障，因为它们中的大多数只使用一个状态估计器[8]。相比之下，松散耦合方法将传感器故障风险分散在多个估计引擎之间，具有更强的鲁棒性能。然而，由于松散耦合方法大多采用帧对帧估计，在精度和鲁棒性方面仍存在一定的局限性。

表I     多模态传感器融合算法比较

基于以上讨论，我们结合紧密耦合方法和松散耦合方法的优点，提出了一个IMU-centric SLAM系统。该系统的设计遵循一个关键的见解:惯性测量单元(IMU)可以产生带有噪声的平滑测量，但很少有异常值，这可以使其估计非常准确，只要偏差漂移可以很好地受到其他传感器的约束。因此，Super Odometry是围绕IMU作为主要传感器而设计的。作者所作的主要工作如下：

• 我们提出了首个以IMU-centric传感器融合框架，能够在极端和感知挑战的环境中实现精确的实时状态估计。
• 该框架结合了紧耦合方法和松耦合方法的优点，为多传感器融合提供了一种简单而有效的方法。
• 我们提出使用一种高效的动态八叉树，显著提高扫描匹配的实时性。
• 该方法被部署在包括无人机和地面机器人在内的多个物理系统上，并在各种具有挑战性的场景下进行了广泛的评估，这些场景包括剧烈运动、光线不足、长走廊，甚至是在重粉尘环境中。

II. 相关工作

近年来，人们对激光-视觉-惯性估计方法进行了大量的研究，可将将其分为松散耦合方法和紧密耦合方法。

A. 松耦合激光-视觉-惯性里程计

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

B. 紧耦合激光-视觉-惯性里程计

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

图1：Super Odometry在DARPA Subter-ranean Challenge Alpha Course中的性能，这是具有挑战性的环境，包括黑暗(b)、大雾(c )和竖井(d)。(a)表示由多机器人（UGV1、UAV1和UAV2）在Alpha Course过程中Super Odometry的重建地图结果；绿线、橙色线和红线分别是UGV1、UAV1和UAV2的估计轨迹。

 

C. 方法要点

与以前的工作相比，Super Odometry的优点是：

• 鲁棒性和可扩展性：我们的IMU-centric的传感器融合架构（见图2）使我们能够实现高精度，并以低故障率运行，因为IMU传感器与环境无关。该系统包括故障安全机制，并提供了一种更简单、灵活的方式来融合多个传感器，如GPS、车轮里程计等。
• 简单但有效：Super Odometry避免了融合多个传感器的复杂推导，因为IMU具有简单而准确的概率模型。只要其他传感器能够提供相对先验位姿去约束IMU预积分因子，这些传感器就可以成功融合到系统中。特别是，作为团队探险家应对DARPA地下挑战的努力的一部分，它在具有挑战性的环境中得到了广泛的评估。
• 低CPU使用率和高实时性能：由于Super Odometry不会将所有传感器数据组合成一个完整的因子图，相反，它将大因子图划分为几个“子因子图”，每个“子因子图”接收来自IMU预积分因子的预测（见图2）。因此，每个里程计因子的运动以并行方式从粗到细地恢复，这显著提高了实时性能。此外Super Odometry采用动态八叉树来保存3D点，这使得扫描匹配非常有效。

III.系统概述

图2：Super Odometry算法概述。IMU里程计利用视觉惯性里程计和激光雷达惯性里程计提供的观测值来限制IMU bias。作为回报，受限IMU里程计为视觉里程计和激光里程计提供预测。

 
我们首先定义了在本文中使用的框架和符号，如图4(e)所示。我们将世界框架表示为W，IMU主体框架表示为B，激光雷达框架表示为L，相机框架表示为C。为简单起见，我们将IMU框架视为我们的机器人主体框架，我们的目标是估计IMU主体框架相对于固定世界框架W的位置。机器人状态$x_k$可以写为：
$$x_k=[p_{b_k}^w,v_{b_k}^w,q_{b_k}^w,b_a,b_g],k\in [0,n](1)$$
其中，第k个机器人状态$x_k$分别包含位置$p_{b_k}^w$、速度$v_{b_k}^w$、IMU中心的旋转$q_{b_k}^w$以及加速度计bias $b_a$和陀螺仪bias $b_g$。
提出的系统的概述如图2(b)所示。它由三个模块组成：IMU里程计、视觉惯性里程计（VIO）和激光雷达惯性里程计（LIO）。我们的系统设计遵循一个关键观点：只要零偏受到其他传感器的良好约束，IMU及其状态估计就可以非常精确。因此，Super Odometry使用IMU作为主要传感器。它使用视觉惯性里程计（VIO）和激光雷达惯性里程计（LIO）提供的观测值来约束加速度计bias $b_a$和陀螺仪bias $b_g$ 。作为回报，受约束的IMU里程计向VIO和LIO提供预测，VIO和LIO又粗到细的方式恢复运动。此外，以IMU为中心使框架对状态估计中的环境相关问题（如几何或视觉退化）更具鲁棒性。最后，该算法架构允许以简单灵活的方式合并其他里程计源，如GPS和车轮里程计。

IV. 方法论

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

A. IMU里程计因子

IMU里程计系数的结构如图2（a）所示。类似于传统的图优化结构。然而，不同之处在于，IMU里程计的估计状态不仅包括位置$p_{b_k}^w$、速度$v_{b_k}^w$、IMU中心的旋转$q_{b_k}^w$，还包括加速度计bias $b_a$和陀螺仪bias $b_g$。图优化中的每个节点都与状态关联。节点密度由最低频率里程计确定。两个连续节点之间的边表示通过IMU预积分获得的相对运动。其他边可以是局部约束或全局约束，这取决于传感器的类型。由于相机和激光雷达不是全局参考的，并且它们的里程计基 于机器人的第一个位姿，因此我们仅使用它们的相对状态估计作为局部约束来校正IMU预积分的bias。

1) IMU预积分因子:

基于[17]中提出的IMU预积分方法，我们可以获得IMU连续的第i帧和第j帧之间的相对运动测量值 $\Delta r=(\widehat{{\alpha }_j^i},\widehat{{\beta }_j^i},\widehat{{\gamma }_j^i})$。因此，我们可以结合视觉惯性里程计和激光惯性里程计来推导IMU预积分因子的局部约束。

其中，旋转矩阵$R$和四元数$q$用于表示旋转。$p_{b_k}^w$、$v_{b_k}^w$和$q_{b_k}^w$是从IMU坐标系到世界坐标系的平移、速度和旋转，可由VIO和LIO获得。$g^w$是世界坐标系中的重力矢量。$b_w$$b_g$分别是加速度计偏置和陀螺仪bias, $\ominus$是对IMU残差的负运算。请注意，加速度计 bias和陀螺仪 bias在图中联合优化。

2) IMU里程计优化:

由于我们同时使用VIO和LIO来约束IMU预积分测量（见等式（2）），因此我们可以同时获得多个IMU预积分残差。必须使用适当的协方差矩阵$W_{ij}$对这些残差进行加权，该协方差矩阵可根据观测的可靠性进行计算。例如，在视觉退化环境中，VIO计算的IMU残差$e_{ij}^{VIO}$的权重较低。相反，在几何退化环境中，LIO计算的IMU残差$e_{ij}^{LIO}$的权重较低。对于每个新的帧，我们最小化一个优化问题，该问题由VIO-IMU项、LIO-IMU项和之前的边缘化项$E_m$组成。

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

B.激光惯性里程计因子

激光惯性里程计系数的结构如图2（c）所示。由于IMU里程计计算效率高，输出频率很高，因此其IMU预积分因子可以自然地添加到激光里程计的因子图中。IMU预积分因子将用作当前扫描地图匹配的运动预测，并连接因子图中的连续激光雷达帧。扫描地图匹配过程可分为三个步骤，即：（a）基于PCA的特征提取（b）多尺度线性平方ICP（c）激光里程计因子（d）动态八叉树。

1) 基于PCA的特征提取

当系统接收到新的激光雷达扫描时，将执行特征提取。使用主成分分析（PCA）方法提取点、线和平面特征。文献[18]中定义几何特征的局部线性度${\sigma }_{1D}$、平面度${\sigma }_{2D}$和曲率${\sigma }_{3D}$。

其中${\sigma }_i=\sqrt{{\lambda }_i}$,而λi是PCA正常计算的特征值。根据式（4）及其原理法线方向，点、线和平面特征Fi+1={Fi+1po，Fi+1li，Fi+1pl}\mathbb{F_{i+1}}=\{ F_{i+1}^{po} ，F_{i+1}^{li} ， F_{i+1}^{pl} \}Fi+1​={Fi+1po​，Fi+1li​，Fi+1pl​}可以很容易地进行分类。

2) 多尺度ICP因子

在提取可靠的几何特征后，我们使用IMU里程计提供的运动预测，并将${\mathbb{F}}_{i+1}$从B坐标系转换为W坐标系。然后，我们使用提出的动态八叉树在地图中找到其点、线和平面对应关系。为了简洁起见，下一节将描述详细的步骤。
为了提高Super Odometry的鲁棒性，我们期望估计联合最小化点到点、点到线和点到平面距离误差度量的最佳变换，可使用以下等式计算：

其中$e_i^{po\to po},e_i^{po\to li},e_i^{po\to pl}$是点到点（直线、平面）距离。$v_j$和$n_k$表示特征的相应线特征方向向量和面特征法线方向。$T_{i+1}=R,t$是我们期望估计的最佳变换。

3) 激光惯性里程计优化

使用levenberg-marquardt方法通过最小化以下各项来求解最佳变换：

$e_{imuodom}^{prior}$和$e^{imu}$是IMU里程计提供的预测姿态约束和IMU预积分因子。当环境几何退化时，由于IMU里程计可以接收来自其他传感器的测量值，IMU里程计仍然可靠，$e_{imuodom}^{prior}$将在优化问题中占主导地位。同时，通过分析信息矩阵$w_i$的特征值来评估当前激光扫描的可观测性，从而拒绝不可靠的激光约束。然而，当环境结构良好时，多尺度ICP因子将在优化问题中占主导地位。

4) 动态八叉树

完成 scan-to-map匹配后，新的激光帧需要在地图上注册。因此，地图将在每个激光雷达注册步骤后通过添加或删除点进行更新。一般来说，大多数激光SLAM方法采用KD树方法来管理三维点并实现数据关联。然而，我们认为传统的KD树并不是管理3D点的最佳选择，尤其是当地图频繁更新时。

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

 

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

关键洞察：我们没有改进三维点集的邻近搜索，而是将更多精力放在如何重用现有树的结构上，并避免在地图更新时重新创建新树。为了实现这一点，我们提出了一种更有效的三维点组织方法，称为动态八叉树，以促进数据关联。

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

C.视觉惯性里程计因子

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

1) 视觉惯性里程计优化

对于每个新的关键帧，我们最小化一个非线性优化问题，该问题由视觉重投影因子$e_{reproj}$、IMU预积分因子$e_{i}mu$、边缘化因子$e_m$和IMU里程计的先验位姿$e_{imuodom}^{prior}$组成。视觉惯性里程计因子的结构如图2（b）所示。

**obs(i)**表示包含由其他帧跟踪的视觉特征点$i$的集合。集合C包含由IMU因子连接的成对视觉节点（a，b）。$W_{reproj}$和$W_{imu}$视觉重投影因子和IMU预积分因子的协方差矩阵

与上一节的解释类似，当环境视觉退化时，先验的$e_{imuodom}^{prior}$将在优化问题中占主导地位，通过分析信息矩阵将拒绝不可靠的视觉因素。然而，当环境光照条件较好时，视觉因素在优化问题中占主导地位，IMU预积分因子仅提供视觉特征跟踪的初始值。

V.实验

未完待续~

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