Map Construction Based on LiDAR Vision Inertial Multi-Sensor Fusion

Abstract

In order to make up for the shortcomings of independent sensors and provide more reliable estimation, a multi-sensor fusion framework for simultaneous localization and mapping is proposed in this paper. Firstly, the light detection and ranging (LiDAR) point cloud is screened in the front-end processing to eliminate abnormal points and improve the positioning and mapping accuracy. Secondly, for the problem of false detection when the LiDAR is surrounded by repeated structures, the intensity value of the laser point cloud is used as the screening condition to screen out robust visual features with high distance confidence, for the purpose of softening. Then, the initial factor, registration factor, inertial measurement units (IMU) factor and loop factor are inserted into the factor graph. A factor graph optimization algorithm based on a Bayesian tree is used for incremental optimization estimation to realize the data fusion. The algorithm was tested in campus and real road environments. The experimental results show that the proposed algorithm can realize state estimation and map construction with high accuracy and strong robustness.

1. Introduction

At present, the simultaneous localization and mapping (SLAM) algorithm plays an important role in mobile robot navigation. The mainstream SLAM technologies fall into two categories: laser and vision SLAM. The laser-based SLAM has some advantages, such as its high resolution, strong anti-interference ability and accurate obstacle distance information. However, LiDAR is vulnerable to the corridor effect in unstructured environments because it is difficult to provide reliable motion estimation. Therefore, in the process of building a point cloud map, motion distortion will be caused by receiving distance measurements data at different times. Although vision-based SLAM has rich image feature information and a low repeatability of spatial features, meaning that the map has better global positioning, the positioning accuracy will be reduced because the image sensor is easily affected by the light, and there are few texture features in the environment. For the problem mentioned above, the mobile robot estimates its position information by wheel odometry and inertial measurement units (IMU), which improve the robustness and stability of the system. For the realization of accurate positioning and navigation, as well as the avoidance of the influence of environmental light changes and rapid scene movement, a multi-sensor fusion scheme has become the main research direction of SLAM technology.

In this paper, the feature extraction method in LiDAR front-end processing is mainly inspired by lightweight and ground-optimized LiDAR odometry and mapping (LEGO-LOAM). It divides the point cloud of LiDAR into ground points and segmentation points. Then, it divides the points into edge points and flat points according to smoothness. In this way, the efficiency and accuracy of the drawing construction can be improved. However, because LEGO-LOAM uses a rotating LiDAR velodyne, the number of point clouds is much smaller than the solid-state LIVOX used in this paper. We therefore screen the point clouds to eliminate unqualified points before extracting the LiDAR feature points, which effectively reduces the loss of computing resources. Note that LEGO-LOAM is a loosely-coupled system, and IMU measurements are not used in the optimization steps. In this paper, the IMU factor is imported into the factor graph for global optimization. Visual-LiDAR odometry and mapping (VLOAM) integrates the monocular camera and LiDAR information to build graphs, but does not add closed-loop detection. In this paper, closed-loop detection is set to periodically eliminate the drift caused by the robot.

This paper proposes a multi-sensor coupled SLAM framework which has four modules, as shown in Figure 1. The front-end module obtains images and laser point clouds from the camera and LiDAR, respectively. For the image data, feature point matching is obtained by feature point extraction and feature point tracking between two adjacent frames. For the LiDAR data, we filter and eliminate the noise point cloud, and then extract and match the features of the laser point cloud. The vision inertial LiDAR odometry module uses the visual features to compensate the LiDAR features, increasing the number of LiDAR feature points, in order to improve the mapping and positioning accuracy. In the pose estimation, the feature points of successive key frames are aligned and registered. Then, the map building module performs point cloud registration and a map update to build a large-scale high-precision map. The closed-loop detection module detects whether the trajectory is closed. As a closure factor, the pose relation of the closed position will be improved into the factor graph. Finally, the initial factor, registration factor, IMU factor and closed-loop factor are inserted into the factor graph for optimization. The motion trajectory can be obtained after incremental smoothing. The main contributions of this paper are as follows:

(1)

Before the LiDAR feature extraction, the point clouds are screened to eliminate the points that do not meet the conditions, so as to effectively reduce the loss of computing resources.

(2)

The visual features filtered through the intensity value of the point clouds are used to supplement the laser point cloud features, improving the accuracy of positioning and mapping.

(3)

A close-coupled LiDAR vision inertial system based on factor graph optimization is proposed in order to realize multi-sensor information fusion and construct a three-dimensional point cloud map with high robustness in real-time.

The rest of the paper is organized as follows. First, Section 2 discusses previous work. Secondly, Section 3 describes the details, state estimation based on factor graph optimization, front-end processing of the vision and LiDAR, IMU pre-integration, odometry of fusion vision IMU LiDAR, map construction, closed-loop detection and factor graph optimization. Then, Section 4 presents the experimental verifications. Finally, Section 5 summarizes the full text.

2. Previous Work

Zhao [1] used the feature parameterization of the Fourier series to optimize the irregular closed features, and reduced the time dependency of the initial guess through a subgraph connection. Mur-Artal et al. proposed a monocular SLAM method based on oriented fast and rotated brief (ORB) features [2], ORB-SLAM [3], which can realize the real-time positioning of indoor and outdoor environments by monocular cameras, and can generate a compact traceable map. In order to solve the problem that monocular cameras can’t restore the realistic scale, Mur-Artal et al. came up with an approach called ORB-SLAM2 [4], which added binocular cameras and red-green-blue-depth (RGB-D) cameras to estimate the depth. Vladyslav et al. put forward the system called binocular vision inertial odometry direct visual-inertial (DVIO) [5], which estimates the attitude, speed and IMU noise of the camera by minimizing the photometric error, as well as taking the minimization of the energy function as the optimization goal. This method is not only better than the algorithm relying only on vision or loose coupling but also has high accuracy under rapid motion and significant illumination changes. Qin et al. proposed the tightly coupled SLAM system [6], which is a sliding window based on visual inertia. This approach is a multi-sensor state estimator based on nonlinear optimization, supporting the data fusion of multiple visual inertial sensors and realizing high-precision autonomous positioning. Campos et al. extended ORB-SLAM2 by fusing camera and IMU measurement. As a result, ORB-SLAM3 [7] was created. However, the cost of high-end IMU is very high, while low-precision IMU is vulnerable to Gaussian white noise and drift. Qin et al. came up with a powerful monocular vision inertial state estimator versatile monocular visual-inertial system monocular (VINS-MONO) [8]. This system uses a local nonlinear optimization algorithm based on a sliding window to keep the computational complexity constant by constraining the number of key frames in the sliding window. At the same time, it can also estimate the monocular scale factor, IMU bias and external parameters between IMU and the camera in real time. Therefore, this system has very good robustness.

