Super Odometry：激光雷达-视觉-惯性多传感器融合，DARPA地下挑

本文转自泡泡机器人

编辑：一点人工一点智能

01  摘要

我们提出了Super Odometry，一个高精度的多模态传感器融合框架，提供了一个简单但高效的方法来融合多种传感器（比如激光雷达，摄像头，IMU），在感知退化的环境中实现了鲁棒的的状态估计。与传统传感器融合方法不同的是，Super Odometry实现了一个IMU为中心的处理流程线，能够融合松耦合与紧耦合方法的优势并且通过从粗到细的方式恢复运动。我们提出的框架由三部分组成，IMU里程计，VIO里程计以及LIO里程计。VIO里程计和LIO里程计提供了先验位姿来约束IMU偏差以及接收来自IMU里程计的运动估计。为了保证实时并且精确的运行，我们使用了一个动态八叉树，同比于KD树只需要花费10%时间。我们提出的系统被部署于无人机和地面机器人上，作为团队探索者一部分参与DARPA地下挑战赛，同时团队分别在隧道赛道和城市电路赛道获得第一名，第二名。

02  主要贡献

我们提出了第一个IMU中心的传感器融合流程线，能够在极端以及视觉挑战的环境下实现精确实时状态估计。

我们提出的流程线结合了紧耦合和松耦合方法的优点，能够提供简单但有效的方法来融合多种传感器。

我们提出使用一种高效的3D点集（动态八叉树）的组织方法，显著的提高了帧匹配的实时性能。

提出的方法在多种物理系统（包括无人机以及地面机器人）实现，并且在不同的具有挑战性的场景（剧烈运动，低光照，长走廊以及甚至于在严重粉尘环境）得到广泛验证。

03  方法概述

3.1 SYSTEM OVERVIEW

机器人系统状态定义：

其中W代表世界坐标系，B代表IMU载体坐标系，L代表激光雷达坐标系，C代表摄像头坐标系。为了简化，设置IMU坐标系作为载体坐标系。

系统流程图如上，我们的系统跟踪这样一个直觉，当IMU的偏差误差被其它传感器很好的约束时，它的状态估计可以非常精准。它使用由VIO和LIO提供的观测来约束线加速度偏差和角加速度。反过来，经过约束的IMU里程计可以为VIO和LIO提供预测，VIO和LIO通过由粗到细的范式来完成运动恢复。另外，对于IMU的重视使得该系统对于环境依赖问题可以达到很好鲁棒结果，例如视觉和激光雷达退化场景。最后该问题架构能够满足一个简单且灵活的方式来集成其它的里程计例如GPS和轮速传感器。

3.2 METHODOLOGY 

Super Odometry使用因子图来建模，我们下面分别介绍三种因子。

A. IMU Odometry Factors

与传统的因子图结构不同的是，IMU里程计的估计状态不仅包含了位置，速度以及旋转，还同时包含了线加速度和角加速度。因子图中的每个节点都和一个状态相关。两个连续的节点之间的边代表着的是从IMU预积分方法得到的相对运动，其它边表示的是局部或者全局约束，这个取决于传感器类型的选择。由于采用的是摄像头和雷达，它们都根据第一个机器人位姿作为参照，我们使用它们的相对位置估计作为局部约束来修正IMU预积分的偏差。

​

1) IMU Preintegration Factor:

​

基于IMU预积分方法，我们能够获取到在连续IMU载体i和j之间的相对运动观测Δr。因此，我们能够通过混合VIO和LIO来构建IMU预积分因子的局部约束。

其中的和 分别代表从载体系到世界坐标系的平移，速度和旋转。值得注意的是，线加速度偏差误差和角加速度偏差误差在图中被联合优化。

​

2) IMU Odometry Optimization:

​

因为我们同时使用VIO和LIO来约束IMU预积分观测，我们可以在同一时刻获得多个IMU预积分残差。这些残差需要使用合适的协方差矩阵W_ij来加权，可以基于观测的可靠程度计算。例如，在视觉退化环境中，使用VIO计算的IMU残差会得到较小的权重。对于新的每一帧，我们最小化一个包含VIO-IMU项，LIO-IMU项以及边缘化先验的优化问题。

B. Laser-Inertial Odometry Factors

IMU预积分因子被用作一个当前帧到地图匹配的运动预测，同时在因子图中将相邻两个雷达帧连接起来。帧到地图的匹配过程可以分为四步。基于PCA的特征提取，多误差尺度的线性平方ICP，LIO因子，动态八叉树。

​

1) PCA Based Feature Extraction:

​

当新一帧雷达帧到来的时候，特征提取将使用主成分分析方法来提取点，线，面特征。局部线性系数，局部平面系数，曲面系数定义如下。

其中

是第个特征值。根据计算得到的三个系数，可以简单的分为点，线，面特征。

​

2) Multi-metric ICP Factors:

​

为了提高Super Odometry的鲁棒性。我们通过联合最小化点到点，点到线，点到平面的距离误差尺度来估计得到最优的运动转移矩阵。我们使用以下公式来求解。

其中的和代表的是对应特征的主成分和法方向。

​

3) Laser-inertial Odometry Optimization:

​

使用LM方法通过最小化下式来得到最优运动估计

其中的 是预测的姿态约束和由IMU里程计提供的IMU预积分因子。当环境存在结构退化时，IMU里程计依旧可靠，所以先验IMU预测位姿误差将在优化问题中起到主导作用。同时，不可靠的激光雷达约束将被拒绝，通过分析它的信息矩阵的特征值来检验当前激光雷达帧的能观性。

​

4) Dynamic Octree:

​

当结束了帧到地图的匹配，新的一帧需要在地图中配准。因此，当每次激光雷达配准完成时，地图将会通过加入或者删除点进行更新。广义的说，大多数激光SLAM方法采用的时KD树来组织3D点以及完成数据关联。同时，我们主张传统KD树并不是组织3D的最好方式，尤其是当这个地图还需要不定时更新。

​

Motivation:

​

上图展示了在全局地图（紫色块）里面加入新点（黄色块）的过程。因为传统的KD树只使用一个树来组织所有点。当加入新的点时，我们需要重建整个KD树。整个树结构需要被随时的改变，我们发现这样是及其消耗时间和计算量的。

​

Key Insight:

​

我们没有改进三维点集的临近点搜索，而是着重研究如何重复利用现有树的结构，并避免在更新地图时重新创建新的KD树。

​

Method Description:

​

我们将地图存储为一个哈希键值表。由于整个世界可以由栅格表示，栅格可以使用栅格世界坐标的哈希函数从哈希表中访问。三维点根据它们的位置坐标存储在不同的栅格中。我们没有只建立一个树，每个栅格都会有它的单独八叉树来存储点，每个八叉树能够通过哈希表来访问。因为我们只需要更新特定的八叉树而不是整个八叉树。它在数据关联上的实时表现相比于传统的方法有显著提升。下面是详细算法流程。

C. Visual-Inertial Odometry Factors

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

​

1) Visual-inertial Odometry Optimization:

​

我们最小化一个非线性优化问题，包含视觉重投影因子，IMU预积分因子，边缘化因子，以及一个来自IMU里程计的先验位姿。以下是它的优化方程。

04  实验结果