Tightly Coupled 3D Lidar Inertial Odometry and Mapping

TL;DR

This paper presents a tightly coupled lidar-IMU fusion approach for robust ego-motion estimation in mobile robotics, achieving high-precision pose estimation even in challenging conditions with degraded lidar data.

Contribution

It introduces a novel joint optimization method for lidar and IMU data fusion and a rotation-constrained refinement algorithm for improved pose accuracy.

Findings

High-precision pose estimation at IMU update rate

Robust performance under fast motion and feature-scarce conditions

Effective long-term drift compensation

Abstract

Ego-motion estimation is a fundamental requirement for most mobile robotic applications. By sensor fusion, we can compensate the deficiencies of stand-alone sensors and provide more reliable estimations. We introduce a tightly coupled lidar-IMU fusion method in this paper. By jointly minimizing the cost derived from lidar and IMU measurements, the lidar-IMU odometry (LIO) can perform well with acceptable drift after long-term experiment, even in challenging cases where the lidar measurements can be degraded. Besides, to obtain more reliable estimations of the lidar poses, a rotation-constrained refinement algorithm (LIO-mapping) is proposed to further align the lidar poses with the global map. The experiment results demonstrate that the proposed method can estimate the poses of the sensor pair at the IMU update rate with high precision, even under fast motion conditions or with insufficient features.

Tables

Table 1. TABLE I : Translational and rotational errors w.r.t. ground-truth. The motions in the 6 sequences vary from fast to slow.

Errors	Sequence	LOAM	LIO- raw	LIO- no-ex	LIO	LIO- mapping
Trans- lation RMSE (m)	fast 1	0.4469	0.2464	0.0957	0.0949	0.0529
	fast 2	0.2023	0.4346	0.1210	0.0755	0.0663
	med 1	0.1740	0.1413	0.1677	0.1002	0.0576
	med 2	0.1010	0.2460	0.3032	0.1308	0.0874
	slow 1	0.0606	0.1014	0.0838	0.0725	0.0318
	slow 2	0.0666	0.1016	0.0868	0.1024	0.0435
Rotation RMSE (rad)	fast 1	0.1104	0.1123	0.0547	0.0545	0.0537
	fast 2	0.0763	0.1063	0.0784	0.0581	0.0574
	med 1	0.0724	0.0620	0.0596	0.0570	0.0523
	med 2	0.0617	0.0886	0.0900	0.0557	0.0567
	slow 1	0.0558	0.0672	0.0572	0.0581	0.0496
	slow 2	0.0614	0.0548	0.0551	0.0533	0.0530

Table 2. TABLE II : Mean running time analyses for 16-line 3D lidar.

Scenarios	Time ($ms$)
	Prediction	Odometry	Mapping
Indoor	0.0127	128.7	108.3
Outdoor	0.0102	213.5	167.6

Equations

$$\begin{array}{rl}{\mathbf{X}}_{B_i}^W& ={\left[\begin{array}{ccccc}{{\mathbf{p}}_{B_i}^W}^T& {{\mathbf{v}}_{B_i}^W}^T& {{\mathbf{q}}_{B_i}^W}^T& {{\mathbf{b}}_{a_i}}^T& {{\mathbf{b}}_{g_i}}^T\end{array}\right]}^T\\ {\mathbf{T}}_B^L& ={\left[\begin{array}{cc}{{\mathbf{p}}_B^L}^T& {{\mathbf{q}}_B^L}^T\end{array}\right]}^T\end{array},$$

$${\mathbf{p}}_j={\mathbf{p}}_i+\sum _{k=i}^{j-1}\left[{\mathbf{v}}_k\Delta t+\frac{1}{2}{\mathbf{g}}^W\Delta t^2+\frac{1}{2}{\mathbf{R}}_k({\hat{\mathbf{a}}}_k-{\mathbf{b}}_{a_k})\Delta t^2\right]$$

$${\mathbf{v}}_j={\mathbf{v}}_i+{\mathbf{g}}^W\Delta t_{ij}+\sum _{k=i}^{j-1}{\mathbf{R}}_k({\hat{\mathbf{a}}}_k-{\mathbf{b}}_{a_k})\Delta t$$

$${\mathbf{q}}_j={\mathbf{q}}_i\otimes \prod _{k=i}^{j-1}\delta {\mathbf{q}}_k={\mathbf{q}}_i\otimes \prod _{k=i}^{j-1}\left[\begin{array}{c}\frac{1}{2}\Delta t({\hat{\mathbf{\omega }}}_k-{\mathbf{b}}_{g_k})\\ 1\end{array}\right],$$

