GVINS文章暴力翻译（仅供自学）

——————————————————————
本文一部分引用了 https://blog.csdn.net/u013019296/article/details/117341876的内容(灰色部分)
其他部分是GVINS论文的粗略翻译，只为理清GVINS论文逻辑，仅供自学用。
——————————————————————

摘要

视觉惯性里程计（VIO）众所周知在长期的运行中会有累计误差。在本文中提出了GVINS，一个基于非线性优化的系统，它将GNSS原始测量值与视觉和惯导信息紧密地融合起来，用于实时和无漂移的状态估计。本文的系统的目标是在复杂的室内外环境下提供精确的全局6自由度姿态估计，在这种环境下，GNSS信号可能被大量丢失甚至完全不可用。为了将全局测量与局部状态联合起来，本文提出了一种由粗到精的初始化方法，可以有效地在线标定变换，并在很短的测量滑动窗口内对GNSS状态进行初始化。然后在因子图框架下，结合视觉和惯导约束，对GNSS伪距和多普勒频移测量进行建模和优化。对于复杂和GNSS不友好的区域，对退化场景进行了讨论和处理，以保证里程计的鲁棒性。该系统所涉及的工程挑战也包括在内，以便于相关的GNSS融合研究。由于采用了紧耦合的多传感器方法和系统设计，我们的系统充分利用了三种传感器的优点，能够无缝地应对室内和室外环境之间的过渡，即便在卫星丢失和重新捕获的情况下。我们通过仿真和实际的实验对所提出的系统进行了广泛的评估，结果表明，尽管GNSS测量有噪声，我们的系统仍然有效地消除了VIO的漂移，并保持了系统的局部精度。此外，实验还表明，我们的系统甚至可以从一颗卫星获得增益，而传统的GNSS算法至少需要四颗卫星。

1. 介绍

基于多传感器融合的状态估计方法已经被证明是有效和鲁棒的，并且在这方面有大量的文献。其中，我们特别感兴趣的是将小型低成本的传感器，如相机、IMU和GNSS接收机结合起来，在未知环境下进行实时准确的估计。视觉与惯性测量的紧密耦合融合可以分为基于滤波的方法和基于优化的方法。MSCKF是一种优秀的基于滤波器的状态估计算法，它利用多摄像机姿态之间的几何约束来有效地优化系统状态。与基于滤波器的方法相比，非线性批处理优化方法通过重新线性化，以牺牲计算量为代价，可以获得更好的性能。OKVIS利用基于关键帧的滑动窗口优化方法进行状态估计。VINS Mono还优化了滑动窗口内的系统状态，但在线重新定位和姿态图优化更为完善。但是无法避免的在长期运行中的累计误差。
图1。我们的系统在复杂的室内外环境中的快照。全局估计结果直接绘制在Google地图上，并与第（a）部分所示的真值RTK轨迹很好地吻合。（b）部分描述了卫星的分布，切线方向表示方位角，径向方向表示仰角。蓝色箭头是一个类似指南针的应用程序，指示相机的全局偏航方向。子图（c）和（d）分别表示姿态信息和局部偏航偏移。每颗被跟踪卫星的测量噪声级如（e）部分所示。注意，当我们走室内楼梯时，RTK轨迹上有一个明显的故障，而我们的系统即使在室内环境中也可以进行全局估计。
本文效果如下图1 ，其贡献：
由于紧耦合的方法和系统设计，我们的系统充分利用了全球导航卫星系统、视觉和惯性测量之间的互补性，即使在复杂的环境中也能够提供局部平滑和全局一致的估计，如图1所示，本文的贡献如下：
*一种从粗到精的在线方法来初始化系统
*一种基于优化的GNSS+VIO紧耦合方法，在概率框架下将视觉惯性数据与多星座GNSS原始测量数据融合一种实时估计器，
*能够在GNSS信号可能被大量丢失甚至完全不可用的复杂环境中提供无漂移的6自由度全局估计
*在仿真和真实环境中对所提出的系统进行了评估。