The fusion of LiDAR inertial odometry (LIO) is usually divided into loose coupling and tight coupling. Loosely-coupled LIO methods typically process the LiDAR and IMU measurements separately and fuse their results later. The 3D laser LiDAR odometry and mapping (LOAM) [9] proposed by Zhang and the LEGO-LOAM [10] based on ground optimization proposed by Shan et al. are loosely coupled methods. LOAM takes the pose integrated from IMU measurements as the initial estimate for the LiDAR scan registration. However, LOAM directly stores the global voxel map rather than the local map, making it difficult to perform closed-loop detection to correct the drift. A common procedure in the loosely-coupled approach is to obtain a pose measurement by registering a new scan and then fusing the pose measurement with the IMU measurements. The separation between scan registration and data fusion reduces the computation load. However, it ignores the correlation between the system’s other states and the pose of the new scan. Moreover, in the case of featureless environments, the scan registration could degenerate in certain directions, and causes unreliable fusion in later stages. The difference between our work and the above is that LOAM directly stores the global voxel map, which makes it impossible for closed-loop detection to correct the cumulative error. This article uses incremental map construction to compare the current point cloud scan with the constructed global map. The point clouds are superimposed together to eliminate the cumulative error caused by the pose estimation. Unlike the loosely-coupled method, the tightly-coupled LiDAR-inertial odometry methods typically fuse the raw feature points of LiDAR with IMU data. There are two main approaches to tightly-coupled LIO: optimization-based and filter-based. LIO-SAM [11] obtains the globally consistent pose of the robot through factor graph optimization. This method uses frame and local map matching instead of LOAM frame and global map matching, which improves the matching efficiency. LIO-Mapping [12] embeds a rotation-constrained thinning algorithm to further align the LiDAR attitude with the global map, but the optimization of all of the measured values can’t ensure real-time performance. Zhao [13] proposed the LIOM package, which uses a similar graph optimization but based on edge and plane features. Geneva [14] used graph optimization with IMU pre-integration constraints and plane constraints from LiDAR feature points. For filter-based methods, Bry [15] used a Gaussian Particle Filter (GPF) to fuse the data of IMU and a planar 2D LiDAR. This method was also used in the Boston Dynamics Atlas humanoid robot.

Recently, LiDAR-visual-inertial systems have attracted increased attention due to their robustness in sensor-degraded tasks [16]. J Zhang et al. raised VLOAM [17], which loosely integrates monocular cameras and LiDAR. Similarly to LOAM, it does not add closed-loop detection. SEO [18] established LiDAR and visual maps in parallel, and used odometry residuals to maintain the global consistency between LiDAR voxel maps and operating environments. Through this close-coupling method, odometry can be estimated more accurately. Jiang et al. [19] developed a cost function execution graph optimization considering LiDAR data and image data, and constructed a 2.5D map to accelerate cycle detection. Lin et al. designed the system LOAM-LIVOX [20]. Through processing on the front and back ends of SLAM, they solved some challenges brought by this non-repetitive scanning LiDAR. High accuracy can be achieved at low speeds, while pose estimation will drift greatly in high-speed operation. In order to improve the loop detection and graphics optimization performance of point cloud matching, Chen [21] built a LiDAR and vision fusion SLAM backend, using the geometric and visual features of the LiDAR to complete loop detection, and re-matching the point cloud to verify closed loop detection and complete the graph optimization. Wang [22] proposed direct visual LiDAR odometry and mapping, which is the first framework that combines a depth-enhanced direct visual odometry module and the LiDAR mapping module into location and mapping. It takes advantage of the efficiency of the direct visual odometry module and the accuracy of the LiDAR scan-to-map matching method.

3. Methods

3.1. State Estimation Based on Factor Graph Optimization

The world coordinate system is represented as $\mathrm{W},$ and the carrier coordinate system is represented as $\mathrm{B}$. Suppose that the starting time of the LiDAR’s i-th scan is $t_{\mathrm{i}}$, scanned point clouds are marked as ${ℱ}_{\mathrm{i}}$, and the data collected by IMU in [$t_{\mathrm{i}},t_{\mathrm{j}}$] is recorded as ${ℐ}_{\left(\mathrm{i},\mathrm{j}\right)}$; the motion state of the system ${\mathrm{x}}_{\mathrm{i}}$ can be expressed as

$${\mathrm{x}}_{\mathrm{i}}={\left[R_{\mathrm{i}}^{\mathrm{T}},P_{\mathrm{i}}^{\mathrm{T}},V_{\mathrm{i}}^{\mathrm{T}},b_{\mathrm{i}}^{\mathrm{T}}\right]}^{\mathrm{T}}$$

(1)

where the rotation matrix is $R\in SO\left(3\right)$, the position vector is $P\in {ℝ}^3$, the velocity vector is $V\in {ℝ}^3$ and zero bias is $b$. $b$ is determined by gyro zero bias $b_{\mathrm{g}}$ and accelerometer zero bias $b_{\mathrm{a}}$.

The rigid body transformation from the carrier coordinate system $\mathrm{B}$ to the world coordinate system $\mathrm{W}$ is carried out under $\mathrm{T}=[R|p]\in SE\left(3\right)$. In order to reduce the computational complexity, the key frame is selected as the state quantity of the optimization estimation. Define ${\mathcal{K}}_{\mathrm{k}}$ is all keyframes at time ${\mathrm{t}}_{\mathrm{k}}$; the corresponding state quantity is ${\mathcal{X}}_{\mathrm{k}}={\left\{{\mathrm{x}}_{\mathrm{i}}\right\}}_{\mathrm{i}\in {\mathcal{K}}_{\mathrm{k}}}$, and the observation is ${\mathcal{Z}}_{\mathrm{k}}={\left\{{ℱ}_{\mathrm{i}},{ℐ}_{\left(\mathrm{i},\mathrm{j}\right)}\right\}}_{\left(\mathrm{i},\mathrm{j}\right)\in {\mathcal{K}}_{\mathrm{k}}}$. The state estimation problem can be expressed as the maximum a posteriori problem. Given the observation information and the prior information, estimate the posterior probability of ${\mathcal{X}}_{\mathrm{k}}$:

$$\mathrm{p}\left({\mathcal{X}}_{\mathrm{k}}|{\mathcal{Z}}_{\mathrm{k}}\right)\propto \mathrm{p}\left({\mathcal{X}}_0\right)\mathrm{p}\left({\mathcal{Z}}_{\mathrm{k}}|{\mathcal{X}}_{\mathrm{k}}\right)=\mathrm{p}\left({\mathcal{X}}_0\right){\displaystyle \prod }_{\left(\mathrm{i},\mathrm{j}\right)\in {\mathcal{K}}_{\mathrm{k}}}\left({ℱ}_{\mathrm{i}},{ℐ}_{\left(\mathrm{i},\mathrm{j}\right)}|{\mathcal{X}}_{\mathrm{k}}\right)=\mathrm{p}\left({\mathcal{X}}_0\right){\displaystyle \prod }_{\left(\mathrm{i},\mathrm{j}\right)\in {\mathcal{K}}_{\mathrm{k}}}\left({ℐ}_{\left(\mathrm{i},\mathrm{j}\right)}{|\mathrm{x}}_{\mathrm{i}},{\mathrm{x}}_{\mathrm{j}}\right){\displaystyle \prod }_{\mathrm{i}\in {\mathcal{K}}_{\mathrm{k}}}\mathrm{p}\left({ℱ}_{\mathrm{i}}{|\mathrm{x}}_{\mathrm{i}}\right)$$

(2)

The maximum a posteriori probability of the variable factor can be derived from Equation (2):