$$\begin{array}{r}{\mathbf{T}}_{L_{\alpha }}^{L_p}={{\mathbf{T}}_B^L{\mathbf{T}}_{B_p}^W}^{-1}{\mathbf{T}}_{B_{\alpha }}^W{{\mathbf{T}}_B^L}^{-1}=\left[\begin{array}{cc}{\mathbf{R}}_{L_{\alpha }}^{L_p}& {\mathbf{p}}_{L_{\alpha }}^{L_p}\\ 0& 1\end{array}\right]\end{array}.$$

$${\mathbf{r}}_{\mathcal{L}}(m,{\mathbf{T}}_{L_p}^W,{\mathbf{T}}_{L_{\alpha }}^W,{\mathbf{T}}_B^L)={\mathbf{\omega }}^T({\mathbf{R}}_{L_{\alpha }}^{L_p}x+{\mathbf{p}}_{L_{\alpha }}^{L_p})+d.$$

$$\begin{array}{r}\mathbf{X}=\left[{\mathbf{X}}_{B_p}^W,\cdots ,{\mathbf{X}}_{B_j}^W,{\mathbf{T}}_B^L\right]\end{array}.$$

$$\begin{aligned} \min_{\mathbf{X}}\frac{1}{2}\Bigg{\{}&\left\lVert\mathbf{r}_{\mathcal{P}}(\mathbf{X})\right\rVert^{2}+\sum_{\begin{subarray}{c}m\in\mathbf{m}_{L_{\alpha}}\\ \alpha\in\{p+1,\cdots,j\}\end{subarray}}{\left\lVert\mathbf{r}_{\mathcal{L}}(m,\mathbf{X})\right\rVert^{2}_{\mathbf{C}^{m}_{L_{\alpha}}}}\\ &+\sum_{\begin{subarray}{c}\beta\in\{p,\cdots,j-1\}\end{subarray}}{\left\lVert\mathbf{r}_{\mathcal{B}}(z^{\beta}_{\beta+1},\mathbf{X})\right\rVert^{2}_{\mathbf{C}^{B_{\beta}}_{B_{\beta+1}}}}\Bigg{\}}\end{aligned},$$

$$\begin{array}{rl}& {\mathbf{C}}_{\mathcal{M}}=\sum _{m\in {\mathbf{m}}_L}{\Vert {\mathbf{r}}_{\mathcal{M}}(m,{\mathbf{T}}_L^W)\Vert }^2\\ & {\mathbf{r}}_{\mathcal{M}}(m,\mathbf{T})={\mathbf{\omega }}^T(\mathbf{R}\mathbf{x}+\mathbf{p})+d\end{array},$$

$$\begin{array}{rl}& {\mathbf{J}}_{{\mathbf{\theta }}_z}^{\mathbf{C}}={\mathbf{J}}_{\mathbf{\theta }}^{\mathbf{C}}\cdot (\breve{\mathbf{R}}{)}^T\cdot {\breve{\mathbf{\Omega }}}_z\\ & {\breve{\mathbf{\Omega }}}_z=\left[\begin{array}{ccc}{ϵ}_x& 0& 0\\ 0& {ϵ}_y& 0\\ 0& 0& 1\end{array}\right]\end{array},$$

$$\begin{array}{rl}& \tilde{\mathbf{p}}=\breve{\mathbf{p}}+\delta \mathbf{p}\\ & \tilde{\mathbf{q}}=\left[\begin{array}{c}\frac{1}{2}\delta {\mathbf{\theta }}_z\\ 1\end{array}\right]\otimes \breve{\mathbf{q}}\end{array}.$$

Full text

I Introduction

Ego-motion estimation plays a major role in many navigation tasks and is one of the key problems for autonomous robots. It offers the knowledge of robot poses and can provide instant feedback to the pose controllers. Besides, together with the various sensors perceiving the environment, it provides crucial information for simultaneous localization and mapping (SLAM). Accurate estimation of the robot pose helps to reduce risks and contributes to successful planning.

The lidar sensor that can provide distance measurements for surrounding environments has been widely used in robotic systems. To be specific, a typical 3D lidar can sense the surroundings at a frequency around 10Hz with a horizontal field of view (FOV) of 360 degrees. Besides, as an active sensor, it is invariant to the illumination. The high reliability and precision make the lidar sensor a popular option for pose estimation.

