High-Precision SLAM Based on the Tight Coupling of Dual Lidar Inertial Odometry for Multi-Scene Applications

Featured Application

This paper fuses different sensors to form a general high-precision SLAM framework for multi-scene applications. The algorithm framework in this paper can be extended to the fields of autonomous driving, robot navigation, and 3D reconstruction.

Abstract

Simultaneous Localization and Mapping (SLAM) is an essential feature in many applications of mobile vehicles. To solve the problem of poor positioning accuracy, single use of mapping scene, and unclear structural characteristics in indoor and outdoor SLAM, a new framework of tight coupling of dual lidar inertial odometry is proposed in this paper. Firstly, through external calibration and an adaptive timestamp synchronization algorithm, the horizontal and vertical lidar data are fused, which compensates for the narrow vertical field of view (FOV) of the lidar and makes the characteristics of vertical direction more complete in the mapping process. Secondly, the dual lidar data is tightly coupled with an Inertial Measurement Unit (IMU) to eliminate the motion distortion of the dual lidar odometry. Then, the value of the lidar odometry after correcting distortion and the pre-integrated value of IMU are used as constraints to establish a non-linear least-squares objective function. Joint optimization is then performed to obtain the best value of the IMU state values, which will be used to predict the state of IMU at the next time step. Finally, experimental results are presented to verify the effectiveness of the proposed method.

1. Introduction

Simultaneous localization and mapping (SLAM) require building a map of an unknown environment by a mobile vehicle and simultaneously localizing the vehicle in such a map [1,2,3]. SLAM is essential for vehicles to fulfill many tasks, including vehicle rescue [4] and exploration [5]. The perception of the unknown external environment by various onboard sensors provides vital information for SLAM. Thus, integrating different sensors to develop a practical SLAM framework that can be applied in multiple scenes is essential.

Generally, both vision-based and lidar-based SLAM [6,7,8,9,10] are used. Although the vision-based SLAM can obtain high-precision positioning [11,12,13], the vision sensors are vulnerable to light change in the environment and cannot work in dark or untextured scenes. In comparison, lidar is not affected by light and can usually measure the angle and distance of obstacles with higher accuracy. Therefore, this paper focuses on the design of lidar-based SLAM to adapt the multi-scene applications.

In recent years, many different lidar-based SLAM schemes have been proposed. Among them, the Lidar Odometry and Mapping in Real-time (LOAM) method, where a single-line lidar and a motor are used to form a multiline lidar to realize low-drift and low-calculation real-time positioning and mapping, has been studied extensively [14]. In the LOAM method, SLAM is divided into two parts: lidar odometry and lidar mapping. In the lidar odometry part, to reduce the computation, the plane smoothness of the lidar point cloud, which is utilized to distinguish the edge points, is calculated according to the curvature and then invalid point clouds are discarded to improve the feature of point cloud. After the point cloud information is obtained through feature extraction, a scan-to-scan method [15] is proposed to realize feature matching between frames, including edge points and planar points matching. To eliminate the motion distortion of lidar, linear interpolation is utilized. After the distortion is eliminated, the pose transformation of the lidar point cloud data of two adjacent frames is acquired to obtain the lidar odometry. Then, after accumulating a certain number of point cloud data, lidar mapping is performed through a map-to-map matching method [16]. Although LOAM can create a high-precision point cloud map, it cannot remove moving people or objects during feature extraction. Furthermore, in an environment with fewer feature points, it is easy for the lidar odometry to fail, resulting in positioning and mapping with worse accuracy.

To solve the above problems, a Lightweight and Ground-Optimized Lidar Odometry and Mapping on Variable Terrain (LeGO-LOAM) [17] is proposed. Its core comprises four modules: point cloud segmentation and denoising, feature extraction, lidar odometry, and lidar mapping. Firstly, the point cloud segmentation technology [18] solves the defects of moving people or objects in LOAM mapping and filters out noise. Then, feature extraction is applied to obtain planar and edge features from the segmented point cloud. The lidar odometry module uses the features to find out the optimal pose transform between two consecutive scans with the help of a two-step Levenberg–Marquardt (LM) optimization method [19]. The extracted features are further processed by a scan-to-map [20] matching method to obtain a global point cloud map in lidar mapping. LeGO-LOAM optimizes LOAM to a certain extent, but in large scenes (e.g., long corridors) or environments with few feature points, the low frequency of lidar and less characteristic information can lead to significant errors in positioning and mapping.

Some recent research has used low-cost IMU [21,22,23] to assist lidar for SLAM. The simplest way to integrate the IMU with lidar is loose coupling [24], in which IMU is regarded as an independent module to assist the lidar. The authors of [25] loosely couple IMU and optional GPS measurement with lidar through the Extended Kalman Filter (EKF) to improve computational efficiency and accuracy. However, the IMU error will continue to accumulate in the case of long distances, so the localization error is still increasing in such cases. To solve this problem, IMU and lidar are generally coupled via a tight coupling method that usually offers improved accuracy. A tightly coupled lidar inertial odometry and mapping framework (LIO-Mapping) is introduced in the literature [26]. It comprises the state optimization for the odometry and the refinement with rotational constraints. Results showed that this method outperformed the state-of-the-art lidar-only and loosely coupled methods. Since LIO-Mapping is designed to process all sensor measurements, real-time performance is not achieved. The authors of [27] proposed a tightly coupled Lidar Inertial Odometry via Smoothing and Mapping (LIO-SAM) based on LeGO-LOAM. LIO-SAM performs highly accurate, real-time mobile vehicle trajectory estimation and map building, suitable for multi-sensor fusion and global optimization. Although the tight coupling of lidar and IMU can improve the localization accuracy, lidar sensors have certain drawbacks. Conventional lidar can only be used to scan the environment in a narrow range of vertical angles and the point cloud feature information obtained is limited. For example, in an indoor corridor or staircase environment, lidar can receive the point cloud information of the sidewall, but only a tiny part of the point cloud information is obtained from the floor and ceiling. In such cases, the matched lidar features can easily cause ill-constrained pose estimation and incomplete mapping.

