1_6 建图定位

一、GICI-LIB: A GNSS/INS/Camera Integrated Navigation Library

        Abstract— Accurate navigation is essential for autonomous robots and vehicles. In recent years, the integration of the Global Navigation Satellite  system (GNSS), Inertial Navigation System (INS), and camera has garnered considerable attention due to its robustness and high accuracy in diverse environments. In such systems, fully utilizing the role of GNSS is cumbersome because of the diverse choices of formulations, error models, satellite constellations, signal frequencies, and service types, which lead to different precision, robustness, and usage dependencies. To clarify the capacity of GNSS algorithms and accelerate the development efficiency of employing GNSS in multi-sensor fusion algorithms, we open source the GNSS/INS/Camera Integration Library (GICI-LIB), together with detailed documentation and a comprehensive land vehicle dataset. A factor graph optimization-based multi-sensor fusion framework is established, which combines almost all GNSS measurement error sources by fully considering temporal and spatial correlations between measurements. The graph structure is designed for flexibility, making it easy to form any kind of integration algorithm. For illustration, four Real-Time Kinematic (RTK)-based algorithms from GICI-LIB are evaluated using our dataset. Results confirm the potential of the GICI system to provide continuous precise navigation solutions in a wide spectrum of urban environments.

        摘要-精确导航对自主机器人和车辆至关重要。近年来，全球导航卫星系统（GNSS）、惯性导航系统（INS）和相机的集成由于其在不同环境中的鲁棒性和高精度而引起了人们的广泛关注。在这样的系统中，充分利用全球导航卫星系统的作用是麻烦的，因为公式、误差模型、卫星星座、信号频率和服务类型的选择多种多样，这导致了不同的精度、稳健性和使用依赖性。为了阐明全球导航卫星系统算法的能力，并加快在多传感器融合算法中使用全球导航卫星网络的开发效率，我们开放了GNSS/INS/相机集成库（GICI-LIB），以及详细的文件和全面的陆地车辆数据集。建立了一个基于因子图优化的多传感器融合框架，该框架通过充分考虑测量之间的时间和空间相关性，结合了几乎所有的GNSS测量误差源。图形结构的设计具有灵活性，使其易于形成任何类型的积分算法。例如，使用我们的数据集评估了GICI-LIB中四种基于实时动态（RTK）的算法。结果证实了GICI系统在广泛的城市环境中提供连续精确导航解决方案的潜力。

