点击下方卡片，关注“自动驾驶之心”公众号
戳我-> 领取自动驾驶近15个方向学习路线
论文作者 | 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]。
平方根逆滑动窗口滤波器是什么？
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、高精地图、局部在线地图）、自动驾驶规划控制/轨迹预测等领域技术方案、大模型、端到端等，更有行业动态和岗位发布！欢迎扫描下方二维码，加入自动驾驶之心知识星球，这是一个真正有干货的地方，与领域大佬交流入门、学习、工作、跳槽上的各类难题，日常分享论文+代码+视频
③【自动驾驶之心】技术交流群
自动驾驶之心是首个自动驾驶开发者社区，聚焦感知、定位、融合、规控、标定、端到端、仿真、产品经理、自动驾驶开发、自动标注与数据闭环多个方向，目前近60+技术交流群，欢迎加入！扫码添加汽车人助理微信邀请入群，备注：学校/公司+方向+昵称（快速入群方式）
④【自动驾驶之心】全平台矩阵