Aiming to broaden the FOV of lidar, lidars can be actuated in a number of ways. The periodic nodding and continuous rotation can be adopted and the resulting configurations can potentially enlarge the FOV. While the implementation of this method depends on a complex mechanical structure, the localization accuracy is relatively low [28,29]. More recently, a new design for a 3D sensor system was introduced [30], involving construction from a 2D range scanner coupled with a passive linkage mechanism, such as a spring. However, the system requires reasonably continual excitation to induce motion of the sensor head. Therefore, the system is not considered appropriate for electric ground vehicles operating with infrequent accelerations on smooth terrain. In order to solve the above problems, some researchers have started investigating the use of multiple 3D lidars for better coverage of the environment. In [31], a high-precision lidar odometry system was proposed to achieve robust and real-time operation under challenging perceptual conditions. In this system, the two lidars are mounted at 180 degrees to each other to make up for the self-similar areas with low lidar observability. The authors of [32] proposed a system to achieve robust and simultaneous extrinsic calibration, odometry, and mapping for multiple lidars, while [33] proposed a scheme to combine multiple lidars with complementary FOV for feature-based lidar-inertia odometry and mapping. While the above methods can work well in single-scene applications, their adaptability to different environments was not considered, especially the environments that need to perceive height information.

This paper proposes a high-precision SLAM framework based on tight coupling of dual lidar inertial to overcome the above problems. In this framework, the horizontal lidar and the vertical lidar are integrated to broaden the FOV. Then the dual lidar and IMU are tightly coupled to improve the localization accuracy. In the outdoor environment, this paper introduces GPS measurements to further improve positioning accuracy. The main contributions of this paper include:

• A general high-precision SLAM framework is provided by fusing different sensors. It can adapt to multi-scene applications, such as a corridor with fewer features, stairs with height, and complex outdoor environments.
• The horizontal and vertical lidars are fused by external calibration and adaptive time synchronization algorithms to solve the narrow vertical FOV of the lidar.
• To improve the positioning accuracy of SLAM in the environment with height information (e.g., stairs), the dual lidar odometry and IMU are tightly coupled. The dual lidar odometry measurement and IMU pre-integration are jointly optimized to obtain more accurate IMU state values, which will be used to eliminate the motion distortion of the dual lidar to improve the localization accuracy.
• In addition, several practical experiments are carried out to verify the feasibility and effectiveness of the proposed method.

2. Prerequisites

2.1. IMU State Prediction and Pre-Integration

Usually, the frequency of IMU is much higher than lidar. IMU can obtain the acceleration and angular velocity at each time step, which can be used to predict the next state of IMU through integration.

In general, the IMU states at time $\mathit{t}$ can be modeled as:

$${\mathit{X}}_{\mathit{t}}^{\mathit{W}}=[{\mathit{p}}_{\mathit{t}},{\mathit{v}}_{\mathit{t}},{\mathit{R}}_{\mathit{t}},{\mathit{b}}_{\mathit{t}}]$$

(1)

where ${\mathit{X}}_{\mathit{t}}^{\mathit{W}}$ represents the state in the world frame $\mathit{W}$ at time $\mathit{t}$; ${\mathit{p}}_{\mathit{t}}$, ${\mathit{v}}_{\mathit{t}}$, and ${\mathit{R}}_{\mathit{t}}$ represent the position, velocity, and rotation matrix, respectively, at time $\mathit{t}$; and ${\widehat{\mathit{a}}}_{\mathit{t}}$ and ${\widehat{\mathit{\omega }}}_{\mathit{t}}$ are the acceleration and angular velocity of the raw IMU measurements (${\widehat{\mathit{a}}}_{\mathit{t}}$ and ${\widehat{\mathit{\omega }}}_{\mathit{t}}$ are affected by a slowly varying bias, ${\mathit{b}}_{\mathit{t}}^{}$).

The next state, ${\mathit{X}}_{\mathit{t}+\Delta \mathit{t}}^{\mathit{W}}$, is predicted through the integration of IMU, where $\Delta \mathit{t}$ is the interval between two consecutive IMU measurements. The state prediction value of the IMU is then used to infer the motion of the vehicle. The velocity, position, and rotation of the vehicle at time $\mathit{t}+\Delta \mathit{t}$ can be computed by Equation (2) [27]:

$$\begin{array}{l}{\mathit{v}}_{\mathit{t}+\Delta \mathit{t}}={\mathit{v}}_{\mathit{t}}+\mathit{g}\Delta \mathit{t}+{\mathit{R}}_{\mathit{t}}\left({\widehat{\mathit{a}}}_{\mathit{t}}-{\mathit{b}}_{\mathit{t}}^{\mathit{a}}-{\mathit{n}}_{\mathit{t}}^{\mathit{a}}\right)\Delta \mathit{t}\\ {\mathit{p}}_{\mathit{t}+\Delta \mathit{t}}={\mathit{p}}_{\mathit{t}}+{\mathit{v}}_{\mathit{t}}\Delta \mathit{t}+\frac{1}{2}\mathit{g}\Delta {\mathit{t}}^2+\frac{1}{2}{\mathit{R}}_{\mathit{t}}\left({\widehat{\mathit{a}}}_{\mathit{t}}-{\mathit{b}}_{\mathit{t}}^{\mathit{a}}-{\mathit{n}}_{\mathit{t}}^{\mathit{a}}\right)\Delta {\mathit{t}}^2\\ {\mathit{q}}_{\mathit{t}+\Delta \mathit{t}}={\mathit{q}}_{\mathit{t}}\otimes \left[\begin{array}{c}\frac{1}{2}\Delta \mathit{t}\left({\widehat{\mathit{\omega }}}_{\Delta \mathit{t}}-{\mathit{b}}_{\Delta \mathit{t}}^{\mathit{\omega }}-{\mathit{n}}_{\Delta \mathit{t}}^{\mathit{\omega }}\right)\\ 1\end{array}\right]\end{array}$$