定位是许多空间感知应用的基本功能，如自动驾驶、无人机导航和增强现实(AR)。用各种传感器估计系统状态已经被广泛研究了几十年。其中，传感器融合方法近年来受到了越来越多的关注。由于异构传感器提供的互补性，传感器融合算法可以显著提高状态估计系统的精度和鲁棒性。
摄像机以较低的成本和较小的占地面积提供了丰富的视觉信息，因此受到了计算机视觉和机器人领域的广泛关注。视觉惯性导航(VIN)算法与MEMS IMU(提供高频和无异常的惯性测量)相结合，通常可以获得较高的精度，在复杂环境中具有更强的鲁棒性。
然而，摄像机和IMU都是在局部帧内工作的，并且已经证明VIN系统有四个不可见方向[1]，即x, y, z和yaw。因此，任何VIN系统的里程表漂移都是不可避免的。另一方面，全球导航卫星系统(Global Navigation Satellite System, GNSS)为定位任务提供了一种无漂移和全球感知的解决方案，已广泛应用于各种场景。GNSS信号可以免费获得，并在接收机和卫星之间传递距离信息。在至少4颗卫星同时被跟踪的情况下，接收器能够获得其在全球地球框架下的独特坐标。考虑到VIN和GNSS系统之间的互补性，通过融合两个系统的信息来进行改进似乎是很自然的。
然而，在两大系统的融合过程中也存在着许多挑战。首先，一个稳定的初始化从噪声GNSS测量是必不可少的。在需要初始化的量中，局部VIN帧和全局GNSS帧之间的4自由度变换是重要的，这是将多个源的测量数据关联在一起的必要条件。不同于相机与IMU之间的外部转换，这种转换不能离线校准，因为VIN系统每次启动这种转换都会发生变化。另外，在GNSS中断的情况下，使用部分序列进行一次性对齐的效果并不好，融合系统的漂移使这种对齐失效。因此，需要在局部帧和全局帧之间进行在线初始化和标定，以融合异构测量并应对复杂的室内外环境。其次，GNSS测量精度与VIN系统不匹配，在GNSS信号传播过程中存在各种误差源。在实际应用中，GNSS系统中用于全球定位的伪距测量只能达到米级精度，而VIN系统能够在短时间内提供厘米级估计。因此，融合系统如果没有精心设计，将会受到不稳定GNSS测量的影响。第三，当融合系统经历纯旋转或锁定卫星数量不足等特定运动时，就会发生退化。通常，gnss -视觉-惯性融合系统可以提供无漂移的六自由度全局估计，但在退化情况下该结论不再适用。此外，在室内和室外环境的转换过程中，所有的卫星都会丢失并逐渐重新被捕获，这也给系统设计带来了挑战。
为了解决上述问题，我们提出了一种基于非线性优化的系统，将GNSS原始测量数据(伪距和多普勒频移)与视觉和惯性数据紧密融合，以精确和无漂移状态估计。在初始化阶段，采用从粗到细的方法恢复局部帧与全局帧之间的4自由度变换，并进一步优化。为了纳入有噪声的GNSS原始测量数据，所有的GNSS约束都是在统一的概率框架下制定的，所有状态都是联合优化的。此外，还讨论了退化的情况，并仔细处理，以确保鲁棒性。由于采用了紧密耦合的方法和系统设计，我们的系统充分利用了GNSS、视觉测量和惯性测量之间的互补特性，即使在复杂环境下也能够提供局部平滑和全局一致的估计，如图1所示。本文的主要贡献如下:一种在线粗精初始化gnss视觉惯性状态的方法。一种基于优化的紧耦合方法，在概率框架下融合视觉惯性数据和多星座GNSS原始测量数据。一个实时估计器，能够提供无漂移的六自由度全局估计，在复杂环境下，GNSS信号可能被大量拦截，甚至完全不可用。在仿真和真实环境中对该系统进行了评估。为了研究社区的利益，这项工作中的源代码和数据集将被公开.
https://github.com/HKUST-Aerial-Robotics/GVINS
其余的工作结构如下:在第二节中，我们讨论现有的相关文献。第三节描述了系统中涉及到的符号和坐标系统。第四部分简要介绍了GNSS的相关背景知识。第五部分介绍了系统的结构和工作流程。问题的形式和方法在第六节中进行了说明。在第七节中，我们解决了GNSS初始化问题，并讨论了几个降低系统性能的退化情况。当我们从系统级别解决问题时，为了社区的利益，系统开发期间的工程挑战在VIII中列出。第九节给出了实验设置和评价。最后，第十部分对本文进行了总结。

2. 相关工作

基于多传感器融合的状态估计方法已经被证明是有效和稳健的，在这方面有大量的文献。其中，我们特别感兴趣的是结合小型和低成本的传感器，如相机，IMU和GNSS接收机，以产生实时准确的估计在未知的环境。
视觉测量与惯性测量的紧密耦合融合可以分为基于滤波器的方法和基于优化的方法。MSCKF[2] 是一种很好的基于滤波的状态估计方法，它利用多个摄像机姿态之间的几何约束来有效地优化系统状态。[3]在MSCKF的基础上改进了其准确性和一致性，[4]的目标是3克服其数值稳定性问题，特别是在移动设备上。与基于滤波器的方法相比，非线性分批优化方法可以通过重新线性化来获得更好的性能，但代价是计算量的减少。OKVIS[5]采用基于关键帧的滑动窗口优化方法进行状态估计。VINS-Mono[6]也在滑动窗口内优化系统状态，但更完整的在线重新定位和姿态图优化。由于相机和IMU只在状态之间施加相对约束，累积漂移是VIN系统的一个关键问题，特别是在长期运行中。
由于GNSS提供了全球地球坐标系下的绝对测量，整合GNSS信息是减少累积漂移的自然方法。[7][8]以松耦合的方式描述了在EKF框架下融合GNSS解决方案与视觉和惯性数据的状态估计系统。[9]提出了一种融合视觉、惯性、激光雷达和GNSS方案的UKF算法，在不同环境下产生平滑一致的轨迹。[10]、[11]和我们之前的工作VINS-Fusion[12]在优化框架下将本地VIO的结果与GNSS解决方案融合。所有上述工作都依赖于GNSS解决方案来进行估计，因此一旦GNSS解决方案严重损坏或在跟踪卫星数量低于4颗的情况下不可用，系统就会出现故障。
也有一些关于将GNSS原始测量数据与视觉和惯性信息紧密融合的工作。[13]和[14]在EKF框架下结合相机、IMU和GNSS RTK测量进行定位，但需要一个静态的GNSS参考站来实现厘米级RTK解决方案。[15]和[16]研究了在混乱的城市环境下追踪不到4颗卫星的融合系统的性能。然而，局部和全局框架之间的转换没有处理和他们的现实世界实验的规模是有限的。此外，[16]底层VIN系统与GNSS融合性能不佳。[17]以EKF方式将GNSS伪距数据和天空指向相机测量数据紧密融合。上向摄像头用于过滤多径GNSS信号和高层建筑的跟踪特征，仅适用于城市环境。在他们的工作中，假设局部车架和全局车架之间的变换是已知的。最近，我们发现了一项类似的工作，将GNSS原始测量数据与视觉惯性SLAM紧密融合。虽然它在室外环境中工作得很好，但它不能处理室内场景，如隧道，这限制了紧密多传感器融合方法的潜力。
为此，我们的目标是建立一个稳健而准确的状态估计器，将GNSS原始测量数据、视觉和惯性数据紧密融合。通过利用GNSS的全球测量，消除了视觉惯性系统的累积误差。在不进行离线标定的情况下，估计局部帧和全局帧之间的变换。该系统能够在复杂的室内和室外环境下工作，并实现局部平滑和全局一致性。