$$\widehat{{\mathcal{X}}_{\mathrm{k}}}=\begin{array}{c}\mathrm{argmin}\\ {\mathcal{X}}_{\mathrm{k}}\end{array}\left(-\ln \mathrm{p}\left({\mathcal{X}}_{\mathrm{k}}|{\mathcal{Z}}_{\mathrm{k}}\right)\right)=\begin{array}{c}\mathrm{argmin}\\ {\mathcal{X}}_{\mathrm{k}}\end{array}\left(\begin{array}{c}\parallel {𝓇}_0{\parallel }_{{\mathsf{\Sigma }}_0}^2+{\displaystyle {\displaystyle \sum }_{\left(\mathrm{i},\mathrm{j}\right)\in {\mathcal{K}}_{\mathrm{k}}}}\parallel {𝓇}_{{ℐ}_{\left(\mathrm{i},\mathrm{j}\right)}}{\parallel }_{{\mathsf{\Sigma }}_{{ℐ}_{\left(\mathrm{i},\mathrm{j}\right)}}}^2+\\ {\displaystyle {\displaystyle \sum }_{\begin{array}{c}i\\ \in {\mathcal{K}}_{\mathrm{k}}\end{array}}}\parallel {𝓇}_{ℱ}{\parallel }_{{\mathsf{\Sigma }}_{{ℱ}_{\mathrm{i}}}}^2+{\displaystyle {\displaystyle \sum }_{\mathrm{i}\in {\mathcal{K}}_{\mathrm{k}}}}\parallel {𝓇}_{{ℱ}_{\left(\mathrm{i},\mathrm{j}\right)}}{\parallel }_{{\mathsf{\Sigma }}_{ℱ\left(\mathrm{i},\mathrm{j}\right)}}^2\end{array}\right)$$

(3)

where ${𝓇}_0,{𝓇}_{{ℐ}_{\left(\mathrm{i},\mathrm{j}\right)}}, {𝓇}_{{ℱ}_{\mathrm{i}}}, {𝓇}_{{ℱ}_{\left(\mathrm{i},\mathrm{j}\right)}}$ represents the residual associated with the measured value. ${\mathsf{\Sigma }}_0,{\mathsf{\Sigma }}_{{ℐ}_{\left(\mathrm{i},\mathrm{j}\right)}},{\mathsf{\Sigma }}_{{ℱ}_{\mathrm{i}}},{\mathsf{\Sigma }}_{ℱ\left(\mathrm{i},\mathrm{j}\right)}$ is the corresponding covariance matrix, and the items in Equation (3) will be described in detail later.

3.2. Vision and LiDAR Front End

3.2.1. Visual Front End

The visual feature points used in this article are ORB feature points, which are composed of two parts: an improved FAST corner point and a BRIEF [23] descriptor. The improved FAST corner points are used to extract pixels with obvious differences in the image. The gray-scale centroid algorithm is added to improve the rotation invariance of the feature points. In addition, the characteristics of the pixels are judged by the BRIEF descriptor to determine the position of the ORB feature points in the image. The principle of FAST corner detection is that if there are enough pixels on the neighborhood circle at a certain point, the gray value of which is quite different from the gray value of the center point of the neighborhood circle, the point may be a key point. Select a certain pixel p in the image as the center, set its brightness value as $I_p$ and the threshold T as 20% of the brightness value $I_p$. Select 16 pixel points on a circle with radius 3 if there are 12 consecutive pixels on the circle. If the brightness value of the point is outside the range $\left(I_p-T,I_p+T\right)$, then this pixel point p can be used as a feature point. The number of corner points extracted by FAST is too large and not very stable. This article specifies the number of corner points to be extracted N. By calculating the Harris response value of the corner points, the response values are arranged from large to small, and the first N is selected. One corner point with the largest response value is used as the final feature point to be extracted in order to eliminate the problem of excessive corner points and instability. In addition, the corner points extracted by FAST do not have scale information and lack directionality. This article constructs an image pyramid. The bottom layer of the pyramid is the original image. For each upper layer, the image is down-sampled with a scaling factor of 1.2 to obtain a total of 8 images. In feature matching, the scale invariance is achieved by matching images on different layers. Calculate the gray centroid in the neighborhood S of the feature point and use it as the end of the vector. The direction of the vector is used as the direction of the feature point. The gray moment of the neighborhood S of the feature point is defined as follows:

$$m_{pq}={\displaystyle \sum }_{x,y\in B}{x}^p{y}^{q}I\left(x,y\right)$$

(4)

where x and y are the coordinates of the pixel, and $I\left(x,y\right)$ is the gray value of the pixel.

Then, the gray-scale centroid C of the image and the dominant direction θ of the feature point can be obtained by using the gray-scale moment of the image as follows:

$$C=\left(\frac{m_{10}}{m_{00}},\frac{m_{01}}{m_{00}}\right)$$

(5)

$$\theta =arctan\left(\frac{m_{01}}{m_{10}}\right)$$

(6)

The FAST feature point does not have the defect of directionality through the improvement of the gray centroid. After extracting the key points, use the BRIEF descriptor to describe the environmental information around the key points, which is a binary descriptor. This description method does not have rotation invariance. It is necessary to add the key point direction in the improved FAST to obtain the rotation characteristics of the descriptor. Then, rotate the image to the dominant direction of the feature point in order to calculate the BRIEF descriptor to obtain the rotation invariance. Finally, using the previous image frame as the initial posture, the optical flow method is used to continuously track the new image frame. Moreover, the rigid body transformation relationship between two adjacent key frames is estimated.

3.2.2. LiDAR Front End

By calculating the characteristics of each 3D point cloud $F\left(x,y,z\right)$, depth D is the distance to each point measured by LiDAR:

$$D\left(F\right)=\sqrt{x^2+y^2+z^2}$$

(7)

Laser deflection Angle φ is the angle between the x axis and laser ray:

$$\varphi \left(F\right)=ta{n}^{-1}\sqrt{\frac{y^2+z^2}{x^2}}$$

(8)

The reflection intensity of LiDAR $I\left(F\right)$ is:

$$I\left(F\right)=R/D{\left(F\right)}^2$$

(9)

where R is the reflectivity of the object measured by LiDAR. The reflection intensity $I\left(F\right)$ is smaller, which indicates that the point is far away from the LiDAR sensor or the object’s reflectivity R is low.

(1)

In order to improve the accuracy of the positioning and mapping, the point cloud scanned by LiDAR is processed, and the irrelevant point clouds are eliminated by the following rules.

The points at the edge of the LiDAR angle of view are eliminated.

(2)

Eliminate the points with particularly large or small reflection intensities. Excessive intensity will lead to the saturation or distortion of the receiving circuit, and a too-small intensity will lead to the reduction of the signal-to-noise ratio, both of which will reduce the ranging accuracy.

(3)

Eliminate the points hidden behind the object, otherwise it will lead to the wrong edge features.

The remaining points are collectively referred to as candidate points. The candidate point cloud is projected into the distance image to judge the vertical dimension characteristics of the pixel points and whether they belong to the ground laser scanning range. The original point cloud scanned by LiDAR is shown in Figure 2a. According to this, the point cloud can be divided into ground points and split points, as shown in Figure 2b. When the point cloud is projected into the image, the distance r from the pixel to the sensor can be calculated as ${\mathrm{r}}_{\mathrm{i}}$. At the same time, the continuous point set in the distance image is collectively referred to as $\mathrm{S}$. Calculate the smoothness $\mathrm{c}$ of each point in the point set, and set the threshold $\mathrm{c}\mathrm{th}$ to distinguish the characteristics of the points. Those with a smoothness greater than the threshold are edge points; otherwise, they are plane points. In order to evenly extract features from all directions, the LiDAR scanning data is divided into two subsets in the horizontal direction. The smoothness of the points in the subset is sorted, and the edge feature point ${ℱ}_{\mathrm{i}}^{\mathrm{e}}$ with the largest roughness and not belonging to the ground is selected from each row of the subset. Then, mark the plane feature point ${ℱ}_{\mathrm{i}}^{\mathrm{f}}$ with the minimum roughness as a ground or segment point in the same way, as shown in Figure 2c.

