【论文翻译】GICI-LIB：A GNSS-INS-Camera Integrated Navigation Library.md

• 软件下载：

• 数据集下载：

• 论文下载：GICI-LIB: A GNSS/INS/Camera Integrated Navigation Library

• 第一作者：上海交通大学航空航天学院池澄博士

[TOC]

摘要

准确的导航定位信息对机器人和车辆至关重要。近年来，全球导航卫星系统（GNSS）、惯性导航 全球导航卫星系统（GNSS）、惯性导航系统（INS）和相机（Camera）量测数据的融合（Integrated），由于在不同环境下的鲁棒性和高精度，受到了广泛关注。但在这些系统中，很难充分发挥 GNSS 的作用，因为 GNSS 可选择的误差模型、卫星星座、信号频率和增强服务类型太多了；不同的 GNSS 解算方式会带来不同的精度、鲁棒性、依赖性。为了阐明 GNSS 在多源融合算法中的作用，并加快在多源融合中 GNSS 的开发效率，我们开源了 GNSS/INS/Camera Integration Library (GICI-LIB)，附带了一套有详细文档的车辆数据集。在此建立了一套基于因子图优化的多传感器融合框架，实现了几乎所有的 GNSS 量测误差模型，充分考虑测量之间的时间相关性和空间相关性。因子图结构非常灵活，很容易就能构建各种多源融合算法。我们使用数据集对 GICI-LIB 的四种基于实时（RTK）的算法进行了评估，结果证实了 GICI 系统在各种城市环境中提供连续精确导航解决方案的潜力。

一、介绍

随着计算能力的大幅提高以及功耗和成本的同步降低，自主机器人和车辆已变得无处不在；对于这些应用，强大而精确的导航是必不可少的。尽管视觉惯性导航系统（VINS）因其高精度和低成本，多年来一直被认为是一种实用的解决方案；但它在复杂环境下性能波动较大，长期运行时的姿态经常发生漂移。

将 VINS 和 GINS 融合在一起，即 GVINS，是一种有效的方法，可大幅提高鲁棒性与可用性。这两套系统是互补的。GINS 可以在大多数室外环境中提供稳健、全局精确的姿态解，而 VINS 则可以在 GNSS 信号差是下大大限制姿态漂移率。GNSS 有四种典型的定位模式：SPP、RTD、RTK、PPP，其中 SPP 和 RTD 可提供米级的精度，RTK 和 PPP 可提供厘米级的定位精度。在过去，厘米级 GNSS 通常用于对成本不敏感的应用，如测量和航空；但如今，随着高精度 GNSS 芯片和增强服务成本的降低，厘米级 GNSS 设备在自动化应用中的发展势头日益强劲；通过采用高精度 GNSS 定位方式，GVINS 系统可在各种环境下提供连续的厘米级解决方案。

构建一个 GVINS 系统并不难，已有多开源软件完成了这一工作；但想充分发挥球导航卫星系统的作用，实现多种量测模型、误差模型、卫星星座、信号频率和服务类，却很麻烦。这些不同的选项会对精度、稳健性和使用依赖性产生重大影响。依赖性。我们可以通过通过 SPP 在复杂的城市环境中实现不那么精确、稳健的解；也可以通过通过观测空间（OSR）服务增强的多星座多频率 RTK，即使在复杂的城市环境中也能实现精确而稳健的解决方案；在因缺乏基准站而无法使用 OSR 服务的环境中仍然可以通过 PPP 的状态空间（SSR）服务。

为了阐明 GNSS 在多源融合算法中的作用，并加快在多源融合中 GNSS 的开发效率，我们开源了 GNSS/INS/Camera Integration Library (GICI-LIB)，附带了一套有详细文档的车辆数据集。亮点如下：