Despite its advantages, the lidar sensor is not perfect and with several shortcomings. From the lidar itself, it has a low vertical resolution and the sparse point cloud it obtains provides limited features, thus making feature tracking an intractable problem. In addition, lidars mounted on moving robots suffer from motion distortion, which directly affects sensing accuracy. In real-world scenarios, there are some lidar-degraded cases in which the lidar receives few or missing points. For example, 3D lidar receives only few usable points in narrow corridor environments. The received points are mainly from the side walls and only a small portion of points are observed from the ceiling and floor. In this case, the matched lidar features can easily lead to ill-constrained pose estimation. Another shortcoming is its low update rate. This limits its application for tasks that require fast response such as the control of a robot pose.

In this paper, we present a tightly coupled 3D lidar-IMU pose estimation algorithm to overcome the aforementioned problems. Measurements from both the lidar and IMU are used for a joint-optimization. To achieve real-time and more consistent estimation, fixed-lag smoothing and marginalization of old poses are applied, followed by a rotation-constrained refinement. The main contributions of our work are as follows:

• •

A tightly coupled lidar-IMU odometry algorithm is proposed. It provides real-time accurate state estimation with high update rate.

• •

Given the prior from the lidar-IMU odometry, a rotational constrained refinement method further optimizes the final poses and the generated point-cloud maps. It ensures a consistent and robust estimation, even in some lidar-degraded cases.

• •

The algorithm is verified with extensive indoor and outdoor tests. It outperforms the state-of-the-art lidar-only or loosely coupled lidar-IMU algorithms.

• •

The source code is available online 111https://sites.google.com/view/lio-mapping or https://ram-lab.com/file/hyye/lio-mapping. This is the first open-source implementation for tightly coupled lidar and IMU fusion available to the community.

The remainder of the paper is organized as follows. Sec. II presents a review of related works. Sec. III explains the notation and some preliminaries. The proposed odometry and refinement methods are presented in Sec. IV and V, respectively. Implemetation and tests are shown in Sec. VI and VII. Conclusion is presented in Sec. VIII.

II Related Works

There are several methods relating to the fusion of IMU and lidar measurements. One important category is loosely coupled fusion. Methods in this category consider the estimation of the lidar and the estimation of the IMU separately. In [1], the lidar odometry with IMU assistance relied on the orientation calculated by the IMU and assumed a zero velocity when using the acceleration. It decoupled the measurements of the lidar and IMU and mainly took the IMU as prior for the whole system, thus it could not utilize IMU measurements for further optimization. In [2], a loosely coupled extended Kalman filter (EKF) was used to fuse an IMU and lidar in the 2D case, but it could not handle 3D or more complex environments. Lynen et al. [3] presented a modular way to fuse IMU measurements with other relative pose measurements, e.g., from a camera, lidar or even pressure sensor, by an EKF in the 3D case. This loosely coupled method is of computational efficiency, but is less accurate than tightly coupled methods [4] since it takes the odometry part as a black box and does not update it with measurements from the IMU.

Tightly coupled methods are another important category. For 2D planar motion estimation, Soloviev et al. [5] proposed a method that extracted and matched lines among the 2D lidar scans, where the tilted lidar was compensated by the predicted orientation from the IMU. A Kalman filter was applied to correct the IMU states in the lidar measurement domain. Hemann et al. [6] proposed a method to tightly couple the IMU propagation and accumulated lidar heightmap in the form of an error-state Kalman filter. The corrections of the states are updated with the matching between the lidar heightmap and a priori digital elevation model (DEM). This method showed the ability in long-range GPS-denied navigation when the environments are known, but it could not work without prior map information. In [7] and [8], the raw measurements directly from the IMU and the predicted IMU measurements from the continuous trajectory were used for calculating the residuals to be optimized. The transition and estimation of the states were not involved in these methods, which made the systems unfeasible under fast motions even with an additional camera [9].

Inspired by other visual-inertial works [10, 11], we design our method under tightly coupled lidar-IMU fusion. We “pre-integrate” and use the raw IMU measurements with the lidar measurements to optimize the states within the whole system, which can work in laser degraded cases or when the motions are rapid. To the best of our knowledge, ours is one of the few 3D lidar-IMU fusion algorithms that are suited for complex 3D environments.

III Notations and Preliminaries

III-A Notations

We denote every line of measurement captured by the 3D lidar sensor as scan $\mathcal{C}$, and denote sweep $\mathcal{S}$ containing all the scans in one measurement. For example, a 16-line 3D lidar contains 16 scans in one sweep.

