VINS系列|VINS-Fusion论文精读：一种通用的基于优化的多传感器局部里程计估计框架（上）

摘要

如今，越来越多的传感器装载在机器人上，以增加鲁棒性和自主能力。我们已经看到装载在不同平台上的各种传感器套件，例如地面车辆上的双目相机、移动手机上的单目相机和惯性测量单元（IMU），以及空中机器人上的双目相机和IMU。尽管用于状态估计的很多算法已经在过去几年中被提出，但是它们通常被应用于单个传感器或者一个特定的传感器套件。其中很少有算法能够用于多种传感器选择。在本文中，我们提出一种通用的基于优化的里程计估计框架，它支持多种传感器套件。每个传感器被作为我们框架中的一个通用因子。共享共同状态变量的因子被加到一起以构建优化问题。我们进一步展示视觉和惯性传感器的通用性，这形成三种传感器套件（双目相机、单目相机和IMU，以及双目相机和IMU）。我们在公开数据集上和通过多传感器的真实世界实验验证了本文系统的性能，其结果与其它最先进的算法进行比较。我们强调，本文系统是一种通用的框架，它能够在位姿图优化中容易地融合各种传感器。我们的实现已经开源。

介绍

实时6自由度（DoF）状态估计是机器人学中的一种基础技术。精确的状态估计在机器人探索、自动驾驶、虚拟现实（VR）和增强现实（AR）等各种智能应用中发挥着重要作用。我们在这些应用中使用的最常见传感器为相机。在过去几十年中，已经提出了大量令人印象深刻的基于视觉的位姿估计算法。除了相机之外，IMU是另一个用于状态估计的主流选择。IMU能够以高频测量加速度和角速度，这对于实时应用中低延迟位姿反馈是必要的。因此，很多研究工作将IMU和视觉融合到一起。另一个用于状态估计的主流传感器为激光雷达。基于激光雷达的方法在受限的局部环境中实现了精确的位姿估计。尽管在过去已经提出了很多算法，但是它们通常被应用于单个输入传感器或者特定的传感器套件。

最近，我们已经看到装载各种传感器组件的平台，例如地面车辆上的双目相机、移动电话上的单目相机和IMU，以及空中机器人上的双目相机和IMU。然而，由于大多数传统算法是针对单个传感器或者特定的传感器组件而设计的，因此它们不能够被移植到不同的平台。即使对于一个平台，我们也需要在不同的场景中选择不同的传感器组合。因此，需要能够支持不同传感器套件的通用算法。另一种实际的需求是，在传感器失效的情况下，应该移除非主动传感器，并且将另一种传感器快速加入到系统中。因此，需要一种与多传感器兼容的通用算法。

在本文中，我们提出一种通用的基于优化的位姿估计框架，它支持多传感器组合。我们使用视觉和惯性传感器进一步进行演示，这形成了三种传感器套件（双目相机、单目相机和IMU，以及双目相机和IMU）。我们能够容易地在不同传感器组合之间进行切换。我们强调本文的贡献如下：

1）一种通用的基于优化的状态估计框架，它支持多传感器；

2）使用视觉和惯性传感器进行状态估计的详细推导，这些传感器形成不同的传感器套件（双目相机、单目相机+IMU以及双目相机+IMU）；

3）所提出系统在公开数据集和现实环境中进行评估；

4）为学习社区开源代码。

相关工作

在过去几十年中，状态估计一直是一个热门的研究课题。大量算法着重于精确的6自由度位姿估计。我们已经看到很多使用一种类型传感器工作的令人印象深刻的方法，例如基于视觉的方法、基于激光雷达的方法、基于RGB-D相机的方法和基于事件相机的方法。使用单目相机工作的方法难以实现6自由度位姿估计，因为无法从单个相机恢复绝对尺度。为了提高可观性和鲁棒性，将具有互补性质的多传感器融合到一起。

多传感器融合方法有两种趋势。一种是基于滤波的方法，另一种是基于优化的方法。基于滤波的方法通常通过扩展卡尔曼滤波器（EKF）实现。视觉和惯性测量通常被一起滤波，以进行6自由度状态估计。高频的惯性传感器被用于状态传播，视觉测量被用于状态更新。MSCKF是一种基于EKF的视觉惯性里程计（VIO），它维护若干相机位姿，并且利用多个相机视图来构成多约束更新。基于滤波的方法通常更早地线性化状态，并且受到不精确线性化点引起的误差的影响。为了克服线性化误差导致的不一致性，提出了可观性约束EKF以提高精度和一致性。工作Multi-sensor fusion for robust autonomous flight in indoor and outdoor environments with a rotorcraft MAV中提出一种无迹卡尔曼滤波（UKF）算法，它将视觉、激光雷达和GPS测量融合到一起。UKF是EKF的一种扩展，其没有解析的雅可比。基于滤波的方法对时间同步非常敏感。任何迟来的测量都会造成麻烦，因为状态无法在滤波过程中被传播回来。因此，需要特殊的排序机制来确保来自多个传感器的所有测量都是有序的。

