Logo
ScienceStack
Table of Contents
Fetching ...
Sign In

Factor Graph Fusion of Raw GNSS Sensing with IMU and Lidar for Precise Robot Localization without a Base Station

Jonas Beuchert, Marco Camurri, Maurice Fallon

TL;DR

A robust approach that tightly fuses raw GNSS receiver data with inertial measurements and, optionally, lidar observations for precise and smooth mobile robot localization and is believed to be the first system that fusesRaw GNSS observations (as opposed to fixes) with lidar in a factor graph.

Abstract

Accurate localization is a core component of a robot's navigation system. To this end, global navigation satellite systems (GNSS) can provide absolute measurements outdoors and, therefore, eliminate long-term drift. However, fusing GNSS data with other sensor data is not trivial, especially when a robot moves between areas with and without sky view. We propose a robust approach that tightly fuses raw GNSS receiver data with inertial measurements and, optionally, lidar observations for precise and smooth mobile robot localization. A factor graph with two types of GNSS factors is proposed. First, factors based on pseudoranges, which allow for global localization on Earth. Second, factors based on carrier phases, which enable highly accurate relative localization, which is useful when other sensing modalities are challenged. Unlike traditional differential GNSS, this approach does not require a connection to a base station. On a public urban driving dataset, our approach achieves accuracy comparable to a state-of-the-art algorithm that fuses visual inertial odometry with GNSS data -- despite our approach not using the camera, just inertial and GNSS data. We also demonstrate the robustness of our approach using data from a car and a quadruped robot moving in environments with little sky visibility, such as a forest. The accuracy in the global Earth frame is still 1-2 m, while the estimated trajectories are discontinuity-free and smooth. We also show how lidar measurements can be tightly integrated. We believe this is the first system that fuses raw GNSS observations (as opposed to fixes) with lidar in a factor graph.

Factor Graph Fusion of Raw GNSS Sensing with IMU and Lidar for Precise Robot Localization without a Base Station

TL;DR

A robust approach that tightly fuses raw GNSS receiver data with inertial measurements and, optionally, lidar observations for precise and smooth mobile robot localization and is believed to be the first system that fusesRaw GNSS observations (as opposed to fixes) with lidar in a factor graph.

Abstract

Accurate localization is a core component of a robot's navigation system. To this end, global navigation satellite systems (GNSS) can provide absolute measurements outdoors and, therefore, eliminate long-term drift. However, fusing GNSS data with other sensor data is not trivial, especially when a robot moves between areas with and without sky view. We propose a robust approach that tightly fuses raw GNSS receiver data with inertial measurements and, optionally, lidar observations for precise and smooth mobile robot localization. A factor graph with two types of GNSS factors is proposed. First, factors based on pseudoranges, which allow for global localization on Earth. Second, factors based on carrier phases, which enable highly accurate relative localization, which is useful when other sensing modalities are challenged. Unlike traditional differential GNSS, this approach does not require a connection to a base station. On a public urban driving dataset, our approach achieves accuracy comparable to a state-of-the-art algorithm that fuses visual inertial odometry with GNSS data -- despite our approach not using the camera, just inertial and GNSS data. We also demonstrate the robustness of our approach using data from a car and a quadruped robot moving in environments with little sky visibility, such as a forest. The accuracy in the global Earth frame is still 1-2 m, while the estimated trajectories are discontinuity-free and smooth. We also show how lidar measurements can be tightly integrated. We believe this is the first system that fuses raw GNSS observations (as opposed to fixes) with lidar in a factor graph.
Paper Structure (18 sections, 10 equations, 4 figures, 1 table)

This paper contains 18 sections, 10 equations, 4 figures, 1 table.

Table of Contents

  1. INTRODUCTION
  2. RELATED WORK
  3. PROBLEM STATEMENT
  4. Frames
  5. State
  6. Measurements
  7. Maximum-a-Posteriori Estimation
  8. FACTOR GRAPH FORMULATION
  9. Pre-integrated Inertial Measurements
  10. ICP Registration
  11. Pseudoranges
  12. Carrier Phases
  13. IMPLEMENTATION
  14. EXPERIMENTAL RESULTS
  15. Experiment 1: Raw GNSS and IMU fusion
  16. ...and 3 more sections

Figures (4)

  • Figure 1: Setups for experimental validation. Left: a car driving on public roads. Center: a quadruped robot moving in urban and forest environments. Right: a GNSS receiver carried handheld. Raw GNSS measurements were tightly fused with inertial sensing and optionally lidar using factor graphs.
  • Figure 2: Reference frames: the Earth-centered-Earth-fixed (ECEF) frame $\mathtt{E}$, the local frame $\mathtt{W}$, and the frames on the platforms, including base $\mathtt{{B}}$, which coincides with the GNSS antenna frame $\mathtt{{A}}$, IMU frame $\mathtt{I}$, and lidar frame $\mathtt{L}$.
  • Figure 3: Factor graph structure with variable nodes (large circles) for states $\boldsymbol{x}_i$ and transformation $\mathbf{T}_{\mathtt{E}\mathtt{W}}$ between local frame and global Earth frame and factor nodes (small, colored) for priors, and properioceptive (IMU), exteroceptive (lidar), and GNSS measurements (pseudorange, carrier phase).
  • Figure 4: Experimental results. (A)$^1$: Trajectory of a car in Hong Kong estimated with our algorithm fusing inertial and GNSS measurements (red) compared to RTK (ground truth, blue) cao2022tro. (B): Trajectory of a quadruped traversing between two buildings estimated using IMU, ICP, and GNSS (red) compared to RTK (blue), part of sequence Thom. The RTK trajectory drifted into the building when there was little GNSS data while our estimate did not. (C): Trajectory of a quadruped in the Bagley Wood estimated using IMU, ICP, and GNSS (red) in comparison to RTK (blue) and single GNSS fixes (orange). (D): Same trajectory with lidar scans overlayed. (E): Trajectory of a handheld device estimated using time-differential carrier-phase factors only and given the starting position. (F): The corresponding factors between different states in time, represented by lines and crosses respectively. Few factors were created to states near the trees on the left. Still, the horizontal localization error at the end was less than 10 cm.