$$\mathrm{c}=\frac{1}{\left|S\right|\cdot \parallel {\mathrm{r}}_{\mathrm{i}}\parallel }\parallel {\displaystyle \sum }_{\mathrm{j}\in S,\mathrm{j}\ne \mathrm{i}}\left({\mathrm{r}}_{\mathrm{j}}-{\mathrm{r}}_{\mathrm{i}}\right)\parallel$$

(10)

In order to create the point cloud images scanned by adjacent LiDAR through the sliding window method, $\mathrm{n}$ latest sub key frames $\left\{{\mathcal{K}}_{\mathrm{i}-\mathrm{n}},\cdots ,{\mathcal{K}}_{\mathrm{i}}\right\}$ are extracted for the estimation, and the sub keyframe transformed by $\left\{{\mathrm{T}}_{\mathrm{i}-\mathrm{n}},\cdots ,{\mathrm{T}}_{\mathrm{i}}\right\}$ is merged into a voxel map ${\mathrm{M}}_{\mathrm{i}}$ medium. ${\mathrm{M}}_{\mathrm{i}}^{\mathrm{e}}$ and ${\mathrm{M}}_{\mathrm{i}}^{\mathrm{f}}$ are the set of all of the edge and plane features in this process, and feature point set $\prime {ℱ}_{\mathrm{i}}^{\mathrm{e}}$ and $\prime {ℱ}_{\mathrm{i}}^{\mathrm{f}}$ are shown in Figure 2d.

3.3. IMU Pre-Integration

The observation model of IMU is

$$\hat{\mathrm{a}}\left(\mathrm{t}\right)=\mathrm{a}\left(t\right)+{\mathrm{b}}_{\mathrm{a}}\left(\mathrm{t}\right)+{\mathrm{R}}_{\mathrm{WB}}^{⊺}\left(\mathrm{t}\right){\mathrm{g}}^{\mathrm{w}}+{\mathsf{\eta }}_{\mathrm{a}}$$

(11)

$$\hat{\mathsf{\omega }}\left(t\right)=\mathsf{\omega }\left(\mathrm{t}\right)+{\mathrm{b}}_{\mathrm{g}}\left(\mathrm{t}\right)+{\mathsf{\eta }}_{\mathsf{\omega }}$$

(12)

where ${\mathsf{\eta }}_{\mathrm{a}},{\mathsf{\eta }}_{\mathsf{\omega }}$ represents observation noise, ${\mathrm{R}}_{\mathrm{WB}}^{⊺}$ represents the rotation matrix from $\mathrm{B}$ to $\mathrm{W}$, and ${\mathrm{g}}^{\mathrm{w}}$ represents the gravitational acceleration in the world coordinate system. The presence of noise and deviation will affect the final measured value. Assuming that noise ${\mathsf{\eta }}_{\mathrm{a}}$ and ${\mathsf{\eta }}_{\mathsf{\omega }}$ obey Gaussian distribution, acceleration offset ${\mathrm{b}}_{\mathrm{a}}$ and gyro offset ${\mathrm{b}}_{\mathrm{g}}$ are modeled as a random walk:

$${\dot{\mathrm{b}}}_{\mathrm{a}}={\mathrm{n}}_{{\mathrm{b}}_{\mathrm{a}}},{\dot{\mathrm{b}}}_{\mathrm{g}}={\mathrm{n}}_{{\mathrm{b}}_{\mathrm{g}}}$$

(13)

where ${\mathrm{n}}_{{\mathrm{b}}_{\mathrm{a}}}$ and ${\mathrm{n}}_{{\mathrm{b}}_{\mathrm{g}}}$ are subject to Gaussian distribution, and the corresponding moments of two consecutive key frames ${\mathcal{K}}_{\mathrm{i}}$ and ${\mathcal{K}}_{\mathrm{j}}$ are respectively ${\mathrm{b}}_{\mathrm{i}}$ and ${\mathrm{b}}_{\mathrm{j}}$. From the state quantity at moment ${\mathrm{b}}_{\mathrm{i}}$ to the state quantity at moment ${\mathrm{b}}_{\mathrm{j}}$, displacement P and velocity V can be obtained; the movement model of quaternion $q$ is as follows:

$$\begin{array}{cc}{\mathrm{P}}_{b_{\mathrm{j}}}^w=& {\mathrm{P}}_{b_i}^w+V_{b_i}^w\mathsf{\Delta }t+\frac{1}{2}{{\displaystyle \iint }}_{\mathrm{t}\in \left[{\mathrm{b}}_{\mathrm{i}},{\mathrm{b}}_{\mathrm{j}}\right]}{\mathrm{R}}_{\mathrm{t}}^w\left(\hat{\mathrm{a}}-{\mathrm{b}}_{{\mathrm{a}}_{\mathrm{t}}}\right)-g^w{\mathrm{dt}}^2\hfill \\ & {\mathrm{V}}_{b_{\mathrm{j}}}^w=V_{b_i}^w+{{\displaystyle \int }}_{\mathrm{t}\in \left[b_{\mathrm{i}},b_{\mathrm{j}}\right]}{\mathrm{R}}_{\mathrm{t}}^w\left(\hat{\mathrm{a}}-{\mathrm{b}}_{{\mathrm{a}}_{\mathrm{t}}}\right)-g^w\mathrm{dt}\hfill \\ & {\mathrm{q}}_{b_{\mathrm{j}}}^w={\mathrm{q}}_{b_{\mathrm{i}}}^w\mathsf{⨂}{{\displaystyle \int }}_{\mathrm{t}\in \left[b_{\mathrm{i}},b_{\mathrm{j}}\right]}\frac{1}{2}\mathsf{\Omega }\left(\hat{\mathsf{\omega }}-{\mathrm{b}}_{{\mathrm{w}}_{\mathrm{t}}}\right){\mathrm{q}}_{\mathrm{t}}^{b_i}\mathrm{dt}\hfill \end{array}$$

(14)

where ${\mathrm{P}}_{b_{\mathrm{j}}}^w$ is the pose of IMU in the world coordinate system at ${\mathrm{b}}_{\mathrm{j}}$, ${\mathrm{V}}_{b_{\mathrm{j}}}^w$ is the velocity of IMU in the world coordinate system at ${\mathrm{b}}_{\mathrm{j}}$, and ${\mathrm{q}}_{b_{\mathrm{j}}}^w$ is the quaternion of IMU in the world coordinate system at ${\mathrm{b}}_{\mathrm{j}}$. If we multiply both sides of the three equations of (14) by $R_w^{b_i}$, the integral part depends only on the measurement of IMU.

$$\begin{array}{c}{R}_w^{b_i}{\mathrm{P}}_{b_{\mathrm{j}}}^w=R_w^{b_i}\left({\mathrm{P}}_{b_i}^w+V_{b_i}^w\mathsf{\Delta }t-\frac{1}{2}{g}^w\mathsf{\Delta }{t}^2\right)+{\mathsf{\alpha }}_{{\mathrm{b}}_{\mathrm{j}}}^{{\mathrm{b}}_{\mathrm{i}}}\\ R_w^{b_i}{\mathrm{V}}_{b_{\mathrm{j}}}^w=R_w^{b_i}(V_{b_i}^w-g^w\mathsf{\Delta }t)+{\mathsf{\beta }}_{{\mathrm{b}}_{\mathrm{j}}}^{{\mathrm{b}}_{\mathrm{i}}}\\ q_w^{b_i}{\mathrm{q}}_{b_{\mathrm{j}}}^w={\mathsf{\gamma }}_{{\mathrm{b}}_{\mathrm{j}}}^{{\mathrm{b}}_{\mathrm{i}}}\end{array}$$

