IMU-Aided High-Frequency Lidar Odometry for Autonomous Driving

Abstract

For autonomous driving, it is important to obtain precise and high-frequency localization information. This paper proposes a novel method in which the Inertial Measurement Unit (IMU), wheel encoder, and lidar odometry are utilized together to estimate the ego-motion of an unmanned ground vehicle. The IMU is fused with the wheel encoder to obtain the motion prior, and it is involved in three levels of the lidar odometry: Firstly, we use the IMU information to rectify the intra-frame distortion of the lidar scan, which is caused by the vehicle’s own movement; secondly, the IMU provides a better initial guess for the lidar odometry; and thirdly, the IMU is fused with the lidar odometry in an Extended Kalman filter framework. In addition, an efficient method for hand–eye calibration between the IMU and the lidar is proposed. To evaluate the performance of our method, extensive experiments are performed and our system can output stable, accurate, and high-frequency localization results in diverse environment without any prior information.

1. Introduction

Precise and high-frequency localization is one of the key problems for autonomous vehicles. In recent years, the fusion of Global Navigation Satellite System (GNSS) and Inertial Measurement Unit (IMU) has been the most popular localization method. However, the GNSS/IMU system will fail in some environments where GNSS signals suffer from satellite blockage or multipath propagation [1], such when an autonomous vehicle is driving in a tunnel, in the mountains, or in an environment with electromagnetic interference. Thus, an alternative localization method is necessary when the GNSS signal is unavailable or is of poor quality.

Currently, most autonomous vehicles are equipped with light detection and ranging (lidar) device, which is a promising sensor that could accurately calculate the range measurements of the surroundings. Some recent navigation approaches have begun to use lidar as a complementary sensor for the GNSS/IMU localization system. A typical lidar odometry algorithm could roughly be divided into three steps: a pre-processing step, which tries to compensate the intra-frame distortion caused by the vehicle’s own movement; an intermediate step, which is the main body of the lidar odometry algorithm, and a last step, which outputs the localization result.

As the lidar mostly spins at 10 Hz, its output frequency is thus less than 10 Hz, which might not be fast enough to meet the needs of other modules, like planning or control. To remedy this, some approaches try to combine the lidar with other sensors, such as IMU. In these approaches, the IMU and the lidar odometry are usually combined in an Extended Kalman Filter (EKF) framework, or by using a factor graph representation [2,3,4,5,6,7] (as shown in the left of Figure 1). In this paper, we claim that IMU information can not only be fused with the lidar odometry at the output level, but it can also aid lidar odometry in the pre-processing level and the intermediate level (as shown in the right of Figure 1).

In the pre-processing step, as the vehicle itself is moving during the lidar spin, the generated point clouds will be distorted. This intra-frame distortion is usually ignored. In this paper, we show that the high-frequency IMU information could naturally help correct this distortion, as long as the IMU and lidar are well-calibrated. We also claim that the problem of intra-frame correction is in fact equivalent to inter-frame registration, and propose an interesting two-step correction procedure which does not rely on the IMU. In the intermediate step, the IMU could also provide a good initial guess for the lidar odometry algorithm. Past works usually use the registration result from the previous two scans as the initial guess. We will do experiments to compare these two approaches. In the output step, we try to combine the lidar odometry with the IMU in the EKF framework.

In summary, we make the following contributions in this paper:

• We propose an efficient method for hand–eye calibration between the IMU and the lidar, which is a pre-requisite for making good use of the IMU.
• In the pre-processing step of the lidar odometry algorithm, we emphasize that the intra-frame correction is equivalent to inter-frame registration, and propose an efficient method for utilizing the IMU information to rectify the distorted point clouds.
• In the intermediate step, we use IMU to obtain the initial guess for the lidar odometry algorithm, and compare this approach with previous methods which usually use the registration results from the previous two scans as the initial guess.
• By combining the lidar odometry with the IMU information, we obtain a complete localization system which can output stable, accurate, and high-frequency localization results at a frequency of around 40 Hz.

The rest of this paper is organized as follows. In Section 2, we introduce some related works. Section 3.1 provides an overview of the proposed approach, and introduces the coordinate systems and notations in our approach. The details of the proposed approach are presented from Section 3.2, Section 3.3, Section 3.4, Section 3.5 and Section 3.6. The experimental results and comparison with state-of-the-art approaches are presented in Section 4. Finally, the conclusions are summarized in Section 5.

2. Related Works

Lidar odometry has been a hot topic in recent years. Lidar odometry is, in essence, equivalent to scan matching, which tries to estimate the relative transform between two frames of point clouds. The scan matching methods can be classified into three categories: point-based, mathematical property-based, and feature-based. The Iterative Closest Point (ICP) algorithm [8] and its variants [9] are the most popular point-based methods which estimate the relative transform through finding the nearest points between the two scans. For the mathematical property-based method, such as the Normal Distribution Transform (NDT) algorithm and its variants (D2D-NDT [10] and P2D-NDT [11]), the observed points are modeled as a set of Gaussian probability distribution. For the feature-based method, there have been various features applied, such as the line segment feature [12], corner feature [13], and grid feature [14]. Zhang et al. proposed a method called Lidar Odometry And Mapping (LOAM) [15,16], which extracts feature points located on sharp edges or planar surfaces, and matches these feature points using either the point-to-line distance or the point-to-plane distance.

All these lidar odometry algorithms can only output localization results at a frequency of less than 10 Hz, which might not be fast enough. Some recent approaches have started to fuse the lidar odometry with the IMU using either the Kalman Filter framework [2,3,4,5,17,18] or the factor graph representation [6,7].

In [2], the authors proposed a localization system which combines a 2D Simultaneous Localization and Mapping (SLAM) subsystem with a 3D navigation subsystem based on IMU. This system can estimate the accurate state of an unmanned ground vehicle (UGV), but the usage of 2D SLAM is not suitable for the off-road environment. In [3], a hybrid lidar odometry algorithm was proposed which combines the feature-based scan matching method with the ICP-based scan matching method, and the lidar odometry result was then fused with the IMU in an EKF framework. In [6], a bunch of IMU measurements were considered as a relative constraint using the pre-integration theory, and the results of lidar odometry were fused with IMU measurements in a factor graph framework. However, in all these previous works, the IMU information was only fused with the lidar odometry in the result level; whilst in this paper, we try to utilize the IMU information to aid the whole process of the lidar odometry algorithm.

3. The Proposed Approach

3.1. Coordinate Systems and Notations

The aim of this work is to accurately estimate the pose of the vehicle. Three coordinate systems are used in our approach, which are defined as follows:

• World Coordinate System $\left\{W\right\}$. In $\left\{W\right\}$, we define the coordinates of a point by latitude, longitude, and altitude, the x-axis points to the east, the y-axis points to the north, and the z-axis points to the sky, following the right-hand rule. This coordinate system is defined in a global scale and will never change.
• Body Coordinate System $\left\{B\right\}$. This coordinate is fixed with the IMU, which is installed in the center of two rear wheels of the vehicle. The x-axis points to the right, the y-axis points to the forward, and the z-axis points to the upward, following the right-hand rule.
• Lidar coordinate system $\left\{L\right\}$. The lidar is mounted on top of the vehicle, and the origin of $\left\{L\right\}$ is located at the center of the lidar. The three axes of this coordinate system are the same as in $\left\{B\right\}$.