3.符号和定义

A. 框架

我们系统涉及的空间框架包括:
1)传感器框:传感器框与传感器相连，是一个本地框，传感器报告其读数。在我们的系统中，传感器帧包括摄像机帧(·)c和IMU帧(·)i，我们选择IMU帧作为我们的估计目标帧，并表示它为体帧(·)b。
2)局部世界框架:我们将视觉惯性系统运行的常规框架表示为局部世界框架(·)w。在VIN系统中，任意设置局部世界坐标系的原点，通常选择z轴与局部接地面正交，如图2所示。
3) ECEF坐标系:ECEF(地球中心，地球固定)坐标系(·)e是一个相对于地球固定的笛卡尔坐标系。如图2所示，ECEF框架的原点位于地球的质心上。z轴垂直于地球赤道，指向正北。x-y平面与地球赤道重合，其中x轴指向本初子午线。
4) ENU框架:为了连接局部世界和全局ECEF框架，引入了半全局框架ENU。ENU坐标系(·)n的x、y、z轴分别指向东、北、上(图2)。在ECEF坐标系中给定一个点，可以确定一个唯一的ENU坐标系，其原点位于该点上。注意，本地世界框架和ENU框架的z轴都平行于重力方向。在时间帧方面，GNSS数据在GNSS时间系统中进行标记(例如，GPST)，而视觉测量和惯性测量在当地时间系统中进行标记。我们假设这两个时间系统事先是对齐的，因此不进行相应的区分。

b .状态

待估计的系统状态包括:
车身坐标系相对于局部世界坐标系的位置pw b和方向qw b，
速度vbw，加速度计偏置ba和陀螺仪偏置bw
每个特征的逆深度ρ，
本地世界帧和ENU帧之间的偏航偏移，
接收机时钟偏差δt和接收机时钟漂移速率δt_。
由于我们的系统支持所有四个星座，因此对GPS、GLONASS、Galileo和北斗的时钟偏差分别进行了估计。注意，每个星座的接收机时钟漂移速率相同。
我们的系统采用滑动窗口优化方式，窗口内的状态X可以概括为:X = [x0;x1;···xn;ρ0;ρ1;···ρm;] xk = HPW bk;vbwk;qw bk;英国航空公司;bw;δt;δt _ I;k 2 [0;n] δt = [δtG;δtR;δtE;δtC);(1)
式中，n为窗口大小，m为窗口中特征点的个数

4.GNSS基本介绍

由于我们的系统需要GNSS原始测量处理，所以有必要了解GNSS的背景知识。在本节中，我们首先对GNSS进行概述。然后介绍了伪距和多普勒频移的原始测量结果，并对其进行了建模。最后介绍了SPP算法进行全局定位的原理

A. GNSS 概述

全球导航卫星系统（GNSS），顾名思义，是一种基于卫星的系统，能够提供全球定位服务。目前有四个独立且全面运行的系统，即GPS、GLONASS、伽利略和北斗。每个GNSS系统由控制段、卫星段和用户段组成，用户段由无限数量的接收机组成。
卫星段是在约20,000公里高度绕地球轨道运行的卫星星座（北斗的GSO/IGSO卫星除外），其状态由控制段监测和更新。导航卫星不断发射特定的无线电信号，接收器可以从中唯一地识别卫星并检索导航信息。 GPS L1 信号的典型结构如图 3 所示。每颗卫星都有一个唯一的伪随机数 (PRN) 码，每 1 毫秒重复一次。包含卫星轨道和GNSS时间参数的导航电文（星历）首先与PRN码组合，然后用于调制高频载波信号。接收器接收到信号后，通过测量接收信号和设计信号之间的频率差来获得多普勒频移（第 IV-C 节）。伪距测量（第 IV-B 节）是从表示传播时间的 PRN 码位移推断出来的。最后，通过反向解调过程发现导航消息。

B. 伪距测量

