美团SRI-GVINS：将GNSS原始观测与视觉惯性深度融合的导航新SOTA-CSDN博客

点击下方卡片，关注“自动驾驶之心”公众号

论文作者 | Jun Hu

编辑 | 自动驾驶之心

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]。

GVINS[2]

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

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积分有以下约束：

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

伪距测量（Psr）：将方程 (14) 和方程 (22) 代入方程 (18) 中，以定义完整的伪距测量残差。
多普勒频移测量（Dopp）：将方程 (15) 和方程 (22) 代入方程 (19) 中，以定义完整的多普勒频移测量残差。
单差伪距测量（Dpsr）：考虑到基站的存在，我们可以定义时间步k时基站的伪距测量：

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

双差载波相位测量（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

投稿作者为『自动驾驶之心知识星球』特邀嘉宾，欢迎加入交流！

① 全网独家视频课程

BEV感知、BEV模型部署、BEV目标跟踪、毫米波雷达视觉融合、多传感器标定、多传感器融合、多模态3D目标检测、车道线检测、轨迹预测、在线高精地图、世界模型、点云3D目标检测、目标跟踪、Occupancy、cuda与TensorRT模型部署、大模型与自动驾驶、Nerf、语义分割、自动驾驶仿真、传感器部署、决策规划、轨迹预测等多个方向学习视频（扫码即可学习）

网页端官网：www.zdjszx.com

② 国内首个自动驾驶学习社区

国内最大最专业，近3000人的交流社区，已得到大多数自动驾驶公司的认可！涉及30+自动驾驶技术栈学习路线，从0到一带你入门自动驾驶感知（2D/3D检测、语义分割、车道线、BEV感知、Occupancy、多传感器融合、多传感器标定、目标跟踪）、自动驾驶定位建图（SLAM、高精地图、局部在线地图）、自动驾驶规划控制/轨迹预测等领域技术方案、大模型、端到端等，更有行业动态和岗位发布！欢迎扫描下方二维码，加入自动驾驶之心知识星球，这是一个真正有干货的地方，与领域大佬交流入门、学习、工作、跳槽上的各类难题，日常分享论文+代码+视频