Figure 2 shows the coordinate system $\left\{W\right\}$ and $\left\{B\right\}$ in the 2D plane and the relationship between them. The body coordinate system $\left\{B\right\}$ changes with the movement of the vehicle. ${\mathit{T}}_{\mathrm{k}}^{\mathrm{WB}}$ represents the transformation between the coordinate system $\left\{W\right\}$ and $\left\{B\right\}$ at time $t_{\mathrm{k}}$, which can also denote the pose of the vehicle at time $t_{\mathrm{k}}$. On the contrary, ${\mathit{T}}_{\mathrm{k}}^{\mathrm{BW}}$ transforms the coordinate system $\left\{B\right\}$ to $\left\{W\right\}$ and ${\mathit{T}}_{\mathrm{k}}^{\mathrm{BW}}={\left({\mathit{T}}_{\mathrm{k}}^{\mathrm{WB}}\right)}^{-1}$. Moreover, $\Delta {\mathit{T}}_{\mathrm{k}-1,\mathrm{k}}^{\mathrm{B}}$, which could be obtained from the IMU, represents the relative transformation of the vehicle between time $t_{\mathrm{k}-1}$ and $t_{\mathrm{k}}$. The relationship between $\Delta {\mathit{T}}_{\mathrm{k}-1,\mathrm{k}}^{\mathrm{B}}$ and ${\mathit{T}}_{\mathrm{k}}^{\mathrm{WB}}$ can be expressed as:

$$\Delta {\mathit{T}}_{\mathrm{k}-1,\mathrm{k}}^{\mathrm{B}}={\left({\mathit{T}}_{\mathrm{k}-1}^{\mathrm{WB}}\right)}^{-1}{\mathit{T}}_{\mathrm{k}}^{\mathrm{WB}}.$$

(1)

In addition, the relationship between the coordinate system $\left\{B\right\}$ and $\left\{L\right\}$ is shown in Figure 3.

We use ${\mathit{T}}_{\mathrm{k}}^{\mathrm{WL}}$ to denote the pose of the lidar at time $t_{\mathrm{k}}$. $\Delta {\mathit{T}}_{\mathrm{k}-1,\mathrm{k}}^{\mathrm{L}}$ represents the relative transformation of the lidar between time $t_{\mathrm{k}-1}$ and $t_{\mathrm{k}}$. The transformation ${\mathit{T}}^{\mathrm{BL}}$, which transforms the coordinate system $\left\{B\right\}$ to $\left\{L\right\}$, can be obtained from the hand–eye calibration.

The transformation $\mathit{T}$ can be expressed by:

$$\mathit{T}=\left[\begin{array}{cc}\mathit{R}& \mathit{p}\\ {\mathbf{0}}^T& 1\end{array}\right],$$

(2)

where $\mathit{R}\in {\mathbb{R}}^{3\times 3}$ represents the rotational matrix and $\mathit{p}\in {\mathbb{R}}^3$ is the translational vector.

In Lie Group [19], $\mathit{T}$ belongs to the Special Euclidean group $\mathrm{SE}\left(3\right)$ and the rotational matrix $\mathit{R}$ belongs to the Special Orthogonal group $\mathrm{SO}\left(3\right)$. Correspondingly, the $\mathfrak{se}\left(3\right)$ and $\mathfrak{so}\left(3\right)$ are two kinds of Lie Algebra. $\mathit{\theta }\in \mathfrak{so}\left(3\right)$ indicates the rotation angle which can be expressed as $\mathit{\theta }=\theta \mathit{n}$, where $\theta$ represents the magnitude of the rotation angle and $\mathit{n}$ denotes the rotation axis. The pose can thus be represented by $\mathit{\xi }={\left[\begin{array}{cc}\mathit{\theta }& \mathit{p}\end{array}\right]}^T\in \mathfrak{se}\left(3\right)$.

The exponential map and logarithmic map can be used to derive the relationship between the Lie Group and the Lie Algebra. For example, $\mathit{T}$ can be represented by the exponential map of the $\mathit{\xi }$, namely, $\mathit{T}=\exp \left({\mathit{\xi }}^{\wedge }\right)$. Conversely, $\mathit{\xi }$ can be represented by the logarithmic map of the $\mathit{T}$, namely, $\mathit{\xi }=\log {\left(\mathit{T}\right)}^{\vee }$.

The exponential map between $\mathit{\theta }$ and $\mathit{R}$ can be formulated by using the Rodriguez formula:

$$\mathit{R}=\cos \theta ·\mathit{I}+\left(1-\cos \theta \right)·\mathit{n}·{\mathit{n}}^T+\sin \theta ·{\mathit{n}}^{\wedge }.$$

(3)

The operator ∧ maps a vector to a skew-symmetric matrix as Equation (4), and the operator ∨ maps a skew symmetric matrix back to a vector.

$${\mathit{\theta }}^{\wedge }={\left[\begin{array}{c}{\theta }^x\\ {\theta }^y\\ {\theta }^z\end{array}\right]}^{\wedge }=\left[\begin{array}{ccc}0& -{\theta }^z& {\theta }^y\\ {\theta }^z& 0& -{\theta }^x\\ -{\theta }^y& {\theta }^x& 0\end{array}\right].$$

(4)

The notation of the data used in our system are specified as follows. Each type of data used in our system is composed of the timestamp and the data itself. The timestamp is defined as the time when the data is generated. $t^{\mathrm{B}}$ denotes the timestamp of the IMU measurement and $t^{\mathrm{L}}$ represents the timestamp of the lidar point clouds. ${\mathit{f}}_{\mathrm{k}}^{\mathrm{L}}$ indicates all points generated from the lidar during $[t_{\mathrm{k}-1}^{\mathrm{L}},t_{\mathrm{k}}^{\mathrm{L}}]$. In addition, the i-th point generated by the b-th laser beam is denoted as ${\mathit{X}}_{(\mathrm{i},\mathrm{b},\mathrm{k})}^{\mathrm{L}}$ in the lidar coordinate system $\left\{L\right\}$. The number of points received per laser beam is denoted as $S_b$ and $i\in [1,S_b]$.

3.2. Motion Prior from IMU and Wheel Encoder

It is well-known that the accelerometer in the IMU is affected by the gravity, and it needs to be integrated twice to obtain the translational vector [20]. In contrast, the gyro in the IMU directly measures the angular velocity, and it only needs to be integrated once to obtain the rotation angle. It is reasonable to believe that the angular output from the IMU is much more accurate than the translational measurement.

For the ground vehicle, besides IMU, it is also equipped with the wheel encoder. The wheel encoder directly measures the rotation of the wheel, and thus provides a much more accurate translational measurement.

In this paper, we try to combine the angular output from the IMU and the translational output from the wheel encoder to obtain the motion prior.

The motion model of the vehicle can be simplified as Figure 4. We mount the IMU at the center of two rear wheels, and mount two wheel encoders on the left and right rear wheels, respectively. The black car denotes the position of the vehicle at the previous moment $t_{\mathrm{k}-1}^{\mathrm{B}}$, and the blue car indicates its position at the current time $t_{\mathrm{k}}^{\mathrm{B}}$. $\Delta sl$ and $\Delta sr$ represent the traveled distances of the left rear wheel and the right rear wheel during the short period of time.

Firstly, we compute the rotation matrix ${\mathit{R}}_{\mathrm{k}}^{\mathrm{WB}}$ by triaxial angular velocities of the IMU. We introduce the following kinematic model [21]:

$$\frac{\mathrm{d}{\mathit{R}}_{\tau }^{\mathrm{BW}}}{\mathrm{d}t}={\mathit{R}}_{\tau }^{\mathrm{BW}}{\left[\begin{array}{c}{\omega }_{\tau }^x\\ {\omega }_{\tau }^y\\ {\omega }_{\tau }^z\end{array}\right]}^{\wedge }.$$

(5)

In Equation (5), ${\mathit{R}}_{\tau }^{\mathrm{BW}}$ indicates the rotation matrix from the coordinate system $\left\{B\right\}$ to $\left\{W\right\}$. ${\omega }_{\tau }^x$, ${\omega }_{\tau }^y$, and ${\omega }_{\tau }^z$ represent the instantaneous angular velocities of the IMU.

By assuming the angular velocity of the IMU constant during $[t_{\mathrm{k}-1}^{\mathrm{B}},t_{\mathrm{k}}^{\mathrm{B}}]$, we integrate Equation (5) from $t_{\mathrm{k}-1}^{\mathrm{B}}$ to $t_{\mathrm{k}}^{\mathrm{B}}$ and solve the differential equation. The rotation matrix ${\mathit{R}}_{\mathrm{k}}^{\mathrm{BW}}$ can be obtained by:

$${\mathit{R}}_{\mathrm{k}}^{\mathrm{BW}}={\mathit{R}}_{\mathrm{k}-1}^{\mathrm{BW}}\mathrm{Exp}\left({\left({\mathit{\omega }}_{\mathrm{k}}\Delta t\right)}^{\wedge }\right).$$

(6)

To compute the translational component ${\mathit{p}}_{\mathrm{k}}^{\mathrm{WB}}$ of the pose ${\mathit{T}}_{\mathrm{k}}^{\mathrm{WB}}$, we approximate the traveled distance $\Delta s$ of the vehicle to be the average of the two rear wheels. $\Delta s$ indicates the relative displacement of the vehicle during $[t_{\mathrm{k}-1}^{\mathrm{B}},t_{\mathrm{k}}^{\mathrm{B}}]$ in the body coordinate system $\left\{B\right\}$ and the instantaneous speed of the vehicle can be denoted as $v=\Delta s/(t_{\mathrm{k}}^{\mathrm{B}}-t_{\mathrm{k}-1}^{\mathrm{B}})$. We assume the velocity of the vehicle to be constant during $[t_{\mathrm{k}-1}^{\mathrm{B}},t_{\mathrm{k}}^{\mathrm{B}}]$, where the wheel encoder always records the distance traveled along the heading direction of the vehicle. Thus, the instantaneous velocity vector can be denoted as ${\mathit{v}}_{\mathrm{k}}^{\mathrm{B}}={\left[v00\right]}^T$. The relationship between the velocity vector ${\mathit{v}}_{\mathrm{k}}^{\mathrm{W}}$ in the coordinate system $\left\{W\right\}$ and the velocity vector ${\mathit{v}}_{\mathrm{k}}^{\mathrm{B}}$ in the coordinate system $\left\{B\right\}$ is:

$${\mathit{v}}_{\mathrm{k}}^{\mathrm{W}}={\mathit{R}}_{\mathrm{k}}^{\mathrm{WB}}{\mathit{v}}_{\mathrm{k}}^{\mathrm{B}}.$$

(7)

Using ${\mathit{p}}_{\mathrm{k}-1}^{\mathrm{WB}}$, ${\mathit{R}}_{\mathrm{k}}^{\mathrm{WB}}$, and ${\mathit{v}}_{\mathrm{k}}^{\mathrm{W}}$, the translational component ${\mathit{p}}_{\mathrm{k}}^{\mathrm{WB}}$ can be computed as:

$$\begin{array}{cc}\hfill {\mathit{p}}_{\mathrm{k}}^{\mathrm{WB}}& ={\mathit{p}}_{\mathrm{k}-1}^{\mathrm{WB}}+{\mathit{v}}_{\mathrm{k}}^{\mathrm{W}}\left(t_{\mathrm{k}}-t_{\mathrm{k}-1}\right)\hfill \\ & ={\mathit{p}}_{\mathrm{k}-1}^{\mathrm{WB}}+{\mathit{R}}_{\mathrm{k}}^{\mathrm{WB}}{\left[\begin{array}{ccc}\Delta s& 0& 0\end{array}\right]}^T.\hfill \end{array}$$

(8)

Since both the IMU and the wheel encoder output data at a frequency of 100 Hz, we use two separate buffers to cache these two kinds of data. These two sources of information are then fused by Equations (6) and (8). This fusion process only involves a few operators, and usually takes less than 1 millisecond. Therefore, the fusion output, which we term as the motion prior data, is also generated at 100 Hz.

3.3. Pre-Processing Step 1: Lidar-IMU Hand–Eye Calibration

As described before, the lidar point clouds are represented in the lidar coordinate system $\left\{L\right\}$. Taking the lidar point clouds of two frames ${\mathit{f}}_{\mathrm{k}-1}^{\mathrm{L}}$ and ${\mathit{f}}_{\mathrm{k}}^{\mathrm{L}}$ as inputs, we can calculate $\Delta {\mathit{T}}_{\mathrm{k}-1,\mathrm{k}}^{\mathrm{L}}$ by the lidar odometry algorithm. $\Delta {\mathit{T}}_{\mathrm{k}-1,\mathrm{k}}^{\mathrm{L}}$ indicates the relative transformation in the coordinate system $\left\{L\right\}$. However, the $\Delta {\mathit{T}}_{\mathrm{k}-1,\mathrm{k}}^{\mathrm{B}}$ obtained from the IMU is expressed in the body coordinate system $\left\{B\right\}$. Therefore, in order to utilize the IMU information, we need to establish the relationship between $\left\{L\right\}$ and $\left\{B\right\}$ first.

The calculation of the relative transformation ${\mathit{T}}^{\mathrm{BL}}$ between $\left\{L\right\}$ and $\left\{B\right\}$ is a typical hand–eye calibration problem. Based on ${\mathit{T}}^{\mathrm{BL}}$, we can transform a point from the lidar coordinate system $\left\{L\right\}$ to the body coordinate system $\left\{B\right\}$:

$${\mathit{X}}_{(\mathrm{i},\mathrm{b},\mathrm{k})}^{\mathrm{B}}={\mathit{T}}^{\mathrm{BL}}{\mathit{X}}_{(\mathrm{i},\mathrm{b},\mathrm{k})}^{\mathrm{L}}.$$

(9)

According to [22], we can formulate the hand–eye calibration problem between the coordinate system $\left\{L\right\}$ and $\left\{B\right\}$ as:

$$\Delta {\mathit{T}}_{\mathrm{k},\mathrm{k}-1}^{\mathrm{B}}{\mathit{T}}^{\mathrm{BL}}={\mathit{T}}^{\mathrm{BL}}\Delta {\mathit{T}}_{\mathrm{k},\mathrm{k}-1}^{\mathrm{L}}.$$

(10)