接收到信号后，信号的飞行时间 (ToF) 是根据 PRN 码移位来测量的。通过乘以光速，接收机获得伪距测量。伪距之所以称为“伪”，是因为它不仅包含卫星与接收机之间的几何距离，还包括信号产生、传播和处理过程中的各种误差。
卫星侧误差源主要包括卫星轨道误差和时钟误差。轨道误差来自其他天体的影响，这些天体不是由星历精确建模的，时钟误差是卫星星载原子钟相对于标准系统时间不完善的结果。轨道和时钟误差由系统控制部分监控并不断纠正。
信号从卫星到接收器的传播过程中，经过电离层和对流层，电磁信号的速度不再与真空中的速度相同，并且信号根据大气成分和传播路径而延迟。信号以不同方式到达接收器的现象称为多径效应，可能会发生并增加额外的延迟，特别是对于低仰角卫星。当信号到达时，ToF 是通过将信号传输时间（由卫星的原子钟标记）与接收器不太准确的本地时钟时间进行比较来计算的。因此，距离信息也被接收器时钟偏差相对于 GNSS 系统时间偏移。总之，伪距测量可以建模为：
(2)
其中pe s和pe r分别为卫星s和接收机r的ECEF坐标。C表示真空中的光速。ζ是一个4 × 1的指标向量，对应的卫星星座实体为1，其他三个实体为0。∆ts为卫星时钟误差，由星历表计算得到。Trs表示对流层延迟，Irs表示电离层延迟。我们用M s r表示由多径效应引起的时延，用s r表示测量噪声。由于GNSS参考系即ECEF随地球旋转，因此GNSS信号的传播必须考虑Sagnac效应[19]。Sagnac术语Srs是由:
E c pe sx pe ry−pe sy pe rx;（3）
其中，E表示地球的角速度，·x和·y分别提取一个矢量的x和y分量

C. 多普勒测量

多普勒频移是由接收到的载波信号与设计载波信号的差值来测量的，反映了接收机-卫星在信号传播路径上的相对运动。由于GNSS信号结构的特点，多普勒测量精度通常比伪距测量精度高一个数量级。的
多普勒频移模型为:
其中，ver、vse分别表示接收机和卫星在ECEF帧内的速度。我们用λ表示载波信号的波长，用κs r表示接收机到卫星的单位方向向量。_ts为星历表中卫星时钟误差的漂移率，ηrs为多普勒测量噪声。
(4)
其中，ver、vse分别表示接收机和卫星在ECEF帧内的速度。我们用λ表示载波信号的波长，用κs r表示接收机到卫星的单位方向向量。_ts为星历表中卫星时钟误差的漂移率，ηrs为多普勒测量噪声。

D. SPP算法

单点定位(SPP)算法利用伪距测量通过三边测量来确定GNSS接收机的三自由度全球位置。因此，理论上，接收机的坐标可以通过3颗不同卫星的辅助来获得。然而，如第IV-B节所述，伪距测量被接收机时钟偏差所抵消。因为接收器的时钟偏差可能导致数百公里的误差，它必须连同位置一起估计，以便得到一个合理的结果。为此，至少需要4个伪距测量来充分约束3自由度全局位置和接收机时钟偏差。由于不同的导航系统使用不同的时间参考，不同系统之间存在时钟偏差。如果卫星来自多个星座，则需要额外的测量来估计系统间的时钟偏差。总之，至少需要同时跟踪(N + 3)颗卫星，才能获得唯一的sat1 sat2 sat1 sat2 (a) (b)接收机定位，其中N为被跟踪卫星中的星座数。
在收集到足够的测量值后，将Eq. 2中的约束条件叠加在一起，形成一系列pe r和δt未知的方程。对伪距测量进行了修正，使其仅为pe r和δt的函数。本系统利用Saastamoinen模型[20]估计对流层延迟Trs，利用Klobuchar模型[21]和星历表参数计算电离层延迟Irs。通过排除低海拔卫星，我们忽略了多径效应引起的时延Mrs。在实践中，将使用超过(N + 3)的测量值来得到一个可靠的解决方案，并通过最小化残差平方和来优化问题。如[22]所示，SPP解的噪声不仅依赖于测量噪声，还与卫星的几何分布有关系，这种关系在后面的Eq. 17中也有体现。图4中简化的二维案例显示了卫星分布对最终解噪声特性的影响。因此，在卫星分布均匀的情况下，即使测量噪声不变，SPP算法的性能也会更好。

图4所示。卫星分布如何影响SPP解的不确定性的一个简化的二维说明。这里我们假设接收卫星是同步的，因此两颗卫星就足够进行定位。虚线表示地面真实伪距，而两实线之间的区域表示可能的噪声测量。最终SPP解的不确定性用阴影面积表示。

5. 系统概述

本文提出的系统结构下图5所示。估计器以原始GNSS、IMU和相机测量值作为输入，然后对每种类型的测量进行必要的预处理。IMU测量值被预积分，整个图像被概括为一系列稀疏的特征点。对于GNSS原始数据，首先过滤掉容易出错的低俯仰角和不良的卫星信号。为了抑制不稳定的卫星信号，只允许连续锁定一定时间的卫星进入系统。由于星历数据是通过慢速卫星接收器无线链路（50位/秒）获取的，因此在相应的星历完全传输之前，全球卫星导航系统的测量是不可用的。在预处理阶段之后，所有的测量都为估计器准备好了，但是在进入优化部分之前，需要一个初始化阶段来正确初始化非线性估计器的系统状态。初始化过程从一个纯视觉的SfM开始，由该SfM联合估计一个最接近的运动和结构，然后将IMU的运动轨迹与SfM结果对齐，以恢复IMU的尺度、速度、重力和IMU偏差。
图5 上图显示了本文提出的系统的工作流程。首先，所有传感器的测量值在进入后续程序之前都经过预处理。在初始化阶段，视觉惯性初始化是通过将惯性信息与纯视觉SfM的结果对齐来完成的。如果视觉惯性系统成功对准，则执行从粗到精的过程，以初始化GNSS状态。一旦全球卫星导航系统介入，该系统将监测和处理全球卫星导航系统退化情况。最后在非线性优化框架下对滑动窗口内的测量约束进行优化。注意，如果GNSS不能初始化，我们的系统仍然可以在视觉惯性模式下工作。同时采用边缘化策略，保证实时估计。