1、介绍

        With the significant increase in computing capability and the simultaneous reduction in power consumption and cost, autonomous robots and vehicles have become ubiquitous [1]. For these applications, robust and accurate navigation is essential. Although the Visual-Inertial Navigation System (VINS) has been recognized as a practical solution due to its high accuracy and low cost over the years [2], it still suffers from performance fluctuations under complex environments and pose drift during long-term operations [3].

        随着计算能力的显著提高以及功耗和成本的同时降低，自主机器人和车辆变得无处不在[1]。对于这些应用程序，稳健和准确的导航至关重要。尽管多年来，视觉惯性导航系统（VINS）因其高精度和低成本而被公认为一种实用的解决方案[2]，但在复杂环境下，它仍然会受到性能波动和长期运行过程中姿态漂移的影响[3]。

        Fusing VINS and GINS together, namely GVINS, is an effective approach to substantially enhance the robustness and availability [4]. The two systems are complementary. GINS can provide robust and globally accurate pose solutions in most outdoor settings and VINS can greatly constrain the pose drift rate under GNSS-challenging conditions. There are four typical GNSS formulations: Single Point Positioning (SPP), Real-Time Differential (RTD), Real-Time Kinematic (RTK), and Precise Point Positioning (PPP). SPP and RTD are meter-level accuracy formulations, while RTK and PPP are centimeter-level accuracy formulations. Previously, centimeter-level GNSS was commonly used in cost-insensitive applications, such as surveying and aviation. With the cost reduction of high-precision GNSS chips and augmentation services, centimeter-level GNSS devices are gaining momentum in automation applications. By incorporating high-precision GNSS positioning formulations, the GVINS system can provide continuous centimeter-level solutions under various environments.

        将VINS和GINS融合在一起，即GVINS，是显著提高稳健性和可用性的有效方法[4]。这两个系统是相辅相成的。GINS可以在大多数户外环境中提供稳健且全局准确的姿态解决方案，并且在GNSS具有挑战性的条件下，VINS可以极大地限制姿态漂移率。有四种典型的GNSS公式：单点定位（SPP）、实时差分（RTD）、实时动态（RTK）和精确点定位（PPP）。SPP和RTD是米级精度公式，而RTK和PPP是厘米级精度公式。此前，厘米级全球导航卫星系统通常用于测量和航空等对成本不敏感的应用。随着高精度GNSS芯片和增强服务的成本降低，厘米级GNSS设备在自动化应用中的势头越来越大。通过结合高精度GNSS定位公式，GVINS系统可以在各种环境下提供连续的厘米级解决方案。

        Constructing a GVINS system is not difficult, which has been done by several works. However, fully utilizing the role of GNSS is cumbersome. Rigorously, there are multiple measurement formulations, error models, satellite constellations, signal frequencies, and service types. These distinct options can significantly impact precision, robustness, and usage dependencies. One can achieve an imprecise and vulnerable solution via SPP formulations, and can also achieve a precise and robust solution even in a typical urban environment via multi-constellations multi-frequency RTK with the augmentation of Observation Space Representation (OSR) services. In environments where OSR service is unavailable because of the absence of reference stations, one can still achieve a precise solution within a few minutes via PPP with the augmentation of State Space Representation (SSR) services.

构建GVINS系统并不困难，这已经由几项工作完成。然而，充分利用全球导航卫星系统的作用很麻烦。严格地说，有多种测量公式、误差模型、卫星星座、信号频率和服务类型。这些不同的选项会显著影响精度、健壮性和使用依赖性。人们可以通过SPP公式实现不精确和脆弱的解决方案，也可以通过多星座多频RTK和观测空间表示（OSR）服务的增强，即使在典型的城市环境中也可以实现精确和稳健的解决方案。在由于缺乏参考站而无法获得OSR服务的环境中，通过增强状态空间表示（SSR）服务，仍然可以通过PPP在几分钟内实现精确的解决方案。

        To clarify the capacity of GNSS algorithms and accelerate the development efficiency of employing GNSS in multisensor fusion algorithms, we present the GNSS/INS/Camera Integration Library (GICI-LIB), together with detailed documentation and a comprehensive land vehicle dataset. The key features are highlighted as follows:
• A factor graph optimization-based multi-sensor fusion framework, which implements most of the possible GNSS loose and tight integration factors, INS factors, visual factors, and motion constraints, together with reliable initialization, measurement sparsification, and outlier rejection algorithms. The GNSS formulations are implemented towards four constellations and full frequencies.
• Unlike other state-of-the-art (SOTA) integrated INS/GNSS/camera systems, our system focuses on combining almost all measurement error sources of GNSS in space, propagation, and ground segments by fully considering temporal and spatial correlations between measurements. It is in this way our system outperforms the SOTA methods on the majority of our open-source datasets and the evaluated INS/GNSS/camera code repositories.
• For ease of use, the software is developed under object-oriented programming features, and the graph is designed to enable the flexible addition of sensors. By simple instantiation, one can easily form any kind of multi-sensor fusion algorithm with considerable robustness.
• An open land vehicle dataset that collects necessary raw data for all the above algorithms. The datasets contain multiple short-term and long-term trajectories, covering open-sky, tree-lined, typical urban, and dense urban environments.

        为了阐明全球导航卫星系统算法的能力，并加快在多传感器融合算法中使用全球导航卫星网络的开发效率，我们介绍了全球导航卫星/导航卫星系统/相机集成库（GICI-LIB），以及详细的文件和全面的陆地车辆数据集。主要功能如下所示：

