[VINS论文笔记]A General Optimization-based Framework for Global Pose Estimation with Multiple Sensors

前言

2019年1月11日，港科大VINS-Mono的团队发布了VINS的扩展版本——VINS-Fusion。其支持多种视觉惯性传感器类型（单相机+ IMU，立体相机+ IMU，立体相机），还支持将VINS与GPS进行融合。本人就VINS-Fusion中多传感器全局位姿图优化的相关论文进行了阅读，并进行内容整理（尽可能简洁）。

原文：A General Optimization-based Framework for Global Pose Estimation with Multiple Sensors
《一种基于通用优化方法的多传感器全局位姿估计框架》

摘要翻译

准确的状态估计是自主机器人的基本问题。为了实现局部精确和全局无漂移的状态估计，多个具有互补特性的传感器通常融合在一起。局部传感器（摄像机，IMU，激光雷达等）在一个小区域内提供精确位姿，而全局传感器（GPS，磁力计，气压计等）在大规模环境中提供带噪声但全局无漂移的定位。在本文中，我们提出了一种传感器融合框架，将局部状态与全局传感器融合，从而实现局部精确和全局无漂移的位姿估计。由现有VO/VIO方法产生的局部估计在位姿图优化中与全局传感器融合。在图优化中，局部估计被对齐到全局坐标，同时消除了累积漂移。我们评估该系统在公共数据集和实验中的表现，将结果与其他最先进算法进行比较。我们强调该系统是一个通用框架，可以在统一的位姿图优化中轻松融合各种全局传感器。我们开源了代码实现。

主要内容

完全可以将前三章的内容放在一起说（因为讲的内容要点差不多）：

文章将传感器划分为局部传感器和全局传感器两类。
1、局部传感器，如相机、激光、IMU等，其应用的算法（如VIO）存在以下局限性：
1）通常将第一个位姿设置为坐标系原点，从相对于原点的局部坐标系开始位姿估计，而不是在全局坐标系下；
2）局部估计时，累计误差随着到时间和距离的增长而增长。

2、全局坐标系，如GPS，磁力计，气压计，总是在一个固定的全局坐标系下工作，如地球坐标系，原点是固定且预知的。但是它也存在问题：噪声大，频率低，没有姿态信息（磁力计有）。因此无法直接用来估计六自由度位姿。

由此引出了通过多传感器融合以提高鲁棒性。在第二章讨论了一些全局传感器与局部传感器融合的算法，如采用EKF融合IMU和GPS，采用UKF融合视觉、激光和GPS等，采用优化的方法融合VIO和GPS等。

其中基于滤波器的方法存在以下问题：
1）需要对不同坐标系之间的转换进行准确的初始预测，以保证收敛；
2）对传感器的时间同步非常敏感。

因此文章提出了一个更为通用的基于优化的全局坐标系位姿估计框架，它可以支持多个全局传感器。其中局部传感器用于局部估计，采用现有的VO/VIO方法来产生局部位姿。并将全局传感器转换为位姿图中的统一因子，和局部估计一起输入到全局位姿图中构造优化问题，最后生成局部精确且全局感知的6自由度位姿。

其中局部位姿估计和VINS论文中提到的一样，就不赘述了。

全局位姿图结构

位姿图中的节点为世界坐标系下的每个位姿，其密度由频率最低的传感器决定；两个连续节点之间的边是由局部估计器（VIO）得到的局部约束；其他边是由全局传感器产生的全局约束。

位姿图优化是一个最大似然估计问题，假设测量独立且误差服从高斯分布，将其转变为非线性最小二乘问题（BA）。

这里的优化目标即 $X=\{x_0,x_1,...,x_n\}$，其中 $x_i=\{p^w_i,q^w_i\}$。

传感器因子

1）局部因子 local factor
采用由VIO得到的连续两帧之间的相对位置和姿态进行构建。比如对于第t-1帧位姿 $(q^l_{t-1},p^l_{t-1})$和第t帧位姿 $(q^l_{t},p^l_{t})$，局部因子表示为：

如果VIO算法产生位姿的协方差矩阵，则将其用作局部测量的协方差，否则所有局部测量使用统一的协方差。

2）GPS因子
将经度、纬度和海拔转换到NED坐标系下的x,y,z，将第一个GPS测量值设置为原点，在ENU世界坐标系中得到GPS测量值 $p^{GPS}_t$。GPS因子表示为：

GPS测量值直接约束每个节点的位置。协方差由接收测量时的卫星数决定，收到的卫星越多协方差就越小。

3）磁力计因子
磁力计测量磁场方向和强度，可以帮助确定在世界坐标系下的方向：假设该区域的磁场强度不变 $Z^w$，将测量得到的磁场强度 $Z^m_t$的方向与其对齐可以得到机器人的方向。将磁力计因子表示为

由于磁场容易受到环境影响，这里使用没有长度的归一化矢量。并将长度用于确定协方差。如果长度相差很多则设置很大的协方差，否则使用小协方差。

4）气压计因子
测量的气压可以线性转换为高度。将第一个测量设置为原点高度，然后得到高度的测量值 $z^m_t$。气压计因子被表征为高度估计的残差：

由于气压计的测量有噪声，我们会在短时间内计算多个测量值的方差，并在代价函数中使用。

位姿图优化

在低频（1Hz）下进行位姿图优化。在每次优化之后，得到局部坐标系到全局坐标系的变换矩阵。通过变换矩阵转换后续的高速率局部位姿（VIO，200Hz），以实现实时高速全局位姿估计。由于位姿图非常稀疏，因此计算复杂度随着位姿数量线性增加。在位姿图中采用滑动窗口法以获得准确且全局无漂移的位姿估计。

个人想法

觉得这篇文章最大的贡献是将VINS扩展成了一个可以进行多传感器融合的框架，可以通过直接构建对应残差项以方便地添加各种全局传感器。因为融合的时候是将VIO得到的局部位姿和其他全局传感器测得的部分位姿量进行融合，算是一种松耦合的方法。不过全局传感器本身的独立性很强，不像相机、IMU和激光的测量信息具有关联性，而且可以直接算得全局位姿，既没必要也难以进行紧耦合。另外就是其实磁力计和高度计的频率还是很快的，能否将其应用到局部姿态估计优化中，或者像9轴IMU那样融合在IMU角度测量上？