(2)

where $\mathit{g}$ is gravitational acceleration; ${\mathit{b}}_{\mathit{t}}^{\mathit{a}}$ is the varying bias of the acceleration ${\widehat{\mathit{a}}}_{\mathit{t}}$; and ${\mathit{n}}_{\mathit{t}}^{\mathit{a}}$ is the white noise of the acceleration ${\widehat{\mathit{a}}}_{\mathit{t}}$. The quaternion ${\mathit{q}}_{\mathit{t}}$ under Hamilton notation (which corresponds to ${\mathit{R}}_{\mathit{t}}$) is used; $\otimes$ is used for the multiplication of two quaternions; ${\mathit{b}}_{\mathit{t}}^{\mathit{\omega }}$ is the varying bias of the angular velocity ${\widehat{\mathit{\omega }}}_{\mathit{t}}$; and ${\mathit{n}}_{\mathit{t}}^{\mathit{\omega }}$ is the white noise of the angular velocity ${\widehat{\mathit{\omega }}}_{\mathit{t}}$.

The motion state of IMU between the $\mathit{i}$-th and the $\mathit{j}$-th time steps can be represented by the IMU pre-integrated measurement value $\Delta {\mathit{p}}_{\mathit{i}\mathit{j}},\Delta {\mathit{v}}_{\mathit{i}\mathit{j}},\Delta {\mathit{q}}_{\mathit{i}\mathit{j}}$, which can be computed by:

$$\begin{array}{l}\Delta {\mathit{v}}_{\mathit{i}\mathit{j}}={\mathit{R}}_{\mathit{i}}^{\mathit{T}}\left({\mathit{v}}_{\mathit{j}}-{\mathit{v}}_{\mathit{i}}-\mathit{g}\Delta {\mathit{t}}_{\mathit{i}\mathit{j}}\right)\\ \Delta {\mathit{p}}_{\mathit{i}\mathit{j}}={\mathit{R}}_{\mathit{i}}^{\mathit{T}}\left({\mathit{p}}_{\mathit{j}}-{\mathit{p}}_{\mathit{i}}-{\mathit{v}}_{\mathit{i}}\Delta {\mathit{t}}_{\mathit{i}\mathit{j}}-\frac{1}{2}\mathit{g}\Delta {\mathit{t}}_{\mathit{i}\mathit{j}}^2\right)\\ \Delta {\mathit{q}}_{\mathit{i}\mathit{j}}={\mathit{q}}_{\mathit{i}}^{-1}\otimes {\mathit{q}}_{\mathit{j}}={\displaystyle \prod _{\mathit{k}=\mathit{i}}^{\mathit{j}-1}\left[\begin{array}{c}\frac{1}{2}\Delta {\mathit{t}}_{\mathit{i}\mathit{j}}\left({\widehat{\mathit{\omega }}}_{\mathit{k}}-{\mathit{b}}_{\mathit{k}}^{\mathit{\omega }}-{\mathit{n}}_{\mathit{k}}^{\mathit{\omega }}\right)\\ 1\end{array}\right]}\end{array}$$

(3)

where $\Delta {\mathit{p}}_{\mathit{i}\mathit{j}},\Delta {\mathit{v}}_{\mathit{i}\mathit{j}},\Delta {\mathit{q}}_{\mathit{i}\mathit{j}}$ is the position, velocity, and rotation matrix of the IMU, respectively, which has the covariance ${\mathit{C}}_{{\mathit{I}}_{\mathit{j}}}^{{\mathit{I}}_{\mathit{i}}}$ in the error-state model. Due to space limitations, we invite readers to refer to the descriptions in [26,34] to understand the detailed derivation of IMU pre-integration.

2.2. Segmentation and Feature Extraction

Before extracting features, to filter out the noise, the point cloud is segmented to reduce noise interference. ${\mathit{D}}_{\mathit{t}}=\{{\mathit{d}}_1,{\mathit{d}}_2,\dots ,{\mathit{d}}_{\mathit{n}}\}$ is the point cloud information acquired at time $\mathit{t}$; ${\mathit{d}}_{\mathit{\tau }}$ is a point in ${\mathit{D}}_{\mathit{t}}$; ${\mathit{D}}_{\mathit{t}}$ is projected onto a range image; the point ${\mathit{d}}_{\mathit{\tau }}$ represents a pixel of the image; and the pixel value ${\mathit{r}}_{\mathit{\tau }}$ represents the Euclidean distance from the point ${\mathit{d}}_{\mathit{\tau }}$ to the lidar. An image-based segmentation method [17] is applied to the range image to group points into many clusters. Points of the same category have the same label (the ground is a particular category). Features are then extracted from ground points and segmented points, with the corresponding edge points and plane points obtained by calculating the curvature $\mathit{E}$ of each point. To evenly extract features from all directions, the range image is divided horizontally into several equal sub-images. Then, the points are sorted in each row of the sub-image based on their curvature values, $\mathit{E}$. The curvature $\mathit{E}$ is computed by Equation (4).