In the following sections, we denote the transformation matrix as $\mathbf{T}^{a}_{b}\in SE(3)$, which transforms the point $\mathbf{x}^{b}\in\mathbb{R}^{3}$ in the frame $\mathcal{F}_{\mathit{b}}$ into the frame $\mathcal{F}_{\mathit{a}}$. $\bar{\mathbf{T}}^{a}_{b}$ is the transform predicted by IMU. $\mathbf{R}^{a}_{b}\in SO(3)$ and $\mathbf{p}^{a}_{b}\in\mathbb{R}^{3}$ are the rotation matrix and the translation vector of $\mathbf{T}^{a}_{b}$, respectively. The quaternion $\mathbf{q}^{a}_{b}$ under Hamilton notation is used, which corresponds to $\mathbf{R}^{a}_{b}$. $\otimes$ is used for the multiplication of two quaternions. We use $\hat{\mathbf{a}}_{k}$ and $\hat{\bm{\omega}}_{k}$ to denote the raw measurements of the IMU at timestamp $k$. The extracted features are denoted as $\mathbf{F}_{a}$ in the original capture frame $\mathcal{F}_{a}$, which can be transformed into the frame $\mathcal{F}_{b}$ as $\mathbf{F}^{b}_{a}$.

III-B IMU Dynamics

III-B1 States

The body frame $\mathcal{F}_{\mathit{B_{i}}}$ and $\mathcal{F}_{\mathit{L_{i}}}$ are the reference of the IMU body and the reference of the lidar center, respectively, while obtaining the lidar sweep $\mathcal{S}_{i}$ at the discrete timestamp $i$. The states we will estimate are the IMU state $\mathbf{X}^{W}_{B_{i}}$ in the world frame $\mathcal{F}_{\mathit{W}}$ and the extrinsic parameters $\mathbf{T}^{L}_{B}$ between the lidar and the IMU sensors. In detail, we can write the IMU states at $i$ and the extrinsic parameters as

[TABLE]

where $\mathbf{p}^{W}_{B_{i}}$, $\mathbf{v}^{W}_{B_{i}}$ and $\mathbf{q}^{W}_{B_{i}}$ are the position, velocity and orientation of the body frame w.r.t. to the world frame, respectively, $\mathbf{b}_{a}$ is the IMU acceleration bias and $\mathbf{b}_{g}$ is the IMU gyroscope bias.

III-B2 Dynamic model

With the inputs from the IMU’s accelerator and gyroscope, we can update the preceding IMU state $\mathbf{X}^{W}_{B_{i}}$ to current IMU state $\mathbf{X}^{W}_{B_{j}}$ by discrete evolution, as shown in Eq. (2), where $\Delta t$ is the interval between two consecutive IMU measurements, and all the IMU measurements between the lidar sweeps’ times $k=i$ and $k=j$ are integrated. With slight abuse of notation, we use $k=j-1$ as the preceding IMU timestamp before $k=j$.

[TABLE]

where $\mathbf{g}^{W}$ is the gravity vector in the world frame. We use the shorthands $(\cdot)_{i}\doteq(\cdot)^{W}_{B_{i}}$, $\Delta t_{ij}=\sum_{k=i}^{j-1}\Delta t$ and $\prod_{k=i}^{j-1}$ as the sequences of quaternion multiplications for clarity.

III-B3 Pre-integration

The body motion between the timestamps $i$ and $j$ can be represented via pre-integration measurement $z^{i}_{j}=\left\{\Delta\mathbf{p}_{ij},\Delta\mathbf{v}_{ij},\Delta\mathbf{q}_{ij}\right\}$, which has the covariance $\mathbf{C}^{B_{i}}_{B_{j}}$ in the error-state model (see details in the supplementary material [12]).

IV Tightly coupled Lidar-IMU Odometry

To ensure efficient estimation, many works on lidar mapping, like [7], [1] and [13], separate the task into two parts, the odometry and the mapping. Inspired by these works, the proposed system comprises two parallel parts. The first part, introduced in Sec. IV, is the tightly coupled lidar-IMU odometry, which optimizes all the states within a local window. The second part, presented in Sec. V, is the rotation constrained refinement (leading to a globally consistent mapping process), which aligns the lidar sweeps to the global map using the information from the optimized poses and gravity constraints.

IV-A Lidar-IMU Odometry Overview

Fig. 1 provides a brief overview of our proposed lidar-IMU odometry. With the previous estimated states, we can use the current lidar raw input $\mathcal{S}_{j}$, and the IMU raw inputs $\mathcal{I}_{i,j}$, from the last timestamp $i$ to the current timestamp $j$, to have a new step of optimization for the states. The odometry estimation performs as follows:

1. Before $S_{j}$ arrives, the IMU states are updated via Eq. (2) iteratively.
2. Meanwhile, these inputs are “pre-integrated” as $\Delta\mathbf{p}_{ij}$, $\Delta\mathbf{v}_{ij}$ and $\Delta\mathbf{q}_{ij}$ to be used in the joint optimization.
3. When the latest lidar sweep $\mathcal{S}_{j}$ is received, de-skewing is applied on the raw data to obtain the de-skewed lidar sweep $\bar{\mathcal{S}}_{j}$ (Sec. IV-B).
4. Next, a feature extraction step is applied to reduce the dimension of the data and extract the most important feature points $\mathbf{F}_{L_{j}}$ (Sec. IV-B).
5. The previous lidar feature points $\mathbf{F}_{L_{o,i}}$ within the local window are merged as a local map $\mathbf{M}^{L_{p}}_{L_{o,i}}$, according to the previous corresponding optimized states $\mathbf{T}^{W}_{B_{o,i}}$ and $\mathbf{T}^{L}_{B}$ (Sec. IV-C).
6. With the predicted lidar pose for $\mathbf{F}_{j}$, we can find the relative lidar measurements $\mathbf{m}_{L_{p+1,j}}$ (Sec. IV-C).
7. The final step is joint non-linear optimization, taking the relative lidar measurements and IMU pre-integration to obtain a MAP estimation of the states within the local window (Sec. IV-D and IV-E). The optimized results are applied to update the prediction states in step 1), avoiding the drift from IMU propagation.

IV-B De-skewing and Feature Extraction

The 3D lidar has rotating mechanism inside to receive data for a whole circle. When the 3D lidar is moving, $\mathcal{S}_{j}$, the raw data from it, suffers from motion distortion, which makes the point in a sweep different from the true positions. To handle this problem, we use the prediction of lidar motion $\bar{\mathbf{T}}^{L_{j}}_{L_{j^{\prime}}}$ from IMU propagation and assume the linear motion model during the sweep. Then, every point $\mathbf{x}(t)\in\mathcal{S}_{j}\subset\mathbb{R}^{3}$ is corrected by linear interpolation of $\bar{\mathbf{T}}^{L_{j}}_{L_{j^{\prime}}}$ to obtain the deskewed sweep $\bar{\mathcal{S}}_{j}$ into the ending pose of the sweep, where $t\in\left(t_{j^{\prime}},t_{j}\right]$ is the timestamp of the point in the sweep, and $t_{j^{\prime}}$ and $t_{j}$ are the timstamps of the sweep start and end, respectively.

For computational efficiency, lidar feature extraction is required. Here, we are only interested in the points which are most alike on a plane or on an edge [7, 1], since these points can be extracted from every scan of a lidar sweep. Such feature points $\mathbf{F}_{L_{j}}$ in $\bar{\mathcal{S}}_{j}$ are selected by the curvature and distance changes, as are those in [1]; i.e., the most planar or edged points are selected.

IV-C Relative Lidar Measurements

With the fusion of the IMU and another sensor, which is capable to provide the relative pose of the sensor pair, the states to be estimated, $\mathbf{X}^{W}_{B}$ and $\mathbf{T}^{L}_{B}$, will be locally observable if we fix the first reference frame [14]. To properly incorporate pre-integration from the IMU, we propose using the relative lidar measurements, between sweeps to constrain the lidar poses, as Algorithm 1. Before finding the point correspondences, we build a local map, because the points in a single sweep are not dense enough to calculate accurate correspondences.

The local map contains the lidar feature points from $N_{m}$ discrete timestamps $\{o,\cdots,p,\cdots,i\}$, where $o$, $p$ and $i$ are the timestamps of the first lidar sweep within the window, the pivot lidar sweep and the last processed lidar sweep, respectively, as shown in Fig. 2. The local map $\mathbf{M}^{L_{p}}_{L_{o,i}}$ is built in the frame of the $pivot$ lidar sweep from the features $\mathbf{F}^{L_{p}}_{L_{\gamma}},\gamma\in\{o,\cdots,i\}$, which is transformed via the previous optimized lidar poses $\mathbf{T}^{L_{p}}_{L_{\gamma}}$ 222For simplification, we denote the predicted transform $\bar{\mathbf{T}}^{L_{p}}_{L_{j}}$ as $\mathbf{T}^{L_{p}}_{L_{j}}$ in this section, and $\mathbf{F}^{L_{p}}_{L_{j}}$ is transformed via $\bar{\mathbf{T}}^{L_{p}}_{L_{j}}$.. The to-be-estimated states are the ones at the $N_{s}$ timestamps $\{p+1,\cdots,i,j\}$, where $p+1$ and $j$ are the timestamp of the lidar sweep next to the $pivot$ one and the current lidar sweep in the window.

