Classic literature reading - DAMS-LIO (lightweight LiDAR inertial odometer based on iEKF)

0. Introduction

The fusion scheme is the key to the multi-sensor fusion method, which is the most promising solution for state estimation in complex and extreme environments such as underground mines and planetary surfaces. This paper presents a lightweight iEKF-based LiDAR inertial odometry system with a degradation-aware modular sensor fusion pipeline that simultaneously measures LiDAR from another odometry during the update process only when degradation is detected. points and relative poses. The CRLB theory and simulation experiments verify that the method has higher accuracy than the single observation method. Furthermore, the proposed system is evaluated on perception-challenged datasets against various state-of-the-art sensor fusion methods. The results show that the system still has high estimation accuracy and real-time performance even in harsh environments and poor observation conditions.

Although this work has not yet been open sourced, it is expected to be open sourced after receiving it. For the specific original content, please refer to " DAMS-LIO: A Degeneration-Aware and Modular Sensor-Fusion LiDAR-inertial Odometry " here.

Figure 1: (a) is the mapping result of DAMS-LIO in the CERBERUS DARPA subsurface challenge dataset, which contains many challenging environments, as shown in (b)-(c), including dark, long tunnels, and textureless areas . We compare our mapping results with the state-of-the-art laser inertial odometry navigation system LIO-SAM in (a) top right.

1. Article contribution

1. A lightweight lidar-inertial odometry system (DAMS-LIO) based on degradation-aware and modular sensor fusion is proposed, which is capable of robust and accurate state estimation in extreme In this case, it provides a clear advantage for robots to perform complex detection tasks.

2. A new sensor fusion method is proposed to fully fuse the information of LiDAR and other odometry, and only when degradation is detected, the relative pose of LiDAR points and other odometry is used as the measurement value in the update process.

3. A theoretical analysis based on the CRLB theorem is carried out to quantify the performance and demonstrate the high accuracy of the proposed sensor fusion method.

4. Extensive experiments on simulated and real datasets verify the robustness and accuracy of our method.

2. System architecture

The definition of each coordinate system in our system is shown in Fig. 2. Following the typical LIO coordinate system definition, $\left\{L\right\}$ sum$\left\{I\right\}$ correspond to the lidar and IMU coordinate systems respectively, while$\left\{G\right\}$ is a local vertical reference frame whose origin coincides with the initial IMU position. $\left\{O\right\}$ denotes other odometry measurements whose origin is marked as{ M } \{M\}{ M}。

${}^{L}p$represents the position of each point relative to the lidar coordinate system. Definitions of some important and commonly used symbols are given in Table I.

A. State vector (this part is more general)

The estimated states in our system include the current IMU state $x_I$, the extrinsic parameter I x L I_{x_L} between LiDAR and IMU$I_{x_L}$and the transformation from other odometry frames to IMU frames $I_{x_O}$. at time $t_k$, the state can be written as:

${}^G{v}_I$is the speed of the IMU in the global coordinate system, $b_{g,k}$and $b_{a,k}$denote the bias of gyroscope and accelerometer respectively, $g$ is in{ G } \{G\} Gravity vector in$\{$$G$$\}$ coordinate system, x L = { LI q ˉ , I p L } x_L = \{^I_L \bar{q}, ^Ip_L\}xL​={ LI​qˉ​,IpL​}和$x_O={\{}_O^I\bar{q}{,}^I{p}_O\}$ means from other odometry coordinate system {O} and LiDAR coordinate system$\left\{L\right\}$ to IMU coordinate system$Transformation\; of\; \{I\}$ .

B. IMU propagation (similar to IMU propagation)

Due to IMU measurements are subject to bias $b$ and zero-mean Gaussian noise$n$ [15], so they can be modeled as:

where,${oh}_m(t)$和$a_m\left(t\right)$ is the original IMU measurement data,$I_{}\left(t\right)$ is the IMU in the local coordinate system{ I } \{I\} Angular velocity at$\{$$I$$\}$$G_g$和$G_{a_I}\left(t\right)$ is gravity and the acceleration of the IMU in the global coordinate system. The IMU kinematics are the same as [16], and for the sake of brevity, we do not repeat the description here. In order to transform the covariance matrix from time$t_k$passed to time $t_{k+1}$, we follow the linear discrete-time model generation format of [17]:

This text is about state vectors and IMU propagation. Among them, in ${Phi}_k$is the linearized system state transition matrix, $n_k=[n_g{n}_{wg}{n}_a{n}_w]$ is the system noise. Except for quaternions, the error state is defined as$\tilde{x}=x-\hat{x}$ . Among them, the quaternion passes the relation$q=\hat{q}\otimes \delta qis$ defined. Same as [16], notation$\otimes$ means quaternion multiplication, and the error quaternion is defined as$q\_\backsimeq [\frac{1}{2}d{i}^{T}1{]}^{T.}$ _
If$n_k$The covariance of $Q_k$, then the state covariance can be changed from $t_k$Propagate to $t_{k+1}$, see the formula for the specific method.

where $Phi=diag({\Phi }_I,{Phi}_O)$，$G=[G_I^T,G_O^T{]}^T$，${Phi}_I$Japanese $G_I$Indicates the part related to variables other than extrinsic parameters between other odometry and IMU, which is the same definition as in [12]. Furthermore, ${Phi}_O=I_{6\times 6},G_O=0_{6\times 12}$。

C. Measurement model (more important knowledge)

1) LiDAR measurements: For measurements from LiDAR, we model them as in [12]. After motion compensation for scans sampled at time τi, we set the $j$ points are defined as${}^L{p}_j$. By backpropagation, the point can be changed to be the same as $t_k$The end-of-scan measurement corresponding to the IMU measurement at . At the same time, since each point should lie on a facet area in the map, we can get

where, ${}^G{q}_j$is a point on the facet, ${}^G{u}_j$is the normal vector of the plane. ${}^I{R}_L$和${}^G{R}_I$is the corresponding %ILqsum ${}^G{I}_{Kq}$The rotation matrix of . ${}^L{n}_j$is point ${}^L{p}_j$ranging and pointing noise. (8) can also be summarized in a more concise form:

In order to linearize the updated measurement model, we set the measurement model at ${\hat{x}}_k$make a first-order approximation.
where $v_l\in N(0,R_l)$ is due to the original measurement noise${}^L{n}_j$resulting in Gaussian noise. $H_L$is the residual $r_l$For error state ${\tilde{x}}_k$The Jacobian matrix for , is given as follows:

2) Another odometry measurement: Since each odometry is issued at a different frequency, we perform a linear interpolation to obtain the estimated state at time $t_k$and $t_{k+1}$Other odometer attitudes at , and denote their coordinate system as $O_{k-1}$and $O_k$. We will transform the matrix ${}^A{T}_B$Defined as ${}^A{T}_B={[}^A{R}_B{,}^A{p}_B;0_{3\times 3},1]$ . Then according to Figure 2, we have the following transformation relationship:

Among them, $^{Q{k-1}}T_{O_k} =KaTeX parse error: Double superscript at position 19: …^{-1}_{O_{k- 1}}^̲GT_{O_k}is the relative pose measure computed by other odometry. Therefore, based on (13), we can easily get the measurement model$z_O=[\begin{array}{c}{z}_r\\ z_p\end{array}]$ , where:

the rotation and translation errors calculated according to formula (10), where$v_r\in N(0,R_r)$ and$v_p\in N(0,R_p)$ is the corresponding Gaussian noise. Therefore, according to equations (14) and (15), we can obtain the Jacobian matrix of the error state of the rotation and translation residuals.

in:

D. Metamorphic perception update (more important knowledge)

Similar to [12], we have the following error states:

$⊞/⊊$ represent the addition and subtraction operators in the Lie group [18]. $M_{}$是$({\hat{x}}_k^{}⊞{\tilde{x}}_k^{})\boxminus {\hat{x}}_k$Relative to ${\tilde{x}}_k^{}$Partial derivatives of , evaluated with a value of zero.

…For details, please refer to Gu Yueju