$\Delta {\mathit{T}}_{\mathrm{k}-1,\mathrm{k}}^{\mathrm{B}}$ and $\Delta {\mathit{T}}_{\mathrm{k}-1,\mathrm{k}}^{\mathrm{L}}$ are inputs of Equation (10). Let the timestamps of frames ${\mathit{f}}_{\mathrm{k}}^{\mathrm{L}}$ and ${\mathit{f}}_{\mathrm{k}-1}^{\mathrm{L}}$ be ${\tau }_{\mathrm{k}}^{\mathrm{L}}$ and ${\tau }_{\mathrm{k}-1}^{\mathrm{L}}$, respectively. We firstly find the nearest timestamps ${\tau }_{\mathrm{k}}^{\mathrm{B}}$ and ${\tau }_{\mathrm{k}-1}^{\mathrm{B}}$ from the IMU, then ${\mathit{T}}_{\mathrm{k}}^{\mathrm{WB}}$ and ${\mathit{T}}_{\mathrm{k}-1}^{\mathrm{WB}}$ can be obtained. The relative transformation $\Delta {\mathit{T}}_{\mathrm{k}-1,\mathrm{k}}^{\mathrm{B}}$ can be computed by Equation (1). We then use $\Delta {\mathit{T}}_{\mathrm{k}-1,\mathrm{k}}^{\mathrm{B}}$ as the initial guess and $\Delta {\mathit{T}}_{\mathrm{k}-1,\mathrm{k}}^{\mathrm{L}}$ can be obtained from the lidar odometry.

Equation (10) can be solved by calculating the rotational and translational component separately. The rotational component is:

$$\Delta {\mathit{R}}_{\mathrm{k},\mathrm{k}-1}^{\mathrm{B}}{\mathit{R}}^{\mathrm{BL}}={\mathit{R}}^{\mathrm{BL}}\Delta {\mathit{R}}_{\mathrm{k},\mathrm{k}-1}^{\mathrm{L}},$$

(11)

and the translational component is:

$$\Delta {\mathit{R}}_{\mathrm{k},\mathrm{k}-1}^{\mathrm{B}}{\mathit{p}}^{\mathrm{BL}}+\Delta {\mathit{p}}_{\mathrm{k},\mathrm{k}-1}^{\mathrm{B}}={\mathit{R}}^{\mathrm{BL}}\Delta {\mathit{p}}_{\mathrm{k},\mathrm{k}-1}^{\mathrm{L}}+{\mathit{p}}^{\mathrm{BL}}.$$

(12)

There are many methods to solve Equations (11) and (12). According to [23], the method proposed by Tsai et al. [24] exhibits the best performance. We therefore chose to use this method.

In our approach, multiple pairs of transformation $\Delta {\mathit{T}}_{\mathrm{i},\mathrm{i}-1}^{\mathrm{L}}$ and $\Delta {\mathit{T}}_{\mathrm{i},\mathrm{i}-1}^{\mathrm{B}}$ are needed. To reduce the error of hand–eye calibration [25,26], the following requirements need to be met:

• To avoid the intra-frame distortion caused by the vehicle’s own movement, the frames should be chosen when the vehicle is stationary.
• The relative rotation angle of the two frames needs to be greater than 90 degrees, and the relative translation distance should be small.
• The rotational axes for these multiple transformation pairs should not be parallel.

After completing the hand–eye calibration, each point of the frame ${\mathit{f}}_{\mathrm{k}}^{\mathrm{L}}$ can be transformed by Equation (9) and be represented in the coordinate system $\left\{B\right\}$.

3.4. Pre-Processing Step 2: Intra-Frame Correction

As mentioned in Section 3.1, it usually takes 100 ms for the lidar to spin a full circle. During this time, the vehicle also keeps moving. Some of these data are received at the previous moment $t_{\mathrm{k}}^{\mathrm{L}}-t_j(t_j\in (0,100)\left(\mathrm{ms}\right))$ and their coordinates are represented in the coordinate system corresponding to the moment $t_{\mathrm{k}}^{\mathrm{L}}-t_j$. These points can be denoted as ${\mathit{X}}_{(\mathrm{i},\mathrm{b},\mathrm{k}-\mathrm{j})}^{\mathrm{B}}$. If we want to represent these points in the coordinate system corresponding to time $t_{\mathrm{k}}^{\mathrm{L}}$, we need to transform the point ${\mathit{X}}_{(\mathrm{i},\mathrm{b},\mathrm{k}-\mathrm{j})}^{\mathrm{B}}$ to ${\mathit{X}}_{(\mathrm{i},\mathrm{b},\mathrm{k})}^{\mathrm{B}}$. This transformation is usually called “correction” or “rectification” of the lidar scan.

There are two common ways to rectify the distorted point clouds. One utilizes the information of IMU [16,27]. We make some improvements in this method which is described in Section 3.4.1. The other one uses the previous registration results [15], which is presented in Section 3.4.2. Furthermore, we also propose an interesting two-step correction procedure which does not rely on the IMU in Section 3.4.3.

3.4.1. Rectification with the Aid of IMU

In this method, we try to use IMU to rectify the raw points. Let the pose at time $t_{\mathrm{k}}^{\mathrm{L}}$ and $t_{\mathrm{k}-1}^{\mathrm{L}}$ be ${\mathit{T}}_{\mathrm{q}}^{\mathrm{WB}}$ and ${\mathit{T}}_{\mathrm{p}}^{\mathrm{WB}}$. Since the IMU data is generated at a frequency of 100 Hz and the lidar point clouds are generated at a frequency of 10 Hz, there are always nine frames of IMU data between the pose ${\mathit{T}}_{\mathrm{p}}^{\mathrm{WB}}$ and ${\mathit{T}}_{\mathrm{q}}^{\mathrm{WB}}$. Therefore, ten relative transformations $\Delta {\mathit{T}}_{\mathrm{m}-1,\mathrm{m}}^{\mathrm{B}}(m\in (p,q])$ can be obtained by Equation (1) and each $\Delta {\mathit{T}}_{\mathrm{m}-1,\mathrm{m}}^{\mathrm{B}}$ represents the relative transformation during $[t_{\mathrm{m}-1}^{\mathrm{B}},t_{\mathrm{m}}^{\mathrm{B}}]$, where $t_{\mathrm{m}}^{\mathrm{B}}-t_{\mathrm{m}-1}^{\mathrm{B}}$ equals to 10 ms.

Since the rotational velocity of the lidar is constant during $[t_{\mathrm{k}-1}^{\mathrm{L}},t_{\mathrm{k}}^{\mathrm{L}}]$, the rotation angle of the lidar per 10 ms is also fixed. We can separate all points of a frame ${\mathit{f}}_{\mathrm{k}}^{\mathrm{L}}$ into ten sets, and the ID of points in each set can be denoted as $\mathrm{i}\in [\frac{m-1-p}{q-p}\times S_b,\frac{m-p}{q-p}\times S_b]$. Let the total number of points in each set be $S_e$, which is equal to $S_b/10$. We further assume that the motion of the vehicle is constant during $[t_{\mathrm{m}-1}^{\mathrm{B}},t_{\mathrm{m}}^{\mathrm{B}}]$. Each set of points now correspond to a unique relative transformation $\Delta {\mathit{T}}_{\mathrm{m},\mathrm{m}-1}^{\mathrm{B}}$ which can also be denoted as $exp\left(\Delta {\mathit{\xi }}_{\mathrm{m}-1,\mathrm{m}}^{\wedge }\right)$.

We linearly interpolated $exp\left(\Delta {\mathit{\xi }}_{\mathrm{m}-1,\mathrm{m}}^{\wedge }\right)$ to transform point ${\mathit{X}}_{(\mathrm{i},\mathrm{b},\mathrm{k}-\mathrm{j})}^{\mathrm{B}}$ to ${\mathit{X}}_{(\mathrm{i},\mathrm{b},\mathrm{m}-1)}^{\mathrm{B}}$. Then, ${\mathit{X}}_{(\mathrm{i},\mathrm{b},\mathrm{m}-1)}^{\mathrm{B}}$ will be transformed to ${\mathit{X}}_{(\mathrm{i},\mathrm{b},\mathrm{k})}^{\mathrm{B}}$ by $\Delta {\mathit{T}}_{\mathrm{q},\mathrm{m}-1}^{\mathrm{B}}$. The whole rectification process can be denoted as:

$${\widehat{\mathit{X}}}_{(\mathrm{i},\mathrm{b},\mathrm{k})}^{\mathrm{B}}=\Delta {\mathit{T}}_{\mathrm{q},\mathrm{m}-1}^{\mathrm{B}}·\exp {\left(\Delta {\mathit{\xi }}_{\mathrm{m}-1,\mathrm{m}}·\frac{i\%S_e}{S_e}\right)}^{\wedge }·{\mathit{X}}_{(\mathrm{i},\mathrm{b},\mathrm{k}-\mathrm{j})}^{\mathrm{B}},$$

(13)

where $A\%B$ is used to compute the remainder when A is divided by B, and ${\widehat{\mathit{X}}}_{(\mathrm{i},\mathrm{b},\mathrm{k})}^{\mathrm{B}}$ represents the undistorted point after rectification.

3.4.2. Rectification by Previous Registration Results

This approach assumes that the motion of the vehicle is constant during $\left[t_{\mathrm{k}-2}^{\mathrm{L}},t_{\mathrm{k}}^{\mathrm{L}}\right]$. Thus, it can assume that the relative transformation $\Delta {\mathit{T}}_{\mathrm{k}-1,\mathrm{k}-2}^{\mathrm{B}}$ between frame ${\mathit{f}}_{\mathrm{k}-1}^{\mathrm{L}}$ and ${\mathit{f}}_{\mathrm{k}-2}^{\mathrm{L}}$ is the same as the relative transformation $\Delta {\mathit{T}}_{\mathrm{k},\mathrm{k}-1}^{\mathrm{B}}$ between frame ${\mathit{f}}_{\mathrm{k}}^{\mathrm{L}}$ and ${\mathit{f}}_{\mathrm{k}-1}^{\mathrm{L}}$. Therefore, we can linearly interpolate the relative transformation $exp\left(\Delta {\mathit{\xi }}_{\mathrm{k}-1,\mathrm{k}-2}^{\wedge }\right)$ to rectify the raw distorted points in frame ${\mathit{f}}_{\mathrm{k}}^{\mathrm{L}}$ as:

$${\widehat{\mathit{X}}}_{(\mathrm{i},\mathrm{b},\mathrm{k})}^{\mathrm{B}}=\exp {\left(\Delta {\mathit{\xi }}_{\mathrm{k}-1,\mathrm{k}-2}·\frac{i}{S_b}\right)}^{\wedge }·{\mathit{X}}_{(\mathrm{i},\mathrm{b},\mathrm{k}-\mathrm{j})}^{\mathrm{B}}.$$

(14)

3.4.3. Rectification by Registering Two Distorted Point Clouds

In this approach, we note that intra-frame correction is in fact quite related to inter-frame registration. As the task of intra-frame correction is to estimate the lidar motion between 0 ms and 100 ms, inter-frame registration tries to estimate the motion between 100 ms and 200 ms. Both frame a (corresponding to the lidar date generated between 0 ms and 100 ms) and frame b (generated between 100 ms and 200 ms) are distorted point clouds. It is reasonable to assume that the degree of distortion of these two frames are quite similar. We could thus directly register these two frames without any intra-frame correction first, and then use the registration result to correct each frame.

Let the relative transformation obtained from registering two distorted point clouds be $\Delta {\tilde{\mathit{T}}}_{\mathrm{k},\mathrm{k}-1}^{\mathrm{B}}$. We assumed that $\Delta {\tilde{\mathit{T}}}_{\mathrm{k},\mathrm{k}-1}^{\mathrm{B}}$ can approximate the lidar motion between time $\left[t_{\mathrm{k}}^{\mathrm{L}},t_{\mathrm{k}-1}^{\mathrm{L}}\right]$ and linearly interpolate $exp\left(\Delta {\tilde{\mathit{\xi }}}_{\mathrm{k},\mathrm{k}-1}^{\wedge }\right)$ to rectify the raw distorted points of ${\mathit{f}}_{\mathrm{k}}^{\mathrm{L}}$, as in Equation (15).

$${\widehat{\mathit{X}}}_{(\mathrm{i},\mathrm{b},\mathrm{k})}^{\mathrm{B}}=\exp {\left(\Delta {\tilde{\mathit{\xi }}}_{\mathrm{k},\mathrm{k}-1}·\frac{i}{S_b}\right)}^{\wedge }·{\mathit{X}}_{(\mathrm{i},\mathrm{b},\mathrm{k}-\mathrm{j})}^{\mathrm{B}}.$$

(15)

The undistorted frame after rectification is denoted as ${\widehat{\mathit{f}}}_{\mathrm{k}}^{\mathrm{L}}$. In Section 4, we do experiments to compare these three approaches.

3.5. Intermediate Step: Inter-Frame Registration

To register two frames of point clouds, we could use any off-the-shelf scan registration approaches, such as ICP, NDT, and LOAM. To successfully apply these approaches, it always requires an accurate initial guess for the estimated transformation. Just as mentioned previously, the problem of inter-frame registration is, in fact, quite related to intra-frame correction. Therefore, the approaches mentioned in Section 3.4 can also be applied here. We can either use the registration result of the previous two frames, or use the IMU data directly. We undertake experiments to compare these two approaches, presented in Section 4.

3.6. Output Step: Fusion in an EKF Framework

3.6.1. Basic Concepts

The Extended Kalman filter (EKF) [28] is one of the most commonly used data fusion algorithms. It is a recursive state estimator for the nonlinear system, and the state is assumed to satisfy a Gaussian distribution. A typical EKF algorithm consists of two steps: a prediction step, and an update step.

In the prediction step, the previous state ${\mathit{x}}_{\mathrm{k}}-1$ is used to compute a priori estimate of the current state by:

$$\begin{array}{cc}\hfill {\overline{\mathit{x}}}_{\mathrm{k}}& =\mathit{f}\left({\widehat{\mathit{x}}}_{\mathrm{k}-1}\right),\hfill \\ \hfill {\overline{\mathit{P}}}_{\mathrm{k}}& =\mathit{F}{\widehat{\mathit{P}}}_{\mathrm{k}-1}{\mathit{F}}^T+\mathit{R},\hfill \end{array}$$

(16)

where R is the covariance of the process noise. ${\overline{\mathit{x}}}_{\mathrm{k}}$ and ${\overline{\mathit{P}}}_{\mathrm{k}}$ are the mean and covariance matrices of the prior distribution of the state ${\mathit{x}}_{\mathrm{k}}$, respectively. ${\widehat{\mathit{x}}}_{\mathrm{k}-1}$ and ${\widehat{\mathit{P}}}_{\mathrm{k}-1}$ are the mean and covariance matrices of the posterior Gaussian distribution of the state ${\mathit{x}}_{\mathrm{k}-1}$. By performing a first-order Taylor expansion to the function $\mathit{f}$ at the mean of the state ${\mathit{x}}_{\mathrm{k}-1}$, the first-order partial derivative can be denoted as the Jacobian matrix $\mathit{F}$:

$$\mathit{F}=\frac{\partial \mathit{f}}{\partial {\mathit{x}}_{\mathrm{k}-1}}{|}_{{\widehat{\mathit{x}}}_{\mathrm{k}-1}}.$$

(17)

