美团无人机新作SRI-GVINS：将GNSS原始观测与视觉惯性深度融合的导航新SOTA

GNSS如何融入VI系统

视觉-惯性里程计（VIO）或视觉-惯性导航系统（VINS）是融合相机测量和惯性测量单元（IMU）数据，以估计传感平台的3D运动，包括方向、位置和速度，可分为基于优化的方法和基于滤波的方法。由于VIO容易积累漂移，并且全局位置和偏航对应的四个不可观测方向，无法提供全局定位。

GNSS测量可以提供绝对测量，通过融合VIO和GNSS，可以实现无漂移的全局定位以及一致的局部轨迹。

VIO和GNSS的融合主要有两种方法：松耦合和紧耦合。

• 在松耦合方法中：VIO和GNSS是独立处理的，大多数工作依赖于GNSS解决方案进行估计，在跟踪卫星数少于四个的情况下无法提供信息。
• 在紧耦合方法中，VIO和GNSS在一个联合优化框架内集成。该框架同时优化GNSS原始测量（伪距、载波相位或多普勒频移）、视觉和惯性信息以最小化其误差。

现有方法的一些问题：

• 不支持在线GNSS-IMU外参标定
• 在多普勒频移测量时不插值速度，这可能引入未建模的误差。

SRI-GVINS是一种紧耦合的GNSS-视觉-惯性导航系统，在一个高效的平方根滤波框架内深度融合GNSS原始测量（SRI-SWF），主要贡献有：

• 首次在SRI-SWF[2]框架内深度融合伪距、多普勒频移、单差伪距和双差载波相位的GNSS原始数据与IMU和相机测量，以提供无漂移的全局定位。提供了数值稳定性和计算效率。
• 一种基于滤波的初始化方法，以自适应收敛参考框架与VIO框架之间的变换。利用IMU积分来更新状态，使用延迟和异步GNSS测量。具备GNSS-IMU外参在线标定的能力，以减轻外部参数退化的影响。
• 在无人机的真实世界数据集上展示了其实时消除VIO漂移的能力和GNSS-IMU外部校准的有效性。在精度和效率方面优于现有的最先进方法，如GVINS[3]和InGVIO[4]。

平方根逆滑动窗口滤波器是什么？

SRI-SWF是提出的SRI-GVINS的基础，其架构如图1所示。

在每个时间步，状态向量包括当前SLAM特征、历史IMU姿态克隆、参数状态向量和IMU额外状态。

其中，表示点特征的状态，通过当前滑动窗口内其首次观测相机位姿的逆深度参数化表示。表示时间步的克隆IMU位姿状态，其中是描述IMU框架{}到本地VIO世界框架{}旋转的四元数，是{}在{}中的位置。 是IMU到相机的外参，是GNSS接收器到IMU的外部参数。 表示接收器相对于GNSS星座的时钟偏移， 是接收器的时钟漂移速率，对每个星座都是相同的。是{}在{}中的速度，而和是时间步时的加速度计和陀螺仪偏置。

IMU传播

给定惯性测量 ，利用惯性运动学进行传播：

其中 ，而 是影响IMU测量的高斯噪声，其协方差为 。

通过在状态估计周围线性化方程 (5)，得到误差状态传播方程：

其中， 和 是相应的雅可比矩阵。在IMU传播步骤中，一个新的位姿状态 被添加到当前状态向量中：

先验（上三角）平方根信息矩阵和残差向量定义如下：

其中，， 和 是雅可比矩阵 对克隆和额外IMU状态的块列。注意， 在下一个边缘化步骤之前不是上三角矩阵。

边缘化

在每个时间步，过去的SLAM特征$ \tilde{x}{DS}\tilde{x}{Ck-M}\tilde{x}_{Ek-1}$ 被边缘化。包含所有这些待边缘化状态的（误差）状态向量定义如下：

将剩余的状态向量记作 ，并通过一个置换矩阵将状态向量变换如下：

通过执行完整的QR分解，获得了先验（上三角）平方根信息矩阵和残差向量，并且仅保留 。

测量更新

通常，测量可以建模如下：

其中， 是描述测量模型的非线性函数， 是获得的测量值（例如，SLAM特征的重新观测和MSCKF特征测量）， 和 分别表示测量的白噪声和其协方差。

通过围绕状态估计 线性化方程 (11)，得到线性化的测量约束：，其中， 是测量雅可比矩阵， 是残差。

最后，我们通过执行QR分解来更新状态：

SRI-GVINS: 初始化和更新

在SRI-SWF框架内，除了VIO中的视觉-惯性测量外，所提出的SRI-GVINS还深度融合了GNSS的伪距、多普勒频移、单差伪距和双差载波相位测量。因此，这里重点介绍使用原始GNSS数据进行的测量更新。

帧初始化

GNSS更新需要从本地VIO世界框架到ECEF框架的变换 、接收器时钟偏移 和接收器时钟漂移速率 。为了初始化变换 ，将变换变量插入滤波器中以自适应收敛4自由度变换。接收器在VIO世界框架中的位置和速度可以表示为：

