编译：廖邦彦
审核：王志勇
这是泡泡点云时空推送的第230篇文章，欢迎个人转发朋友圈；其他机构或自媒体如需转载，后台留言申请授权
摘要
我们提出了Super Odometry，一个高精度的多模态传感器融合框架，提供了一个简单但高效的方法来融合多种传感器（比如激光雷达，摄像头，IMU），在感知退化的环境中实现了鲁棒的的状态估计。与传统传感器融合方法不同的是，Super Odometry实现了一个IMU为中心的处理流程线，能够融合松耦合与紧耦合方法的优势并且通过从粗到细的方式恢复运动。我们提出的框架由三部分组成，IMU里程计，VIO里程计以及LIO里程计。VIO里程计和LIO里程计提供了先验位姿来约束IMU偏差以及接收来自IMU里程计的运动估计。为了保证实时并且精确的运行，我们使用了一个动态八叉树，同比于KD树只需要花费10%时间。我们提出的系统被部署于无人机和地面机器人上，作为团队探索者一部分参与DARPA地下挑战赛，同时团队分别在隧道赛道和城市电路赛道获得第一名，第二名。
主要贡献
方法概述
SYSTEM OVERVIEW
机器人系统状态定义
其中W代表世界坐标系，B代表IMU载体坐标系，L代表激光雷达坐标系，C代表摄像头坐标系。为了简化，设置IMU坐标系作为载体坐标系。
系统流程图如上，我们的系统跟踪这样一个直觉，当IMU的偏差误差被其它传感器很好的约束时，它的状态估计可以非常精准。它使用由VIO和LIO提供的观测来约束线加速度偏差和角加速度。反过来，经过约束的IMU里程计可以为VIO和LIO提供预测，VIO和LIO通过由粗到细的范式来完成运动恢复。另外，对于IMU的重视使得该系统对于环境依赖问题可以达到很好鲁棒结果，例如视觉和激光雷达退化场景。最后该问题架构能够满足一个简单且灵活的方式来集成其它的里程计例如GPS和轮速传感器。
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:
当新一帧雷达帧到来的时候，特征提取将使用主成分分析方法来提取点，线，面特征。局部线性系数，局部平面系数，曲面系数定义如下。
其中
λ_i是第i个特征值。根据计算得到的三个系数，可以简单的分为点，线，面特征。
2) Multi-metric ICP Factors:
为了提高Super Odometry的鲁棒性。我们通过联合最小化点到点，点到线，点到平面的距离误差尺度来估计得到最优的运动转移矩阵。我们使用以下公式来求解。
其中的v_j和n_k代表的是对应特征的主成分和法方向。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里程计的先验位姿。以下是它的优化方程。
实验结果
Abstract
We propose Super Odometry, a high-precision multi-modal sensor fusion framework, providing a simple but effective way to fuse multiple sensors such as LiDAR, camera, and IMU sensors and achieve robust state estimation in perceptually-degraded environments。 Different from traditionalsensor-fusion methods, Super Odometry employs an IMUcentric data processing pipeline, which combines the advantages of loosely coupled methods with tightly coupled methods and recovers motion in a coarse-to-fine manner。
The proposed framework is composed of three parts: IMU odometry, visualinertial odometry, and laser-inertial odometry。 The visualinertial odometry and laser-inertial odometry provide the pose prior to constrain the IMU bias and receive the motion prediction from IMU odometry。 To ensure high performance in real-time, we apply a dynamic octree that only consumes 10% of the running time compared with a static KD-tree。 The proposed system was deployed on drones and ground robots, as part of Team Explorer’s effort to the DARPA SubterraneanChallenge where the team won 1st and 2 nd place in the Tunnel and Urban Circuits 1, respectively。
如果你对本文感兴趣，想要下载完整文章进行阅读，可以关注【泡泡机器人SLAM】公众号。
欢迎来到泡泡论坛，这里有大牛为你解答关于SLAM的任何疑惑。
有想问的问题，或者想刷帖回答问题，泡泡论坛欢迎你！
泡泡网站：
泡泡论坛：
泡泡机器人SLAM的原创内容均由泡泡机器人的成员花费大量心血制作而成，希望大家珍惜我们的劳动成果，转载请务必注明出自【泡泡机器人SLAM】微信公众号，否则侵权必究！同时，我们也欢迎各位转载到自己的朋友圈，让更多的人能进入到SLAM这个领域中，让我们共同为推进中国的SLAM事业而努力！
泡泡机器人SLAM的原创内容均由泡泡机器人的成员花费大量心血制作而成，希望大家珍惜我们的劳动成果，转载请务必注明出自【泡泡机器人SLAM】微信公众号，否则侵权必究！同时，我们也欢迎各位转载到自己的朋友圈，让更多的人能进入到SLAM这个领域中，让我们共同为推进中国的SLAM事业而努力！
商业合作及转载请联系paopaorobot@163.com
限 时 特 惠： 本站每日持续更新海量各大内部创业教程，一年会员只需98元，全站资源免费下载 点击查看详情
站 长 微 信： lzxmw777