(15)

where ${\mathsf{\alpha }}_{{\mathrm{b}}_{\mathrm{j}}}^{{\mathrm{b}}_{\mathrm{i}}}$ is the position, ${\mathsf{\beta }}_{{\mathrm{b}}_{\mathrm{j}}}^{{\mathrm{b}}_{\mathrm{i}}}$ is the speed and ${\mathsf{\gamma }}_{{\mathrm{b}}_{\mathrm{i}}}^{{\mathrm{b}}_{\mathrm{i}-1}}$ is the rotation. All can be propagated according to the measured values of IMU during the [${\mathrm{b}}_{\mathrm{i}},{\mathrm{b}}_{\mathrm{j}}$] time interval as follows:

$$\begin{array}{c}{\mathsf{\alpha }}_{{\mathrm{b}}_{\mathrm{j}}}^{{\mathrm{b}}_{\mathrm{i}}} \approx {{\displaystyle \iint }}_{\mathrm{t}\in \left[{\mathrm{t}}_{\mathrm{i}},{\mathrm{t}}_{\mathrm{j}}\right]}{\mathrm{R}}_{\mathrm{t}}^{{\mathrm{b}}_{\mathrm{i}}}\left(\hat{\mathrm{a}}-{\mathrm{b}}_{{\mathrm{a}}_{\mathrm{t}}}\right){\mathrm{dt}}^2\\ {\mathsf{\beta }}_{{\mathrm{b}}_{\mathrm{j}}}^{{\mathrm{b}}_{\mathrm{i}}} \approx {{\displaystyle \int }}_{\mathrm{t}\in \left[{\mathrm{t}}_{\mathrm{i}},{\mathrm{t}}_{\mathrm{j}}\right]}{\mathrm{R}}_{\mathrm{t}}^{{\mathrm{b}}_{\mathrm{i}}}\left(\hat{\mathrm{a}}-{\mathrm{b}}_{{\mathrm{a}}_{\mathrm{t}}}\right)\mathrm{dt}\\ {\mathsf{\gamma }}_{{\mathrm{b}}_{\mathrm{j}}}^{{\mathrm{b}}_{\mathrm{i}}} \approx {{\displaystyle \int }}_{\mathrm{t}\in \left[{\mathrm{t}}_{\mathrm{i}},{\mathrm{t}}_{\mathrm{j}}\right]}\frac{1}{2}\mathsf{\Omega }\left(\hat{\mathsf{\omega }}-{\mathrm{b}}_{{\mathrm{w}}_{\mathrm{t}}}\right){\mathsf{\gamma }}_{{\mathrm{t}}_{\mathrm{i}}}^{\mathrm{b}}\mathrm{dt}\end{array}$$

(16)

where

$$\mathsf{\Omega }\left(\mathsf{\omega }\right)=\left\{\begin{array}{ll}-[\mathsf{\omega }{]}_{\times }\hfill & \mathsf{\omega }\hfill \\ -{\mathsf{\omega }}^{\mathrm{T}}\hfill & 0\hfill \end{array}\right\},[\mathsf{\omega }{]}_{\times }=\left\{\begin{array}{ccc}0\hfill & -{\mathsf{\omega }}_{\mathrm{z}}& {\mathsf{\omega }}_{\mathrm{y}}\\ {\mathsf{\omega }}_{\mathrm{z}}\hfill & 0& -{\mathsf{\omega }}_{\mathrm{x}}\\ -{\mathsf{\omega }}_{\mathrm{y}}\hfill & {\mathsf{\omega }}_{\mathrm{x}}& 0\end{array}\right\}$$

Using the associated covariance estimation ${{\displaystyle \widehat{\sum }}}_{\mathrm{s},\mathrm{j}}$ of the prior state ${\mathrm{s}}_{\mathrm{i}}$, acceleration ${ \mathrm{a}}_{\mathrm{i}}$ and angular velocity ${\mathsf{\omega }}_{\mathrm{i}}$, the pose, speed and deviation of the next frame can be predicted. Therefore, the error equation of IMU is shown in Equation (17).

$${\hat{\mathrm{s}}}_{\mathrm{j}}≔h\left({\mathsf{\alpha }}_{{\mathrm{b}}_{\mathrm{j}}}^{{\mathrm{b}}_{\mathrm{i}}},{ \mathsf{\beta }}_{{\mathrm{b}}_{\mathrm{j}}}^{{\mathrm{b}}_{\mathrm{i}}},{\mathsf{\epsilon }}_{\mathrm{j}},{\mathrm{a}}_{\mathrm{i}},{\mathsf{\omega }}_{\mathrm{i}}\right)$$

(17)

The IMU deviation term ${\mathsf{\epsilon }}_{\mathrm{j}}\in {ℝ}^6$ is used to measure the 3-axis acceleration and 3-axis angular velocity of IMU. Finally, the error equation of IMU is:

$${𝓇}_{{ℐ}_{\left(\mathrm{i},\mathrm{j}\right)}}≔{\left[\begin{array}{c}{\mathsf{\alpha }}_{{\mathrm{b}}_{\mathrm{j}}}^{{\mathrm{b}}_{\mathrm{i}}}\circ {\mathsf{\alpha }}_{{\mathrm{b}}_{\mathrm{j}}}^{{\mathrm{b}}_{\mathrm{i}}}{}^{\prime }\\ {\mathsf{\beta }}_{{\mathrm{b}}_{\mathrm{j}}}^{{\mathrm{b}}_{\mathrm{i}}}+{\mathsf{\beta }}_{{\mathrm{b}}_{\mathrm{j}}}^{{\mathrm{b}}_{\mathrm{i}}}{}^{\prime }\\ \mathsf{\epsilon }+{\mathsf{\epsilon }}_{\mathrm{j}}^{\prime }\end{array}\right]}^{\mathrm{T}}{\widehat{{{\displaystyle \sum }}^{ } }}_{\mathrm{s},\mathrm{j}}^{-1}\left(\left[\begin{array}{c}{\mathsf{\alpha }}_{{\mathrm{b}}_{\mathrm{j}}}^{{\mathrm{b}}_{\mathrm{i}}}\circ {\mathsf{\alpha }}_{{\mathrm{b}}_{\mathrm{j}}}^{{\mathrm{b}}_{\mathrm{i}}}{}^{-1}\\ {\mathsf{\beta }}_{{\mathrm{b}}_{\mathrm{j}}}^{{\mathrm{b}}_{\mathrm{i}}}-{\mathsf{\beta }}_{{\mathrm{b}}_{\mathrm{j}}}^{{\mathrm{b}}_{\mathrm{i}}}{}^{\prime }\\ \mathsf{\epsilon }-{\mathsf{\epsilon }}_{\mathrm{j}}^{\prime }\end{array}\right]\right)$$

(18)

3.4. LiDAR-IMU-Vision Odometry