In the update step, the priori estimate of the current state is combined with the current observation ${\mathit{z}}_{\mathrm{k}}$ to refine the state estimate and obtain the posteriori state estimate ${\mathit{x}}_{\mathrm{k}}$, which can be formulated as:

$$\begin{array}{cc}\hfill {\mathit{K}}_{\mathrm{k}}& ={\overline{\mathit{P}}}_{\mathrm{k}}{\mathit{H}}^T{\left(\mathit{H}{\overline{\mathit{P}}}_{\mathrm{k}}{\mathit{H}}^T+\mathit{Q}\right)}^{-1},\hfill \\ \hfill {\widehat{\mathit{x}}}_{\mathrm{k}}& ={\overline{\mathit{x}}}_{\mathrm{k}}+{\mathit{K}}_{\mathrm{k}}\left({\mathit{z}}_{\mathrm{k}}-\mathit{h}\left({\overline{\mathit{x}}}_{\mathrm{k}}\right)\right),\hfill \\ \hfill {\widehat{\mathit{P}}}_{\mathrm{k}}& =\left(\mathit{I}-{\mathit{K}}_{\mathrm{k}}\mathit{H}\right){\overline{\mathit{P}}}_{\mathrm{k}},\hfill \end{array}$$

(18)

where $\mathit{Q}$ represents the covariance of the measurement noise. ${\mathit{K}}_{\mathrm{k}}$ is the Kalman gain and the posteriori state estimation ${\mathit{x}}_{\mathrm{k}}\sim \mathcal{N}\left({\widehat{\mathit{x}}}_{\mathrm{k}},{\widehat{\mathit{P}}}_{\mathrm{k}}\right)$. Similarly, a first-order Taylor expansion to the function $\mathit{h}$ can be performed at the mean of the prior distribution of the state ${\mathit{x}}_{\mathrm{k}}$. The first-order partial derivative could be defined as the Jacobian matrix $\mathit{H}$:

$$\mathit{H}=\frac{\partial \mathit{h}}{\partial {\mathit{x}}_{\mathrm{k}}}{|}_{{\overline{\mathit{x}}}_{\mathrm{k}}}.$$

(19)

In this section, We use an EKF framework to fuse the IMU and the results obtained from the lidar odometry, as shown in Figure 5. Let the motion prior model presented in Section 3.2 be the system model and the results from the lidar odometry (which is denoted as the red star in Figure 5) be the measurements. The system model and measurement model are described as follows.

3.6.2. System Model

Since the main purpose of our system is to obtain the pose of the vehicle, we could use the Lie Algebra $\mathit{\xi }$ to define the state vector ${\mathit{x}}_{\mathrm{k}}$ at time $t_{\mathrm{k}}$ as:

$${\mathit{x}}_{\mathrm{k}}={\left[\begin{array}{cc}{\mathit{\theta }}_{\mathrm{k}}& {\mathit{p}}_{\mathrm{k}}\end{array}\right]}^T\in \mathfrak{se}\left(3\right).$$

(20)

By utilizing the logarithmic map and the approximate Baker-Campbell-Hausdorff (BCH) formula [21], the ${\mathit{R}}_{\mathrm{k}}^{\mathrm{WB}}$ obtained from the Equation (6) can be converted to:

$${\mathit{\theta }}_{\mathrm{k}}={\mathit{J}}_l^{-1}\left({\mathit{\theta }}_{\mathrm{k}-1}\right)·\left({\mathit{\omega }}_{\mathrm{k}}\Delta t\right)+{\mathit{\theta }}_{\mathrm{k}-1},$$

(21)

where ${\mathit{J}}_l^{-1}\left({\mathit{\theta }}_{\mathrm{k}}\right)$ denotes the right BCH Jacobian of $\mathrm{SO}\left(3\right)$ and it equals to:

$${\mathit{J}}_l^{-1}\left({\mathit{\theta }}_{\mathrm{k}}\right)=\frac{{\theta }_{\mathrm{k}}}{2}\cot \frac{{\theta }_{\mathrm{k}}}{2}\mathit{I}+\left(1-\frac{{\theta }_{\mathrm{k}}}{2}\cot \frac{{\theta }_{\mathrm{k}}}{2}\right){\mathit{n}}_{\mathrm{k}}{\mathit{n}}_{\mathrm{k}}^T-\frac{{\theta }_{\mathrm{k}}}{2}{\mathit{n}}_{\mathrm{k}}^{\wedge },$$

(22)

and ${\mathit{J}}_l^{-1}\left({\mathit{\theta }}_{\mathrm{k}}\right)$ can be denoted as:

$${\mathit{J}}_l^{-1}\left({\mathit{\theta }}_{\mathrm{k}}\right)=\left[\begin{array}{ccc}{J}_1^{-1}\left({\mathit{\theta }}_{\mathrm{k}}\right)& J_2^{-1}\left({\mathit{\theta }}_{\mathrm{k}}\right)& J_3^{-1}\left({\mathit{\theta }}_{\mathrm{k}}\right)\\ J_4^{-1}\left({\mathit{\theta }}_{\mathrm{k}}\right)& J_5^{-1}\left({\mathit{\theta }}_{\mathrm{k}}\right)& J_6^{-1}\left({\mathit{\theta }}_{\mathrm{k}}\right)\\ J_7^{-1}\left({\mathit{\theta }}_{\mathrm{k}}\right)& J_8^{-1}\left({\mathit{\theta }}_{\mathrm{k}}\right)& J_9^{-1}\left({\mathit{\theta }}_{\mathrm{k}}\right)\end{array}\right].$$

(23)

Combining the Equations (8), (21) and (23), the state transition equation could be defined as:

$${\mathit{x}}_{\mathrm{k}}={\mathit{x}}_{\mathrm{k}-1}+\left[\begin{array}{c}{\mathit{J}}_l^{-1}\left({\mathit{\theta }}_{\mathrm{k}-1}\right)·{\mathit{\omega }}_{\mathrm{k}}\Delta t\\ {\mathit{R}}_{\mathrm{k}}^{\mathrm{WB}}(1:3,1)·\Delta s\end{array}\right],$$

(24)

where ${\mathit{R}}_{\mathrm{k}}^{\mathrm{WB}}$ is computed by ${\theta }_{\mathrm{k}}^x$, ${\theta }_{\mathrm{k}}^y$ and ${\theta }_{\mathrm{k}}^z$ according to Equation (3) and ${\mathit{R}}_{\mathrm{k}}^{\mathrm{WB}}(1:3,1)$ means the first column of the ${\mathit{R}}_{\mathrm{k}}^{\mathrm{WB}}$.

According to Equations (17) and (24), $\mathit{F}$ can be derived as:

$$\mathit{F}=\left[\begin{array}{cc}{\mathit{F}}_{\theta \theta }& {\mathbf{0}}_{3\times 3}\\ {\mathit{F}}_{p\theta }& {\mathit{I}}_{3\times 3}\end{array}\right]\in {\mathbb{R}}^{6\times 6},$$

(25)

where ${\mathit{F}}_{\theta \theta }\in {\mathbb{R}}^{3\times 3}$, and it can be computed by:    