With the built local map, the correspondences can be found between $\mathbf{M}^{L_{p}}_{L_{o,i}}$ and the original $\mathbf{F}_{L_{\alpha}},\alpha\in\{p+1,\cdots,j\}$. We define such correspondence as relative lidar measurements, since they are relative to the $pivot$ pose, and the $pivot$ pose will change with the sliding window. The original features we extracted in Sec. IV-B are the most planar or edged points in $\mathcal{F}_{L_{\alpha}}$. In practice, we found that the edged points cannot improve the results of the lidar-IMU odometry. Thus, in the following, we only discuss the planar features. KNN is used for each transformed feature point $\mathbf{x}^{L_{p}}\in\mathbf{F}^{L_{p}}_{L_{\alpha}}$ to find the $\mathbf{K}$ nearest points $\pi(\mathbf{x}^{L_{p}})$ in $\mathbf{M}^{L_{p}}_{L_{o,i}}$. Then for the planar points, we fit these neighbor points into a plane in $\mathcal{F}_{L_{p}}$. The coefficient of a planar point can be solved by the linear equation defined by $\bm{\omega}^{T}\mathbf{x}^{\prime}+d=0,\mathbf{x}^{\prime}\in\pi(\mathbf{x}^{L_{p}})$, where $\bm{\omega}$ is the plane normal direction and $d$ is the distance to the origin of $\mathcal{F}_{L_{p}}$. We denote $m=\left[\mathbf{x},\bm{\omega},d\right]\in\mathbf{m}_{L_{\alpha}}$ for each planar feature point $\mathbf{x}\in\mathbf{F}_{L_{\alpha}}$ as one of the relative lidar measurements. To be mentioned, in each relative lidar measurement $m\in\mathbf{m}_{L_{\alpha}}$, $\mathbf{x}$ is defined in $\mathcal{F}_{L_{\alpha}}$, and $\bm{\omega}$ and $d$ are defined in $\mathcal{F}_{L_{p}}$.

IV-D Lidar Sweep Matching

The relative lidar measurements can provide relative constraints between the $pivot$ lidar pose and the following lidar poses. Our method optimizes all the poses in the optimization window, including the first pose $\mathbf{T}^{W}_{L_{p}}$, i.e., $\mathcal{F}_{L_{p}}$ is not fixed. Thus, each item in the lidar cost function involves the poses of two lidar sweeps, $\mathbf{T}^{W}_{L_{p}}$ and $\mathbf{T}^{W}_{L_{\alpha}},\alpha\in\{p+1,\cdots,j\}$. Optimizing the $pivot$ pose will help to minimize the pre-integration error better and ensure the sensor pair align with gravity. The states we estimate are those of the IMU; thus we need to introduce extrinsic parameters to represent the lidar constraints by IMU states. The relative transformation from the latter lidar poses to the $pivot$ one in the window can be defined as

[TABLE]

With the previous correspondences, the residual for each relative lidar measurement $m=\left[\mathbf{x},\bm{\omega},d\right]\in\mathbf{m}_{L_{\alpha}},\alpha\in\{p+1,\cdots,j\}$ can be represented as point-to-plane distance

[TABLE]

IV-E Optimization

To obtain the optimized states, a fixed-lag smoother and marginalization are applied. The fixed-lag smoother keeps $N_{s}$ IMU states in the sliding window, from $\mathbf{X}^{W}_{B_{p}}$ to $\mathbf{X}^{W}_{B_{j}}$, as shown in Fig. 2. The sliding window helps to bound the amount of computation. When new measurement constraints come, the smoother will include the new states and marginalize the oldest states in the window. The whole of the states to be estimated in detail is

[TABLE]

Then the following cost funtion with a Mahalanobis norm is minimized to obtain the MAP estimation of the states $\mathbf{X}$,

[TABLE]

where $\mathbf{r}_{\mathcal{P}}(\mathbf{X})$ is the prior items from marginalization. $\mathbf{r}_{\mathcal{L}}(m,\mathbf{X})$ is the residual of the relative lidar constraints and $\mathbf{r}_{\mathcal{B}}(z^{\beta}_{\beta+1},\mathbf{X})$ is the residual of the IMU constraints. The cost function in the form of a non-linear least square can be solved by the Gauss-Newton algorithm, which takes the form $\mathbf{H}\delta\mathbf{X}=-\mathbf{b}$. We use Ceres Solver [15] to solve the problem.

