Reading classic literature--LIW-OAM (LiDAR-IMU-encoder fusion SLAM)

0. Introduction

We have often been exposed to the complementary information of lidar and inertial measurement units (IMU) before. However, if the IMU undergoes severe bumps during actual use, it may cause the IMU to fail. The widely used Iterative Closest Point (ICP) algorithm can only provide constraints for attitude, while velocity can only be constrained by IMU preintegration. Therefore, velocity estimates tend to be updated with attitude results. There is a recent blog that introduces the method of integrating radar, imu and odometry " LIW-OAM: Lidar-Inertial-Wheel Odometry and Mapping ", which proposes an accurate and powerful LiDAR inertial wheel odometry and mapping system. Measurements from LiDAR, IMU and wheel encoders are integrated in a beam adjustment (BA) based optimization framework. The involvement of wheel encoders can provide velocity measurements as an important observation, which helps LI-OAM provide more accurate state predictions. In addition, constraining the speed variable through the observations of the wheel encoder in the optimization can further improve the accuracy of the state estimation. Experimental results on two public datasets show that our system outperforms all state-of-the-art LI-OAM systems in terms of absolute trajectory error (ATE), and embedding wheel encoders can significantly improve the performance of LI-OAM based on the BA framework. performance. This project is currently open source on Github

1. Article contribution

In this paper, we use an additional wheel encoder sensor, and the cost of the wheel encoder sensor is much lower than that of AHRS. Experimental results on the public datasets nclt [3] and kaist [7] show that our system outperforms the existing state-of-the-art LI-OAM systems in terms of absolute trajectory error (ATE). There are two main contributions:

1. We propose a novel BA-based LIW-OAM system that embeds the wheel encoder velocity observations into BA-based LI optimization. Our LIW-OAM system outperforms most state-of-the-art LI-OAM systems in terms of accuracy.

2. We have released the source code of this work to facilitate the development of the community.

2. Preliminary knowledge

2.1 Coordinate system

We use $(\cdot {)}^w$ 、$(\cdot {)}^l$ 、$(\cdot {)}^o$ and$(\cdot {)}^k$ represents a three-dimensional point in the world coordinate system, LiDAR coordinate system, IMU coordinate system and odometer (i.e. wheel encoder) coordinate system. World coordinates and$(\cdot {)}^l$ overlap at the starting position. In all coordinate systems, the x-axis points forward, the y-axis points to the left, and the z-axis points upward. We use$l_i$Indicates that at time $t_i$Carry out the iiLiDAR coordinates of scan$i$$o_i$expressed in $t_i$The corresponding IMU coordinates at the time, and then from the LiDAR coordinates $l_i$Arrive at IMU $o_i$The transformation matrix (ie, external parameters) is expressed as $T_{l_i}^{o_i}\in SE\left(3\right)$：

Among them, $T^{o_i} {l_i} $From\; the\; rotation\; matrix$ R^{o_i}{l_i} ∈ SO(3)$and\; translation\; vector$ t^{o_i}{l_i} ∈ \mathbb{R}^{3$. The extrinsic parameters are usually calibrated offline and remain constant during online pose estimation; therefore, we can simply use $T}o_l$ to denote $T^{o_i}{l_i}$.Similarly,\; the\; transformation\; from\; odometer\; coordinates\; toIMUcoordinates\; is\; expressed\; asT$ ^o_k$, which includes$R_k^o$andtkot $t_k^o$ 。

We represent rotations using a rotation matrix R and a Hamilton quaternion q. In state vectors we mostly use quaternions, but rotation matrices are also used to rotate 3D vectors conveniently. $\otimes$ represents the multiplication operation between two quaternions. Finally, we use$(\hat{\cdot })$represents a noisy measurement or estimate of a certain quantity. In addition to the attitude, we also estimate the velocity$v$ . Accelerometer deviation$b_a$and gyroscope deviation $b_{}$, they are all uniformly represented by a state vector:

2.2 Scan status expression (need to review CT-ICP)

Inspired by CT-ICP [5], we represent scan SS using$S$ status:

1）$S$ start time$t_b$status (e.g. $x_b$）；

2）$S$ end time$t_e$state (e.g. $x_e$）。

In this way, [tb, te] [t_b, t_e] can be$[t_b，t_e]$ The state of each point during the period is expressed as$x_b$和$x_e$The function. For example, for time $t_p\in [t_b，t_e]$ The collection belongs to$S$ 's$p$ , you can calculate$t_p$status at the time:

Among them, slerp(·) is the spherical linear interpolation operator of quaternion.

2.3 IMU-odometer measurement model (skip)

IMU-Odometer consists of a wheel encoder and an IMU which includes an accelerometer and a gyroscope. Raw gyroscope and accelerometer measurements from the IMU, i.e. ${\hat{a}}_t$和${\widehat{oh}}_t$, respectively:

IMU measurements are measured in the IMU coordinate system, combining forces counteracting gravity and platform dynamics, and subject to accelerometer bias $b_{a_t}$, Gyroscope deviation $b_{{oh}_t}$and the effects of additive noise. As mentioned in VINS-Mono [12], the additive noise in accelerometer and gyroscope measurements is modeled as Gaussian white noise, $n_a\sim N(0,p_a^2)，n_{}\sim N(0,p_{oh}^2)$ . The accelerometer bias and gyroscope bias are modeled as random walks with derivatives as Gaussian distributions,${\dot{b}}_{a_t}=n_{b_a}\sim N(0,p_{ba}^2)，{\dot{b}}_{{oh}_t}=n_{b_{}}\sim N(0,p_{b_{}}^2)$。

The wheel encoder obtains the rotational speed $\tau$ , then according to$\tau$ and wheel radius calculate the speed of the left and right rear wheels.

其中，$n_{t_{left}}$和$n_{t_{right}}$是$t_{left}$和$t_{right}$The corresponding zero-mean white Gaussian noise, ${\hat{v}}_{left}$和${\hat{v}}_{right}$is from $\hat{t}\cdot$and$r\cdot$ Calculated measured linear velocity of the two wheels. Then, the final measurement model of the wheel encoder odometry, measured in the odometry coordinate system, can be defined as:

Among them, $[n_v{]}_x$are two zero-mean Gaussian distributions $n_v\sim N(0,p_v^2)$ , is still a zero-mean Gaussian distribution.

3. System Overview

Figure 1 illustrates our LIW-OAM framework, which consists of four main modules: preprocessing, initialization, state estimation, and point cloud registration. The preprocessing module downsamples the input raw points and preintegrates the IMU-odometry measurements at the same frequency as the input scans. The initialization block estimates some state parameters, including gravitational acceleration, accelerometer bias, gyroscope bias, and initial velocity. The state estimation module first integrates the IMU-odometer measurements to the last state to predict the current state, and then performs BA-based LIW optimization to optimize the current scanned state. Finally, point cloud registration adds new points to the map and removes points further away from the map.

Figure 1. Overview of LIW-OAM, including four main modules: preprocessing module, initialization module, state estimation module and point registration module.

4. Preprocessing (skimmed)

4.1 Downsampling

Processing a large number of 3D points incurs high computational cost. To reduce computational complexity, we downsample the input points as follows. We scan the current input $S_{i+1}$The points fit into a volume of $0.5\times 0.5\times 0.5$ (unit: meter) in the voxel, and make each voxel contain only one point, to obtain the downsampling scan$P_{i+1}$. This downsampling strategy ensures that the density distribution of points is uniform in 3D space after downsampling.

4.2 Pre-integration

Typically, IMU-odometry sends data at a higher frequency than LiDAR. Pre-integrate two consecutive scans $S_i$Sum $S_{i+1}$All IMU-odometry measurements between hardware platforms can be nicely summarized from time $t_{e_i}$tei $t_{e_{i+1}}$The dynamic characteristics of $e_i$sumei $e_{i+1}$Separation is $S_i$Sum $S_{i+1}$The end timestamp of . In this work, we adopt a quaternion-based discrete-time IMU-odometry pre-integration method [10] and use the method in [12] to incorporate the IMU bias. Specifically, at the corresponding IMU coordinates $o_{e_i}$Japanese $o_{e_{i+1}}$S i S_i is calculated in$S_i$Sum $S_{i+1}$全电影图发动，即${\hat{a}}_{e_{i+1}}^{e_i}，{\widehat{the}}_{e_{i+1}}^{e_i}，{\hat{b}}_{e_{i+1}}^{e_i}$和${\hat{c}}_{e_{i+1}}^{e_i}$, among themα ${\hat{a}}_{e_{i+1}}^{e_i}，{\hat{b}}_{e_{i+1}}^{e_i}$和${\hat{c}}_{e_{i+1}}^{e_i}$are the translation, velocity and rotation pre-integrated from the IMU measurements respectively, and ${\widehat{the}}_{e_{i+1}}^{e_i}$is the pre-integrated translation from gyroscope and wheel encoder odometer measurements. In addition, the pre-integrated Jacobian matrix with respect to the bias is also calculated based on the error state kinematics, namely $J_{b_a}^{}，J_{b_{}}^{}，J_{b_a}^{}，J_{b_{}}^{}，J_{b_{}}^{}，J_{b_a}^{}and{J}_{b_{}}^{}$。

5. Initialization (need to review SR-LIO)

The initialization module is designed to estimate all necessary values, including initial attitude, velocity, gravitational acceleration, accelerometer bias, and gyroscope bias, for subsequent state estimation. Similar to our previous work SR-LIO [19], we adopt motion initialization and static initialization for handheld devices and vehicle-mounted devices respectively. For more details about our initialization module, see [19].

6. State Estimation

6.1 State Prediction

When each new downsampling scan $P_{i+1}$When done, we use the IMU-odometry measurements to predict $P_{i+1}$start timestamp (i.e. $x_{b_{i+1}}^w$) and end timestamp (ie $x_{e_{i+1}}^w$) states to provide LIW-optimized prior motion. Specifically, the predicted state $x_{b_{i+1}}^w$（即$t_{b_{i+1}}^w，R_{b_{i+1}}^w，v_{b_{i+1}}^w，b_{a_{b_{i+1}}},b_{w_{b_{i+1}}}$) is assigned the value

where $\widehat{oh}\cdot 、\hat{a}\cdot and\hat{v}\cdot$are measurements from the IMU gyroscope, IMU accelerometer and wheel encoder,$g^w$ is the acceleration due to gravity in the world coordinate system,$n$ and$n+1$ is in$t_{e_i}$、$t_{e_{i+1}}$Two time instants during which the IMU odometer measurement value is obtained, $d_t$is $n$ and$n+1$ interval between. We iteratively increase n from 0 to$t_{e_{i+1}}-t_{e_i}/d_t$to get $x_{e_{i+1}}^w$. When n = 0, $x_n^w=x_{e_i}^w$. 对于$b_{a_{e_{i+1}}}$sum $b_{w_{e_{i+1}}}$、Our general guide: $b_{a_{e_{i+1}}}=b_{a_{e_i}}，b_{w_{e_{i+1}}}=b_{w_{e_i}}$。

6.2 BA-based LIW-optimization (main innovation)

We jointly use lidar, inertial and wheel encoder measurements to optimize the current scan $P_{i+1}$The starting state of (ie $x_{b_{i+1}}^w$) and the end state (i.e. $x_{b_{i+1}}^e$), where the variable vector is expressed as:

…For details, please refer to Gu Yueju