$$\mathit{E}=\frac{1}{\left|\mathit{S}\right|\cdot \Vert {\mathit{r}}_{\mathit{\tau }}\Vert }\Vert {\displaystyle \sum _{\mathit{\lambda }\in \mathit{S},\mathit{\lambda }\ne \mathit{\tau }}\left({\mathit{r}}_{\mathit{\lambda }}-{\mathit{r}}_{\mathit{\tau }}\right)}\Vert$$

(4)

where $\mathit{S}$ is the set of continuous points ${\mathit{d}}_{\mathit{\tau }}$ from the same row of the range image; $\mathsf{\lambda }$ is a point in the set $\mathit{S}$; ${\mathit{E}}_{\mathit{t}\mathit{h}}$ is a threshold which is set to distinguish different types of features; the points are called with $\mathit{E}>{\mathit{E}}_{\mathit{t}\mathit{h}}$ edge features; and the points are called with $\mathit{E}<{\mathit{E}}_{\mathit{t}\mathit{h}}$ planar features.

3. Multi-Sensor Fusion

3.1. Hardware System Description

The configuration of our system is shown in Figure 1, with the left model obtained through Unigraphics NX and the designed equipment in our paper shown on the right. The designed equipment is a scanning arm composed of various sensors that can be installed on a backpack or a vehicle. The sensor suite used in the scanning arm includes two 10 HZ Velodyne VLP-16 lidars, a 200 HZ YIS510A IMU, and a 5 HZ HY-269 GPS. The panoramic camera prepares for the follow-up research. The lidar has a horizontal FOV of 360° with 0.4° resolution, a vertical FOV 15° with 2° resolution, and an accuracy of ±3 cm. The Roll/Pitch of IMU accuracy is 0.25°, HY-269 GPS is a small-volume positioning and directional receiver, and the positioning accuracy of GPS is 1.2 m.

3.2. System Overview

In this paper, horizontal and vertical lidars are fused to make up for the shortcomings of lidar’s narrow FOV. When indoors, accurate positioning information is obtained through the tight coupling of dual lidar inertial odometry. When outdoors, GPS measurements are added, providing a relatively accurate initial position for positioning, thereby further improving outdoor positioning accuracy.

Figure 2 provides a brief overview of our proposed framework. Firstly, after obtaining the IMU data, the IMU state prediction and pre-integration are updated, as detailed in Section 2.1. IMU pre-integration is used to construct a factor graph constraint, which will be used for joint optimization. Secondly, the dual lidar point clouds are obtained through external calibration and an adaptive timestamp synchronization algorithm, as detailed in Section 3.3. Since lidar has continuous measurement, the lidar measurement is obtained in continuous motion and it is almost certain that motion distortion will occur. When dual lidar point cloud is received, deskewing is applied on the dual lidar raw point cloud to obtain deskewed dual lidar point cloud [14]. To be ground-optimized and reduce the amount of calculation, point cloud segmentation is applied to filter out noise (e.g., moving people or objects) and the planar points and edge points are distinguished by calculating the curvature of the lidar point, as detailed in Section 2.2. After feature extraction, the dual lidar point cloud is registered to construct the lidar odometer factor. Then, the sliding window factor graph optimization method of local finite frame is used, with the IMU pre-integration factor and lidar odometer factor jointly optimized to realize the mutual parameter optimization of the dual lidar and IMU. Joint optimization is taken to obtain a maximum a posteriori (MAP) estimation of the IMU states, as detailed in Section 3.4, avoiding the drift from IMU propagation. To further improve outdoor positioning accuracy, GPS is introduced in this framework. When GPS is loosely coupled with IMU through an Unscented Kalman Filter (UKF), GPS measurements are used to further improve the positioning accuracy of IMU, with the optimized IMU state then used for the state estimation of IMU at the next time step. GPS measurements are added, providing a relatively accurate initial position for localization, thereby further improving outdoor localization accuracy [25]. However, the drift of lidar inertial odometry grows very slowly; in practice, GPS measurements are used when the estimated position covariance is larger than the received GPS position covariance. Finally, the output of the figure is the global map and localization.

3.3. Fusion of Horizontal Lidar and Vertical Lidar

To accurately fuse the horizontal and vertical lidar data, their individual coordinate systems must be transformed into a unified coordinate system, which is termed “frame coordinate system”. The horizontal lidar coordinate system is used as the frame coordinate system; the vertical lidar coordinate system is registered in this frame coordinate system through external parameter calibration, with the external parameters obtained by the CAD model of the scanning arm. This paper adopts the adaptive time synchronization algorithm to ensure that the timestamps of the horizontal and vertical lidars can be simultaneously output. The fusion process of dual lidar is divided into two parts: (1) external parameter calibration of the horizontal and vertical lidars and (2) the adaptive time synchronization algorithm. Both parts are described as follows.

3.3.1. External Parameter Calibration of Horizontal Lidar and Vertical Lidar

Assuming that ${\mathit{L}}_1$ is the horizontal lidar coordinate system and ${\mathit{L}}_2$ is the vertical lidar coordinate system, the horizontal lidar coordinate system is used as the frame coordinate system and the vertical lidar coordinate system ${\mathit{L}}_2$ is registered in this frame coordinate system ${\mathit{L}}_1$ through the rotation matrix ${ℝ}_{{\mathit{L}}_2}^{{\mathit{L}}_1}$ and translation matrix ${\mathit{H}}_{{\mathit{L}}_2}^{{\mathit{L}}_1}$, which can be computed by Equation (5):

