Fast and robust LiDAR-inertial state estimation

Abstract

Mobile robots play an essential role in numerous scenarios. On the one hand, the ability to robustly and accurately estimate the actual states (position, velocity, acceleration, etc.) is the base of autonomous control and navigation. On the other hand, the emerging advanced sensors like LiDAR (light detection and ranging sensor) usually generate thousands to millions of points measurements per second. Processing such a large amount of data requires the state estimation algorithm to be highly efficient.

The first problem this thesis address is the system observability analysis, which is the theoretical foundation for a state estimator to converge. The existing observability analysis method is much more complicated due to the over-parameterization in a Euclidean space, such as the quaternion. To address this issue, this thesis proposes new theoretical tools to ease the observability analysis of robotic systems operating on manifolds with minimum parameterization. This new paradigm is more straightforward and natural, and its effectiveness is demonstrated in two popular robotics systems.

The second problem addressed in this thesis is the estimation of robots' {\it high-order dynamics states} (e.g., translational and angular acceleration), which is fundamentally important for many robots and robotic techniques. This thesis presents a generic statistical motion model to capture mobile robots' dynamic behaviors (translation and rotation). After proving the observability of the system augmented with the proposed statistical model, this thesis shows the applications of the proposed statistic motion model in simulated and actual robotic systems. It is able to estimate the {\it high-order dynamic states} with the IMU (inertial measurement unit) measurements noises being effectively depressed online. Then with the estimated acceleration and angular velocity, the inter-IMU calibration without the requirement of any other sensors is also shown.

Subsequently, this thesis proposes a fast and robust LiDAR-inertial odometry system, FAST-LIO, to estimate robots' kinematic state. FAST-LIO is developed based on the extended Kalman filter framework. A novel forward and backward propagation method is proposed for the points undistortion and state propagation. Then an equivalent Kalman gain computation method is proposed to improve the efficiency. The proposed FAST-LIO is tested in various indoor and outdoor environments where it produces reliable navigation results in real-time when running on an onboard computer.

This thesis finally presents FAST-LIO2, which improves the efficiency and robustness of FAST-LIO. It has two key novelties. The first one is directly registering raw points to the map without extracting features. It enables the exploitation of subtle elements in the environment and hence increases accuracy. The elimination of a hand-engineered feature extraction also makes it naturally adaptable to LiDARs of different scanning patterns; The second main novelty is maintaining a map by an incremental k-d tree data structure, named \textit{ikd-Tree}, that enables incremental updates and dynamic re-balancing. This thesis conducts an exhaustive benchmark comparison and challenging experiments. FAST-LIO2 is computationally-efficient (e.g., 100 $Hz$ real-time in large outdoor environments), robust (e.g., reliable pose estimation with rotation speed up to 1000 $deg/s$), versatile (i.e., applicable to various LiDAR types and robots platforms), while still achieving higher accuracy than existing state-of-the-art methods.