$${\mathit{F}}_{\theta \theta }={\mathit{I}}_{3\times 3}+{\left[\begin{array}{c}{\left(\frac{\partial {\mathit{J}}_l^{-1}\left({\mathit{\theta }}_{\mathrm{k}-1}\right)}{\partial {\theta }_{\mathrm{k}-1}^x}·{\mathit{\omega }}_{\mathrm{k}}\Delta t\right)}^T\\ {\left(\frac{\partial {\mathit{J}}_l^{-1}\left({\mathit{\theta }}_{\mathrm{k}-1}\right)}{\partial {\theta }_{\mathrm{k}-1}^y}·{\mathit{\omega }}_{\mathrm{k}}\Delta t\right)}^T\\ {\left(\frac{\partial {\mathit{J}}_l^{-1}\left({\mathit{\theta }}_{\mathrm{k}-1}\right)}{\partial {\theta }_{\mathrm{k}-1}^z}·{\mathit{\omega }}_{\mathrm{k}}\Delta t\right)}^T\end{array}\right]}^T.$$

(26)

${\mathit{F}}_{p\theta }$ can be computed by:

$${\mathit{F}}_{p\theta }=\left[\begin{array}{ccc}\frac{\partial {\mathit{R}}_{\mathrm{k}}^{\mathrm{WB}}(1:3,1)}{\partial {\theta }_{\mathrm{k}-1}^x}& \frac{\partial {\mathit{R}}_{\mathrm{k}}^{\mathrm{WB}}(1:3,1)}{\partial {\theta }_{\mathrm{k}-1}^y}& \frac{\partial {\mathit{R}}_{\mathrm{k}}^{\mathrm{WB}}(1:3,1)}{\partial {\theta }_{\mathrm{k}-1}^z}\end{array}\right].$$

(27)

The prior probability distribution of the state ${\mathit{x}}_{\mathrm{k}}$ could then be obtained by the Equation (16).

3.6.3. Measurement Model

The measurement vector ${\mathit{z}}_{\mathrm{k}}$ is denoted as:

$${\mathit{z}}_{\mathrm{k}}={\left[\begin{array}{cccccc}{\theta }_{z_{\mathrm{k}}}^x& {\theta }_{z_{\mathrm{k}}}^y& {\theta }_{z_{\mathrm{k}}}^z& p_{z_{\mathrm{k}}}^x& p_{z_{\mathrm{k}}}^z& p_{z_{\mathrm{k}}}^z\end{array}\right]}^T.$$

(28)

According to Figure 5, the lidar odometry algorithm starts to run at time $t_{\mathrm{m}}^{\mathrm{L}}$, but the result is obtained at time $t_{\mathrm{k}}$, which is several tens of milliseconds after $t_{\mathrm{m}}^{\mathrm{L}}$. We assume that the pose of the vehicle at time $t_{\mathrm{m}}^{\mathrm{L}}$ and $t_{\mathrm{k}}$ were approximately equal to ${\mathit{T}}_{\mathrm{i}}^{\mathrm{WB}}$ and ${\mathit{T}}_l^{\mathrm{WB}}$. Both ${\mathit{T}}_{\mathrm{i}}^{\mathrm{WB}}$ and ${\mathit{T}}_l^{\mathrm{WB}}$ can be obtained from the IMU data.

The relationship between the measurement ${\mathit{z}}_{\mathrm{k}}$ and the real state ${\mathit{x}}_{\mathrm{k}}$ of the vehicle at time $t_{\mathrm{k}}$ can be indicated by $\Delta {\mathit{T}}_{\mathrm{i},l}^{\mathrm{B}}$, which can also be denoted as $\Delta {\mathit{\xi }}_{\mathrm{i},l}={\left[\begin{array}{cc}\Delta {\mathit{\theta }}_{\mathrm{i},l}& \Delta {\mathit{p}}_{\mathrm{i},l}\end{array}\right]}^T$. The observation equation could then be represented as:

$${\mathit{z}}_{\mathrm{k}}=\left[\begin{array}{c}{\mathit{\theta }}_{\mathrm{k}}\\ \Delta {\mathit{p}}_{\mathrm{i},l}\end{array}\right]+\left[\begin{array}{c}{\mathit{J}}_l^{-1}\left({\mathit{\theta }}_{\mathrm{k}}\right)·\Delta {\mathit{\theta }}_{\mathrm{i},l}\\ \Delta {\mathit{R}}_{\mathrm{i},l}^{\mathrm{B}}·{\mathit{p}}_{\mathrm{k}}\end{array}\right].$$

(29)

According to Equations (19) and (29), $\mathit{H}$ can be derived as:

$$\mathit{H}=\left[\begin{array}{cc}{\mathit{H}}_{\theta \theta }& {\mathbf{0}}_{3\times 3}\\ {\mathbf{0}}_{3\times 3}& {\mathit{H}}_{pp}\end{array}\right],$$

(30)

where ${\mathit{H}}_{pp}=\Delta {\mathit{R}}_{\mathrm{i},l}^{\mathrm{WB}}\in {\mathbb{R}}^{3\times 3}$, ${\mathit{H}}_{\theta \theta }\in {\mathbb{R}}^{3\times 3}$ and ${\mathit{H}}_{\theta \theta }$ can be computed by:

$${\mathit{H}}_{\theta \theta }={\left[\begin{array}{c}{\left(\frac{\partial {\mathit{J}}_l^{-1}\left({\mathit{\theta }}_{\mathrm{k}}\right)}{\partial {\theta }_{\mathrm{k}}^x}·\Delta {\mathit{\theta }}_{\mathrm{i},l}\right)}^T\\ {\left(\frac{\partial {\mathit{J}}_l^{-1}\left({\mathit{\theta }}_{\mathrm{k}}\right)}{\partial {\theta }_{\mathrm{k}}^y}·\Delta {\mathit{\theta }}_{\mathrm{i},l}\right)}^T\\ {\left(\frac{\partial {\mathit{J}}_l^{-1}\left({\mathit{\theta }}_{\mathrm{k}}\right)}{\partial {\theta }_{\mathrm{k}}^z}·\Delta {\mathit{\theta }}_{\mathrm{i},l}\right)}^T\end{array}\right]}^T.$$

(31)

Based on the covariance matrix ${\overline{\mathit{P}}}_{\mathrm{k}}$ from Equation (16) and the Jacobian matrix $\mathit{H}$ from Equation (30), the Kalman gain ${\mathit{K}}_{\mathrm{k}}$ and the posteriori state estimation could be computed by the Equation (18).

This fusion framework is summarized in Algorithm 1.

Algorithm 1: The fusion framework of our system.

input: The history pose set $\left\{{\mathit{T}}_{\mathrm{i}}^{\mathrm{WB}}\right\}$ and pose ${\mathit{T}}_{{\mathrm{B}}_{\mathrm{k}}}^{\mathrm{WB}}$ at time $t_{\mathrm{k}}^{\mathrm{B}}$ from the IMU; pose ${\mathit{T}}_{{\mathrm{L}}_{\mathrm{k}}}^{\mathrm{WB}}$ at time $t_{\mathrm{k}}^{\mathrm{L}}$ from the lidar odometry; previous pose ${\mathit{T}}_{\mathrm{k}-1}^{\mathrm{WB}}$     output: Current pose ${\mathit{T}}_{\mathrm{k}}^{\mathrm{WB}}$

4. Experimental Results

The platform used in the following experiments is shown in Figure 6. The vehicle is equipped with a Velodyne HDL-64E lidar, a Xsens MTI300 IMU, two photoelectric encoders, a NovAtel SPAN-CPT GNSS/INS system, and a Nova 5100 industrial computer. The lidar is mounted on the top of the car, and it has 64 beams which could generate over 1.3 million points per second with a high range accuracy. The horizontal Field of View (FOV) is ${360}^{\circ }$, the vertical FOV is $26.8^{\circ }$, and the maximal detection distance is 120 m. The NovAtel GNSS/INS system provides localization results with centimeter-level accuracy, which can be served as the ground truth.

