标题：GVINS: Tightly Coupled GNSS-Visual-Inertial Fusion for Smooth and Consistent State Estimation
作者：Shaozu Cao, Xiuyuan Lu, and Shaojie Shen
编译：王宇杰
审核：lionheart
摘要
众所周知，视觉惯性里程计 (VIO) 会出现漂移，尤其是在长时间运行的条件下。在本文中，我们提出了 GVINS，这是一种基于非线性优化的系统，它将 GNSS 原始测量与视觉和惯性信息紧密融合，以进行实时和无漂移的状态估计。我们的系统旨在在 GNSS 信号可能被大量丢失甚至完全不可用的复杂室内外环境下提供准确的全局 6-DoF 估计。为了将全局测量与局部状态联系起来，我们提出了一种从粗到细的初始化程序，可以有效地完成在线标定，并在很短的测量滑动窗口内对 GNSS 状态进行初始化。然后在因子图框架下，结合视觉和惯性约束，对 GNSS 伪距和多普勒频移测量进行建模和优化。对于复杂且对 GNSS 不友好的区域，我们将讨论并仔细处理退化情况以确保系统的鲁棒性。在本文中我们还包含了搭建系统过程中所涉及的工程难点，以促进 GNSS 融合相关领域的研究。由于采用了紧耦合的多传感器融合方法和系统设计，我们的系统充分利用了三种传感器的优点，能够无缝应对室内和室外环境之间的过渡，卫星丢失和重新捕获的情况。我们在仿真和真实世界中开展了广泛的实验以评估所提出的系统，结果表明尽管 GNSS 测量有噪声，我们的系统基本上消除了 VIO 的漂移并保持了局部精度。此外，实验还表明，我们的系统甚至可以从一颗卫星即能获得增益，而传统的 GNSS 算法至少需要四颗。
主要贡献
1、 一种在线的由粗到细的初始化GNSS -视觉-惯性状态的方法。
2、 一种基于优化的紧耦合融合方法，在概率框架下将视觉惯性数据与多星座 GNSS 原始测量值融合。
3、 一种实时的状态估计器，能够在 GNSS 信号可能被大量拦截甚至完全不可用的复杂环境中提供无漂移的 6-DoF 全局位姿估计。
4、 在仿真和真实环境中对所提出的系统进行了广泛的评估。
在复杂的室内外环境中测试情况。全局估计结果直接绘制在谷歌地图上，与地面实况 RTK 轨迹很好地对齐，如 (a) 部分所示。(b)部分描绘了卫星的分布，切线方向代表方位角，径向方向为仰角。蓝色箭头是一个类似指南针的应用程序，它指示相机的全局偏航方向。子图 (c) 和 (d) 分别说明了姿态信息和本地坐标系与 ENU坐标系之间偏航角的偏移量。每颗被跟踪卫星的测量噪声等级显示在（e）部分。请注意，当我们位于室内楼梯时，RTK 轨迹存在明显的故障，而我们的系统即使在室内环境中仍然可以稳定地进行全局估计。
本地世界坐标系、ECEF坐标系 和 ENU 坐标系的图示。
算法框架
我们提出的系统的结构如下图所示。状态估计器将原始 GNSS、IMU 和相机测量作为输入，然后对每种类型的测量值进行必要的预处理。其中，IMU 测量值需要做预积分，图像则被表示为一系列的稀疏特征点。对于GNSS原始数据，我们过滤掉容易出错的卫星数据，同时为了滤除不稳定的卫星信号，只使用了已经连续锁定一定时间的卫星数据。
GVINS的算法框图
接下来是初始化阶段，首先从基于视觉的 SfM 开始，从中联合估计最相似的运动和结构，然后将来自 IMU 的轨迹与 SfM 结果对齐，以恢复尺度、速度、重力和 IMU 偏差。VI 初始化完成后，进行由粗到细的 GNSS 初始化过程。首先通过 SPP 算法获得粗定位结果，然后在偏航角对齐的阶段使用来自 VI 初始化和 GNSS 多普勒测量的局部速度关联局部和全局帧。最后初始化阶段以锚点细化结束，它利用精确的局部轨迹并施加时间同步约束来进一步细化锚点的全局位置。
从粗到精的初始化过程的说明。该模块从 VIO 获取局部位置和速度结果，并在全局 ECEF 框架中输出相应的轨迹。
在初始化阶段之后，会检查并仔细处理 GNSS 退化情况，以确保鲁棒的性能。然后结合不同测量项的约束，在非线性优化的框架下联合估计滑动窗口内的系统状态。请注意，如果 GNSS 不可用或无法正确初始化，我们的系统自然会降级为 VIO。为了确保实时性能和处理视觉惯性退化运动，在每次优化后也应用双向边缘化策略。
基于概率的因子图优化：如下图所示，整个问题被建模成一个因子图，来自传感器的测量形成了一系列因子，这些因子反过来又约束了系统状态。
因子图模型，其中系统状态由彩色的大圆圈表示，因子由黑色的小圆圈表示。来自各种传感器测量的因子包括惯性因子 i、视觉因子 f、伪距和多普勒因子 g 以及时钟因子 c。
主要结果
GVINS、VINS-Fusion 和 VINS-Mono 在仿真环境中的相对位姿误差。上面两个图对应VIO的四个不可观测方向（x、y、z和yaw），下面的图是整体相对旋转误差。
GVINS、VINS-Fusion、VINS-Mono 和 RTKLIB 在仿真环境中的绝对轨迹误差。
我们实际实验中使用的设备是带有 VI 传感器和 u-blox ZED-F9P 的头盔。VI-Sensor 本身可以很好地同步相机和 IMU 测量。来自 GNSS 接收器的 PPS 信号用于触发 VI-Sensor 以将全球时间与本地时间对齐。
GVINS、VINS-Fusion、VINS-Mono和RTKLIB在运动场实验中的定位误差。三个子图分别对应ENU帧的三个方向。GVINS、VINS-Fusion 和 RTKLIB 的结果直接与 RTK 地面实况进行比较，没有任何对齐，而 VINS-Mono 的结果事先与地面实况轨迹对齐。
GVINS、VINS-Fusion、VINS-Mono和RTKLIB在运动场实验中的运动轨迹。我们提出的系统的最终轨迹是平滑的，并且与 RTK 的轨迹很好地对齐。
在锁定卫星数量不足的情况下，我们提出的系统的定位误差。
我们提出的系统在卫星配置下的轨迹。GVINS 通过利用所有可用卫星表现最佳，并在零卫星配置下降级为 VIO。仅使用3颗卫星时会出现小偏差，当卫星数进一步减少到2颗时会出现平移漂移。如果只有1颗可用卫星，yaw估计也开始漂移，但与VIO相比幅度较小（ 0 卫星）。
GVINS、VINS-Mono和RTKLAB在复杂的室内外实验中的定位误差。我们只与 RTK fix 解决方案进行比较，因此图中的差距对应于地面实况不可用的情况。由于巨大的误差和振荡，VINS-Fusion 的结果未显示。
RTK和GVINS在复杂室内外实验中的定位结果。
复杂的室内外实验中的最终轨迹。由于噪声和抖动较大，RTKLIB 和 VINS-Fusion 的结果未绘制。RTK 路径的不连续性是不良 GNSS 信号和定位丢失事件的结果。
正常 GVINS、无多普勒因子的 GVINS 和无伪距因子的 GVINS 的定位误差。
Abstract
Visual-Inertial odometry (VIO) is known to suffer from drifting especially over long-term runs. In this paper, we present GVINS, a nonlinear optimization based system that tightly fuses GNSS raw measurements with visual and inertial information for real-time and drift-free state estimation. Our system is aiming to provide accurate global 6-DoF estimation under complex indoor-outdoor environment where GNSS signals may be largely intercepted or even totally unavailable. To connect global measurements with local states, a coarse-to-fine initialization procedure is proposed to efficiently online calibrate the transformation and initialize GNSS states from only a short window of measurements. The GNSS pseudorange and Doppler shift measurements are then modelled and optimized under a factor graph framework along with visual and inertial constraints. For complex and GNSS-unfriendly areas, the degenerate cases are discussed and carefully handled to ensure robustness. The engineering challenges involved in the system are also included to facilitate relevant GNSS fusion researches. Thanks to the tightly-coupled multi-sensor approach and system design, our system fully exploits the merits of three types of sensors and is capable to seamlessly cope with the transition between indoor and outdoor environments, where satellites are lost and recaptured again. We extensively evaluate the proposed system by both simulation and real-world experiments, and the result demonstrates that our system substantially eliminates the drift of VIO and preserves the local accuracy in spite of noisy GNSS measurements. In addition, experiments also show that our system can gain from even a single satellite while conventional GNSS algorithms need four at lease.
