GVINS：初始化（GNSS与VIO融合）

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.

 一、系统结构图

估计器以原始GNSS、IMU和相机测量值作为输入，然后对每种类型的测量进行必要的预处理。IMU测量值被预积分，整个图像被概括为一系列稀疏的特征点。对于GNSS原始数据，首先过滤掉容易出错的低俯仰角和不良的卫星信号。为了抑制不稳定的卫星信号，只允许连续锁定一定时间的卫星进入系统。由于星历数据是通过慢速卫星接收器无线链路（50位/秒）获取的，因此在相应的星历完全传输之前，全球卫星导航系统的测量是不可用的。在预处理阶段之后，所有的测量都为估计器准备好了，但是在进入优化部分之前，需要一个初始化阶段来正确初始化非线性估计器的系统状态。初始化过程从一个纯视觉的SfM开始，由该SfM联合估计一个最接近的运动和结构，然后将IMU的运动轨迹与SfM结果对齐，以恢复IMU的尺度、速度、重力和IMU偏差。

二、初始化

1、GNSS 初始化决定了state的预测结果；
2、在线初始化可以从任意情况下恢复初始state；
3、anchor point 的局部和全局坐标已知（用于GNSS 全局系和VIO的局部系融合）；
前提：

a. VIO系统已经完成初始化，如重力向量，初始速度，初始IMU bias，尺度）；
b. 局部地图系上的光滑轨迹可用于GNSS-VI 的初始化；

步骤：
a. Coarse anchor point localization: 使用GNSS 的SPP算法（输入：滑窗内的所有伪距），由于滑窗是位于一段空间和时间上的，SPP的结果是当前串口的平均位置；
b. Yaw offset calibration: 校准位于ENU frame和local frame间的yaw offset航线偏移（使用低噪声的Doppler Measurement），通过下式可获得初始yaw offset和接收器时钟漂移速率；

n为滑窗的大小，pk为第k个epoch观察到的的卫星数量。滑窗内，假设速度恒为VIO求得的结果，k时刻的时钟漂移率恒定。上一步求得的 Coarse anchor point localization用于计算方向向量和旋转矩阵。优化变量为Yaw offset ψ和滑窗内平均时钟漂移速率。优化后，ENU frame和local frame完成校准。
c. Anchor Point Refinement: 完善上一步得到的coarse anchor point,并调整local frame下的轨迹实现在ECEF frame中的alignment. Anchor Point由之前ENU frame的原点变为local frame的原点。 图示见下图：
 

Frame介绍

1、sensor frame(传感器系)
2、local world frame（局部系:VIO系统所在系，原点可为任意点，Z轴垂直与lcoal ground plane–红色虚线；
 3、ECEF frame（Earth-Centered,Earth-Fixed:相对于地球的Cartesian坐标系，原点在地球中心处，Z轴垂直于赤道指向正北，下，x,y轴与赤道相交，x轴指向本初子午线—蓝色实线；
 4、ENU frame(Semi-globla frame):作用是连接local frame和全局ECEF frame，x指向东，y指向北，z向上。对于任意一个ECEF上的点，都有唯一的ENU frame（此ENU原点在ECEF目标点上—黑色实线；
 注： 对于local frame和ENU frame，Z轴都平行于重力方向；
 

ECEF 和local frame间转化关系

• 已知接收器在ECEF下坐标，欲求接收器在local下的坐标，需通过anchor point
• ENU坐标的建立就为了解决此问题
• 已知anchor point在ECEF下坐标，从ENU到ECEF的旋转矩阵为：

• φ and λ是纬度和经度；
• 从ENU到local的旋转矩阵R为yaw offset ψ；

因此，ECEF到local frame

 接收器在ECEF位置 = 旋转矩阵 *（接收器与anchor在local 下位置差）+ anchor在ECEF中位置

理解：

 1、 需要求接收器ECEF上位置，已知local 位置，所以需要R矩阵；
 2、   因为接收器在移动，所以R矩阵需要从Anchor确定；
 3、   anchor用于ENU的引入，因为anchor的local 和ECEF坐标都已知，所以可以作为frame转换的中间媒介；
4、    anchor值固定，R矩阵也因此可以得出；

三、基于概率的因子图优化

如下图所示，整个问题被建模成一个因子图，来自传感器的测量形成了一系列因子，这些因子反过来又约束了系统状态。

 

因子图模型，其中系统状态由彩色的大圆圈表示，因子由黑色的小圆圈表示。来自各种传感器测量的因子包括惯性因子 i、视觉因子 f、伪距和多普勒因子 g 以及时钟因子 c。

 

6.泡泡机器人解读

GVINS：基于GNSS-视觉-惯性紧耦合融合的平滑一致状态估计方法