• 基于因子图优化框架，实现了几乎所有的 GNSS、IMS、Camera 因子和运动约束，有可靠的初始化、量测值稀疏化、粗差剔除算法，GNSS 支持四大系统全部频率。
• 与其他最先进的（SOTA）集成系统不同，我们的系统侧重于将空间、传播和地面段的几乎所有测量误差源，充分考虑测量之间的时空相关性。因此，我们的系统在大部分开源数据集上都优于 SOTA 模型方法。
• 为了方便使用，软件采用面向对象开发模式，因子图可以很方便的添加传感器，可以很方便的在此基础上拓展。
• 有详细文档的数据集，包含多个短期和长期轨迹、涵盖开阔天空、绿树成荫、典型城市和密集城市环境。

本文的其余内容如下：

• 在第二节，我们回顾了研究现状。
• 在第三节，展示了系统的结构。
• 在第四节，算法模型。
• 在第五节，我们将比较我们的算法和其他开源算法。
• 在第六节，是对文章的总结。

二、研究现状

在过去的几十年，几乎所有的 GNSS 算法都采用基于卡尔曼滤波的模型；与 GNSS 不同，视觉导航分为基于优化的方法和基于滤波的方法两大类，滤波的计算量小、优化的准确性更高。

• 空白表示我们无法获取相应的信息。

• GNSS 松组合的精度受到所采用 GNSS 定位模式的影响。我们在此介绍研究中报告的精度。

• 我们实现了 SSR-II 和 SSR-III 功能，但没有将其实例化，因为目前还没有标准、稳定和开放的实时功能。因为目前还没有标准和稳定的开放式实时服务。

显然，我们填补了基于优化的 GNSS 算法的许多空白。除了表中列出的指标表中列出的指标外，我们的系统还在全球导航卫星系统误差处理、模糊度解算、稀疏化和初始化等方面也取得了重大进展，这些细节为我们的系统带来了非凡的性能。

三、系统概述

GICI-LIB 系统很广泛，由多个硬件 I/O 控制器、数据解码器和编码器以及估算处理器组成，这些节点以多个线程运行，以确保并发性。通过配置文件定义的节点，该系统不仅可用于实时和后处理算法处理，还可以进行数据流转换。本文中我们只介绍基于实时数据流的解算，更多信息请参考文档。

实时估算数据流中有两种主要线程：stream 线程和 estimator 线程。stream 线程处理数据输入、输出、解码和编码；estimator 线程使用 stream 线程收集的原始数据，进行解算，并将解果返回。一个 estimator 线程有三个子线程组成：frontend、backend、export。frontend 线程处理耗时的预处理任务，在 GIC 系统中只处理来自相机的原始数据，进行特征检测和跟踪。backend 线程进行初始化和图优化估计，得到的解算结果反过来可以提高 frontend 线程的精度和效率。export 线程用于集成数据，对齐到后端需要的时间戳。

作为示例，我们为估计器节点实例化了几种算法，包括 SPP、SDGNSS、DGNSS、RTK、PPP、SPP-based LC GINS、TC GINS、RTK-based LC GINS、SRR GVINS、RRR GVINS。

四、算法

在本节中，我们首先介绍 FGO 结构，然后介绍 GNSS 因子，由于视觉和惯性因子算法已经很成熟了就不在此介绍了，之后介绍 GNSS 模糊度解算，最后介绍稀疏化和初始化算法。所有算法软件都支持，且在文档中有介绍。

1、因子图优化

GVINS 融合的结构如下图所示，纯 GNSS 和 GNSS-INS 是 GVINS 融合的子集，因此不再单独介绍。

因子图的结构可以描述成非线性最小二乘问题： $$ \begin{array}{r}\hat{\boldsymbol{\chi}}=\underset{\boldsymbol{\chi}}{\arg \min }\left{\left|\boldsymbol{z}{p}-\boldsymbol{H}{p} \boldsymbol{\chi}\right|^{2}+\left|\boldsymbol{z}{r}-h{r}\left(\boldsymbol{\chi}{r}, \boldsymbol{\chi}{I}\right)\right|^{2}+\right. \ \left.\left|\boldsymbol{z}{I}-h{I}\left(\boldsymbol{\chi}{I}\right)\right|^{2}+\left|\boldsymbol{z}{c}-h_{c}\left(\boldsymbol{\chi}{c}, \boldsymbol{\chi}{I}\right)\right|^{2}\right}\end{array} $$