通过利用第一个GNSS测量作为参考点，我们使用GNSS单点定位（SPP）输出来获得接收器在东北下（NED）框架 {}中的位置和速度。在每个时间步，有以下几何约束：

注意，时间步时的接收器位置和速度可以通过IMU积分从时间步的GNSS测量预测出来。由于 对偏航角是1个自由度，因此变换状态可以表示为 。

通过将状态向量与变换变量以及无限协方差先验一起扩展，执行标准测量更新（见公式16）。当偏航角的协方差矩阵 低于决策阈值 时，判断变换已经收敛。初始化完成后将变换状态边缘化。

给定从参考点计算的 和 ，变换 通过以下方式获得：

GNSS接收器时钟偏移

我们使用伪距和多普勒频移测量来初始化GNSS接收器的时钟偏移 和接收器时钟漂移速率 。为此，我们计算时间步k时伪距测量 的残差如下：

其中， 表示光速。 和 分别表示接收器和卫星 的时钟偏移。、 和 分别表示对流层延迟、电离层延迟和萨格纳克效应项。 表示伪距测量噪声。

类似地，多普勒频移测量 的残差表示如下：

其中， 表示GNSS载波波长。 是从接收器到卫星的单位方向向量。 表示卫星 在ECEF框架中的速度。 和 分别表示接收器和卫星 的时钟漂移速率。 表示多普勒频移测量噪声。

通过使用所有GNSS测量残差（见公式18和19），求解以下线性约束 ，其中 是 和 的堆栈。最初，应用RANSAC算法来消除任何错误的约束。随后，求解上述最小二乘问题以获得GNSS接收器时钟偏移和时钟漂移速率的初始值。然后通过延迟初始化进一步将初始值扩展到状态中。

一旦初始化，按如下方式传播 和 ：

先验（上三角）平方根信息矩阵如下：

其中 表示雅可比矩阵 对参数状态的块列。

GNSS测量更新

由于GNSS消息的延迟和异步到达，GNSS测量时间通常早于惯性状态时间。由于滑动窗口中没有保存每个克隆位姿的速度状态，因此无法通过插值获得GNSS测量时间的速度状态。假设在时间步i的惯性状态和时间步k的GNSS测量，通过IMU积分有以下约束：

其中 、 和 是预积分项。对于时间区间 内的惯性测量，这些项计算如下：

1. 伪距测量（Psr）：将方程 (14) 和方程 (22) 代入方程 (18) 中，以定义完整的伪距测量残差。

2. 多普勒频移测量（Dopp）：将方程 (15) 和方程 (22) 代入方程 (19) 中，以定义完整的多普勒频移测量残差。

3. 单差伪距测量（Dpsr）：考虑到基站的存在，我们可以定义时间步k时基站的伪距测量：

当基站和接收器距离较近时，电离层和对流层延迟应通过减去方程 (18) 和方程 (24) 来大致抵消：

4. 双差载波相位测量（Ddcp）：载波相位测量通常比伪距测量更精确，载波相位测量的残差表示如下：

其中 是整数模糊度， 是载波相位测量， 是其噪声。通过对基站和基准卫星si进行差分测量，双差载波相位测量的残差定义如下：

其中 和 是通过RTK算法获得的双差整数模糊度。

这些GNSS测量的雅可比矩阵可以计算为：

总体测量协方差矩阵 表示为：

其中 是GNSS原始测量协方差矩阵， 是来自IMU积分测量的噪声。

GNSS-IMU在线标定

GNSS-IMU外参在状态向量中表示为 （见公式3）。使用多普勒频移测量进行外部参数估计，并且仅在IMU激励足够时更新外部参数。多普勒频移残差相对于外部参数的雅可比矩阵如下：

实验效果

总结一下

SRI-GVINS是一种紧耦合的GNSS-视觉-惯性导航系统，深度融合了视觉、惯性和原始GNSS数据（包括伪距、多普勒频移、单差伪距和双差载波相位）在高效的SRI-SWF框架内。使用基于滤波器的初始化方法自适应收敛参考框架变换，利用IMU积分更新状态，并在线校准GNSS-IMU外参。未来改进方向：

• 使用原始GNSS测量（如多普勒频移）来辅助参考框架变换的初始化
• 直接采用LAMBDA算法解决SRI-GVINS中的双差模糊度问题
• 对SRI-GVINS进行可观性分析，以识别潜在的退化运动

参考

[1] Square-Root Inverse Filter-based GNSS-Visual-Inertial Navigation

[2] A square root inverse filter for efficient vision-aided inertial navigation on mobile devices

[3] GVINS: Tightly Coupled GNSS-Visual-Inertial Fusion for Smooth and Consistent State Estimation

[4] Ingvio:Aconsistentinvariantfilterfor fast and high-accuracy gnss-visual-inertial odometry