LiDAR has a long detection distance and a wider radiation range, and can capture many structural details in the environment. However, such details are often discarded during descriptor extraction, which may lead to false positive detections when surrounded by repetitive structures. At the same time, during the LiDAR measurement process, the ranging error on objects with a high reflection intensity is large, which is not conducive to the fusion of vision and LiDAR features. In this paper, the extracted visual feature points are used as the feature compensation points of LiDAR to eliminate the influence of repeated structure and ranging errors on LiDAR. Using solid-state LiDAR to provide a more dense point cloud, through the external parameter calibration of LiDAR and the vision sensor, we can obtain the rotation matrix R and the translation matrix T. What is more, the conversion relationship of points in different coordinate systems can be obtained. For example, any point collected by LiDAR is represented by $F_L\left(x,y,z\right)$, and the image data collected by the camera is represented by $F_{Img}\left(u,v\right)$. Through the internal parameter matrix and the external parameter matrix, the point $F_L\left(x,y,z\right)$ is mapped to the point $F_{Img}\left(u,v\right)$:

$$\left(\begin{array}{c}u\\ v\\ 1\end{array}\right)=\frac{1}{Z_C}\left(\begin{array}{ccc}{f}_u& 0& u_0\\ 0& f_v& v_0\\ 0& 0& 1\end{array}\right){\left[\left(\begin{array}{cc}R& T\\ 0& 1\end{array}\right)\left(\begin{array}{c}x\\ y\\ z\\ 1\end{array}\right)\right]}_{3\times 3}=\frac{1}{Z_C}\times K\times {\left[E\times \left(\begin{array}{c}x\\ y\\ z\\ 1\end{array}\right)\right]}_{3\times 3}$$

(19)

where $Z_C$ is the Z-axis value of the point in the camera coordinate system, K is the internal parameter matrix of the camera, and E is the external parameter matrix of the LiDAR and the camera.

According to formula (19), it can be shown that all points in the LiDAR coordinate system have one-to-one corresponding pixel points. Through the LiDAR front-end processing, we can calculate the reflection intensity $I\left(F_L\right)$ of $F_L$ at any point, so that we equivalently obtain the reflection intensity $I\left(F_{Img}\right)$ of $F_{Img}$. Through feature extraction on the intensity image of the point cloud, according to the set laser reflection intensity range, the visual feature points with robustness and high distance confidence are selected. The total number of visual features screened is recorded as $\mathbb{B}$. As the feature points in $\prime {ℱ}_{\mathrm{i}}^{\mathrm{e}}$ and $\prime {ℱ}_{\mathrm{i}}^{\mathrm{f}}$ are associated with each pixel in the intensity image, the LiDAR feature points are also associated with the visual feature points in $\mathbb{B}$. The final fusion result is shown in Figure 3.

Convert the newly obtained LiDAR frame $\left\{{ℱ}_{\mathrm{i}+1}^{\mathrm{e}},{ℱ}_{\mathrm{i}+1}^{\mathrm{f}}\right\}$ from the $\mathrm{B}$ coordinate system to the $\mathrm{W}$ coordinate system, then obtain $\left\{\prime {ℱ}_{\mathrm{i}+1}^{\mathrm{e}},\prime {ℱ}_{\mathrm{i}+1}^{\mathrm{f}}\right\}$. This initial transformation is obtained by using the robot motion predicted by IMU. For every feature of $\prime {ℱ}_{\mathrm{i}+1}^{\mathrm{e}}$ and $\prime {ℱ}_{\mathrm{i}+1}^{\mathrm{f}}$, find its edge or plane correspondence in ${\mathrm{M}}_{\mathrm{i}}^{\mathrm{e}}$ or ${\mathrm{M}}_{\mathrm{i}}^{\mathrm{f}}$. The matching point of ${ }_{{\mathrm{B}}_{\mathrm{i}+1}}{\prime \mathrm{F}}_{\mathrm{j}}^{ }\in \prime {ℱ}_{\mathrm{i}+1}^{\mathrm{e}}$ consists of the nearest point ${ }_{{\mathrm{B}}_{\mathrm{i}}}{\prime \mathrm{F}}_{\mathrm{k}}^{ }\in \prime {ℱ}_{\mathrm{i}}^{\mathrm{e}}$ and the sub adjacent point ${ }_{{\mathrm{B}}_{\mathrm{i}}}{\prime \mathrm{F}}_{\mathrm{l}}^{ }\in \prime {ℱ}_{\mathrm{i}}^{\mathrm{f}}$; it can be obtained by a KD tree search. The distance ${\mathrm{d}}_{\mathrm{E}}$ from point ${ }_{{\mathrm{B}}_{\mathrm{i}+1}}{\prime \mathrm{F}}_{\mathrm{j}}^{ }$ to line $\left\{{ }_{{\mathrm{B}}_{\mathrm{i}}}{\prime \mathrm{F}}_{\mathrm{k}}^{ },{ }_{{\mathrm{B}}_{\mathrm{i}}}{\prime \mathrm{F}}_{\mathrm{l}}^{ }\right\}$ is as follows:

$${\mathrm{d}}_{\mathrm{E}}=\frac{\parallel \left({ }_{{\mathrm{B}}_{\mathrm{i}+1}}{\prime \mathrm{F}}_{\mathrm{j}}^{ }-{ }_{{\mathrm{B}}_{\mathrm{i}}}{\prime \mathrm{F}}_{\mathrm{k}}^{ }\right)\times \left({ }_{{\mathrm{B}}_{\mathrm{i}+1}}{\prime \mathrm{F}}_{\mathrm{j}}^{ }-{ }_{{\mathrm{B}}_{\mathrm{i}}}{\prime \mathrm{F}}_{\mathrm{l}}^{ }\right)\parallel }{\parallel { }_{{\mathrm{B}}_{\mathrm{i}}}{\prime \mathrm{F}}_{\mathrm{k}}^{ }-{ }_{{\mathrm{B}}_{\mathrm{i}}}{\prime \mathrm{F}}_{\mathrm{l}}^{ }\parallel }$$

(20)

The matching point of ${ }_{{\mathrm{B}}_{\mathrm{i}+1}}{\prime \mathrm{F}}_{\mathrm{j}}^{ }\in \prime {ℱ}_{\mathrm{i}+1}^{\mathrm{f}}$ consists of ${ }_{{\mathrm{B}}_{\mathrm{i}}}{\prime \mathrm{F}}_{\mathrm{l}}^{ }\in \prime {ℱ}_{\mathrm{i}}^{\mathrm{f}}$ and ${ }_{{\mathrm{B}}_{\mathrm{i}}}{\prime \mathrm{F}}_{\mathrm{m}}^{ }\in \prime {ℱ}_{\mathrm{i}}^{\mathrm{p}}$, and the distance ${\mathrm{d}}_{\mathrm{F}}$ from point ${ }_{{\mathrm{B}}_{\mathrm{i}+1}}{\prime \mathrm{F}}_{\mathrm{j}}^{ }$ to plane $\left\{{ }_{{\mathrm{B}}_{\mathrm{i}}}{\prime \mathrm{F}}_{\mathrm{k}}^{ },{ }_{{\mathrm{B}}_{\mathrm{i}}}{\prime \mathrm{F}}_{\mathrm{l}}^{ },{ }_{{\mathrm{B}}_{\mathrm{i}}}{\prime \mathrm{F}}_{\mathrm{m}}^{ }\right\}$ is as follows:

$${\mathrm{d}}_{\mathrm{F}}=\frac{\parallel \left({ }_{{\mathrm{B}}_{\mathrm{i}+1}}{\prime \mathrm{F}}_{\mathrm{j}}^{ }-{ }_{{\mathrm{B}}_{\mathrm{i}}}{\prime \mathrm{F}}_{\mathrm{k}}^{ }\right)\left(\left({ }_{{\mathrm{B}}_{\mathrm{i}}}{\prime \mathrm{F}}_{\mathrm{k}}^{ }-{ }_{{\mathrm{B}}_{\mathrm{i}}}{\prime \mathrm{F}}_{\mathrm{l}}^{ }\right)\times \left({ }_{{\mathrm{B}}_{\mathrm{i}}}{\prime \mathrm{F}}_{\mathrm{k}}^{ }-{ }_{{\mathrm{B}}_{\mathrm{i}}}{\prime \mathrm{F}}_{\mathrm{m}}^{ }\right)\right)\parallel }{\parallel \left({ }_{{\mathrm{B}}_{\mathrm{i}}}{\prime \mathrm{F}}_{\mathrm{k}}^{ }-{ }_{{\mathrm{B}}_{\mathrm{i}}}{\prime \mathrm{F}}_{\mathrm{l}}^{ }\right)\times \left({ }_{{\mathrm{B}}_{\mathrm{i}}}{\prime \mathrm{F}}_{\mathrm{k}}^{ }-{ }_{{\mathrm{B}}_{\mathrm{i}}}{\prime \mathrm{F}}_{\mathrm{m}}^{ }\right)\parallel }$$

(21)

where ${ }_{{\mathrm{B}}_{\mathrm{i}}}{\prime \mathrm{F}}_{\mathrm{k}}^{ }$ is the nearest point of the target searched by a KD tree,${ }_{{\mathrm{B}}_{\mathrm{i}}}{\prime \mathrm{F}}_{\mathrm{l}}^{ }$ is the nearest point on the adjacent line, and ${ }_{{\mathrm{B}}_{\mathrm{i}}}{\prime \mathrm{F}}_{\mathrm{m}}^{ }$ is the secondary adjacent point on the unified line. Here, there is a three-dimensional transformation relationship between ${ }_{{\mathrm{B}}_{\mathrm{i}}}{\prime \mathrm{F}}_{\mathrm{k}}^{ }\in \prime {ℱ}_{\mathrm{i}}^{ }$ and ${ }_{{\mathrm{B}}_{\mathrm{i}+1}}{\prime \mathrm{F}}_{\mathrm{k}}^{ }\in \prime {ℱ}_{\mathrm{i}+1}^{ }$.

$${ }_{{\mathrm{B}}_{\mathrm{i}+1}}{\prime \mathrm{F}}_{\mathrm{k}}^{ }={\tilde{\mathrm{R}}}_{\left(\mathrm{i}+1,\mathrm{i}\right)}·{ }_{{\mathrm{B}}_{\mathrm{i}}}{\prime \mathrm{F}}_{\mathrm{k}}^{ }+{ }_{{\mathrm{B}}_{\mathrm{i}}}{\tilde{\mathrm{t}}}_{\left(\mathrm{i}+1,\mathrm{i}\right)}$$

(22)

By introducing it into Equations (20) and (21), we can obtain Equation (20).

$$\mathrm{d}=\mathrm{f}\left({\tilde{\mathrm{R}}}_{\left(\mathrm{i}+1,\mathrm{i}\right)},{ }_{{\mathrm{B}}_{\mathrm{i}}}{\tilde{\mathrm{t}}}_{\left(\mathrm{i}+1,\mathrm{i}\right)}\right)$$

(23)

In this paper, the Levenberg Marquardt algorithm is used to minimize $\mathrm{d}$, given an initial value $x_0$ and a termination condition $\mathsf{\Delta }x\le \epsilon$, where $\epsilon$ is a minimum value greater than zero. Start the iteration for the k-th iteration process, calculate the Jacobian matrix and construct the incremental equation to find ∆x. If the termination condition is met, the iteration will be stopped; otherwise, the iteration will continue until the maximum number of iterations is reached or the termination condition is met. To solve the estimation of rotation and translation ${\overline{\mathrm{T}}}_{\left(\mathrm{i}+1,\mathrm{i}\right)}={\overline{\mathrm{R}}}_{\left(\mathrm{i}+1,\mathrm{i}\right)},{ }_{{\mathrm{B}}_{\mathrm{i}}}{\overline{\mathrm{t}}}_{\left(\mathrm{i}+1,\mathrm{i}\right)}$.

3.5. Map Construction

The high-precision map construction and optimization thread can overlay the scanning point cloud at the current time with the constructed global map point cloud, so as to update the global map incrementally. In order to reduce the cumulative error of point cloud matching caused by pose estimation, this paper matches the current point cloud with the global point cloud, again to optimize the results.

When the point cloud ${\{ℱ\}}_{\mathrm{k}}$ scanned in position pose estimation and LiDAR pose ${\dot{\mathrm{x}}}_{\mathrm{k}}$ at time ${\mathrm{t}}_{\mathrm{k}}$ are received, ${\{ℱ\}}_{\mathrm{k}}$ and ${\left\{\mathrm{M}\right\}}_{\mathrm{k}-1}$ are registered, and the global map ${\left\{\mathrm{M}\right\}}_{\mathrm{k}}$ is updated. ${\{ℱ\}}_{\mathrm{k}}$ is converted to the global coordinate system to obtain ${\left\{\varrho \right\}}_{\mathrm{k}}$, then register ${\left\{\varrho \right\}}_{\mathrm{k}}$ and ${\left\{\mathrm{M}\right\}}_{\mathrm{k}-1}$. Because the estimated LiDAR pose ${\dot{\mathrm{x}}}_{\mathrm{k}}$ has error accumulation, the deviation ${\mathsf{\Delta }\mathrm{x}}_{\mathrm{k}}$ of ${\left\{\varrho \right\}}_{\mathrm{k}}$ from the global map ${\left\{\mathrm{M}\right\}}_{\mathrm{k}-1}$ can be solved. Then, the updated global map ${\left\{\mathrm{M}\right\}}_{\mathrm{k}}$ and the optimized unmanned parking position and attitude ${\mathrm{x}}_{\mathrm{k}}$ at time ${\mathrm{t}}_{\mathrm{k}}$ are obtained. Figure 4 is a schematic diagram of the map construction and optimization, in which the black line is the constructed global point cloud; the blue line is the point cloud currently scanned in the global coordinate system, corresponding to the LiDAR scanning at the green point; the yellow line is the optimized laser point cloud, which corresponds to the optimization of the LiDAR posture by laser point cloud scanning at the red point.

3.6. Closed Loop Detection

The closed loop module detects whether the trajectory of the carrier is closed, that is, whether the carrier returns to the previous position. If a closed loop is formed, the ICP algorithm is used to register the current feature point with the feature point cloud at the closed loop to obtain the relative pose relationship t. The closed-loop detection factor is directly input into the factor graph. In this paper, a closed-loop detection method based on Euclidean distance is adopted. By detecting the previous state close to the current key frame in Euclidean space, the pose transformation relationship is obtained by scan matching, and it is added to the factor graph as a closed-loop factor.

3.7. Factor Graph Optimization

The overall framework of the system is shown in Figure 5. The system uses the observations of the sensors to estimate the robot state and its trajectory. The state estimation problem can be expressed as a problem. In this paper, a factor graph is used to model the problem. Under the assumption of a Gaussian noise model, the inference for the maximum posteriori problem is equivalent to solving the nonlinear least squares problem. The odometry framework established based on the factor graph in this paper includes four types of factors: the starting factor, registration factor, IMU factor and closed-loop factor. When the change of the unmanned vehicle posture exceeds the set threshold, a new robot state node will be added to the factor graph for incremental smoothing and mapping by the use of a Bayesian tree. Assuming that the initial state is ${\mathcal{X}}_0$, the map building module inserts the registration factor into the factor map corresponding to $\parallel {𝓇}_{{ℱ}_{\mathrm{i}}}{\parallel }_{{\mathsf{\Sigma }}_{{ℱ}_{\mathrm{i}}}}^2$ in Equation (3).

