众所周知，视觉惯性里程计 (VIO) 会出现漂移，尤其是在长时间运行的条件下。本文提出了一种基于非线性优化的系统——GVINS，它将 GNSS 原始测量、视觉和惯性信息紧耦合以进行实时和无漂移的状态估计。我们的系统旨在为 GNSS 信号可能大量丢失甚至完全不可用的复杂室内外环境下提供准确的全局 6-DoF 估计。为了将全局测量与局部状态联系起来，我们提出了一种从粗到细的初始化程序，可以有效地完成在线标定，并在很短的滑动窗口内对 GNSS 状态进行初始化。然后在因子图框架下，结合视觉和惯性约束，对 GNSS 伪距和多普勒频移测量进行建模和优化。对于复杂且对 GNSS 不友好的区域，我们将讨论并仔细处理退化情况以确保系统的鲁棒性。本文中还包含了搭建系统过程中所涉及的工程难点，以促进 GNSS 融合相关领域的研究。由于采用了紧耦合的多传感器融合方法和系统设计，该系统充分利用了三种传感器的优点，能够无缝应对室内和室外环境之间的过渡以及卫星丢失和重新捕获的情况。仿真和真实世界的实验结果表明，尽管 GNSS 测量有噪声，但该系统基本上消除了 VIO 的漂移并保持了局部精度。此外，实验还表明，该系统甚至可以从一颗卫星即能获得增益，而传统的 GNSS 算法至少需要四颗。
定位是许多空间感知应用程序的一个基本功能，如自动驾驶、UAV导航和增强现实(AR)。用各种传感器估计系统状态已经被广泛研究了几十年。其中，传感器融合方法近年来越来越流行。由于异构传感器提供的互补特性，传感器融合算法可以显著提高状态估计系统的准确性和鲁棒性。
相机提供了丰富的视觉信息，同时成本比较低和结构紧凑，因此吸引了计算机视觉和机器人领域的广泛关注。结合提供高频和无异常点惯性测量，视觉-惯性导航(VIN)算法通常在复杂环境中可以实现高精度、更鲁棒。尽管如此，不过相机和 IMU 都是局部坐标系中运行，并且已经证明 VIN 系统具有四个不可观察的方向 【1】，即 x、y、z 和偏航角。因此，对于任何 VIN 系统，里程计漂移都是不可避免的。另一方面，全球导航卫星系统（GNSS）为定位任务提供了无漂移和全局感知的解决方案，因此已广泛应用于各种场景。 GNSS 信号可免费获取，并在接收机和卫星之间传送距离信息。 通过同时跟踪至少 4 颗卫星，接收机能够获得其在地球坐标系下的唯一坐标。 考虑到 VIN 和 GNSS 系统之间的互补特性，通过将来自两个系统的信息融合在一起来进行改进似乎很自然。
然而，在融合两个系统的过程中存在许多挑战。首先，从嘈杂的 GNSS 测量中进行稳定的初始化是必不可少的。在需要初始化的数量中，局部 VIN 帧和全局 GNSS 帧之间的 4-DoF 转换很重要，这是将来自多个源的测量值关联在一起所必需的。与相机和 IMU 之间的外部转换不同，这种转换无法离线校准，因为每次 VIN 系统启动此类转换都会有所不同。此外，使用一部分序列的一次性对齐效果不佳，因为融合系统的漂移使这种对齐在 GNSS 中断情况下无效。因此，需要在局部框架和全局框架之间进行在线初始化和校准，以融合异构测量并应对复杂的室内外环境。其次，GNSS测量精度与VIN系统不匹配，GNSS信号传播过程中存在各种误差源。在实践中，GNSS 系统中用于全球定位的伪距测量只能达到米级精度，而 VIN 系统能够在短距离内提供厘米级估计。因此，如果不仔细制定，融合系统将容易受到不稳定的 GNSS 测量的影响。第三，当融合系统经历某些运动，如纯旋转或卫星数量不足时，就会发生退化。通常 GNSS 视觉惯性融合系统可以提供无漂移的 6-DoF 全局估计，但该结论在退化情况下不再成立。此外，室内和室外环境之间的过渡，在此期间所有卫星都丢失或逐渐重新捕获，这也对系统设计提出了挑战。
为了解决上述问题，我们提出了一种基于非线性优化的系统，将 GNSS 原始测量（伪距和多普勒频移）与视觉、惯性数据紧耦合，以实现准确且无漂移的状态估计。局部和全部之间的 4-DoF 变换在初始化阶段通过从粗到细的方法恢复，并随后进一步优化。为了结合噪声较大的 GNSS 原始测量，所有 GNSS 约束都在统一的概率框架下制定，其中所有状态都被联合优化。 此外，还讨论并仔细处理退化情况以确保稳健性。 由于紧耦合的方法和系统设计，我们的系统充分利用了 GNSS、视觉和惯性测量之间的互补特性，即使在复杂的环境中也能够提供局部平滑和全局一致的估计，如图 1 所示。
图1 我们系统在复杂内外环境中的快照。全局估计结果被直接绘制在谷歌地图上，并与地面真实的RTK轨迹很好的吻合，如（a）部分所示。
Part (b) depicts the distribution of satellites with tangential direction representing the azimuth andradial direction being the elevation angle. The blue arrow is a compass-like application which indicts the global yaw orientation of the camera.
Subplot (c)and (d) illustrate the attitude information and the local-ENU yaw offset respectively. The measurement noise level of each tracked satellite is shown in part(e).
Note that there is an obvious failure on the RTK trajectory when we walk the indoor stairs, while our system can still perform global estimation even inindoor environment
本文的贡献如下：
一种在线的由粗到细的初始化GNSS -视觉-惯性状态的方法；
一种基于优化的紧耦合方法，在概率框架下将视觉惯性数据与多星座 GNSS 原始测量值融合；
一种实时的状态估计器，能够在 GNSS 信号可能被遮挡甚至完全不可用的复杂环境中提供无漂移的 6-DoF 全局位姿估计；
在仿真和真实环境中对所提出的系统进行了广泛的评估。
为了造福研究界，此次工作的源码和数据集将开源。
本文剩余的章节结构安排如下：第二节讨论当前的相关文献。第三节详述系统中涉及的符号定义和坐标系。第四节简要介绍GNSS的相关背景知识。第五节展示GVINS的结构和算法流程。The problem formulation and methodology are illustrated in Section VI。第七节引出了全球导航卫星系统的初始化问题，并讨论了一些降低系统性能的退化情况。As we solve the problem from a system level, engineering challenges during the development of the system are listed in VIII for the benefit of the community。第九节给出了实验装置和测试评估。最后第十节对本文进行了总结。
通过多传感器融合方法进行状态估计已被证明是有效且稳健的，并且在该领域有大量文献。其中，我们对相机、IMU 和 GNSS 接收机等小尺寸和低成本传感器的组合特别感兴趣，以在未知环境中产生实时准确估计。
紧耦合的视觉和惯性测量根据解算方案可分为滤波方法和优化方法。 MSCKF【2】 是一种出色的基于滤波器的状态估计，它利用多个相机姿态之间的几何约束来有效地优化系统状态。根据MSCKF，【3】对其准确性和一致性进行了改进，【4】旨在克服其数值稳定性问题，尤其是在移动设备上。与基于滤波器的方法相比，非线性批量优化方法可以通过重新线性化以牺牲计算成本来获得更好的性能。 OKVIS【5】利用基于关键帧的滑动窗口优化方法进行状态估计。 VINS-Mono 【6】 还优化了滑动窗口内的系统状态，但通过在线重定位和姿态图优化更加完整。由于相机和 IMU 仅在状态之间施加相对约束，累积漂移是 VIN 系统中的一个关键问题，尤其是在长期运行中。
由于 GNSS 在地球坐标系中提供绝对测量，因此合并 GNSS 信息是减少累积漂移的自然方式。在松耦合方面，【7】【8】描述了在 EKF 框架下将 GNSS与视觉和惯性数据融合的状态估计系统。 【9】提出了一种融合视觉、惯性、激光雷达和 GNSS的 UKF 算法，以在不同环境下产生平滑一致的轨迹。 【10】、【11】 和我们之前的工作 VINS-Fusion 【12】 在优化框架下将本地 VIO 的结果与 GNSS融合。 上述所有工作都依赖于 GNSS进行估计，因此在跟踪卫星数量低于 4 的情况下，一旦 GNSS严重损坏或不可用，就会发生系统故障。
还有一些关于将 GNSS 原始测量与视觉和惯性信息紧耦合的工作。【13】和 【14】 在 EKF 框架下结合相机、IMU 和 GNSS RTK进行定位，但厘米级 RTK需要GNSS 参考站。 【15】 和 【16】 研究了在跟踪少于 4 颗卫星的复杂城市环境下融合系统的性能。然而，没有处理局部和全局框架之间的转换，并且他们的真实世界实验的规模是有限的。此外，【16】中底层 VIN 系统与 GNSS 融合的性能不好。 【17】 以 EKF方式将 GNSS 伪距数据和指向天空的相机测量紧耦合。上置摄像头用于过滤多径 GNSS 信号以及跟踪来自顶部建筑物的特征，因此仅适用于城市环境。局部框架和全局框架之间的转换在他们的工作中是已知的。最近我们发现了一项类似的工作 【18】，它将 GNSS 原始测量与视觉惯性 SLAM 紧耦合。虽然它在室外环境中运行良好，但无法处理隧道等室内场景，这限制了紧密多传感器融合方法的潜力。
为此，我们的目标是建立一个健壮和准确的状态估计器，将与GNSS原始测量、视觉和惯性数据紧耦合。利用GNSS的全局测量，可以消除来自视觉惯性系统的累积误差。将估计局部框架和全局框架之间的转换，而无需进行任何离线校准。该系统能够在复杂的室内外环境中工作，实现局部平滑和全局一致性。
在我们的系统中包含了以下坐标系：
传感器坐标系：传感器坐标系连接到传感器，是传感器报告其读数的本地坐标系。在我们的系统中，传感器帧包括相机坐标系 和imu坐标系 ，我们选择imu坐标系作为估计的目标坐标系，并表示为body坐标系 。
局部世界坐标系：我们将传统的视觉惯性系统作为局部世界坐标系 。在 VIN 系统中，局部世界坐标系的原点是任意设置的，z 轴通常选择与局部地平面正交，如图 2 所示。
ECEF坐标系：ECEF（地心、地固）坐标系 是相对于地固定的笛卡尔坐标系，如图2所示。ECEF坐标系的原点附着在质心上。z轴垂直于地球的赤道，并指向真正的北方。x-y平面与地球赤道重合，x轴指向本向子午线。
ENU坐标系：为了连接局部世界坐标系和全局ECEF坐标系，引入了半全局坐标系， ENU坐标系 。其 x、y、z 轴分别指向东、北和上方向（图 2）。 给定 ECEF 坐标系中的一个点，可以确定一个独特的 ENU 坐标系，其原点位于该点上。请注意，局部世界坐标系和 ENU 坐标系的 z 轴都平行于重力方向。
在时间坐标系方面，GNSS 数据在 GNSS 时间系统（例如 GPST）中标记，而视觉和惯性测量在本地时间系统中标记。 我们假设这两个时间系统是预先对齐的，并没有相应地区分它们。
需要估计的系统状态如下：
位置 和方位 (the body frame with respect to the local world frame)
速度 ，加速度零偏 和陀螺仪零偏
每个特征点的逆深度ρ
偏航角ψ（between the local world frame and ENU frame）, receiver clock bias δt and receiver clock drifting rate δ ṫ. Because our system support all four constellations, the clock biases for GPS, GLONASS, Galileo and BeiDou are estimated separately. Note that the receiver clock drifting rate for each constellation is the same.
我们的系统采用滑动窗口法进行优化，窗口中的状态可以总结如下：
其中n 代表了窗口的大小，m代表了窗口中特征点的数量。
由于我们的系统需要GNSS原始测量处理，因此关于GNSS的背景知识是必要的。在本部分中，我们首先概括介绍GNSS。然后引入了原始测量结果，即伪距和多普勒频移，并进行了介绍和建模。最后介绍了SPP（伪距单点定位，定位精度5-10米）全局定位算法的原理。
我们提出的系统的结构如图 5 所示。估计器将原始 GNSS、IMU 和相机测量作为输入，然后对每种类型的测量应用必要的预处理。与【6】中一样，IMU 测量值是预积分，整个图像被总结为一系列稀疏特征点。对于GNSS原始数据，我们首先设置截至高度角剔除多径影响可能较大的和不健康的卫星。为了拒绝不稳定的卫星信号，只允许连续锁定一定时期的卫星进入系统。由于星历数据是通过慢速卫星接收机无线链路（50 比特/秒）获取的，因此在其相应的星历完全传输之前，GNSS 测量无法使用。在预处理阶段之后，所有的测量都为估计器做好了准备，但在进入优化部分之前，需要一个初始化阶段来正确初始化非线性估计器的系统状态。
初始化从仅视觉的 SfM 开始，从中联合估计最相似的运动和结构，然后将来自 IMU 的轨迹与 SfM 结果对齐，以恢复尺度、速度、重力和 IMU 偏差。 VI初始化完成后，进行从粗到精的GNSS初始化过程。 首先通过 SPP 算法获得粗略的定位结果，然后在偏航对齐阶段使用来自 VI 初始化和 GNSS 多普勒测量的局部速度关联局部和全局 frame。 最后初始化阶段以anchor refinement结束，它利用精确的局部轨迹并施加imposes clock来进一步细化body的全局位置。
在初始化阶段之后，会检查并仔细处理 GNSS 退化情况，以确保稳健的性能。然后formulate 来自所有测量的约束，以在非线性优化框架下联合估计滑动窗口内的系统状态。 请注意，如果 GNSS 不可用或无法正确初始化，我们的系统自然会降级为 VIO。 为了确保实时性能和处理视觉惯性退化运动，在每次优化后也应用two-way边缘化策略。
在本节中，我们首先在概率框架下formulate并推导出我们的状态估计问题，如下所示，整个问题被组织成一个因子图，来自传感器的测量形成了一系列因子，这些因子反过来又约束了系统状态。 本节将详细讨论概率图中的每种类型的因子。 请注意，视觉和惯性因子的公式是从【6】【23】【24】 继承的，因此不是本次工作的贡献。 仅出于本文献的完整性而列出相关内容。
我们将最优系统状态定义为在给定所有测量值时最大似然估计（MAP）的状态。假设所有测量都相互独立，每次测量的噪声均为零平均高斯分布，MAP问题可以进一步转化为一系列成本和最小的问题，每个代价对应于一个特定的测量：
其中， 代表了 个独立传感器测量数据的集合， 封装了系统的先验信息。 转存失败重新上传取消 表示每次测量的残差函数， 是马氏范数。
注意，这种公式自然适合因子图表示【25】，因此我们将优化问题分解为状态和测量关联的单个因子，如图6所示。在下面，我们将详细讨论每一个因素。
惯性因子所涉及的测量结果包括平台的偏置的、有噪声的线性加速度和角速度。当加速度计在地球表面附近工作时，线性加速度测量也包含重力分量，如下所示：
其中， 代表IMU的输出， 代表了IMU坐标系下平台的线加速度和角速度。混入的噪声 认为是0均值的高斯分布，即 ， 。加速度计和陀螺仪缓慢变化的零偏认为是如下的随机游走噪声：
其中 ， 。