本文提出的系统结构如图5所示。该估计器将GNSS、IMU和相机的原始测量值作为输入，然后对每种测量值进行必要的预处理。在[6]中，对IMU测量值进行预积分，将整个图像总结为一系列稀疏特征点。对于GNSS原始数据，我们首先过滤掉低海拔和不健康的卫星，这些卫星容易出错。为了抑制不稳定的卫星信号，只允许连续锁定一定时点的卫星进入系统。由于星历数据是通过缓慢的卫星接收器无线链路(50位/秒)获取的，因此在相应的星历被完全传输之前，GNSS测量是不可用的。在预处理阶段之后，所有的测量都为评估人员准备好了，但是在进入优化部分之前，为了正确地初始化非线性估计器的系统状态，需要一个初始化阶段。
初始化首先从一个仅基于视觉的SfM开始，从该SfM中联合估计到相似的运动和结构，然后将IMU的轨迹对准SfM结果，以恢复尺度、速度、重力和IMU偏差。VI初始化完成后，进行由粗到细的GNSS初始化过程。首先通过SPP算法得到粗锚定位结果，然后利用VI初始化和GNSS多普勒测量得到的局部速度和全局速度在偏航对准阶段进行关联。最后，初始化阶段以锚点精化结束，利用精确的局部轨迹和施加时钟约束进一步精化锚点的全局位置。
初始化阶段结束后，对GNSS的退化情况进行检查和仔细处理，以确保鲁棒性能。然后在非线性优化框架下，利用所有测量值的约束来联合估计滑动窗口内的系统状态。请注意，如果GNSS不可用或不能正确初始化，我们的系统将自然降级为VIO。为保证实时性能和处理视觉惯性退化运动，每次优化后均采用双向边缘策略。

6. 概率公式

在这一节中，我们首先在概率框架下表述并导出状态估计问题。如后面所示，整个问题被组织为一个因子图，来自传感器的测量形成一系列因子，这些因子反过来又约束了系统状态。本节将详细讨论概率图中的每种因素。注意，视觉和惯性因素的形成是继承自[6][23][24]，因此不是本工作的贡献。仅为保证文献的完整性，列出相关内容。

A.地图估计

我们将最优系统状态定义为在给定所有测量值的情况下使后验(MAP)最大化的状态。假设所有测量值相互独立，且每个测量值的噪声均为零均值高斯分布，那么MAP问题可以进一步转化为使一系列代价之和最小的问题，每个代价对应一个特定的测量值。
其中z表示n个独立传感器测量值的聚合，frpHpg封装了系统状态的先验信息。e(·)为每个测量的残差函数，k·kP为马氏范数。注意，这样的公式自然适合于因子图表示[25]，因此我们将优化问题分解为与状态和测量相关的单个因子，如图6所示。下面我们将详细讨论每个因素。

B .惯性因子

b .惯性因子
惯性因子的测量包括平台的偏置、噪声线加速度和角速度。由于加速度计在地球s7表面附近工作，线性加速度测量也包含了重力分量，如下所示:
~t = !t + bwt + nw;(6）
其中fatg为IMU的输出，fatg为IMU传感器框架中平台的线加速度和角速度。假设加性噪声na和nw为零均值高斯分布，如na N (0;Σa)， nw N (0;Σw)。与加速度计和陀螺仪相关的缓慢变化的偏差被建模为如下的随机行走:
B_ at = nba;B_ wt = nbw;（7)
与nba N (0;Σba)， nbw N (0;Σbw)。在实际应用中，IMU的频率通常比相机的频率高一个数量级，因此很难估算IMU测量的每个状态。为此，采用IMU预集成方法[23]，将多个度量值聚合为一个度量值。对于时间间隔[tk, tk+1]内的惯性测量，导出的测量值计算如下:
αbk bk+1 = Zt2[tk;tk+1] Rb tk(a~t bat)dt2 βbbkk+1 = Zt2[tk;tk+1]~ t bwt)γtkdt;（8)
与
= b !!cT ! 0;b !C = 0!zy ! 0 !xz ! 0 !Yx3 5:(9)
这里bk代表时间tk中的身体框架。fα;β;γg封装了坐标系bk和坐标系bk+1之间的相对位置、速度和旋转信息，可以在没有给定IMU偏差的初始位置、速度和旋转轮廓的情况下构造。最后，残差与系统状态有关，预集成IMU测量结果可表示为:
rB (z ~ b bk k + 1;X) (10)
其中δθbk bk+1建模三维欧氏空间中的相对旋转误差，·xyz运算返回四元数的虚部。
图6所示。我们系统的优化问题的因子图表示，其中系统状态用大的彩色圆表示，因子用小的黑色圆表示。各种测量因素包括惯性因子i、视觉因子f、伪距和多普勒因子g和时钟因子c。

