GNSS/LiDAR/IMU Fusion Odometry Based on Tightly-Coupled Nonlinear Observer in Orchard

Abstract

High-repetitive features in unstructured environments and frequent signal loss of the Global Navigation Satellite System (GNSS) severely limits the development of autonomous robot localization in orchard settings. To address this issue, we propose a LiDAR-based odometry pipeline GLIO, inspired by KISS-ICP and DLIO. GLIO is based on a nonlinear observer with strong global convergence, effectively fusing sensor data from GNSS, IMU, and LiDAR. This approach allows for many potentially interfering and inaccessible relative and absolute measurements, ensuring accurate and robust 6-degree-of-freedom motion estimation in orchard environments. In this framework, GNSS measurements are treated as absolute observation constraints. These measurements are tightly coupled in the prior optimization and scan-to-map stage. During the scan-to-map stage, a novel point-to-point ICP registration with no parameter adjustment is introduced to enhance the point cloud alignment accuracy and improve the robustness of the nonlinear observer. Furthermore, a GNSS health check mechanism, based on the robot’s moving distance, is employed to filter reliable GNSS measurements to prevent odometry crashed by sensor failure. Extensive experiments using multiple public benchmarks and self-collected datasets demonstrate that our approach is comparable to state-of-the-art algorithms and exhibits superior localization capabilities in unstructured environments, achieving an absolute translation error of 0.068 m and an absolute rotation error of 0.856°.