因子图的设计非常灵活，体现在三个方面：

• 不同传感器的测量时间戳不必对齐。
• 新的测量数据可以在任何时候添加到图表中，而不一定是在图表结束时。
• 解算结果可在任何所需的时间点和频率生成。

通常情况下，即使传感器在硬件上实现了同步、它们仍会在不同的时间点进行测量，从而导致时间戳错位。此外，不同传感器之间的硬件延迟不同，导致可能在处理完时间戳较新的测量数据后才接收到时间戳较早的测量数据。在这种情况下 INS 被用来连接或重建状态、而不是插值或等待测量、这可能会降低精度或增加延迟。

2、GNSS 因子

GNSS 因子分为两个级别： LC 因子和 TC 因子。LC 因子主要包括位置和速度因子，而 TC 因子主要包括伪距因子、多普勒因子和载波相位因子。由于 LC 因子比较简单，我们在此只介绍 TC 因子。

伪距、载波相位和多普勒测量的非线性模型描述如下： $$ \begin{array}{c}P_{r, i}^{s}=\rho_{r}^{s}+c\left(d t_{r}-d t^{s}\right)+I_{r, i}^{s}+T_{r}^{s}+d_{r, i}-d_{i}^{s}+\varepsilon_{P} \ L_{r, i}^{s}=\rho_{r}^{s}+c\left(d t_{r}-d t^{s}\right)+I_{r, i}^{s}+T_{r}^{s}+b_{r, i}-b_{i}^{s}+N_{r, i}^{s}+\varepsilon_{L} \ D_{r, i}^{s}=\dot{\rho}{r}^{s}+c\left(d f{r}-d f^{s}\right)+\varepsilon_{D}\end{array} $$

除上述情况外，还应特别考虑其他一些错误源。我们将所有误差源列于下表。处理这些误差的处理方法因算法而异，在公式中假定都已经得到很好的处理了。

3、模糊度解算

模糊解算（AR）是高精度 GNSS 算法实现厘米级精度的主要程序。是高精度全球导航卫星系统算法实现厘米级解算的重要步骤，适用于 RTK 和 PPP (以及相应的组合算法）。我们在此介绍 RTK AR。

4、稀释化（Sparsification）

人们普遍认为，基于视觉的估计算法采用关键帧进行稀疏化，以节省计算负荷。我们认为，高精度的紧组合 GNSS 计算也应该制定类似的策略，因为相应的参数维度也很高。遗憾的是，目前还没有相关工作的报告。

在视觉估算中选择关键帧的原则是尽量跳过场景相似的帧。这些帧对解算的贡献较小，会导致估算参数的协方差出现无意义的下降。这一原则的本质是相邻帧中未建模误差之间的相关性很高，这使得将误差建模为白噪声是不正确的。对帧进行稀疏化处理可使误差白化，使其更符合 FGO 的基本假设。

此方法也可以用于 GNSS 中，其原始数据历元间也有相关性，且 GNSS 原始观测数据未建模误差的自相关特性主要是由多径引起的，相关时间取决于场景的变化率。下图显示了 GNSS 测量自相关性和稀疏化的影响。我们可以看到，相关时间随运动而变化，相关性对解算的影响也随运动而变化。相关性对解算的影响可以通过稀疏化得到缓解。

由于 GNSS 测量的相关时间会随时间变化，因此很难为稀疏化设置一致的时间步长。时间步长很难确定。不过，场景变化率与关键帧选择标准类似。因此，我们的 GNSS 稀疏策略设计为保留关键帧附近的测量值，而忽略其他测量值。

5、初始化

GICI-LIB 算法采用了两级初始化：全局帧初始化和估计器状态初始化。

全局帧初始化定义了一个参考点，以便将状态从 ECEF 全局帧转换到 ENU 本地帧，该参考点还用于计算本地重力。我们总是运行 SPP 估计器，并将参考点作为估计器的第一个有效解。SPP 的精度足以（米级）设定参考点，使得 ENU 到ECEF 的转换模型在几百公里的距离内都能保持精确度。