$${\mathit{L}}_1={ℝ}_{{\mathit{L}}_2}^{{\mathit{L}}_1}\times {\mathit{L}}_2+{\mathit{H}}_{{\mathit{L}}_2}^{{\mathit{L}}_1}$$

(5)

where ${ℝ}_{{\mathit{L}}_2}^{{\mathit{L}}_1}$ is the rotation matrix and ${\mathit{H}}_{{\mathit{L}}_2}^{{\mathit{L}}_1}$ is the translation matrix which can be obtained by external parameters. From the hardware structure of the scanning arm, the vertical lidar coordinate system first rotates around the $\mathit{y}$-axis and then translates to the horizontal lidar coordinate system, where

$${ℝ}_{{\mathit{L}}_2}^{{\mathit{L}}_1}=\left[\begin{array}{ccc}\cos \mathit{\theta }& 0& \sin \mathit{\theta }\\ 0& 1& 0\\ -\sin \mathit{\theta }& 0& \cos \mathit{\theta }\end{array}\right]$$

(6)

where the angle of the rotation matrix $\mathit{\theta }=-{77.94}^{°}$ and the translation matrix ${\mathit{H}}_{{\mathit{L}}_2}^{{\mathit{L}}_1}=(\mathit{x},0,\mathit{z})$, with $\mathit{x}=-0.177 \mathrm{m}$ and $\mathit{z}=-0.213 \mathrm{m}$.

The point cloud of the lidar after external parameter calibration in the frame coordinate system is shown in Figure 3, with the point clouds of the vertical and horizontal lidars not affecting each other.

3.3.2. The Adaptive Time Synchronization Algorithm

In this paper, the lidar time synchronization is achieved through hardware time synchronization and GNSS is used as the master clock. However, there is a time offset between the two lidars. As the time offset is constant, an adaptive timestamp synchronization algorithm is used to solve it. The adaptive time synchronization algorithm is used to match its timestamp and realize the simultaneous localization and mapping of the lidars. The timestamps of the dual lidar is shown in Figure 4, where lidar_201/scan represents the timestamp of the horizontal lidar and lidar_202/scan represents the timestamp of the vertical lidar.

The output of the adaptive time synchronization algorithm only depends on the timestamp, not on the arrival time of dual lidar point cloud messages. It means that the adaptive time synchronization algorithm can be safely used on dual lidar point cloud messages that have suffered arbitrary processing delays. As shown in Figure 5, time goes from left to right, with the first row representing the horizontal lidar timestamp and the second row representing the vertical lidar timestamp. Each dot represents the lidar point cloud with the timestamp. The blue dot represents the pivot of the lidar point clouds, with the broken line linking the dual lidar point clouds in a set. Suppose the horizontal lidar point clouds’ queue is $\mathit{P}{\mathit{L}}_{\mathit{P}}$ and the vertical lidar point clouds’ queue is $\mathit{P}{\mathit{V}}_{\mathit{P}}$, with lidar point clouds inserted in a topic-specific queue ($\mathit{P}{\mathit{L}}_{\mathit{P}}$ or $\mathit{P}{\mathit{V}}_{\mathit{P}}$) as they arrive. Once each topic-specific queue contains at least one message, the latest message is found at the head of the queues, known as the pivot. Assume that the queue with pivots is $\mathit{P}{\mathit{L}}_{\mathit{P}}$, which matches $\mathit{P}{\mathit{V}}_{\mathit{P}}$, being the smallest difference between the timestamps, $\mathit{T}$. The two queues are combined into a set; finally, this set is synchronous output.

3.4. Tight Coupling of Dual Lidar and IMU

Although the fusion of dual lidar makes up for the shortcomings of lidar’s narrow FOV, the localization accuracy is improved. However, lidars mounted on moving vehicles suffer from motion distortion, which directly affects the localization accuracy. IMU can accurately measure the three-axis acceleration and three-axis angular velocity of moving vehicles, and provide a priori information for lidar odometry. However, in the case of long or short distances, the IMU error will continue to accumulate, which directly affects positioning accuracy. In this paper, the tight coupling of dual lidar and IMU is proposed to solve the above problems. The process is divided into two parts: (1) external parameter calibration of horizontal lidar and IMU and time synchronization, and (2) joint optimization, which are described as follows:

3.4.1. External Parameter Calibration of Horizontal Lidar and IMU and Time Synchronization

Similar to Section 3.3, the external parameters of the lidar and IMU are also obtained based on the CAD model. To accurately fuse data from the horizontal lidar, the vertical lidar, and the IMU, the IMU coordinate system $\mathit{I}$ is also registered in this frame coordinate system ${\mathit{L}}_1$ through the rotation matrix ${ℝ}_{\mathit{I}}^{{\mathit{L}}_1}$ and the translation matrix ${\mathit{H}}_{\mathit{I}}^{{\mathit{L}}_1}$. It can be seen from the structure of the scanning arm in Figure 1 that the IMU is located directly under the horizontal lidar. Therefore, the IMU coordinate system can be registered in the horizontal lidar coordinate system only by translation, where

$${ℝ}_{\mathit{I}}^{{\mathit{L}}_1}=\left[\begin{array}{ccc}1& 0& 0\\ 0& 1& 0\\ 0& 0& 1\end{array}\right]$$

(7)

where the translation matrix ${\mathit{H}}_{\mathit{I}}^{{\mathit{L}}_1}=[-\mathrm{0.62.66},0,-0.125]$.

In this paper, YIS510A IMU is used, which supports trigger correction of its internal clock by PPS signal. Lidar-IMU time synchronization is achieved through hardware time synchronization, with GNSS used as the master clock. GNSS can output PPS signals to unify the clock source of lidar-IMU and achieve time synchronization.