$${𝓇}_{{ℱ}_{\mathrm{i}}}=\left({\mathrm{x}}_{\mathrm{i}}⨁{\mathrm{x}}_{\mathrm{i}-1}\right)⨁{\overline{\mathrm{T}}}_{\left(\mathrm{i},\mathrm{i}-1\right)}$$

(24)

The IMU pre-integration module inserts the IMU factor into the factor graph, corresponding to $\parallel {𝓇}_{{ℐ}_{\left(\mathrm{i},\mathrm{j}\right)}}{\parallel }_{{\mathsf{\Sigma }}_{{ℐ}_{\left(\mathrm{i},\mathrm{j}\right)}}}^2$ in Equation (3):

$${𝓇}_{{ℐ}_{\left(\mathrm{i},\mathrm{j}\right)}}=\left({\mathrm{x}}_{\mathrm{i}}⨁{\mathrm{x}}_{\mathrm{j}}\right)⨁{{\displaystyle \int }}^{ }{ℐ}_{\left(\mathrm{i},\mathrm{j}\right)}$$

(25)

The closed-loop detection module inserts the closed-loop factor into the factor graph, corresponding to $\parallel {𝓇}_{{ℱ}_{\left(\mathrm{i},\mathrm{j}\right)}}{\parallel }_{{\mathsf{\Sigma }}_{ℱ\left(\mathrm{i},\mathrm{j}\right)}}^2$ in Equation (3):

$${𝓇}_{{ℱ}_{\left(\mathrm{i},\mathrm{j}\right)}}=\left({\mathrm{x}}_{\mathrm{i}}⨁{\mathrm{x}}_{\mathrm{j}}\right)⨁{\overline{\mathrm{T}}}_{\left(\mathrm{i},\mathrm{j}\right)}$$

(26)

where $⨁$ represents the inverse operation of the pose transformation. Whenever new nodes are inserted into the factor graph, the affected nodes are identified and only optimized and updated, so as to maintain the fully optimized sparse characteristics and realize multi-sensor fusion positioning and mapping.

4. Experimental Verification

In order to objectively verify the performance and accuracy of the visual inertial and LiDAR simultaneous localization and mapping (VIAL-SLAM) algorithm proposed in this paper, we conducted mapping and localization experiments in a campus environment and an urban road environment, respectively, and compared them with the FAST-LIO algorithm and the LIO-LIVOX algorithm. The two environments were equipped with different hardware equipment. The campus environment was equipped with an industrial computer with Xeon CPU E3-1275 on the Pix wire control site, while the urban environment was equipped with a laptop computer with I7 10700F. The operating system used the robot operating system (ROS) in Ubuntu18.04. The sensors are divided into LIVOX LIDAR, vision sensor ZED and RTK for inertial navigation. In the campus environment and the urban environment, the sensor parameters were kept consistent to eliminate the influence of the sensors on the experimental results.

4.1. School Environment Test

We controlled the Pix wire control site to build an internal map at the school. The experimental mileage was about 0.75 Km, the time was 600 s, and the average speed was 4.5 Km/h. The point cloud map constructed in the campus environment was superimposed on the real map, as shown in Figure 6. The specific drawing effects of a, b, c, and d in Figure 6 are shown in Figure 7, and the direction of the arrow indicates the moving direction of the vehicle. Figure 8a is the trajectory diagram of the mapping process, in which the RTK trajectory as the reference is a gray dashed line, the FAST-LIO trajectory is blue, the LIO-LIVOX trajectory is green, and the VIAL-SLAM trajectory is red. At the same time, the comparison of the x-axis and y-axis on the time series is plotted in Figure 8b.

It can be seen from the figure that the VIAL-SLAM proposed in this paper has the smallest error. In order to further evaluate the positioning results, the Absolute Pose Error (APE) index in the EVO evaluation tool [24] is used to evaluate the system performance. Comparing the VIAL-SLAM algorithm, FAST-LIO algorithm and LIO-LIVOX algorithm with the benchmark RTK, the changes of the APE in the time series are shown in Figure 8c, and the error distribution status of the APE is shown in Figure 8d.

Table 1. The campus environment test results.

Error Term	VIAL-SLAM	LIO-LIVOX	FAST-LIO
Root mean square error	0.67 m	1.49 m	2.92 m
Mean error	0.56 m	1.35 m	2.49 m
Error ratio	0.095%	0.213%	0.417%

calculates the error ratio of different algorithms in the campus environment in detail. It can be seen from the results that our algorithm, VIAL-SLAM, has excellent positioning effects.

4.2. Real Urban Road Environment Test

In order to verify the system’s autonomous positioning and map construction performance in the real urban road environment, we installed the vision sensor ZED, Lidar sensor LIVOX and inertial navigation RTK on the actual vehicle, as shown in Figure 9.

The mileage of the experiment was about 3.3 km; it took 800 s, and the average speed was 14.85 Km/h. As shown in Figure 10, this article superimposed the results of automatic positioning and map construction on the real map for visual comparison. The specific rendering effects of a, b, c, and d in Figure 10 are shown in Figure 11. The arrow direction indicates the moving direction of the vehicle. According to the result, it can be found that the map constructed in the urban road environment basically matches the real road.

The trajectory estimated by VIAL-SLAM is shown in red in Figure 12a, the trajectory estimated by FAST-LIO is blue, the trajectory estimated by LIO-LIOVX is green, and the reference provided by RTK is gray. The comparison of the x-axis and y-axis in the time series is plotted in Figure 12b, and it can be found that the VIAL-SLAM proposed in this paper is closer to the benchmark RTK. In order to evaluate the positioning accuracy of the system, the absolute positioning accuracy the APE is used for the error calculation and analysis. The autonomous positioning error evaluation results and error distribution status are shown in Figure 12c,d, respectively.

Table 2. The autonomous positioning test results in the urban road environment.

Error Term	VIAL-SLAM	LIO_LIVOX	FAST-LIO
Root mean square error	3.54 m	9.18 m	15.77 m
Mean error	3.08 m	7.92 m	13.5 m
Error ratio	0.107%	0.278%	0.478%

records the position error ratio of the VIAL-SLAM algorithm, the FAST-LIO algorithm and the LIO-LIVOX algorithm proposed in this paper. The position error rate of the FAST-LIO algorithm and the LIO-LIOVX algorithm are 0.478% and 0.278%, respectively, while the position error rate of VIAL-SLAM is only 0.107%. Compared with FAST-LIO and LIO-LIVOX, it is reduced by 71.43% and 61.51%. The results show that the algorithm still has good performance in the urban road environment.

5. Conclusions

This paper presented a slam system integrating LIDAR IMU vision for autonomous positioning and high-precision map construction in complex environments. The proposed system combines the visual image information, LIDAR point cloud and IMU data. After the point cloud alignment and registration, the pose estimation was carried out and the motion trajectory was optimized by the factor graph. In the system, closed-loop detection was used to eliminate the cumulative error. The point cloud map was registered and updated through the motion estimation and laser point cloud data to build a high-precision map. The algorithm was tested in a campus environment and an urban road environment. Furthermore, we compared it with the FAST-LIO algorithm and the LIO-LIVOX algorithm. The experimental results show that the proposed VIAL-SLAM can achieve accurate autonomous positioning and high-precision map construction in complex environments, and still has high accuracy and robustness.