The lidar constraint $\mathbf{r}_{\mathcal{L}}(m,\mathbf{X})$ can be derived from Eq. (4) for each relative lidar measurement. The covariance matrix $\mathbf{C}^{m}_{L_{\alpha}}$ is determined by the lidar accuracy. Similar to the one in [10], IMU constraint $\mathbf{r}_{\mathcal{B}}(z^{\beta}_{\beta+1},\mathbf{X})$ can be obtained from the states and IMU pre-integration, $\left\lVert\mathbf{r}_{\mathcal{P}}(\mathbf{X})\right\rVert^{2}=\mathbf{b}^{T}_{\mathcal{P}}\mathbf{H}^{+}_{\mathcal{P}}\mathbf{b}_{\mathcal{P}}$ can be obtained by Schur complement (detailed in [12]).

V Refinement with Rotational Constraints

Registering the feature points to a global map, instead of local maps, can constrain the lidar poses to a consistent world frame $\mathcal{F}_{W}$. Our refinement method uses the relative lidar measurements $\mathbf{m}_{L}$ as the ones in Sec. IV. Since the global map is a by-product of the refinement, we also refer to it as a mapping method. A cost function to align the latest lidar feature points with the global map can be formed as

[TABLE]

where $\mathbf{T}=\mathbf{T}^{W}_{L}$ is the latest to-be-estimated lidar pose, and $m$ is the relative lidar measurement with the feature point $\mathbf{x}$ in $\mathcal{F}_{L}$ and the coefficients $\bm{\omega},d$ defined in $\mathcal{F}_{W}$. Then we can use a similar Gauss-Newton method to minimize $\mathbf{C}_{\mathcal{M}}$. The optimization is carried out by the residual $\mathbf{C}_{\mathcal{M}}$ and Jacobians $\mathbf{J}^{\mathbf{C}}_{\mathbf{p}}$ and $\mathbf{J}^{\mathbf{C}}_{\bm{\theta}}$, where $\bm{\theta}$ is the error state of the corresponding quaternion $\mathbf{q}$. However, with the accumulated rotation error and after long-term operation, the merged global map cannot align with gravity accurately. This can lead further mapping to wrongly align with a tilted map. Inspired by [16] which optimizes SE(3) with SE(2)-constraints, we propose a constrained mapping strategy. This strategy utilizes the rotational constraints from the lidar-IMU odometry, which ensures the final map always aligns with gravity. Fig. 3 illustrates the structure of the rotationally constrained mapping.

Given the property that the orientation along the $z$-axis has higher uncertainty, and that the other two DoF of the orientation are much more close to the true value, we can constrain the cost function by modifying the Jacobian of the orientation as (detailed derivations in [12]),

[TABLE]

where $\breve{(\cdot)}$ denotes the estimation of the state in the last iteration, and $\breve{\mathbf{\Omega}}_{z}$ is an approximiation of the information matrix of the orientation w.r.t $\mathcal{F}_{W}$, and $\epsilon_{x}$ and $\epsilon_{y}$ can be obtained by the information ratio of the $x$- and $y$-axes orientation to the $z$-axis orientation in $\mathcal{F}_{W}$.

After that, we use $\mathbf{J}^{\mathbf{C}}_{\mathbf{p}}$ and $\mathbf{J}^{\mathbf{C}}_{\bm{\theta}_{z}}$ as the Jacobians instead, and these are needed for the optimization step. The incremental lidar poses can be obtained as $\delta\bm{\theta}_{z}$ and $\delta\mathbf{p}$, which lead to the updated lidar states $\tilde{\mathbf{p}}$ and $\tilde{\mathbf{q}}$

[TABLE]

VI Implementation

Different sensor configurations, system initialization and different parameters for indoor and outdoor tests are introduced in this section.

VI-A Different Sensor Configurations

Sensor pairs can be configured differently. For a hand-held sensor pair, such as the one in Fig. 4(a), the lidar and IMU are close to each other. Thus the pipeline remains the same as what was introduced before. But for sensor pairs mounted on cars, the two sensors are usually farther away from each other. For example, Fig. 4(b) shows an IMU mounted above the car’s base link, while the lidar is mounted at the front of the car. Instead of auto-calibrating all translation parameters, a prior item for the extrinsic translational parameters is added to Eq. (6) for the tests on the cars.

VI-B Initialization