C .视觉因素

本系统采用的视觉测量方法是从图像帧中提取稀疏特征点。检测图像内部的强角[26]作为特征点，并进一步采用迭代Lucas-Kanade方法[27]进行跟踪。对特征点进行畸变校正[28]后，投影过程可以建模为:
Pe = πc(Rc b(Rb w xw + pb w) + pc b) + nc;
其中Pe = [u, v]T为图像平面上的特征坐标，xw为其在局部世界帧中相应的3D地标位置。πc(·)为摄像机投影函数，nc为测量噪声。因此，对于一个在坐标系i中具有逆深度ρl的特征l，如果在坐标系j中再次观察到它，两个坐标系之间的残差可以表示为:
rC (z ~ l;(X) = Pelcj πc(X ^ cl j) X ^cj l = Rc b(Rb wj (Rw bi(Rb c1 ρl πc 1(Pelci) + pb c)+ pb wj)+ pc b(12))
与fRb cb cg IMU和相机之间的转换。

D .伪距的因素

考虑一个锁定导航卫星s的GNSS接收机r，它通过测量码移位来获得如Eq.(2)所示的伪距信息。卫星时钟误差和大气延迟通过第IV-D节中描述的模型进行补偿。 在我们的系统中，伪噪声s r被假定为零均值高斯分布，如s r N(0; pr = ns npr sin2 θel:(13)
这里ns是广播卫星空间精度指数，npr是接收机报告的伪距测量噪声指数。 θel表示从接收机角度看卫星的仰角，这个分母项有两个原因。
首先，它可以抑制低海拔卫星经常出现的GNSS多径效应引起的噪声。此外，导航系统广泛采用的Klobuchar模型得到的电离层延迟误差仍高达50%[21]。由于低海拔卫星将经历一个显著的电离层延迟，分母项也可以减少与电离层补偿带来的误差。通过一个定位点，将接收机的ECEF坐标映射到本地世界坐标系中相应的坐标，在这个定位点上建立ENU坐标系。给定定位点的ECEF坐标，从ENU坐标系到ECEF坐标系的旋转为:
Re = 24 sin λ sin φ cos λ cos φ cos λ cos λ sin φ sin λ cos φ sin λ 0 cos φ sin φ 35;（14）
其中φ、λ为地理坐标系中参考点的经纬度。ENU与局部世界坐标系Rn w之间的1-DOF旋转由偏航偏移给出。那么ECEF坐标与local world坐标的关系可以表示为:
pe r = Re nRn w(pw r pw anc) + pe anc:(15)
在我们的实现中，我们将锚点设置为本地世界框架的原点，也就是说，本地世界框架的原点与ENU框架的原点重合，如图2所示。此外，接收机天线和IMU之间的偏移量(在我们的平台上只有几厘米)被省略，因此我们不区分pw r和pw b。那么连接体bk和卫星sj的tk中伪距测量的残差可以表示为
（16,17,18）略
如式(4)所示，多普勒频移反映了接收机与卫星之间信号传播路径线上的相对速度。与伪距噪声类似，假设多普勒测量噪声ηr;dp s为高斯分布，其方差模型为:
dp = ns NDP sin2 θel;(19)
其中NDP为接收机上报的测量噪声指数。在ECEF坐标系中，接收机的速度可以通过以下方法得到:
ve r = Re nRn wvrw:(20)
这里把vw r看作vbw。最后，连接体bk和卫星sj的tk中与多普勒测量相关的残差可表示为:
rD(z~s rkj, X) =λ1 κs rkj T (vsej Re nRn wvbwk)+ c λ (δt _ k t_sj)+ ff s rkj:(21)
关于态hvbwk δ t_ k iT的雅可比矩阵列示如下:
dp = 1 λ h κs rkj T Re nRn w c κs rkj T Re nGvbwki: (22)

F.接收机时钟因子

接收器的tk和tk+1时钟偏差与时钟漂移速率相关:
1 Ztkt k 1 δt dt;_ (23)
其中1n m表示n × m全1矩阵，离散情况下残差为:
z~k k 1;X) = δtk δtk 1 14 1δ t_ k 1τkk 1;（24）
其中τ k k 1是测量k 1和k之间的时间差。与此残差相关的协方差矩阵定义为4 × 4对角矩阵Dt;k及其元素描述离散误差。另一方面，GNSS接收机时钟漂移率由接收机时钟的频率稳定性决定。TCXO常被选作低成本GNSS接收机的时钟源。由于TCXO的噪声特性，接收机时钟漂移速率被建模为随机游走过程，因此残差为:
rW (z ~ k k 1;X) = δ t_ k δ t_ k 1;（25)
相应的方差σdt;k由时钟频率漂移的稳定性决定。

7. GNSS初始化和退化

上一节描述的状态估计过程对于系统状态是非线性的，因此它的性能很大程度上依赖于初始值。通过在线初始化，可以很好地从未知情况恢复初始状态，无需任何假设或人工干预。在系统运行过程中，估计器也可能遇到一些传感器失效或退化的不完美情况。由于已经有大量关于视觉惯性系统的初始化和退化主题的文献，在本节中，我们将范围限制在GNSS部分。接下来，我们首先介绍了所提出的由粗到细的GNSS初始化方法，然后我们讨论了几个降低系统性能的场景。