3.4.2. Joint Optimization

In this paper, a fixed-lag smoother and marginalization are introduced to obtain the optimal state. The fixed-lag smoother maintains a certain amount of IMU state in the sliding window, and the sliding window can effectively control the amount of calculation. When the new state enters the sliding window, the past state is marginalized. The state variable to be estimated for the whole window is $\mathit{X}=[{\mathit{X}}_{\mathit{\eta }}^{\mathit{W}},\dots ,{\mathit{X}}_{\mathit{\kappa }}^{\mathit{W}},{\mathbb{Z}}_{\mathit{I}}^{{\mathit{L}}_1}]$, where ${\mathit{X}}_{\mathit{\eta }}^{\mathit{W}}$ is the state of IMU at the starting point $\mathit{\eta }$ of the sliding window; ${\mathit{X}}_{\mathit{\kappa }}^{\mathit{W}}$ is the state of IMU at the end of the sliding window; and ${\mathbb{Z}}_{\mathit{I}}^{{\mathit{L}}_1}=[{ℝ}_{\mathit{I}}^{{\mathit{L}}_1},{\mathit{H}}_{\mathit{I}}^{{\mathit{L}}_1}]$ is the external parameter between lidar and IMU. Then, the following cost function with a Mahalanobis norm is minimized to obtain the MAP estimation of the states $\mathit{X}$,

$$\mathit{X}=\underset{\mathit{X}}{\min }\frac{1}{2}\{{\Vert {\mathit{u}}_{\mathit{o}}(\mathit{X})\Vert }^2+\sum _{\mathit{\chi }\in {\mathit{Q}}_{{\mathit{L}}_{\mathit{\beta }}}}{\Vert {\mathit{u}}_{\mathit{\varphi }}(\mathit{\chi },\mathit{X})\Vert }_{{\mathit{C}}_{{\mathit{L}}_{\mathit{\beta }}}^{\mathit{\chi }}}^2+\sum _{\mathit{\varsigma }\in \{\mathit{\eta },\dots ,\mathit{\kappa }-1\}}{\Vert {\mathit{u}}_{\mathit{\gamma }}({\mathit{Z}}_{\mathit{\varsigma }+1}^{\mathit{\varsigma }},\mathit{X})\Vert }_{{\mathit{C}}_{{\mathit{I}}_{\mathit{\varsigma }+1}}^{{\mathit{I}}_{\mathit{\varsigma }}}}^2\}$$

(8)

where ${\mathit{u}}_{\mathit{o}}(\mathit{X})$ is the prior information from marginalization [26]. ${\mathit{u}}_{\mathit{\varphi }}(\mathit{\chi },\mathit{X})$ is the residual of the relative lidar constraints that can be represented as point-to-plane distance [26], where $\mathit{\chi }\in {\mathit{Q}}_{\mathit{\beta }}$ is the residual for each relative lidar measurement with the previous correspondences. $\mathit{\beta }\in \left\{\mathit{\eta }+1,\dots ,\mathit{\kappa }\right\}$, $\mathit{\eta }+1$ and $\mathit{\kappa }$ are the timestamps of the lidar sweep next to the starting one and the current lidar sweep in the window, with the covariance matrix ${\mathit{C}}_{{\mathit{L}}_{\mathit{\beta }}}^{\mathit{\chi }}$ determined by the lidar accuracy [35]. ${\mathit{u}}_{\mathit{\gamma }}({\mathit{Z}}_{\mathit{\varsigma }+1}^{\mathit{\varsigma }},\mathit{X})$ is the residual of the IMU constraints, where

$${\mathit{u}}_{\mathit{\gamma }}({\mathit{Z}}_{\mathit{\varsigma }+1}^{\mathit{\varsigma }},\mathit{X})=\left[\begin{array}{c}{\mathit{R}}_{\mathit{\varsigma }}^{\mathit{T}}\left({\mathit{p}}_{\mathit{\varsigma }+1}-{\mathit{p}}_{\mathit{\varsigma }}-{\mathit{v}}_{\mathit{\varsigma }}\Delta \mathit{t}-\frac{1}{2}\mathit{g}\Delta {\mathit{t}}_{\mathit{\varsigma },\mathit{\varsigma }+1}^2\right)-\Delta {\mathit{p}}_{\mathit{\varsigma },\mathit{\varsigma }+1}\\ {\mathit{R}}_{\mathit{\varsigma }}^{\mathit{T}}\left({\mathit{v}}_{\mathit{\varsigma }+1}-{\mathit{v}}_{\mathit{\varsigma }}-\mathit{g}\Delta {\mathit{t}}_{\mathit{\varsigma },\mathit{\varsigma }+1}\right)-\Delta {\mathit{v}}_{\mathit{\varsigma },\mathit{\varsigma }+1}\\ 2{\left[\Delta {\mathit{q}}_{\mathit{\varsigma },\mathit{\varsigma }+1}{}^{-1}\otimes {\mathit{q}}_{\mathit{\varsigma }}^{-1}\otimes {\mathit{q}}_{\mathit{\varsigma }+1}\right]}_{\mathit{x}\mathit{y}\mathit{z}}\\ {\mathit{b}}_{\mathit{\varsigma }+1}^{\mathit{a}}-{\mathit{b}}_{\mathit{\varsigma }}^{\mathit{a}}\\ {\mathit{b}}_{\mathit{\varsigma }+1}^{\mathit{\omega }}-{\mathit{b}}_{\mathit{\varsigma }}^{\mathit{\omega }}\end{array}\right]$$

(9)

