TL;DR
This paper presents a novel LiDAR scan matching-aided Inertial Navigation System (INS) designed for GNSS-denied environments. By loosely coupling a LiDAR SLAM system with an INS using an Extended Kalman Filter (EKF), the approach overcomes the limitations of each standalone system, achieving centimeter-level positioning accuracy even in featureless indoor spaces.
What Problem Does This Paper Address?
Accurate navigation is crucial for Unmanned Aerial Vehicles (UAVs) and Unmanned Ground Vehicles (UGVs). While integrating Global Navigation Satellite Systems (GNSS) with Inertial Navigation Systems (INS) offers high accuracy when GNSS signals are available, these systems fail in GNSS-denied environments like indoor spaces, dense forests, or urban canyons. In such scenarios, INS alone suffers from accumulating drift errors, while Simultaneous Localization and Mapping (SLAM) techniques, especially visual SLAM, struggle in featureless or low-light conditions. Existing LiDAR-aided INS solutions often rely on complex feature extraction, which can be unstable. This paper addresses the need for a robust, accurate, and stable navigation solution in GNSS-denied environments by proposing a fusion of INS and LiDAR scan matching that leverages the strengths of both.
Methods
The core of the proposed system is a loosely coupled Extended Kalman Filter (EKF) that fuses data from a commercial-grade Inertial Measurement Unit (IMU) and a 2D LiDAR scanner.
- INS Modeling: The paper details the INS mechanization equations in the navigation frame (n-frame) and the body frame (b-frame), including the transformation matrices and error propagation models. It uses a first-order Taylor series expansion for the error state vector, which includes position, velocity, attitude, and sensor biases.
- LiDAR Scan Matching (IMLE): Instead of traditional feature extraction, the paper employs a probabilistic scan matching method called Improved Probabilistically Motivated Maximum Likelihood Estimation (IMLE). This method uses a likelihood map (M) built from previous scans. New scans (St) are matched against this map to find the optimal transformation (T*) that maximizes the likelihood P(St|M). The likelihood of a point is proportional to its distance to the nearest environmental feature, modeled by a Gaussian probability. The map is organized as a quad-tree pyramid for efficient multi-resolution storage.
- EKF Fusion: The EKF is used to fuse the INS mechanization outputs and the LiDAR scan matching results. The IMU's higher output frequency is used to predict the state, and the EKF updates the state when LiDAR observations are available. This fusion aims to mitigate INS drift and improve SLAM performance in featureless environments.
Datasets
Not discussed in the paper. The experiments were conducted using a self-developed Unmanned Ground Vehicle (UGV) platform named NAVIS, equipped with an Xsens MTi-G IMU and a Hokuyo UTM-30LX-EW laser scanner. The data was collected during field tests in a corridor at the Finnish Geospatial Research Institute (FGI).
Key Insights and Ideas
- Complementary Strengths: The fusion of INS and LiDAR scan matching effectively combines the short-term accuracy of INS with the long-term stability and feature-based localization of LiDAR. This is particularly beneficial in environments where one sensor's performance degrades.
- IMLE for Featureless Environments: The IMLE scan matching algorithm, by not relying on explicit feature extraction, offers better stability and performance in featureless environments compared to traditional SLAM methods.
- IMU as a Refiner for LiDAR: The IMU's role is crucial not only for providing a baseline navigation solution but also for refining the search scope for the LiDAR scan matching, preventing local optima and accelerating computation.
- Attitude Estimation Advantage: The IMU significantly contributes to attitude estimation, especially in dynamic scenarios and feature-poor environments, mitigating heading errors that can arise from LiDAR-only solutions.
- Commercial IMU Limitations: The study highlights that while a commercial-grade IMU improves accuracy, its inherent drift can still degrade the overall performance, suggesting that higher-grade IMUs might yield even better results.
Key Results
- Stationary Tests:
- LiDAR scan matching alone achieved RMS errors of 0.007 m (X), 0.004 m (Y), and 0.0° (Heading).
- The combined IMU + LiDAR solution had RMS errors of 0.009 m (X), 0.007 m (Y), and 0.065° (Heading).
- In static conditions, the LiDAR-only solution showed slightly better accuracy, attributed to the lack of dynamic motion and feature changes.
- Dynamic Tests:
- The combined IMU + LiDAR solution achieved a significantly lower RMS error of 0.084 m, maintaining centimeter-level accuracy throughout the trajectory.
- The LiDAR-only solution showed a much higher RMS error of 0.433 m, drifting to 1.195 m by the end of the trajectory.
- The IMU's contribution was vital in mitigating mapping errors and improving heading estimation in feature-poor dynamic environments.
Limitations and Future Work
- Commercial IMU Performance: The paper notes that the drift from the commercial-grade IMU can degrade the accuracy of the combined solution, especially in stationary surveys.
- Future Work:
- Integrating several tactical IMUs into the NAVIS setup to evaluate the benefit of higher-accuracy IMUs for indoor mapping.
- Investigating 3D navigation by installing two LiDARs vertically and horizontally on the mobile platform for more complex situations.
Summary updated