At first, there are no estimated poses for the lidar feature points. Therefore, roughly accurate matching algorithms are needed. We adopt [1]’s lidar odometry in the initialization step. With the provided lidar poses, sufficient motions of the sensor pair are required to make the IMU states observable [14]. Then the poses of the lidar and the IMU measurements are used to initialize the IMU states, which can be solved by the methods introduced in [10] and [17]. For the tests in Sec. VII, we follow the initialization method in [10], which also linearly initializes the extrinsic parameters. Next, with the initial states and the newly coming measurements, the non-linear optimization within local windows will be carried out iteratively to estimate the states.

VII Tests and Analyses

Indoor and outdoor tests are conducted to evaluate our method. The quantitative and qualitative results are provided in the following sections.

VII-A Quantitative Analysis

To quantitatively analyze our method, the sensor pair shown in Fig. 4(a) is used. A Velodyne VLP-16 lidar with 16 lines is mounted above an Xsens MTi-100 IMU. The reflective markers can provide the ground-truth poses using the motion capture system. The lidar is configured to have a 10Hz update rate, and IMU updates at 400Hz. The estimated trajectories from different methods are aligned with the ground-truth using [18].

VII-A1 Tests under Different Motion Conditions

Table I shows the root mean square error (RMSE) results under different motion speeds and different methods, where LOAM [1] is regarded as the baseline. LIO is our local window optimized odometry method. LIO-raw and LIO-no-ex are the same as LIO expect that the motion compensation or the online extrinsic parameter estimation is cut off, respectively. LIO-mapping is from the results of the mapping with rotaional constraints. The two best results are shown in bold.

From the results, we see that LIO-mapping can always provide accurate estimation of translational (position) and rotational (orientation) states in all cases. LIO has better performance when motion is faster, which produces more IMU excitation. But it suffers from drift if the motion is slow, since the local map is relatively sparse at this time. The table also shows that with motion compensation and online extrinsic parameter estimation, LIO can provide better performance, especially when motions are rapid.

VII-A2 Tests of Drift over Time

To evaluate how the error changes with time, we test the algorithms in a longer test. The first 50 estimated poses are aligned with the ground-truth. The final trajectories from the different methods are shown in Fig. 5, and the translational and rotational errors are shown in Fig. 6. The results show that LIO can provide relatively accurate poses and constrain $roll$ and $pitch$ close to the ground-truth, but it suffers from drift. Neither the method without IMU fusion (LOAM) nor the one with loosely coupled fusion (LOAM+IMU) can provide robust estimation when the motion becomes rapid (in the latter half of the test). LIO-mapping benefits from rotational constraints provided by LIO, and further registers the current sweep to the global map. Thus, it results in less drift of the trajectory and greater consistency of the state estimation.

VII-B Qualitative Results

Several tests with different sensor configurations and environments are carried out in order to show the improvements in challenging scenarios, including indoor hand-held and outdoor campus golf cart tests, with the configurations as shown in Fig. 4, and tests on the KAIST Urban dataset [19]. Due to the limited space, these pose estimation and mapping results are shown in the supplementary video.

VII-C Running Time Analysis

We run this test with an Intel i7-7700K CPU at 4.20GHz, 16GB RAM. The lidar intervals vary indoors (0.2s) and outdoors (0.3s), depending on the number of feature points in a sweep (typically more feature points outdoors, around 3000, than indoors, 1000). These intervals help to build larger maps and skip some of the lidar sweeps to fulfill real-time computation. The mean running time of our method can be found in Table II using the data from a 16-line 3D lidar. The time stands for the processing time of each of the new inputs for a module, i.e., raw IMU measurements, lidar measurements and odometry outputs. Note that the odometry and mapping are in different threads. The mapping thread processes the outputs from the odometry thread. The prediction of the IMU is operated based on the optimized states, alongside the optimization. Thus, it can run as fast as the output rate of the IMU.

VIII Conclusion

A novel tightly coupled lidar-IMU fusion method was presented. It comprised the state optimization for the odometry and the refinement with rotational constraints. The results showed that our method outperformed the state-of-the-art lidar-only method and loosely coupled methods.

Despite the limitation that the proposed method requires initialization, our method indeed showed robust pose estimations results with fast update rate, even under challenging test scenarios, e.g. fast-motion cases, lidar-degraded cases and lidar sweeps with limited overlapping, empowered by sufficient IMU excitation.

Acknowledgements

This work was supported by the National Natural Science Foundation of China (Grant No. U1713211); partially supported by the HKUST Project IGN16EG12 and Shenzhen Science, Technology and Innovation Comission (SZSTI) JCYJ20160428154842603, awarded to Prof. Ming Liu.