Robust, high-bandwidth, and consistent LiDAR-inertial odometry

Abstract

Autonomous navigation system offers significant potential to various industries. As a fundamental component of autonomous navigaion systems, accurate and robust localization is crucial for supporting the downstream modules of the autonomous navigation system, such as decision-making, path-planning, and control modules. The performance of localization relies on the utilization of sensors and the multi-sensor fusion algorithm of state estimation. Light Detection And Ranging (LiDAR) is attracting more research interest for its accurate range measurement and Extended Kalman Filter (EKF) is adopted widely as the state estimation method thanks to its high efficiency. This thesis focuses on the research of LiDAR-based autonomous localization and the theoretical foundation for improving the performance of EKF-based state estimation.

Modeling system states on manifolds favours (iterated) EKF with significant benefits including improved stability, minimum-parameterization, and singularity-avoidance. Based on this, the thesis develops a generic and symbolic framework of on-manifold iterated EKF, called IKFoM. The proposed IKFoM is based on a canonical system representation and symbolically separates manifold-related calculations from system behaviors, resulting in the first framework of iterated EKF naturally evolving on manifolds, which is generic to arbitrary manifolds. What’s more, to benefit the research community, a C++ toolkit of IKFoM has been implemented and open-sourced. By integrating manifold-related calculations into IKFoM, this toolkit makes it easier to precisely deploy an on-manifold iterated EKF on an arbitrary system.

Building upon IKFoM, a robust and high-bandwidth LiDAR-inertial odometry (LIO) system called Point-LIO is developed to extend LIO’s capabilities of handling aggressive motions characterized by serious vibrations and high-speed movements. Point-LIO introduces two key innovations: a point-by-point framework where the state is updated at each LiDAR point measurement, and a stochastic process-augmented kinematic model that treats Inertial Measurement Unit (IMU) measurements as outputs rather than inputs. These innovations enable Point-LIO to provide extremely high-frequency odometry output (i.e. 4, 000-8, 000 Hz), significantly increasing the measuring bandwidth, and to effectively mitigate in-frame motion distortion during severe vibrations. The novel IMU modeling approach of Point-LIO ensures robustness to noisy and biased IMU measurements, enabling accurate localization and reliable mapping even under high-speed motions, including scenarios where IMU measurements are saturated, reaching 75 rad/s.

The estimation performance of the iterated EKF used in Point-LIO is largely impacted by its consistency. Existing methods for designing consistent EKFs are either restricted by less accurate first estimates impacting the estimation performance or rely on system-specific insights challenging to generalize. To tackle this issue, the thesis proposes a more general method for designing a consistent on-manifold iterated EKF for a wider class of non-linear systems, leveraging more accurate recent state estimates. The proposed method is proven to be valid through a set of solid theoretical proofs. The key point of the proposed method is to transform the system to an equivalent system through a novel non-linear transformation, then design the consistent (iterated) EKF on the transformed system. Furthermore, a consistent on-manifold iterated EKF-based Point-LIO is presented as an exemplary application of the proposed method, along with two additional examples: a visual-inertial system and a multi-robot system.