${\mathit{u}}_{\mathit{\gamma }}({\mathit{Z}}_{\mathit{\varsigma }+1}^{\mathit{\varsigma }},\mathit{X})$ can be obtained by IMU state prediction (Equation (2)) and IMU pre-integration (Equation (3)), and ${\left[\Delta {\mathit{q}}_{\mathit{\varsigma },\mathit{\varsigma }+1}{}^{-1}\otimes {\mathit{q}}_{\mathit{\varsigma }}^{-1}\otimes {\mathit{q}}_{\mathit{\varsigma }+1}\right]}_{\mathit{x}\mathit{y}\mathit{z}}$ stands for the vector part of a quaternion. With the continuous-time linearized propagation of the error states and the IMU noise parameters, the covariances ${\mathit{C}}_{{\mathit{I}}_{\mathit{\varsigma }+1}}^{{\mathit{I}}_{\mathit{\varsigma }}}$ of the pre-integration measurements and biases can be estimated. The cost function, in the form of a non-linear least-square, can be solved by the Gauss–Newton algorithm. Ceres Solver [36] is used for solving this nonlinear problem.

It can be seen from Section 3.2 that the new states $\mathit{X}$ are obtained by the joint optimization, which is used as the next state of the IMU, avoiding the drift from IMU propagation. The state of the IMU is applied to deskewing, thereby eliminating the motion distortion of the dual lidar.

4. Experiment

4.1. Data Acquisition Equipment

To verify the performance of the proposed high-precision SLAM framework based on the tight coupling of Dual Lidar Inertial Odometry (HSDLIO) for multi-scene applications, this paper describes a series of experiments to qualitatively and quantitatively analyze our proposed framework. The device runs on an industrial computer with a processor of i7-9700 through the port and network port, and the system configured of the industrial computer is ubuntu16.04. The scanning arm is installed on the backpack, with the industrial computer and battery installed in the backpack. The small display screen of the backpack shows the localization and mapping in real-time, with the backpack equipment shown in Figure 6. Data of three representative environments are collected, including the featureless corridor, stairs with height, and complex outdoor environments. To illustrate the effectiveness of the HSDLIO algorithm, LeGO-LOAM and LIO-SAM algorithms are compared with it.

4.2. Indoor Experiment 1

Indoor experiment 1 was carried out in a corridor with few feature points. Since there was no GPS signal indoors, SLAM was mainly carried out through the tight coupling of dual lidar odometry and IMU.

To highlight the performance of the HSDLIO algorithm, LeGO-LOAM and LIO-SAM algorithms are compared with it. Figure 7a shows the environment map of the corridor, while Figure 7b–d shows the overall mapping of LeGO-LOAM, LIO-SAM, and HSDLIO algorithms in the corridor scene, with the white points representing backpack motion trajectories. Compared with the HSDLIO algorithm in Figure 7d, the overall mapping of LeGO-LOAM (Figure 7b) has a significant deviation and the structure is unclear. Figure 8a,b shows that the mapping of LeGO-LOAM is incomplete because the lidar has a considerable drift in a scene with fewer feature points. The mapping results of LIO-SAM (Figure 7c) is close to the real environment, with its structure complete. The main reason is that LIO-SAM tightly couples the horizontal lidar and IMU to make up for the lidar drift. However, Figure 9a,b indicates the mapping result of HSDLIO is more abundant than LIO-SAM in the top surface and ground information. The reason is that HSDLIO fuses horizontal and vertical lidars so that its angle of FOV in both horizontal and vertical directions is 360°. The dual lidars and IMU are tightly coupled to make up for the lidar drift.

In the long corridor scene, compared with LeGO-LOAM and LIO-SAM, the structure of HSDLIO mapping is more complete and the point cloud information on the ground and top surface is richer. To verify the positioning accuracy of the HSDLIO, the trajectories of LeGO-LOAM, LIO-SAM, and HSDLIO are compared in Figure 10. It can be observed in Figure 10 that the LeGO-LOAM trajectory has drifted. The main reason is that in the LeGO-LOAM algorithm, the lidar odometry error continues to increase in a long corridor environment with fewer feature points. Both LIO-SAM and HSDLIO use the tight coupling of IMU and lidar for positioning, which improves positioning accuracy. HSDLIO further improves the accuracy of positioning through the tight coupling of dual lidars and IMU.

To further improve the positioning accuracy of HSDLIO, the relative translational error (RTE)—the distance from the start point to the end point—is introduced. In all experiments, the data collection started and ended at the same point; when the positioning accuracy of the device is very high, the start and end point will coincide. The RTE, when the backpack returns to the start point, is shown in

Table 1. Relative translational error when the backpack returns to the starting point (meters).

Algorithm	LeGO-LOAM	LIO-SAM	HSDLIO
${\mathit{x}}_{\mathit{o}}$	Fail	0.079	0.077
${\mathit{y}}_{\mathit{o}}$	Fail	0.092	0.090
${\mathit{z}}_{\mathit{o}}$	Fail	0.121	0.001
RTE	Fail	0.171	0.118

. The RTE can be computed by Equation (10):

$$\mathit{R}\mathit{T}\mathit{E}=\sqrt{{\mathit{x}}_{\mathit{o}}^2+{\mathit{y}}_{\mathit{o}}^2+{\mathit{z}}_{\mathit{o}}^2}$$

(10)