•基于因子图优化的多传感器融合框架，实现了大多数可能的GNSS松散和紧密集成因子、惯性导航系统因子、视觉因子和运动约束，以及可靠的初始化、测量稀疏化和异常值抑制算法。全球导航卫星系统的公式是针对四个星座和全频执行的。

•与其他最先进的（SOTA）集成INS/GNSS/相机系统不同，我们的系统专注于通过充分考虑测量之间的时间和空间相关性，将全球导航卫星系统在空间、传播和地面段中的几乎所有测量误差源相结合。通过这种方式，我们的系统在大多数开源数据集和评估的INS/GNSS/相机代码库上都优于SOTA方法。

•为了便于使用，该软件是在面向对象的编程功能下开发的，图形设计用于灵活添加传感器。通过简单的实例化，可以很容易地形成任何一种具有相当鲁棒性的多传感器融合算法。

•一个开放式陆地车辆数据集，为所有上述算法收集必要的原始数据。数据集包含多个短期和长期轨迹，涵盖开阔的天空、绿树成荫的典型城市和密集的城市环境。

        The rest of this letter is structured as follows. In section II, we review existing relevant works. Section III shows the structure of the proposed system. Section IV illustrates the formulation and methodology. In section V, we evaluate our algorithms together with other open-source algorithms. Finally section VI concludes this letter.

        这封信的其余部分结构如下。在第二节中，我们回顾了现有的相关工作。第三节显示了拟议系统的结构。第四节说明了提法和方法。在第五节中，我们将评估我们的算法以及其他开源算法。最后，第六节结束了这封信。

