专栏名称: 计算机视觉life

兼具系统性、严谨性、易读性，分享计算机视觉、机器学习、人工智能及相关领域前沿资讯、技术干货、产业理解。

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

计算机视觉life · 公众号 · · 2024-06-21 11:08

正文

请到「今天看啥」查看全文

本文转自：3D视觉之心

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外参。 未来改进方向 ：