where ${\mathit{x}}_{\mathit{o}}={\mathit{x}}_{\mathit{s}}-{\mathit{x}}_{\mathit{e}}$, ${\mathit{y}}_{\mathit{o}}={\mathit{y}}_{\mathit{s}}-{\mathit{y}}_{\mathit{e}}$, ${\mathit{z}}_{\mathit{o}}={\mathit{z}}_{\mathit{s}}-{\mathit{z}}_{\mathit{e}}$, ${\mathit{x}}_{\mathit{s}}$, ${\mathit{y}}_{\mathit{s}}$, and ${\mathit{z}}_{\mathit{s}}$ are the coordinates of the starting point and ${\mathit{x}}_{\mathit{e}}$, ${\mathit{y}}_{\mathit{e}}$, and ${\mathit{z}}_{\mathit{e}}$ are the coordinates of the end point. The result of LeGO-LOAM is not shown because its trajectory has severely shifted. Table 1 shows that the error of HSDLIO in the $\mathit{x}$ and $\mathit{y}$ direction is similar to LIO-SAM, but the accuracy in the $\mathit{z}$ direction is higher than LIO-SAM and the RTE of HSDLIO is smaller than LIO-SAM.

4.3. Indoor Experiment 2

Indoor experiment 2 aimed to show the effectiveness of HSDLIO in the indoor environment with a certain height information. In this experiment, the backpack was carried up and down four flights of stairs. Figure 11a shows the stairs scene, while Figure 11b–d shows the overall mapping of LeGO-LOAM, LIO-SAM, and HSDLIO algorithms. The white points are backpack localization trajectories. LeGO-LOAM (Figure 11b) cannot obtain the structure of the stairs. Because the vertical FOV of the horizontal lidar is relatively narrow, the mapping result of LIO-SAM (Figure 11c) does not show the height information. With the help of the vertical lidar and the tight coupling of the IMU, the mapping of HSDLIO (Figure 11d) is the most complete and accurate, which is closer to the real stairs’ scene.

Figure 12 shows the trajectories obtained by LeGO-LOAM, LIO-SAM, and HSDLIO. The red, black, and green lines represent the trajectories of HSDLIO, LeGO-LOAM, and LIO-SAM, respectively, with the trajectory of HSDLIO approximating the real trajectory. LeGO-LOAM has no IMU correction, which causes the drift of the trajectory. Although the trajectory of LIO-SAM does not drift, due to the narrow vertical FOV of the horizontal lidar, a significant error occurs in the $\mathit{z}$ direction information. Comparing the height information of LIO-SAM and HSDLIO in the $\mathit{z}$ direction in Figure 13, it can be seen that when going up and down four floors of stairs, the localization results of LIO-SAM provide no height information and also indicate a great drift, while HSDLIO provides accurate height information. Therefore, the tight coupling of dual lidars and IMU makes the advantages of HSDLIO clearly apparent.

Table 2. Relative translational error when the backpack returns to the starting point (meters).

Algorithm	LeGO-LOAM	LIO-SAM	HSDLIO
${\mathit{x}}_{\mathit{o}}$	Fail	0.033	0.011
${\mathit{y}}_{\mathit{o}}$	Fail	0.036	0.024
${\mathit{z}}_{\mathit{o}}$	Fail	0.062	0.004
RTE	Fail	0.078	0.026

shows that the error of HSDLIO in the $\mathit{x}$ and $\mathit{y}$ direction is smaller than LIO-SAM, but the accuracy in the $\mathit{z}$ direction is much higher than LIO-SAM and the RTE of HSDLIO is much smaller than LIO-SAM.

4.4. Outdoor Experiment

The outdoor experiment was carried out on a city street. This scene has rich feature points, so the drift of lidar odometry grows very slowly. The localization accuracy of LeGO-LOAM, LIO-SAM, and HSDLIO have very little difference, but the mapping of HSDLIO can provide finer structural details.

The trajectories obtained by LeGO-LOAM, LIO-SAM, and HSDLIO are shown in Figure 14. Because there is more feature information outdoors, the drift of lidar odometry grows very slowly. LeGO-LOAM can obtain relatively accurate localization only through horizontal lidar. In LIO-SAM and HSDLIO, GPS measurement is introduced by loosely coupling with IMU through UKF, which can provide relatively accurate initial positioning. The localization accuracy of HSDLIO and LIO-SAM is higher than LeGO-LOAM. The available data in

Table 3. Relative translational error when the backpack returns to the starting point (meters).

Algorithm	LeGO-LOAM	LIO-SAM	HSDLIO
RTE	0.082	0.036	0.031

further supports the performance of the HSDLIO algorithm.

The 3D city street scene is shown in Figure 15a. In Figure 15a–d, the marks in the white circle represent trees and the marks in the yellow circle represent the building structure. Figure 15b–d shows the overall mapping of LeGO-LOAM, LIO-SAM, and HSDLIO algorithms in the city street scene, with the white points being their localization trajectories. Figure 15b,c shows that the overall effect of the mapping lacks building height information, while the structure of buildings and trees is blurred. The mapping details are analyzed at the marker, compared with the mapping results of LeGO-LOAM and LIO-SAM (Figure 15b,c), while the HSDLIO mapping result is shown in Figure 15d. It can be seen in Figure 15d that the structure of the trees and buildings is complete and has finer details.

To further demonstrate the advantages of HSDLIO mapping, the mapping details of the building are shown in Figure 16, with the mapping results of LeGO-LOAM and LIO-SAM lacking the structural details and height information of the building. However, the mapping result of HSDLIO show finer structural details and more height information of the building.

5. Conclusions

This paper proposes a high-precision SLAM framework for multi-scene applications. In this framework, dual lidars are fused to make up for the shortcomings of lidars’ narrow FOV and hence improve the completeness of mapping. Meanwhile, dual lidars and IMU are tightly coupled to improve the localization accuracy. Extensive experiments were carried out and the results showed that compared with the commonly used LeGO-LOAM and LIO-SAM methods, our proposed method can produce more precise localization and more accurate mapping results with more details.