2、相关工作

        In the past decades, almost all GNSS algorithms utilize filter-based methods. Unlike GNSS, the state estimation problem for visual navigation can be categorized as either filter-based methods [5], [6] or optimization-based methods [7], [8], [9]. Filter-based methods have lower computational costs, whereas optimization-based methods can offer higher accuracy.

        在过去的几十年里，几乎所有的GNSS算法都使用基于滤波器的方法。与GNSS不同，视觉导航的状态估计问题可以分为基于滤波器的方法[5]、[6]或基于优化的方法[7]、[8]、[9]。基于滤波器的方法具有较低的计算成本，而基于优化的方法可以提供更高的精度。

        There are several optimization-based GNSS or GNSS integration algorithms reported. To clarify their usage of GNSS capacities, we summarize them together with our algorithms in Table I. The fusion types are categorized into GNSS-only (G), GNSS/INS (GI) LC, GI TC, GNSS/INS/Camera (GIC) SS (Fusing GNSS solution with VINS solution), GIC SRR, and GIC RRR. The frequencies are categorized into Single (S), Double (D), and Multiple (M). The constellations are categorized into GPS (G), GLONASS (R), Galileo (E), and BDS (C). The robustness is categorized into Fair (F), Good (G), and Extraordinary (E). There are three levels of SSR services: SSR-I contains precise ephemeris and code biases, SSR-II contains phase biases, and SSR-III contains local atmosphere delays, which enables PPP, PPP with ambiguity resolution (PPP-AR), and PPP with local reference network augmentation (PPP-RTK), respectively. We use the term PPP to represent the three kinds of algorithms in GICI-LIB. I

        已经报道了几种基于优化的GNSS或GNSS集成算法。为了阐明它们对全球导航卫星系统能力的使用，我们将它们与我们的算法一起总结在表I中。融合类型分为仅全球导航卫星（G）、全球导航卫星/惯性导航系统（GI）LC、全球导航系统TC、全球导航星系统/惯性导航卫星/相机（GIC）SS（将全球导航系统解决方案与VINS解决方案融合）、全球定位系统SRR和全球定位系统RRR。频率分为单频（S）、双频（D）和多频（M）。星座分为GPS（G）、GLONASS（R）、Galileo（E）和BDS（C）。稳健性分为一般（F）、良好（G）和非凡（E）。SSR服务有三个级别：SSR-I包含精确的星历表和代码偏差，SSR-II包含相位偏差，而SSR-III包含局部大气延迟，这分别实现了PPP、具有模糊度分辨率的PPP（PPP-AR）和具有局部参考网络增强的PPP（PPS-RTK）。我们使用术语PPP来表示GICI-LIB中的三种算法。

        It is clear that we fill lots of gaps for the optimization-based GNSS algorithms. Except for the indexes listed in the table, our system also has significant progress on GNSS error handling, ambiguity resolution, temporal sparsification, and initialization. These details bring our system extraordinary performance.

        很明显，我们为基于优化的GNSS算法填补了许多空白。除了表中列出的索引外，我们的系统在GNSS错误处理、模糊度解决、时间稀疏化和初始化方面也取得了显著进展。这些细节为我们的系统带来了非凡的性能。

 3、系统概览

        GICI-LIB is a system-wide software, which consists of multiple hardware I/O controllers, data decoders and encoders, and estimation processors. The nodes run in multiple threads to ensure concurrency. By configuring nodes defined by our configuration file, the system can be used not only for real-time and post-processed algorithm processing, but also for stream transformation and conversion. We only focus on the real-time estimation data flow in this letter. For further information, please refer to our documentation.

        GICI-LIB是一种全系统软件，由多个硬件I/O控制器、数据解码器和编码器以及估计处理器组成。节点在多个线程中运行以确保并发性。通过配置我们的配置文件定义的节点，该系统不仅可以用于实时和后处理的算法处理，还可以用于流转换和转换。在这封信中，我们只关注实时估计数据流。有关更多信息，请参阅我们的文档。

        There are two kinds of major threads in the real-time estimation data flow: stream threads and estimator threads. The stream threads handle data input, output, decoding, and encoding. The estimator threads process the raw data collected from the stream threads and feed the solutions back. An estimator thread consists of three threads: frontend, backend, and export. The frontend thread handles time consuming preprocessing tasks. In cases considering GNSS, INS, and camera, the front-end thread will solely process the raw data from the camera since only the feature detection and tracking procedures are time-consuming. The backend conducts initialization and optimization with the Factor Graph Optimization (FGO) algorithm. Its solution can be used by frontend to help improve the quality of feature tracking. Finally, the export thread will integrate the datafrom the backend thread according to desired timestamps. The integrated solution can be delivered to the stream threads for hardware output or file storage. Additionally, it can also   be fed into other estimator threads for loose coupling. As examples, we instantiate several algorithms for the estimator nodes, including SPP, RTD, RTK, PPP, SPP-based loosely coupled (LC) and tightly coupled (TC) GINS, SPP-based Solution/Raw/Raw (SRR) and Raw/Raw/Raw (RRR) GVINS, RTK-based LC GINS, TC GINS, SSR GVINS, and RRR GVINS.

        实时估计数据流中有两种主要线程：流线程和估计器线程。流线程处理数据输入、输出、解码和编码。估计器线程处理从流线程收集的原始数据，并反馈解决方案。估计器线程由三个线程组成：前端、后端和导出。前端线程处理耗时的预处理任务。在考虑GNSS、INS和相机的情况下，前端线程将仅处理来自相机的原始数据，因为只有特征检测和跟踪过程是耗时的。后端使用因子图优化（FGO）算法进行初始化和优化。它的解决方案可以被前端用来帮助提高特征跟踪的质量。最后，导出线程将根据所需的时间戳集成来自后端线程的数据。集成解决方案可以交付给流线程用于硬件输出或文件存储。此外，它还可以被馈送到用于松耦合的其他估计器线程中。例如，我们为估计器节点实例化了几种算法，包括SPP、RTD、RTK、PPP、基于SPP的松耦合（LC）和紧耦合（TC）GINS、基于SPP的Solution/Raw/Raw（SRR）和Raw/Raw/Raw（RRR）GVINS、基于RTK的LC GINS、TC GINS、SSR GVINS和RRR GVINS。