https://blog.csdn.net/ryankaros/article/details/117847424
A.初始化
初始化是需要一个已知全局和局部坐标的锚点，将全球GNSS测量与局部视觉和惯性信息进行融合。由于锚点已经设置到局部世界框架的原点，因此需要事先校准局部的世界原点的ECEF坐标。在本文中，我们提出了一个多阶段GNSS-VI初始化程序来在线校准锚点和偏航偏移ψ 在ENU和局部世界框架之间。在GNSS-VI初始化之前，假设VIO已成功初始化，即重力矢量、初始速度、初始IMU偏差和比例已获得初始值。之后，在局部世界框架中形成一条平滑的轨迹，并准备用于GNSS-VI初始化阶段。如图7所示，在线GNSS-VI初始化以从粗到精的方式进行，包括以下三个步骤：
1）粗定位点定位；
首先，GNSS SPP算法在没有任何先验信息的情况下生成一个粗略的ECEF坐标。SPP算法将所有来自滑动窗口的伪距测量值作为输入。由于滑动窗口内的测量是时间和空间跨度的，从SPP得到的结果是当前窗口的平均位置
2）偏航角偏移校准；
在第二步中，我们使用低噪声的多普勒测量来校准ENU坐标系与局部世界坐标系之间的偏航偏移。初始偏航偏置和接收机时钟漂移率通过以下优化问题得到:
其中n为滑动窗口大小，pk为窗口内第k历元观测卫星数。这里我们将速度vbw固定为VIO的结果，并假设δ t_ k在窗口内是恒定的。利用第一步得到的粗锚点坐标计算方向向量κs r和旋转Re n。κs r和Re n对接收器的位置不敏感，因此一个粗锚点坐标就足够了。估计的参数仅包括整个窗口测量的偏航偏移和平均时钟偏差漂移率δt_。在此基础上，对ENU坐标系与局部世界坐标系之间的转换进行了充分的标定。
3）锚点优化。
最后，我们准备完善之前粗糙的锚点，并将本地世界轨迹与ECEF框架对齐。与第一步不同的是，来自VIO的位置结果用作先验信息。下面的问题是经过滑动窗口测量优化的:
最小化δt;nX k =1 pk X j =1 rP(z~s rkj;r + nX k =1 rT (z~k k 1;X) 2 Dt;k (27)
通过对上述问题的优化，得到了与各GNSS历元相关联的锚点坐标和接收机时钟偏差。在这一步之后，锚点，ENU框架的原点，被设置为本地世界框架的原点。最后，完成整个估计器的初始化阶段，并生成启动系统所需的所有初始量。

B.退化
毫无疑问，我们的融合系统将在开放区域表现最好，那里的GNSS信号稳定，卫星分布良好。下面我们将讨论几种可能会降低系统性能的情况。

1） 低速运动：由于多普勒频移测量的噪声级比伪距测量的噪声级低一个数量级，因此局部帧和ENU帧之间的偏航偏移可以通过多普勒频移测量的短窗口得到很好的约束。一旦GNSS接收机的速度低于多普勒频移的噪声水平，估计的偏航偏移量就可能被测量噪声所破坏。此外，低速运动也意味着窗口内的平移距离较短，因此偏航估计也可能受到伪距的影响。在极端情况下，当平台经历纯旋转运动时，GNSS不能提供任何关于旋转方向的信息，而偏航分量又会像VIO那样漂移。因此，如果窗口内的平均速度低于阈值vths，我们固定偏航偏移变量。在我们的系统中，vths设置为0.5m/s，即使行人也能很容易地满足要求。
2） 少于4颗被跟踪的卫星：如果被跟踪的卫星数量少于4颗，SPP或松散耦合的方法将无法解析接收器的位置。然而，在紧耦合结构的帮助下，我们的系统仍然能够利用可用的卫星并随后更新状态向量。
3） 无GNSS信号：在GNSS信号完全不可用的室内或杂乱环境中，与全局信息有关的状态，即偏航偏移ψ, 接收机时钟偏差δt和漂移率˙δ它们不再是可见的。然而，在优化过程中，仍然保持等式（24）和（25）的约束。

8. 工程的挑战

多个异构传感器的集成往往带来很多挑战，如时间同步、标定等，如果不加以考虑，可能会给估计器带来较大的误差。在本节中，我们将讨论提出的多传感器融合系统开发过程中的几个工程挑战。

A.时间同步

在[30]中已经表明，视觉测量和惯性测量之间的毫秒级不对准可能严重影响VINS系统的性能。不幸的是，对于涉及GNSS的多传感器系统，情况会变得更糟，因为时间信息对GNSS至关重要，如果时间被误用，误差可能会被光速放大。如前所述，GNSS系统工作在自己的时间系统中，而当地的传感器如相机和IMU工作在当地的时间系统中，因此GNSS与当地的时间系统正确对齐是系统集成的必要条件。根据对系统性能的影响，可以将GNSS与视觉惯性测量的对准分为三个级别，如图8所示。在理想的情况下，来自不同传感器的测量值被标记在一个统一的时间系统中，并通过一个脉冲信号被硬件触发。如果测量是在不同的时间捕获的，但仍然是在同一时间系统下，情况会变得更糟，因此必须应用插值来正确地融合不同时间戳的测量。同步的最坏情况是两个时间系统没有对齐，因此GNSS和视觉惯性测量之间的时间关系是完全未知的。在这种情况下，估计结果是不可靠的，系统行为是不可预测的。在我们的系统中，两个时间系统通过硬件触发对齐。由精度在纳秒级的GNSS接收机产生的PPS信号馈入vissensor。一旦VI-Sensor检测到时间脉冲的边缘，它就会在当地时间系统中报告相应的时间戳。同时，接收机提供与PPS信号相关的时间信息，从而实现全球GNSS时间系统与本地时间系统的良好对齐。然而，在我们目前的系统中，GNSS和VI测量仍然是在不同的时间捕获的，两种测量之间的差距取决于VI- sensor的启动时间。为此，我们对系统状态采用线性插值，以最小化不同捕获时间的影响。

b .电磁

接收机天线感知到的GNSS信号功率很低，完全淹没在热噪声中。因此，接收机的性能可能会受到附近射频源的影响。请注意，虽然特定的频带是专门为GNSS使用保留的，但许多设备仍然在无意中向GNSS频带发射强大的射频功率。正如[31]所指出的，许多现代数字设备，如计算机或相机，往往会产生一个很宽的频谱，从而与GNSS频段重叠。此外，外围设备的I/O电缆可以作为天线，随后将有噪声的射频信号传递给接收器。不幸的是，这种推断RF信号的功率级通常比近地表GNSS信号的功率级大得多，因此必须认真考虑电磁干扰(EMI)。在我们早期的实验中，我们发现一些USB3相机在图像数据传输过程中严重降低了接收器的性能。通过适当的接地设计和对电缆和连接器的屏蔽，可以在一定程度上减轻干扰。后来，当我们切换到VI-Sensor进行时间同步时，干扰问题不再存在，因为它通过以太网电缆传输视觉和惯性数据。

C.接收时钟跳变

由于底层廉价振荡器的频率稳定性，导致低成本接收机的时钟经常出现累积漂移。当漂移达到一定程度时，一些厂商会选择重置时钟，以防止与GNSS时间有较大偏差。这种操作将引起微秒级或毫秒级的时钟跳变，通过信号s ToF[32]获得的伪距测量可以观察到相应的不连续。如果以光速放大，这种不连续性可能达到106米，进而导致系统失效。注意，当我们在滑动窗口内引入接收时钟之间的约束时，情况会变得更糟。在长时间静态实验中，我们观测到20毫秒的时钟跳变和2106米的伪距间隔。为此，必须避免或小心处理发生时钟跳转的数据序列。

9. 实验结果

我们进行了仿真和真实世界的实验来验证我们提出的系统的性能。在本节中，我们将我们的系统与VINS-Mono[6]、VINS-Fusion [12] (Monocular+IMU+GNSS)和RTKLIB[33]进行比较。由于我们只关心实时的估计结果，因此关闭了VINS-Mono和VINS-Fusion的环路函数。我们使用RTKLIB3计算GNSS的SPP解，并将得到的GNSS位置输入VINS-Fusion得到一个松耦合的结果。
https://blog.csdn.net/u013019296/article/details/117341876

我们进行了模拟和真实世界的实验来验证我们提出的系统的性能。在本节中，我们的系统与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信号错误和修复丢失事件造成的。

10. 总结

本文提出了一种基于非线性优化框架下的紧密耦合系统来融合相机、IMU和GNSS接收机的测量数据。我们的系统从初始化阶段开始，在初始化阶段，采用从粗到细的过程在线校准局部帧和全局帧之间的转换。在优化阶段，GNSS原始测量在概率因子图框架下建模和制定。考虑并仔细处理退化情况，以保持系统在复杂环境下的鲁棒性。此外，还讨论了系统集成过程中的工程挑战，以促进其他GNSS融合研究。我们在仿真和真实环境下进行了实验，评估了系统的性能，结果表明，我们的系统有效地消除了累积漂移，保持了典型VIO系统的局部精度。为此，我们声明，我们的系统可以实现局部平滑和全局一致性。在未来的工作中，我们将在各种退化情况下进行理论的可观测性分析，我们的目标是建立一个在线的可观测感知状态估计器，以应对复杂环境和可能的传感器故障。此外，我们还希望利用GNSS测量组合[36]或精确点定位(PPP)[37]技术来处理群系统中的分布式定位任务，以减小绝对定位误差。

在本文中，我们提出了一个基于非线性优化框架下的紧耦合系统来融合来自相机、IMU和GNSS接收机的测量。我们的系统从一个初始化开始，在初始化阶段采用从粗到精的过程来在线校准局部和全局帧之间的转换。在优化阶段，在概率因子图框架下对GNSS原始测量值进行建模和表达。为了保证系统在复杂环境中的鲁棒性，对退化情况进行了仔细的分析和处理。此外，还讨论了系统集成过程中遇到的工程难题，以便于其他GNSS融合研究。我们在仿真和真实环境下进行了实验，结果表明，我们的系统有效地消除了累积漂移，保持了典型VIO系统的局部精度。为此，我们声明我们的系统可以实现局部平滑和全局一致性。