基于优化的方法维护大量的测量值，并且同时优化多个变量，这也称为捆集调整（BA）。与基于滤波的方法相比，基于优化的方法在时间同步方面具有优势。由于大的捆集作为自然缓冲区，因此当来自多个传感器的测量出现混乱时，它能够容易地处理这种情况。基于优化的算法在精度方面也优于基于滤波的算法，其代价为计算复杂度。早期的优化求解器（例如G2O）利用Gauss-Newton和Levenberg-Marquardt方法来解决优化问题。尽管优化求解器中使用了稀疏的结构，但是复杂度随着状态和测量的数量呈平方增长。为了实现实时性能，一些算法已经探索了增量式求解器，而其它算法限制了位姿图的规模。iSAM2是一种高效的增量式求解器，当新的测量数据传入时，它复用了先前的优化结果以减少计算量。优化迭代仅更新一小部分状态，而不是整个位姿图。随后，工作Ice-ba: Incremental, consistent and efficient bundle adjustment for visual-inertial slam中提出一种加速的求解器，它通过将稠密结构重建为稀疏块来提高效率。保持位姿图尺寸固定的方法被称为滑动窗口方法。一些令人印象深刻的基于优化的VIO方法在有限规模的滑动窗口内优化变量。先前的状态被边缘化为先验因子，而不损失信息。在本文中，我们采用了基于滑动窗口优化的状态估计框架。

系统概述

所提出框架的结构如图1所示，多种类型的传感器被自由地组合。

每个传感器的测量作为一个通用的因子。因子和它们相关的状态构成位姿图。位姿图的一种示例如图2所示。

每个节点表示一个时刻的状态（位置、姿态、速度等）。每条边表示一个因子，它通过一次测量获得。因子约束一个状态、两个状态或者多个状态。对于IMU因子，它通过连续的运动限制约束两个连续的状态。对于一个视觉路标，其因子约束了多个状态，因为它在多帧上被观测到。一旦构建了因子图，优化它等价于找到尽可能匹配所有边的节点配置。

在本文中，我们具体演示了使用视觉和惯性传感器的系统。视觉和惯性传感器能够形成三种组合用于6自由度状态估计，即双目相机、单目相机和IMU、双目相机和IMU。图3显示了使用视觉和惯性传感器的所提出框架的图示。位姿图中存在若干相机位姿、IMU测量和视觉测量。IMU和其中一个相机是可选的。

方法

A.问题定义

1）状态：我们需要估计的主要状态包括机器人中心的三维位置和姿态。除此之外，我们有其它可选的状态，它们与传感器有关。对于相机，需要估计视觉路标的深度或者三维位置。对于IMU，它产生另一个运动变量，即速度，还需要估计IMU的时变加速度计偏置和陀螺仪偏置。因此，对于视觉和惯性传感器，我们需要估计的整个状态定义如下：

B.传感器因子

1）相机因子：该框架支持单目和双目相机。每个相机的内参和相机之间的外参变换假设是已知的，它们能够容易地通过离线标定获取。对于每个相机帧，提取角点特征。KLT跟踪器在前一帧中跟踪这些特征。对于双目设置，跟踪器还在左图像和右图像之间匹配特征。根据特征关联，我们使用每一帧中的每个特征来构建相机因子。相机因子为重投影过程，其根据一个特征的首次观测将该特征投影到之后的帧中。

2）IMU因子：我们使用著名的预积分算法来构建IMU因子。我们假设，加速度计和陀螺仪测量中的附加噪声为高斯白噪声。时变的加速度计和陀螺仪偏置被建模为随机游走过程，其导数为高斯白噪声。由于IMU以比其它传感器更高的频率获取数据，因此两帧之间通常存在多个IMU测量数据。所以，我们在流形上预积分IMU测量，并且传递协方差矩阵。

3）其它因子：尽管我们仅说明了相机和IMU因子，但是本文系统不限于这两种传感器。其它传感器（例如轮速计、激光雷达和毫米波雷达）也能够毫不费力地加入到我们的系统中。关键是将这些测量值建模为通用的残差因子，并且将这些残差因子加入到代价函数中。

C.优化

D.边缘化

与公式3相比，上述公式仅增加一个先验项。它与公式3相同，都是通过Ceres求解器来求解。

E.讨论

所提出系统是一个通用的框架。各种传感器能够被容易地加入到我们的系统中，只要它能够被导出为一个通用的残差因子。由于本文系统不是专门为特定传感器设计的，因此它能够处理传感器失效的情况。当传感器失效时，我们只删除非活跃传感器的因子，并且从其它可选的传感器中加入新的因子。

文章转载自公众号：自动驾驶专栏