Classic Literature Reading--Point-LIO (Robust High Bandwidth Laser Inertial Odometer)

0. Introduction

In the algorithms we have come into contact with before, it is basically necessary to deal with the inter-frame radar distortion, similar to the VSLAM system, the frequency is fixed (for example, 10Hz), but in fact, the lidar points are sampled according to the order of different time instants, and the Accumulating these points into frames introduces artificial motion distortion and can negatively impact map results and odometry accuracy. A low frame rate also increases the latency of the odometry and limits the achievable bandwidth, where the odometry bandwidth is analogous to the bandwidth of a dynamic system, i.e. the frequency at which the system gain drops to 0.707. The odometry bandwidth indicates how fast the motion can be before the odometry can satisfactorily estimate it. Mars Laboratories presented the article " Point-LIO: Robust High-Bandwidth LiDAR-Inertial Odometry ", Point-LIO can provide accurate high-frequency odometry under severe vibration and intense motion (up to 75 rad/s angular velocity) (4-8 kHz) and reliable mapping, beyond the range of IMU measurements.

1. Main contributions

In this work, we address these issues with two key new techniques: peer-to-peer state updates and stochastic process-augmented kinematics models. Specifically, our contributions are as follows:

1. A point-wise LIO framework is proposed that fuses lidar points at actual sampling time without accumulating into frames. Removing point accumulation eliminates intra-frame motion artifacts and allows high odometry output and mapping updates at near point sampling rates, which further enables the system to track very fast motion.
2. To further increase the system bandwidth beyond the IMU measurement range, the IMU measurement is modeled with a stochastic process model. Extend this model to system kinematics and consider IMU measurements as system outputs. Even when the IMU is saturated, the stochastic process-enhanced kinematics model can smoothly estimate the system state, including angular velocity and linear acceleration.
3. Integrating these two key technologies into a fully tightly coupled LIO system is called Point-LIO. The system uses a manifold extended Kalman filter to update the system state by fusing each LiDAR point or IMU data at its respective sampling time. By exploiting the sparsity and linearity of the system, the developed system enables real-time state estimation even on a low-power ARM-based computer on a MAV.
4. The developed system is tested on various challenging real-world data collected by emerging solid-state LiDARs with very small FoV. The results show that Point-LIO is capable of motion distortion compensation with high odometry output rate (4-8 kHz) and high bandwidth (>150Hz). The system is also able to estimate the state under extremely aggressive motion (angular velocity greater than 75 rad/s) by saturating the IMU measurements after the initial phase. Furthermore, an exhaustive benchmark comparison of 12 sequences from various publicly available LiDAR datasets shows that Point-LIO achieves consistently comparable accuracy and efficiency to other methods while consuming fewer computational resources. Finally a practical application on an actual drone is demonstrated.

2. Systematic review

The design concept of this article is based on the following two points:

1) Lidar points are sampled in their respective time order, not frames sampled at the same time;

2) IMU data is a measurement of the system, not an input.

Once the respective measurements (per lidar point or IMU data) are received, we fuse the two measurements in a manifold-based extended Kalman filtering framework [55]. An overview of the system we designed is shown in Figure 1, where sequentially sampled lidar points and IMU data are both used to update the state at their respective timestamps, enabling an extremely high rate of odometry output, i.e., effectively 4- 8 kHz. In particular, for each lidar point received, we search the map for the corresponding plane. If the point matches the plane fitted to the point in the map, the residual is computed to update the system state using a manifold-based Kalman filter. The final optimized pose registers lidar points to the global coordinate system and merges into the map, then proceeds to the next measurement (either lidar points or IMU data). Otherwise, if there is no matching plane for that point, the pose predicted by the Kalman filter is added directly to the map. To simultaneously enable fast planar correspondence search and allow the joining of new registration points, we use the incremental kd tree structure ikd-Tree, which was originally developed in FAST-LIO2 [29]. For each IMU measurement, a saturation check is done separately, and channels with saturation values ​​are not used for status updates.

Figure 1: Overview of the Point-LIO system. ⊕ means adding information.

3. Explanation of symbols

The state estimation of Point-LIO is a tightly coupled manifold-based Kalman filter. Here, we briefly introduce the basic formulation and workflow of the filter, and refer to [55] for a more detailed and theoretical explanation of the manifold-based Kalman filter.

For the convenience of explanation, we adopt the following notation:

Additionally, we introduce two wrapping operations $⊞$ ("boxplus") and its inverse$⊊$ (“boxminus”), defined in [55], is used todescribeManifoldMM$of\; n$The system on$M$${\mathbb{R}}^{}$Parameterized state error in${}^{n\; .}$At the same time, these operations can more compactly describe the state-space model of the system in discrete time. We refer to [55] for more detailed definitions and derivations. In this paper, we only focus on the manifold$SO\left(3\right)$ and$R^n$：

Among them, $Exp®= I + sin(∥r∥)\frac{|r|}{∥r∥} + (1 − cos(∥r∥))∕\frac{|r| 2}{ ^∥r∥ 2} $is$ an exponential map onSO(3)$The\; exponential\; map\; on\; ,$ and Log$is\; its\; inverse\; mapping.For\; a\; composite\; manifold$ M = SO(3) × \mathbb{R}^n$,\; which\; are\; two\; submanifolds$ M = SO(3)$ and${\mathbb{R}}^{}$Cartesian product of${}^{n\; ,\; we\; have:}$

4. Kinematic model

We first derive the system model, which consists of a state transition model and a measurement model.

4.1 State transition model (similar to Fast-LIO2)

Taking the IMU coordinate system (expressed as I) as the body coordinate system, and the first IMU coordinate system as the global coordinate system (expressed as G), the continuous kinematics model is:

Among them, ${}^G{R}_I$，${}^G{p}_I$和${}^G{v}_I$Indicates the attitude, position and velocity of the IMU in the global coordinate system, ${}^{G}g$represents the gravity vector in the global coordinate system. $b_g$Japanese $b_a$is the random drift IMU bias driven by Gaussian noise, respectively $n_{b_g}\sim N(0,Q_{b_g})$和$n_{b_a}\sim N(0,Q_{b_a})$ . Symbol$\lfloor a\rfloor$是$a\in {\mathbb{R}}^{}$The antisymmetric cross-product matrix of${}^{3\; .}$$I_{}$和$I_a$Indicates the angular velocity and acceleration of the IMU in the body coordinate system, that is, the IMU coordinate system. As proposed in [14], some robot motion (angular velocity $I_{}$and linear acceleration $I_a$) can always be viewed as a sample of a signal set or population, which allows us to statistically describe robot motion by stochastic processes. Furthermore, as suggested by [14], since the motion of robotic systems usually has some smoothness (e.g., due to actuator delays), rapid changes in angular velocity and acceleration are relatively unlikely, so an Nth order integrator stochastic process is often Enough for practical use. In particular, we choose the Gaussian noise $w_g\sim N(0,Q_g)$和$w_a\sim N(0,Q_a)$ driven first-order integrator model to simulate the angular velocity$I_{}$and linear acceleration $I_a$。

Then, at each measurement step kkDiscretize the continuous model at$k\; (2).$∆tk$∆t_k$Denoted as the current measurement interval, it is the time difference between the previous measurement (IMU data or LiDAR point) and the current measurement (IMU data or LiDAR point). By assuming that the input is in the interval $∆t_k$, the continuous model (2) is discretized, resulting in

Among them, the manifold $M$ , function$f$ , status$x$ and process noise$w$ is defined as follows:

For example, $Q=diag(Q_{b_g}，Q_{b_a}，Q_g，Q_a)$ is the process noisewwThe covariance matrix of$w\; .$

4.2 Measurement model (emphasis)

The system has two measurements, a LiDAR point or an IMU data (including angular velocity and acceleration measurements). These two measurements are usually sampled and received by the system at different times, so we model them separately. Assuming that the LiDAR coordinate system coincides with the body (ie IMU) coordinate system or has pre-calibrated external parameters, then the LiDAR point ${}^I{p}_{m_k}$Equal to the real position I pkgt ^Ip^{gt}_k in the local IMU coordinate system${}^I{p}_k^{gt}$, the position is unknown, subject to additive Gaussian noise $n_{L_k}\sim N(0,R_{L_k})$ pollution:

Use the real point with the real (but unknown) IMU pose ${}^G{T}_I={(}^G{R}_I{，}^G{p}_I)$ projected into the global coordinate system, should lie exactly on a local facet patch in the map (see Figure 2), ie:

Figure 2: Schematic of direct registration of LiDAR points to a map. Blue represents a flat vector. ${}^G{q}_i$sum $u_i$Represents a point and normal in the map.

Among them, ${}^G{u}_k$is the normal vector of the corresponding plane, ${}^G{q}_k$is any point on the plane. Note that ${}^G{T}_{I_k}$Contained in the state vector $x_k$middle. (6) For state vector $x_k$An implicit measurement model is introduced.

