文章：GVINS: Tightly Coupled GNSS-Visual-Inertial Fusion for Smooth and Consistent State Estimation
作者：Shaozu Cao, Xiuyuan Lu, and Shaojie Shen
代码：https://github.com/HKUST-Aerial-Robotics/GVINS
编译：点云PCL
本文仅做学术分享，如有侵权，请联系删除。内容如有错误欢迎评论留言，未经允许请勿转载！
摘要
视觉惯性里程计（VIO）众所周知在长期的运行中会有累计误差。在本文中提出了GVINS，一个基于非线性优化的系统，它将GNSS原始测量值与视觉和惯导信息紧密地融合起来，用于实时和无漂移的状态估计。本文的系统的目标是在复杂的室内外环境下提供精确的全局6自由度姿态估计，在这种环境下，GNSS信号可能被大量丢失甚至完全不可用。为了将全局测量与局部状态联合起来，本文提出了一种由粗到精的初始化方法，可以有效地在线标定变换，并在很短的测量滑动窗口内对GNSS状态进行初始化。然后在因子图框架下，结合视觉和惯导约束，对GNSS伪距和多普勒频移测量进行建模和优化。对于复杂和GNSS不友好的区域，对退化场景进行了讨论和处理，以保证里程计的鲁棒性。该系统所涉及的工程挑战也包括在内，以便于相关的GNSS融合研究。由于采用了紧耦合的多传感器方法和系统设计，我们的系统充分利用了三种传感器的优点，能够无缝地应对室内和室外环境之间的过渡，即便在卫星丢失和重新捕获的情况下。我们通过仿真和实际的实验对所提出的系统进行了广泛的评估，结果表明，尽管GNSS测量有噪声，我们的系统仍然有效地消除了VIO的漂移，并保持了系统的局部精度。此外，实验还表明，我们的系统甚至可以从一颗卫星获得增益，而传统的GNSS算法至少需要四颗卫星。
介绍
基于多传感器融合的状态估计方法已经被证明是有效和鲁棒的，并且在这方面有大量的文献。其中，我们特别感兴趣的是将小型低成本的传感器，如相机、IMU和GNSS接收机结合起来，在未知环境下进行实时准确的估计。视觉与惯性测量的紧密耦合融合可以分为基于滤波的方法和基于优化的方法。MSCKF是一种优秀的基于滤波器的状态估计算法，它利用多摄像机姿态之间的几何约束来有效地优化系统状态。与基于滤波器的方法相比，非线性批处理优化方法通过重新线性化，以牺牲计算量为代价，可以获得更好的性能。OKVIS利用基于关键帧的滑动窗口优化方法进行状态估计。VINS Mono还优化了滑动窗口内的系统状态，但在线重新定位和姿态图优化更为完善。但是无法避免的在长期运行中的累计误差。
图1。我们的系统在复杂的室内外环境中的快照。全局估计结果直接绘制在Google地图上，并与第（a）部分所示的真值RTK轨迹很好地吻合。（b）部分描述了卫星的分布，切线方向表示方位角，径向方向表示仰角。蓝色箭头是一个类似指南针的应用程序，指示相机的全局偏航方向。子图（c）和（d）分别表示姿态信息和局部偏航偏移。每颗被跟踪卫星的测量噪声级如（e）部分所示。注意，当我们走室内楼梯时，RTK轨迹上有一个明显的故障，而我们的系统即使在室内环境中也可以进行全局估计。
本文效果如下图1 ，其贡献：
由于紧耦合的方法和系统设计，我们的系统充分利用了全球导航卫星系统、视觉和惯性测量之间的互补性，即使在复杂的环境中也能够提供局部平滑和全局一致的估计，如图1所示，本文的贡献如下：
*一种从粗到精的在线方法来初始化系统
*一种基于优化的GNSS VIO紧耦合方法，在概率框架下将视觉惯性数据与多星座GNSS原始测量数据融合一种实时估计器，
*能够在GNSS信号可能被大量丢失甚至完全不可用的复杂环境中提供无漂移的6自由度全局估计
*在仿真和真实环境中对所提出的系统进行了评估。
主要内容
本文提出的系统结构下图5所示。估计器以原始GNSS、IMU和相机测量值作为输入，然后对每种类型的测量进行必要的预处理。IMU测量值被预积分，整个图像被概括为一系列稀疏的特征点。对于GNSS原始数据，首先过滤掉容易出错的低俯仰角和不良的卫星信号。为了抑制不稳定的卫星信号，只允许连续锁定一定时间的卫星进入系统。由于星历数据是通过慢速卫星接收器无线链路（50位/秒）获取的，因此在相应的星历完全传输之前，全球卫星导航系统的测量是不可用的。在预处理阶段之后，所有的测量都为估计器准备好了，但是在进入优化部分之前，需要一个初始化阶段来正确初始化非线性估计器的系统状态。初始化过程从一个纯视觉的SfM开始，由该SfM联合估计一个最接近的运动和结构，然后将IMU的运动轨迹与SfM结果对齐，以恢复IMU的尺度、速度、重力和IMU偏差。
图5 上图显示了本文提出的系统的工作流程。首先，所有传感器的测量值在进入后续程序之前都经过预处理。在初始化阶段，视觉惯性初始化是通过将惯性信息与纯视觉SfM的结果对齐来完成的。如果视觉惯性系统成功对准，则执行从粗到精的过程，以初始化GNSS状态。一旦全球卫星导航系统介入，该系统将监测和处理全球卫星导航系统退化情况。最后在非线性优化框架下对滑动窗口内的测量约束进行优化。注意，如果GNSS不能初始化，我们的系统仍然可以在视觉惯性模式下工作。同时采用边缘化策略，保证实时估计。
GNSS初始化和退化 本流程的状态估计过程相对于系统状态是非线性的，因此其性能在很大程度上依赖于初始值。通过在线初始化，可以很好地从未知情况中恢复初始状态，无需任何假设或人工干预。在系统运行过程中，估计器还可能遇到一些传感器发生故障或退化的不完美情况。由于关于视觉惯性系统的初始化和退化已经有大量的文献。
初始化
初始化是需要一个已知全局和局部坐标的锚点，将全球GNSS测量与局部视觉和惯性信息进行融合。由于锚点已经设置到局部世界框架的原点，因此需要事先校准局部的世界原点的ECEF坐标。在本文中，我们提出了一个多阶段GNSS-VI初始化程序来在线校准锚点和偏航偏移ψ 在ENU和局部世界框架之间。在GNSS-VI初始化之前，假设VIO已成功初始化，即重力矢量、初始速度、初始IMU偏差和比例已获得初始值。之后，在局部世界框架中形成一条平滑的轨迹，并准备用于GNSS-VI初始化阶段。如图7所示，在线GNSS-VI初始化以从粗到精的方式进行，包括以下三个步骤：
1）粗定位点定位； 2）偏航角偏移校准； 3）锚点优化。
图7。本文提出的从粗到精初始化过程的图示。该模块从VIO中获取局部位置和速度结果，并在全局ECEF帧中输出相应的轨迹。
GNSS实际案例 毫无疑问，我们的系统将在GNSS系统信号稳定、卫星分布均匀的开阔地区发挥最佳性能。下面我们将讨论几种可能会降低系统性能的情况
1） 低速运动：由于多普勒频移测量的噪声级比伪距测量的噪声级低一个数量级，因此局部帧和ENU帧之间的偏航偏移可以通过多普勒频移测量的短窗口得到很好的约束。一旦GNSS接收机的速度低于多普勒频移的噪声水平，估计的偏航偏移量就可能被测量噪声所破坏。此外，低速运动也意味着窗口内的平移距离较短，因此偏航估计也可能受到伪距的影响。在极端情况下，当平台经历纯旋转运动时，GNSS不能提供任何关于旋转方向的信息，而偏航分量又会像VIO那样漂移。因此，如果窗口内的平均速度低于阈值vths，我们固定偏航偏移变量。在我们的系统中，vths设置为0.5m/s，即使行人也能很容易地满足要求。
2） 少于4颗被跟踪的卫星：如果被跟踪的卫星数量少于4颗，SPP或松散耦合的方法将无法解析接收器的位置。然而，在紧耦合结构的帮助下，我们的系统仍然能够利用可用的卫星并随后更新状态向量。 3） 无GNSS信号：在GNSS信号完全不可用的室内或杂乱环境中，与全局信息有关的状态，即偏航偏移ψ, 接收机时钟偏差δt和漂移率˙δ它们不再是可见的。然而，在优化过程中，仍然保持等式（24）和（25）的约束。
实验
我们进行了模拟和真实世界的实验来验证我们提出的系统的性能。在本节中，我们的系统与VINS Mono、VINS Fusion[（Monocular IMU GNSS）和RTKLIB进行比较。由于我们只对实时估计结果感兴趣，因此禁用了VINS Mono和VINS融合的循环函数。我们使用RTKLIB3来计算GNSS SPP解，并将得到的GNSS位置反馈给VINS融合，得到松散耦合的结果。
A 仿真环境
1） 设置：模拟环境为30m× 30米 具有随机生成的三维地标的30m立方体。这些地标被投影到一个10hz的虚拟摄像机上，这个摄像机有75度的水平视场和55度的垂直视场，然后每帧产生大约100个可见特征。在所有特征点上增加一个标准偏差为0.5像素的白噪声项。一个虚拟的200hz IMU与摄像机刚性连接，并沿着预先设计的3D路径移动。
图9.在仿真环境中，GVINS、VINS融合和VINS-mono相对于评估距离的相对姿态误差。上面两个图对应VIO的四个不可观察方向（x、y、z和偏航），下面的图是整体相对旋转误差。
图10描绘了绝对轨迹误差（ATE）随行驶距离的变化。
2） 结果：图9显示了相对于评估距离的相对位姿误差（RPE）。从图中可以看出，在平移和旋转方向上，VINS Mono的相对误差都随着评估距离的增加而增加。其中转动误差主要来自偏航分量。这表明VINS Mono在四个不可观察的方向（即x、y、z和偏航）上遭受累积漂移。当评价距离较短时，VINS融合的误差表现出相似的趋势，当评价距离进一步增大时，融合误差保持在一个恒定水平。这意味着VINS融合能够通过松耦合GNSS解决方案来限制累积漂移。然而，相对于VINS-Mono和GVINS的估计结果，其相对误差要大得多，因此GNSS测量噪声对估计的平滑度有很大的影响。由于采用了紧耦合的方法，我们提出的系统结合了VINS Mono和VINS融合的优点。一方面，在较短的范围内，相对误差与VINS-Mono相当，从而保持了平滑度。另一方面，误差不再在各个方向累积，全局一致性也得到了保证。
B 实际环境中的测试
1） 设置：如图11所示，我们在实际实验中使用的设备是，带有VI传感器和一个u-blox ZE-F9P GNSS接收器。在图像传感器方面，实验中只使用了VI传感器的左摄像头。u-blox ZED-F9P是一款低成本的多波段接收机，支持多种星座。此外，ZED-F9P拥有一个内部RTK引擎，能够在开阔区域提供精度为1厘米的接收器定位。来自3km外基站的实时RTCM流被馈送到ZED-F9P接收机，用于真值RTK解决方案。
图11. 实际实验中使用的设备。
图12.运动场实验中GVINS、VINS融合、VINS-MONO和RTKLIB的定位误差。三个子图对应于ENU框架的三个方向。GVINS、VINS-Fusion和RTKLIB的结果直接与RTK真值进行比较，而VINS-Mono的结果事先与真值轨迹对齐。
图13.运动场实验中RTK、GVINS、VINS融合、VINS Mono和RTKLIB的运动轨迹。本文提出的系统的轨迹是平滑的，与RTK的轨迹吻合良好。
图14.在锁定卫星数量不足的情况下，本文提出的系统的定位误差。
左图复杂室内外实验中GVINS、VINS Mono和RTKLIB的定位误差。这里只与RTK-fix解决方案进行比较，因此图中的差距对应于无法获得基本事实的情况。由于存在较大的误差和振荡，VINS融合的结果没有显示出来。右图 RTK和GVINS在复杂室内外实验中的定位结果。
图18。复杂的室内外实验的最终轨迹。RTKLIB和VINS融合的结果由于噪声和抖动较大而没有绘制出来。RTK路径的不连续性是由于GNSS信号错误和修复丢失事件造成的。
总结
在本文中，我们提出了一个基于非线性优化框架下的紧耦合系统来融合来自相机、IMU和GNSS接收机的测量。我们的系统从一个初始化开始，在初始化阶段采用从粗到精的过程来在线校准局部和全局帧之间的转换。在优化阶段，在概率因子图框架下对GNSS原始测量值进行建模和表达。为了保证系统在复杂环境中的鲁棒性，对退化情况进行了仔细的分析和处理。此外，还讨论了系统集成过程中遇到的工程难题，以便于其他GNSS融合研究。我们在仿真和真实环境下进行了实验，结果表明，我们的系统有效地消除了累积漂移，保持了典型VIO系统的局部精度。为此，我们声明我们的系统可以实现局部平滑和全局一致性。