4.1. Experiments on Hand–Eye Calibration

As described in Section 3.3, in order to perform the lidar-IMU hand–eye calibration, we need to utilize multiple pairs of relative transformations $\{\Delta {\mathit{T}}_{\mathrm{k}-1,\mathrm{k}}^{\mathrm{B}},\Delta {\mathit{T}}_{\mathrm{k}-1,\mathrm{k}}^{\mathrm{L}}\}$.

We choose the frame pairs $\{{\mathit{f}}_{\mathrm{i}}^{\mathrm{L}},{\mathit{f}}_{\mathrm{i}-1}^{\mathrm{L}}\}$ before and after the vehicle makes a turn. In addition, all the frames are collected when the vehicle is stationary. Figure 7 shows several transformation pairs we have chosen. In each sub-figure, the left part is the registration result before the hand–eye calibration, whilst the right half is the result after hand–eye calibration. It can be seen that ${\mathit{f}}_{\mathrm{i}}^{\mathrm{L}}$ and ${\mathit{f}}_{\mathrm{i}-1}^{\mathrm{L}}$ are much better-registered in the right half of Figure 7a–d than the left half, which indicates the importance of hand–eye calibration.

Since the ground truth of ${\mathit{T}}^{\mathrm{BL}}$ is impossible to be obtained, we could not verify the calibration result directly. However, we could use the IMU data to assemble different frames together, and the quality of the assembled scene can be used to evaluate the accuracy of the hand–eye calibration result. Figure 8 shows the assembled scene when the vehicle makes a turn at the intersection. Figure 8a is the assembled result before hand–eye calibration, which is very vague, while the result after the hand–eye calibration in Figure 8b is much clearer. This suggests that the result of our hand–eye calibration is accurate.

To quantitatively test the effect of hand–eye calibration, we choose the LOAM algorithm as the lidar odometry algorithm, and compare the estimated trajectories with the ground-truth trajectory generated by the GNSS/INS system. Figure 9 illustrates the localization result before and after hand–eye calibration. The translation error and rotation error are shown in Figure 9b,c, where the horizontal axis represents the traveled distance and the vertical axis shows the errors. It is obvious that the translation error and the rotation error are significantly reduced after the hand–eye calibration.

4.2. Experiments on Intra-Frame Correction

Based on the hand–eye calibration results, we could also use the 3D reconstruction results to qualitatively test the three rectification methods introduced in Section 3.4. In order to test the result of the rectification, the vehicle should run fast to amplify the intra-frame distortion. Therefore, we carry out this experiment when the vehicle drives at around 60 km/h, and the results are shown in Figure 10.

Figure 10a–d represent different 3D reconstruction results when the vehicle is making a turn. Figure 10a is the result without any rectification, and the reconstructed scene is quite blurry. Figure 10b–d show the results when the distorted point clouds are rectified by the previous registration result, by registering two distorted point clouds and by the IMU data. The results obtained by these three approaches are all much better than the one without rectification. Moreover, the average processing time for these three methods applied on 2000 frames of point clouds is displayed in

Table 1. Computational efficiency of three rectification methods.

Rectification Methods	Average Processing Time (ms)
By previous registration	2.565
By registering two distorted point clouds	68.504
By IMU data	4.546

. It is seen that the approach of registering two distorted point clouds is the most computational expensive approach.

To quantitatively test the effect of intra-frame correction, LOAM is also used as the lidar odometry algorithm, and the localization accuracy is compared with the ground-truth trajectory. Figure 11 compares the localization results with/without rectification. The trajectories generated by the three rectification methods and without rectification are compared with the ground truth in Figure 11a, where “Method 1” represents rectification by previous registration, “Method 2” is the approach of registering two distorted point clouds, and “Method 3” indicates rectification by IMU data. In addition, Figure 11b,c compare the translation error and rotation error of these approaches.

It can be seen that both the translation error and the rotation error are significantly reduced after rectification, and “Method 3” exhibits the best performance, followed by “Method 1” and “Method 2”. A detailed error comparison is presented in

Table 2. Error comparison of three rectification methods.

Rectification Methods	Mean Relative Position Error
Method 1	0.79%
Method 2	0.94%
Method 3	0.57%
Without Rectification	2.28%

.

4.3. Experiments on Inter-Frame Registration

4.3.1. Tests with Different Methods of Assigning Initial Guess

In this experiment, two methods of assigning initial guess for the lidar odometry algorithm are compared—namely, using the IMU data or the registration result from the previous two scans. In both approaches, the raw distorted point clouds are rectified by the IMU data, and LOAM is used as the lidar odometry algorithm.

The tests are conducted when the vehicle drives at different speeds: a relatively slow speed of around 25 km/h, and a faster speed of around 60 km/h. A detailed error comparison is shown in

Table 3. Error comparison of different methods at the intermediate step.

Speed (km/h)	Methods of Assigning Initial Guess	Mean Relative Position Error
25	IMU	0.31%
	Previous regist	0.46%
60	IMU	0.57%
	Previous regist	1.32%

. It can be seen that using the IMU data as the initial guess always produces a better result than using the registration result from the previous two scans, especially when the vehicle drives at a faster speed.

4.3.2. Tests with Different Lidar Odometry Algorithms

Here, we compare three popular lidar odometry algorithms: ICP, NDT, and LOAM. During the experiments, the point clouds are rectified by the IMU data. The vehicle drives at a speed around 25 km/h, and the overall distance is about 1.1 km. A detailed error comparison is shown in

Table 4. Error comparison of three lidar odometry algorithms.

Lidar Odometry Methods	Mean Relative Position Error
ICP	6.28%
NDT	3.85%
LOAM	0.31%

. It is obvious that LOAM performs much better than NDT and ICP.

4.3.3. Tests in Both Urban and Off-Road Environments

Our algorithm is tested in both urban and off-road environment. We choose to use LOAM as the lidar odometry algorithm. The point clouds are rectified by the IMU data. In the urban environment, the vehicle drives at a speed of around 25 km/h, and the overall distance is about 1.1 km. In the off-road environment, the vehicle drives at a speed of around 15 km/h and the overall distance is about 1.0 km. The trajectories generated by our method are compared with the ground truth in Figure 12. A detailed error comparison is shown in

Table 5. Error comparison of diverse environments.

Environment	Distance (km)	Mean Relative Position Error
Urban	1.097	0.31%
Off-road	0.983	0.93%

. It is seen that our approach is applicable for both the urban and off-road environments, whilst the error in the off-road scenario is slightly higher than the urban environment.

4.4. Experiments in GNSS-Denied Environment

Finally, we test our whole localization system in the GNSS-Denied environment. Figure 13a shows the trajectory generated by our localization system when the UGV drives at a speed of 20 km/h. By projecting the trajectory to the satellite image, we find that the position drift is quite small. Figure 13b compares the trajectories generated by our localization system and by the disturbed GNSS/INS system. Figure 13c shows the output frequency of our localization system. The average output frequency is around 39.7 Hz. In summary, no matter whether the GNSS signal is available or not, our system can output precise and high-frequency localization results.

5. Conclusions

In this paper, we present a novel approach for combining the IMU data with lidar odometry. The proposed localization system could generate stable, accurate, and high-frequency localization results. Since our system does not recognize the loop closure, we can further extend our work to enable the ability of loop-closure detection.