The IMU measurement is determined by the angular velocity measurement ( ${}^I{\omega }_m$) and acceleration measurements ( ${}^I{a}_m$)composition:

ng $n_g\sim N(0,R_g)，n_a\sim N(0,R_a)$ equal high voice. $n_I=[n_g^T{n}_a^T{]}^T\sim N(0,R_I)=N(0,diag(R_g,R_a))$ represents the measurement noise of the IMU. It can be seen that in the angular velocity measurement${oh}_m$(or accelerometer $a$ ), two states$\omega$，$b_g$(and similarly $a$，$b_a$) were separated in the equation of state (2), but are now correlated.

In summary, the measurement model of the system can be expressed in the following compact form:

5. Extended Kalman filter

Point Cloud - Laser Inertial Navigation System uses tightly coupled Extended Kalman Filter for state estimation. This section introduces the workflow of EKF.

5.1 State Propagation

Suppose we have received $A\; measurement\; at\; k$ steps, and the updated state at that time step is${\bar{x}}_k$, the updated covariance matrix is ${\bar{P}}_k$. from the $k$ steps to the next measurement step$k+The\; state\; propagation\; of\; 1$ directly follows the state transition model in Equation (3), where$w_k$set to 0:

The propagation of the covariance matrix is:

where $Q_k$is the process noise $w_k$The covariance of the matrix $F_{x_k}$，$F_{w_k}$Calculated as follows:

where $x_{k+1}$is time step $k+$The truth value of the state vector of$1$$F_{11}=Exp({-}^I{\tilde{w}}_k∆t_k)$，$F_{31}={-}^G{\bar{R}}_{I_k}{\lfloor }^I{\bar{a}}_k\rfloor ∆t_k$， $F_{38}{=}^G{\bar{R}}_{I_k}∆t_k$。

5.2 Residual calculation LiDAR measurement: (emphasis)

Radar measurement: According to the predicted attitude GT in Kalman propagation (9) ${}^G{\hat{T}}_{I_{k+1}}={（}^G{\hat{R}}_{I_{k+1}}{，}^G{\hat{p}}_{I_{k+1}})$ , the measured LiDAR point${}^I{p}_{m_{k+1}}$G p ^ k + 1 projected into the global coordinate system ${}^G{\hat{p}}_{k+1}{=}^G{\hat{R}}_{I_k}^I{p}_{m_k+1}{+}^G{\hat{p}}_{I_{k+1}}$, and search its nearest 5 points in the map organized by ikd-Tree (distance ${}^G{\hat{p}}_{k+1}$not more than 5 m). Then, use the found nearest neighbors to fit a local facet patch whose normal vector is ${}^G{u}_{k+1}$, the center of gravity is ${}^G{q}_{k+1}$, as indicated by the measurement model (see Eq. (6) and Fig. 2). If the nearest 5 points are not on the fitted plane path (i.e., any point is more than 0.1 m away from the plane), then the current LiDAR point G p ^ k + 1 ${}^G{\hat{p}}_{k+1}$measurements are incorporated directly into the map without residual calculations or state updates. Otherwise, if the local plane fitting is successful, the residual is calculated according to equation (8) ( $r_{L_{k+1}}$）。

where, $\delta x{\_}_{k+1}=x_{k+1}\boxminus {\hat{x}}_{k+1}$, where $x_{k+1}$is time step $k+The\; truth\; value\; of\; the\; state\; vector\; of\; 1$ ,${\hat{x}}_{k+1}$is time step $k+$An estimate of the state vector of$1\; .$

IMU measurement: judge whether the IMU measurement channel is discarded by checking the gap between the current measurement value and the rated measurement range. If the gap is too small, that IMU measurement channel will be discarded without updating the state. Then, the acceleration and angular velocity measurements of the unsaturated IMU channel are collected, and the IMU residual is calculated according to equation (7) ( $r_{I_{k+1}}$) (for notational simplicity, we use here measurements from all six channels).

where, $\delta x{\_}_{k+1}=x_{k+1}\boxminus {\hat{x}}_{k+1}$, where $x_{k+1}$is time step $k+The\; truth\; value\; of\; the\; state\; vector\; of\; 1$ ,${\hat{x}}_{k+1}$is time step $k+$An estimate of the state vector of$1\; .$

In summary, the residuals from LiDAR point measurements (Equation 12) or IMU measurements (Equation 14) are related to the state x k+1 and the corresponding measurement noise as follows:

…For details, please refer to Gu Yueju