Session A5: Sensor-Fusion for GNSS-Challenged Navigation

PF-LIO:Tightly-Coupled LiDAR-Inertial Odometry Based on Plane Fusion
Daifang Huang, Yong Li, Wenhui Yang, and Zhihang Qu, University of Electronics Science and Technology of China
Location: Beacon A

In GNSS-challenged environments, satellite signals are often obstructed or reflected, resulting in multipath interference and non-line-of-sight (NLOS) measurements, making it difficult to achieve accurate navigation. Multi-sensor fusion provides an effective supplement to GNSS, enabling localization systems to deliver high-precision and robust pose estimation[1] by integrating data from various sensors.
Inertial Measurement Unit (IMU) estimates position by integrating acceleration and angular velocity data, but errors accumulate over time, especially with low-cost microelectro-mechanical-system (MEMS) IMU. Light Detection and Ranging (LiDAR) generates high-resolution point cloud data for 3D mapping and localization. In recent years, solid-state LiDAR systems based on MEMS scanning[2] and rotating prisms[3] have become mainstream due to their low cost and lightweight design. However, in GNSS- challenged environments such as long tunnels or buildings, low-beam-count LiDAR lacks sufficient vertical features, weakening the system's constraint on the z-axis. Without the aid of additional sensors, the state estimation errors in pitch angle and elevation positioning for LiDAR increase, and over time, the positioning and attitude errors along the z-axis progressively accumulate[4].Considering that many co-planar features exist in such environments, sub-plane estimation can be viewed as covariance measurements of a larger plane. The fusion of sub-planes allow for more accurate estimation of the parent plane, significantly reducing uncertainty. Based on this plane-fusion strategy, this paper proposes a robust, lightweight and low-cost LiDAR-Inertial Odometry (LIO) system. The system fuses multi-modal sensor data, fully leveraging the geometric structure and kinematic information of the environment to achieve high-precision 3D reconstruction and localization, even in GNSS-challenged environments with limited vertical features.
Recent research has focused on optimizing data structures to improve LiDAR registration efficiency. FAST-LIO[5] is a classical ICP-based framework that utilizes a kd-tree for data management and improves Kalman gain computation using the SMW formula, achieving tightly coupled LIO at nearly 100 Hz. FAST-LIO2[6] introduces incremental kd-trees, reducing update times and improving registration efficiency. FASTER-LIO[7] replaces tree-based structures with sparse incremental voxels (iVox), offering superior performance in incremental updates and k-NN searches. These advancements primarily enhance efficiency by optimizing KNN search indexing methods. LiTAMIN[8] and LiTAMIN2[9] achieve faster registration by reducing the number of registration points and combining NDT, but their improvements depend more on point reduction than on optimizing data structures. Voxelmap[10] employs adaptive voxel construction, using octree hashing to optimize voxel creation, updates, and queries, performing point-to-plane registration by parameterizing planes within voxels, and further improving accuracy through noise handling. Voxelmap++[11] builds on this by introducing least-squares estimation, simplifying plane fitting and covariance estimation to 3DOF, reducing memory consumption and improving efficiency. It also proposes an online plane-merging method to reduce map uncertainty and memory usage, though its merging efficiency is low when traversing and querying converged nodes, and it is prone to local merging issues.
The proposed LIO system integrates voxel down sampling and motion distortion correction to preprocess point cloud data, effectively reducing computational load and enhancing robustness. A spatial hashing function maps point clouds directly to a voxel grid, generating residuals and avoiding KNN searches, significantly improving registration efficiency. The system also optimizes plane parameters using weighted least squares (WLS) and dynamically adjusts point weights to improve point cloud registration accuracy. Inspired by Voxelmap++, the system simplifies plane fitting and covariance estimation, reducing it from 6DOF to 3DOF, minimizing memory consumption and making it suitable for low-cost pedestrian and vehicular applications. Additionally, the system incorporates the Density-Based Spatial Clustering of Applications with Noise(DBSCAN) clustering algorithm and introduces a novel plane-merging method based on a union–find set data structure. This merging strategy enhances both plane-fitting accuracy and merging efficiency while reducing map uncertainty and memory usage. Moreover, the system introduces a slope detection module combined with an Iterative Error-State Kalman Filter (IESKF), ensuring accurate attitude and velocity estimation in complex structured environments, mitigating z-axis drift, and improving localization accuracy and stability in GNSS-challenged scenarios such as long tunnels and indoor spaces lacking vertical features.
Experimental results show that, in a 100m * 80m indoor loop test, which includes challenging LiDAR degradation scenarios such as long corridors and narrow corners. the system achieved a horizontal error of only 0.6m and a vertical error of 0.1m. The system demonstrates superior localization performance in GNSS-challenged environments with limited vertical features, highlighting its potential as a robust, efficient, and cost-effective navigation solution.
Index Terms: SLAM, sensor fusion, voxel,WLS, DBSCAN, IESKF
[1] “M2DGR: A Multi-Sensor and Multi-Scenario SLAM Dataset for Ground Robots.” Accessed: Sep. 14, 2024. [Online]. Available: https://ieeexplore.ieee.org/abstract/document/9664374
[2] “Micromachines | Free Full-Text | MEMS Mirrors for LiDAR: A Review.” Accessed: Sep. 14, 2024. [Online]. Available: https://www.mdpi.com/2072-666X/11/5/456
[3] Z. Liu, F. Zhang, and X. Hong, “Low-Cost Retina-Like Robotic Lidars Based on Incommensurable Scanning,” IEEEASME Trans. Mechatron., vol. 27, no. 1, pp. 58–68, Feb. 2022, doi: 10.1109/TMECH.2021.3058173.
[4] J. Xu, Y. Li, and J. Shi, “GO-LIO: Lidar-IMU SLAM Algorithm Based on Ground Optimization,” in 2023 China Automation Congress (CAC), Nov. 2023, pp. 2809–2814. doi: 10.1109/CAC59555.2023.10452011.
[5] W. Xu and F. Zhang, “FAST-LIO: A Fast, Robust LiDAR-Inertial Odometry Package by Tightly-Coupled Iterated Kalman Filter,” IEEE Robot. Autom. Lett., vol. 6, no. 2, pp. 3317–3324, Apr. 2021, doi: 10.1109/LRA.2021.3064227.
[6] W. Xu, Y. Cai, D. He, J. Lin, and F. Zhang, “FAST-LIO2: Fast Direct LiDAR-Inertial Odometry,” IEEE Trans. Robot., vol. 38, no. 4, pp. 2053–2073, Aug. 2022, doi: 10.1109/TRO.2022.3141876.
[7] C. Bai, T. Xiao, Y. Chen, H. Wang, F. Zhang, and X. Gao, “Faster-LIO: Lightweight Tightly Coupled Lidar-Inertial Odometry Using Parallel Sparse Incremental Voxels,” IEEE Robot. Autom. Lett., vol. 7, no. 2, pp. 4861–4868, Apr. 2022, doi: 10.1109/LRA.2022.3152830.
[8] M. Yokozuka, K. Koide, S. Oishi, and A. Banno, “LiTAMIN: LiDAR-based Tracking And Mapping by Stabilized ICP for Geometry Approximation with Normal Distributions,” in 2020 IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS), Oct. 2020, pp. 5143–5150. doi: 10.1109/IROS45743.2020.9341341.
[9] M. Yokozuka, K. Koide, S. Oishi, and A. Banno, “LiTAMIN2: Ultra Light LiDAR-based SLAM using Geometric Approximation applied with KL-Divergence,” in 2021 IEEE International Conference on Robotics and Automation (ICRA), May 2021, pp. 11619–11625. doi: 10.1109/ICRA48506.2021.9560947.
[10] C. Yuan, W. Xu, X. Liu, X. Hong, and F. Zhang, “Efficient and Probabilistic Adaptive Voxel Mapping for Accurate Online LiDAR Odometry,” IEEE Robot. Autom. Lett., vol. 7, no. 3, pp. 8518–8525, Jul. 2022, doi: 10.1109/LRA.2022.3187250.
[11] C. Wu et al., “VoxelMap++: Mergeable Voxel Mapping Method for Online LiDAR(-Inertial) Odometry,” IEEE Robot. Autom. Lett., vol. 9, no. 1, pp. 427–434, Jan. 2024, doi: 10.1109/LRA.2023.3333736.