Research on Multi-Sensor SLAM Technology for Complex Environment of Construction Machinery

1. Overview of the system

The framework of multi-sensor fusion SLAM technology proposed in this paper is shown in Figure 1, consisting of a LiDAR-inertial system and a visual-inertial system. LIO subsystem uses feature-based methods to detect edge and planar features of point cloud, matches the LiDAR key frames to the global map, estimates the state of the system by minimizing the point-to-plane residuals, executes LiDAR odometry, and constructs the geometric structure of the global map. To ensure the real-time and efficient performance of the system, the feature map maintains key frames in a sliding window. VIO system initializes using the state estimated by LIO system. The visual front-end of the system adopts direct methods, updates the system state by minimizing the photometric error from the frame to the map, performs visual odometry, and constructs the texture information of the map. Two subsystems are tightly coupled to jointly perform pose estimation. The whole system incorporates IMU preintegration constraint, visual odometry constraint, LiDAR odometry constraint and Loop closure detection constraint as factors into the factor graph to achieve global pose optimization.

2. LIDAR-inertial odometry

As early as 2005, the framework of LiDAR SLAM was initially determined, and many studies have been achieved later on LiDAR-based SLAM. For example, the early 2D LiDAR SLAM frameworks cartographer [8] and hector [9] can build environmental maps in a 2D plane, but the localization accuracy is poor. Later, 3D LiDAR SLAM was proposed, such as the LOAM series [10,11], which significantly improved the performance of localization and mapping. Pure LiDAR SLAM relies on simple scanning and matching methods, which has poor performance in dynamic and complex environments and cannot achieve reliable localization and mapping. Therefore, the SLAM framework with fusion of LiDAR and inertial measurement unit (IMU) is proposed. IMU can measure the acceleration and angular velocity of robots, and is not easily affected by external interference. The fusion of IMU can solve the problem of localization drift of pure LiDAR SLAM in fast motion scenarios. For example, Lego-LOAM [12] eliminates unreliable feature points based on LOAM, but adopts a loosely coupled approach to fuse IMU, which does not fully utilize sensor data. The later proposed LIOM [13], LIO-SAM [14], and FAST-LIO [15] are all tightly coupled LiDAR-inertial SLAM systems based on factor graph optimization, which does not require making Gaussian distribution assumptions compared to the traditional filtering method, by calculating the conditional probability between variables and passing the results to Bayesian trees [16] for posterior inference, it can efficiently handle large scale sparse matrices, improve the accuracy and robustness of the model, and has become the mainstream SLAM optimization method.

Table 1 shows the explanation of some important notations in this section. The LiDAR-inertial subsystem in this framework is based on the LIO-SAM, which has better localization and mapping effects. Assuming that the IMU coordinate frame is the same as the robot body coordinate frame, the state of the robot is defined

$$x={\left[R^{\mathrm{T}}{p}^{\mathrm{T}}{\mathrm{v}}^{\mathrm{T}}{b}^{\mathrm{T}}\right]}^{\mathrm{T}}$$

(1)

As shown in Figure 2, the factor graph of the LIO subsystem is composed of four constraints: IMU preintegration constraint, LiDAR odometry constraint, loop closure constraint, and robot state. The raw measurement data of IMU is used to estimate the sensor motion during LiDAR scanning. The estimated motion can be used not only to eliminate point cloud distortion, but also as the initial guess for LiDAR odometry optimization. The obtained initial guess can then be used to estimate the IMU bias and form a loop closure constraint through closed-loop detection. Finally, the relationships among all constraints are considered to achieve global factor graph optimization.

2.1. LiDAR odometry

The front-end of LIO subsystem is mainly processes sensor data. Following the methods of LOAM and Lego-LOAM. Firstly, the distorted point cloud is corrected, and then feature extraction is started to extract edge points and plane points by calculating the curvature of the point cloud in the frame. ${\mathrm{F}}_i^e$ and ${\mathrm{F}}_i^p$ are the sets of edge features and planar features extracted at moment i, respectively, and they together form the LiDAR frame sets ${\mathrm{F}}_i$ at moment i, ${\mathrm{F}}_i=\left\{{\mathrm{F}}_i^e,{\mathrm{F}}_i^p\right\}$, where LiDAR frame F is represented under B. After obtaining the LIDAR frames, to avoid the factor graph being too large, which leads to data complexity and cumulative errors, each LIDAR frame extracted cannot be directly added to the factor graph as a factor. The widely used key frame strategy is used to select the most representative LiDAR frames at each moment as key frames. If the robot’s pose $x_i$ exceeds the defined threshold at moment i , the LiDAR frame ${\mathrm{F}}_{i+1}$ of the next moment will be used as the keyframe and the LiDAR frames between keyframes ${\mathrm{F}}_i$ and ${\mathrm{F}}_{i+1}$ will be discarded to reduce the computational effort. The latest saved keyframe ${\mathrm{F}}_{i+1}$ will be associated with the new robot state $x_{i+1}$ for data association. Then, the set $\left\{{\mathrm{F}}_{i-n},...,{\mathrm{F}}_i\right\}$ of n closest keyframes at moment i, also known as subkeyframes, are extracted and transformed to W through the corresponding transformation matrix set $\left\{{\mathrm{T}}_{i-n},...,{\mathrm{T}}_i\right\}$. This results in a voxel map ${\mathrm{M}}_i$ consisting of a edge voxel map ${\mathrm{M}}_i^e$ and a planar voxel map ${\mathrm{M}}_i^p$. The relationship between the LiDAR frame and voxel map is

$${\mathrm{M}}_i=\{{\mathrm{M}}_i^e,{\mathrm{M}}_i^p\}$$

(2)

$${\mathrm{M}}_i^e{=}^{\prime }{\mathrm{F}}_i^e{\cup }^{\prime }{\mathrm{F}}_{i-1}^e\cup ...{\cup }^{\prime }{\mathrm{F}}_{i-n}^e$$

(3)

$${\mathrm{M}}_i^p{=}^{\prime }{\mathrm{F}}_i^p{\cup }^{\prime }{\mathrm{F}}_{i-1}^p\cup ...{\cup }^{\prime }{\mathrm{F}}_{i-n}^p$$

(4)

In order to preserve the basic shape and geometric structure of the original point cloud as much as possible and improve the efficiency of voxel map processing, it is necessary to divide the map into several voxels and keep only one representative point in each voxel to realize the downsampling of point cloud data.

After filtering out the redundant point cloud features, the new keyframe set ${\mathrm{F}}_{i+1}$ is feature-matched to the voxel map ${\mathrm{M}}_i$ that has already been created. Following the point to line and point to plane point cloud registration methods [17,18], ${\mathrm{F}}_{i+1}$ is converted from B to W to obtain ${}^{\prime }{\mathrm{F}}_{i+1}$, and the distance equation from the point cloud feature to the corresponding edge or plane block is

$$d_{e_k}=\frac{\left|\left({\mathrm{P}}_{i+1,k}^e-{\mathrm{P}}_{i,u}^e\right)\times \left({\mathrm{P}}_{i+1,k}^e-{\mathrm{P}}_{i,v}^e\right)\right|}{\left|{\mathrm{P}}_{i,u}^e-{\mathrm{P}}_{i,v}^e\right|}$$

(5)

$$d_{p_k}=\frac{\left|\left({\mathrm{P}}_{i+1,k}^p-{\mathrm{P}}_{i,u}^p\right)\left({\mathrm{P}}_{i,u}^p-{\mathrm{P}}_{i,v}^p\right)\times \left({\mathrm{P}}_{i,u}^p-{\mathrm{P}}_{i,w}^p\right)\right|}{\left|\left({\mathrm{P}}_{i,u}^p-{\mathrm{P}}_{i,v}^p\right)\times \left({\mathrm{P}}_{i,u}^p-{\mathrm{P}}_{i,w}^p\right)\right|}$$

(6)

where $k,u,v,w$ denotes the index in the corresponding feature set. With the IMU preintegration factor (Section 2.2), frame-by-frame matching is not required, and the method of matching features from frame to local map is directly used, which saves processing time for global map and improves matching efficiency. ${\mathrm{P}}_{i,u}^e$ and ${\mathrm{P}}_{i,v}^e$ are the two points closest to the edge feature ${}^{\prime }{\mathrm{P}}_{i+1,k}^e$ of the previous frame, which form the edge line. ${\mathrm{P}}_{i,u}^p$ , ${\mathrm{P}}_{i,v}^p$, and ${\mathrm{P}}_{i,w}^p$ are the three points closest to the previous frame plane feature ${}^{\prime }{\mathrm{F}}_{i+1,k}^p$ and not on the same line, which together form a plane block in ${\mathrm{M}}_i^p$. Following the principle that edge features correspond to edge lines and plane features correspond to plane blocks, the equation for minimizing the positional transformation is

$$\underset{{\mathrm{T}}_{{}_{i+1}}}{min}\left\{\sum _{{\mathrm{P}}_{i+1,k}^e{\in }^{\prime }{\mathrm{F}}_{i+1}^e}{d}_{e_k}+\sum _{{\mathrm{P}}_{i+1,k}^p{\in }^{\prime }{\mathrm{F}}_{i+1}^p}{d}_{p_k}\right\}$$

(7)

Using the Gauss-Newton method to solve for the optimal pose transformation relationship of the robot at moments i and $i+1$: $\mathrm{\Delta }{\mathrm{T}}_{i,i+1}={\mathrm{T}}_i^{\mathrm{T}}{\mathrm{T}}_{i+1}$, and added to the factor graph as a LiDAR odometry factor.

2.2. IMU preintegration

During the optimization process of the factor graph, the robot’s state is inevitably adjusted continuously. Each adjustment requires a re-integration of the pose from the current moment and the previous moment, which can be very time-consuming. To avoid reintegration and make the IMU integration independent of the previous state, a preintegration strategy [19,20,21] is adopted.

Firstly, modeling the angular velocity and acceleration observed by IMU under B

$${\widehat{\omega }}_t={\omega }_t+b_t^w+n_t^w$$

(8)

$${\widehat{a}}_t=R_t^{BW}(a_t-g)+b_t^a+n_t^a$$

(9)

where ${\widehat{\omega }}_t$ and ${\widehat{a}}_t$ are the original measurements at moment t, and $R_t^{BW}$ denotes the rotation matrix from W to B. $b_t$,$n_t$ are the bias and white noise, respectively. g is the constant gravity vector under W. Assuming that ${\widehat{\omega }}_t$ and ${\widehat{a}}_t$ in B remain unchanged, the motion of the robot is inferred from the measurement values of the IMU. The velocity v, pose p, and rotation q at moment $t+\mathrm{\Delta }t$ are

$${\mathrm{v}}_{t+\mathrm{\Delta }t}={\mathrm{v}}_t+g\mathrm{\Delta }t+R_t({\widehat{a}}_t-b_t^a-n_t^a)\mathrm{\Delta }t$$

(10)

$$p_{t+\mathrm{\Delta }t}=p_t+{\mathrm{v}}_t\mathrm{\Delta }t+\frac{1}{2}g\mathrm{\Delta }{t}^2+\frac{1}{2}{R}_t({\widehat{a}}_t-b_t^a-n_t^a)\mathrm{\Delta }{t}^2$$

(11)

$$R_{t+\mathrm{\Delta }t}=R_{t}exp\left(({\widehat{\omega }}_t-b_t^{\omega }-n_t^{\omega })\mathrm{\Delta }t\right)$$

(12)

where $R_t=R_t^{WB}=R_t^{B{W}^T}$, here it is assumed that the angular velocity and acceleration of the B are constant during the integration process. The relative motion of the robot between moments i and j is calculated using the IMU preintegration, and the preintegrated measurements $\mathrm{\Delta }{\mathrm{v}}_{ij}$, $\mathrm{\Delta }{\mathrm{p}}_{ij}$, and $\mathrm{\Delta }{\mathrm{R}}_{ij}$ between these two moments are

$$\mathrm{\Delta }{\mathrm{v}}_{ij}={\mathrm{R}}_i^{\mathrm{T}}({\mathrm{v}}_j-{\mathrm{v}}_i-g\mathrm{\Delta }{t}_{ij})$$

(13)

$$\mathrm{\Delta }{\mathrm{p}}_{ij}={\mathrm{R}}_i^{\mathrm{T}}({\mathrm{p}}_j-{\mathrm{p}}_i-{\mathrm{v}}_{\mathrm{i}}\mathrm{\Delta }{\mathrm{t}}_{ij}-\frac{1}{2}g\mathrm{\Delta }{t}_{ij}^2)$$

(14)

$$\mathrm{\Delta }{\mathrm{R}}_{ij}={\mathrm{R}}_i^{\mathrm{T}}{\mathrm{R}}_j$$

(15)

By using IMU preintegration, the poses of the robot can be jointly optimized using IMU bias and LiDAR odometry in the factor graph.

2.3. Loop closure detection

Loop closure detection [22] means that the robot can recognize the scenes it has reached before during its motion, forming loopback edges, and matching the map generated at this moment with the previous one to minimize the error between predicted and observed values. Using the closed-loop detection method proposed by Kaess et al. [23], the LiDAR odometry is used to determine whether there is a loop closure. Based on the latest keyframe, the keyframe within a distance of 15 m is searched for, and the timestamp difference between the two frames is greater than 30 s. Only when both conditions are satisfied, the corresponding loopback frame is considered to be found. After detecting loop closure, the ICP matching algorithm is used to calculate the Euclidean distance between the two key frames, iteratively update the matching relationship between them continuously to get the robot’s pose. The indexes of the two frames, inter-frame pose, and noise (ICP score) are added into the loop closure constraint. As shown in Figure 2, assuming that the latest state of the robot is ${\mathrm{X}}_{i+1}$, and ${\mathrm{X}}_2$ is the prior state detected by loop closure, the relative transformation of ${\mathrm{F}}_{i+1}$ to the subkeyframe $\left\{{\mathrm{F}}_{2-m},...,{\mathrm{F}}_2,...,{\mathrm{F}}_{2+m}\right\}$ is calculated as $\mathrm{\Delta }{\mathrm{T}}_{2i+1}$.

3. Visual-inertial odometry

The SLAM systems that combining vision and IMU have also been widely studied. With the measurements and short-term motion constraints of IMU can assist visual odometry to recover scale information, and effectively solve the problem of localization drift in scenes with a lack of texture and fast motion. Currently, feature-based methods are commonly used in the front-end of visual-inertial SLAM, with adjacent frames detecting features for matching and estimating camera motion, such as the newer ORB-SLAM framework [24,25] and MonoSLAM [26]. There are also visual-inertial SLAM frameworks based on direct methods, such as LSD-SLAM [27], which directly estimate the motion of the camera using pixel information. Compared with feature-based methods, the direct method does not require feature extraction, data association, and minimization of feature reprojection errors. They are computationally more efficient and applicable to sparse, semi-dense, and dense point clouds.

The VIO subsystem in this paper, is a tightly coupled visual-inertial odometry based on the direct method. It estimates the camera pose and establishes the environment’s texture information by jointly minimizing the photometric error and IMU measurement error between image frames.The estimation is further optimized by factor graph optimization to obtain the best estimate state. In order to reduce the complexity of the system and to ensure that the nonlinear optimization problem is handled in a reasonable time range, this paper adopts marginalization technique [28], which continuously removes the old variables from the factor graph to optimize the latest state.

Table 2 shows the explanation of some important notations in this section, where gravity is defined as perpendicular to the negative Z-axis in the world coordinate frame. This system is based on nonlinear optimization, the camera’s pose and scene depth are estimated by continuously iteratively minimizing the energy function. Firstly, establish the minimum energy function

$${\mathrm{E}}_{all}=\lambda ·E_{picture}+E_{inertial}$$

(16)

where $E_{picture}$ denotes the photometric error and $E_{inertial}$ denotes the inertial error. $\lambda$ denotes the balance coefficient, which is used to weigh the effects of photometric error and inertial error.

3.1. Visual odometry

The task of the VIO front-end is to execute visual odometry. Firstly, roughly estimating the camera pose by minimizing the photometric error. Assuming that there is a point p in the reference frame i,$p\in {\mathrm{\Omega }}_i$, which is also observable under another frame j, the photometric error of the two adjacent frames images is

$$E_{pj}=\sum _{p\in N_p}{w}_p{∥(I_j\left[p^{\prime }\right]-b_j)-\frac{t_j{e}^{a_j}}{t_i{e}^{a_i}}(I_i\left[p\right]-b_i)∥}_{\gamma }$$

(17)

where $N_p$ is the pixel around point p. $I_i$, $I_j$ is the image of frame i and frame j. $t_i$, $t_j$ is the exposure time of the image. $a_i$, $a_j$, $b_i$, $b_j$ is the coefficient for correcting affine illumination variation. $\gamma$ is the Huber parametric number. ${\omega }_p$ is the weight related to the gradient. $p^{\prime }$ is the projection on $I_i$. Then the total photometric error is

$$E_{picture}=\sum _{i\in \mathcal{F}}\sum _{p\in {\mathcal{P}}_i}\sum _{j\in obs\left(p\right)}{E}_{pj}$$

(18)

where $\mathcal{F}$ is the set of keyframes to be optimized. ${\mathcal{P}}_i$ is the sparse set of points in the keyframes. $obs\left(p\right)$ is the set of keyframes where the point p is observed.

3.2. Inertial measurement and initialization

The frequency of IMU detection data is usually higher than that of the camera. In order to avoid the repetition of IMU integration, the VIO system still adopts preintegration method. The acceleration value $a_{ij}$ and angular velocity value ${\omega }_{ij}$ measured by the IMU are added between two image frames $s_i$ and $s_j$. The visual observations and inertial measurements are combined to further estimate the pose of the most nearest frame. According to the nonlinear dynamic model [28], establish the error term of speed and linear acceleration, and obtain the prediction ${\widehat{s}}_j$ and covariance $\stackrel{\wedge }{\sum {}_{s,j}}$. The inertial error function is

$$E_{\mathrm{inertial}}\left(s_i,s_j\right):={\left(s_j\boxminus {\widehat{s}}_j\right)}^{T}s{\widehat{\sum }}_{s,j}^{-1}\left(s_j\boxminus {\widehat{s}}_j\right)$$

(19)

Accurate initial values are crucial for guiding monocular systems. Monocular cameras cannot recover environmental scale information, but can use inertial data to obtain metric scale and gravity direction. However, there is inevitably bias in the IMU, so these quantities need to be properly initialized before processing in a tightly-coupled system. To address the issue that monocular visual-inertial systems cannot be immediately initialized during constant velocity or zero-acceleration motion, this paper treats scale and gravity direction as parameters of the system, and optimizes them together with pose to achieve initialization at any scale. The parameters that need to be initialized include the rough pose estimate between two frames, the average depth of nearby points, the initial gravity direction, velocity, scale, and IMU bias. Before initialization, the visual-inertial frame needs to be converted to the metric frame, with a transformation matrix of $T_{m\_d}\in \left.\left\{T\in SIM\left(3\right)\right.|translation\left(T\right)=0\right\}$, where ${\xi }_{m\_d}=log\left(T_{m\_d}\right)\in sim\left(3\right)$. The average depths are all normalized to 1, the gravity direction is calculated using acceleration measurements, the velocity and IMU bias are 0, and the scale is 1. Then, all variables and parameters at the initial frame are transformed into the world coordinate frame, and these values based on real scale will continue to be used in the VIO system.

3.3. Optimization of visual-inertial systems

As shown in Figure 3, the poses of the keyframes, IMU bias, and velocity are combined as constraints to form a factor graph for global optimization of the visual-inertial system.The optimized keyframe poses are in the visual-inertial coordinate system and are not affected by environmental scale.The IMU factors are connected between two consecutive keyframes in the form of pre-integration as described in Section 2.2.The time interval threshold between two consecutive keyframes is set to 0.5 seconds to avoid errors in preintegration results due to long time intervals.

3.3.1. Position optimization

Based on the least squares problem, common optimization methods include Gaussian-Newton method, L-M method, and global nonlinear optimization method. To ensure computational efficiency and good convergence. In this paper, the Gauss-Newton method is used for positional optimization. Firstly, define the state vector of each effective key frame

$$s_i:={\left[{\left({\xi }_{cam\_\omega }^D\right)}^T,{v_i}^T,b_i^T,a_i,b_i,d_i^1,...,d_i^m\right]}^T$$

(20)

where $v_i\in R^3$ denotes the velocity. $b_{{}_i}\in R^6$ denotes the IMU bias. $a_i^{}$, $b_i$ are the affine illumination coefficients. $d_i^j$ is the inverse depth of the point in the key frame. The full state vector is established as

$$s={\left[c^T,{\xi }_{m\_d}^T,s_1^T,s_2^{\mathrm{T}},...,s_n^T\right]}^{{}^T}$$

(21)

where c denotes the geometric parameters of the camera and ${\xi }_{m\_d}^T$ denotes the rotational transformation from visual-inertial frames to metric frames.

Using stacked residual vector r to establish equation

$$J=\frac{dr(s⊞ϵ)}{dϵ}{|}_{ϵ=0},H=J^{T}WJ,b=-J^{T}Wr$$

(22)

where W is the weight matrix. The photometric error term $E_{picture}$ and the inertial error term $E_{imu}$ have no common residual. The update equation is established as Equation (23), and the state vector of inertial residual in metric frame is defined as Equation (25)

$$\delta =H^{-1}b$$

(23)

$$H=H_{picture}+H_{imu},b=b_{picture}+b_{imu}$$

(24)

$${s^{\prime }}_i:={\left[{\xi }_{w\_im{u}_i}^M,{\upsilon }_i,b_i\right]}^T,s^{\prime }={\left[{s_1^{\prime }}^T,{s_2^{\prime }}^T,...,{s_n^{\prime }}^T\right]}^T$$

(25)

The inertial residuals lead to

$${H^{\prime }}_{{}_{imu}}={J_{imu}^{\prime }}^T{W}_{imu}{J^{\prime }}_{imu},{b^{\prime }}_{imu}=-{J_{imu}^{\prime }}^T{W}_{imu}{\gamma }_{imu}$$

(26)

To achieve the joint optimization of the data. Firstly, the values of $H_{imu}$ and $b_{imu}$ are calculated from Equation (21) , and $J_{rel}$ is calculated according to the method proposed by Von Stumberg et al. [29]

$$H_{imu}=J_{rel}^T·{H^{\prime }}_{imu}·J_{rel}$$

(27)

and

$$b_{imu}=J_{rel}^T·{b^{\prime }}_{imu}$$

(28)

3.3.2. Marginalization

As time goes on, there will be more and more landmark points and camera poses, leading to an increasing amount of computation. To achieve large-scale Gauss-Newton nonlinear optimization, the number of optimization variables needs to be limited. This paper uses the Schur-Complement method to marginalize out older keyframes and all variables related to that keyframe continuously. When marginalizing the visual factors between two keyframes, the method proposed by Engel et al. [30] is used. Firstly, all points in the keyframe are marginalized, and then remove the residual terms that affect the sparsity of the system. To maintain the consistency of the system, it is necessary to evaluate the Jacobian matrix for the marginal factors related variables and ensure that their values are the same. For visual factors, the Jacobian matrix can be evaluated at the linearized points. When computing the inertial factors, it is necessary to determine the evaluation points for all variables related to the marginal factors and evaluate their Jacobian matrices. As shown in Figure 3b, it can be intuitively understood how marginalization affects the connection between the factors.

4. Real vehicle experiments and results analysis

In order to verify the performance of the proposed system in real environment, a private dataset was self-made for real-vehicle experiments, and the public dataset M2DGR is used to compare this algorithm with several other mainstream LiDAR SLAM algorithms.

4.1. Establishment of experimental platform

The mobile robot experimental platform is shown in Figure 4. The electronically controlled crawler chassis can realize omnidirectional movement. Mobile power supply, display, on-board computing platform JETSON AGX ORIN (built-in ubuntu 20.04 OS), sensor equipment including ZED2i camera, 16-line LIDAR VLP-16 (vertical field of view of 30°, horizontal field of view of 360°), GUI-610GNSS combined navigation suite (positioning accuracy of 1cm+1ppm, attitude accuracy of 0.1°),and WHEELTEC N100 nine-axis IMU have been deployed on the car.

4.2. Collection of private datasets

In this paper, two groups of experimental datasets were collected to verify the performance of the system. The first group was an outdoor large-scale scene, as shown in Figure 5a, which collects a scene driving around a teaching building. It includes buildings, pedestrians, trees, and moving vehicles. The walking trajectory is shown as the green path in the bird’s-eye view satellite map in Figure 5b. The second group was a dimly-lit long corridor environment inside a teaching building, as shown in Figure 5c, the geometric features on the white walls on both sides of the corridor are lacking and the scene repeatability is high, which is in line with the degradation scenario. The experimental car was controlled to move in the above two scenarios and collect datasets.

4.3. Experimental results and analysis

Conduct real vehicle experiments using the collected dataset. Figure 6 shows the positioning and mapping results obtained by the experimental car driving around the teaching building. The entire trajectory did not experience any drift, and a dense 3D environmental point cloud was reconstructed with a clear outline. To preliminarily verify the accuracy of the trajectory obtained by this algorithm, the GPS trajectory during the driving process was first obtained using combined inertial navigation as the true trajectory, and the two complete trajectories were compared, as shown in Figure 7. The two trajectories have a high degree of overlap except for a slight deviation in position shown in Figure 7b. Figure 8 shows the localization and mapping result of the indoor long corridor. In the face of this scene with insufficient features and easy degradation, there was no positioning tilt or displacement occurred, and the constructed map has distinct edges that reproduce the long corridor environment very well.

In order to evaluate the system’s performance using a better dataset, the high-quality public dataset M2DGR is adopted to compare this algorithm with several other mainstream LiDAR SLAM algorithms. The widely used M2DGR dataset is provided by Shanghai Jiaotong University, which contains rich sensor suites and diverse scenarios. Moreover, the speed of the ground robot in each test sequence is close to the actual speed in real environments, which helps to evaluate the effects of various SLAM algorithms more fairly. This paper chose sequence 1 of the M2DGR dataset for testing, with its scene being a relatively empty street and includes significant rotation during movement. Firstly, the trajectories of this algorithm, A-LOAM, Lego LOAM, and LIO-SAM are obtained. Then, the trajectory evaluation tool - evo is used to compare and analyze the obtained trajectories with the true value trajectories. Figure 9 shows the trajectory in a three-dimensional coordinate frame, and Figure 10 is the trajectory projected on the xy two-dimensional plane. It can be observed intuitively that compared to other algorithms, the localization trajectory obtained by this algorithm is generally closer to the ground truth trajectory, even in sections with significant shaking (Figure 10b), where it still performs well.

The positioning accuracy of each algorithm is further evaluated through absolute trajectory error (ATE) and relative trajectory error (RPE). The ATE is the direct difference between the estimated pose and the ground truth pose, which can intuitively reflect the overall accuracy of the algorithm in estimating poses and the global consistency of the estimated trajectory. The RPE is used to measure local accuracy and is suitable for evaluating the drift of the system. Table 3 and Table 4 show the ATE and RPE values obtained by the four algorithms in sequence 1. There are seven indicators used to measure the localization accuracy of a system, including maximum error (max), mean error (mean), median error (median), minimum error (min), root mean square error (rmse), sum of squared errors (sse), and standard deviation (std). The values of each indicator are inversely proportional to the localization accuracy. Among them, the most commonly used and representative indicator are mean, median, and rmse. Therefore, this section focuses on these three indicators. As shown in Figure 11, for absolute trajectory error, the three error values obtained by this algorithm are all the smallest. Since the backend of ALOAM is a grid-based map, the accumulated error after long-distance mapping is significant. However, Lego-LOAM incorporates a loop closure detection module, resulting in a significant reduction of the error. Compared with Lego-LOAM and LIO-SAM, the absolute positional error of this algorithm is reduced by about 4.6% and 24%, respectively. For relative pose error, Lego LOAM has the largest error. The three error values of this algorithm are very close to LIO-SAM, but there is still a decrease, and the rmse is also the smallest among the four algorithms. Compared to LOAM, Lego-LOAM and LIO-SAM, the relative positional errors of this algorithm are reduced by about 5.7%, 48% and 0.3%, respectively.

5. Conclusions

This paper proposes a multi-sensor fusion SLAM system for complex environments in construction machinery. The system integrates visual texture information into LiDAR-inertial odometry to generate a new visual-inertial subsystem. The visual odometry front end uses direct method to establish the matching relationship between adjacent frames for pose estimation. The LIO and VIO subsystems are tightly coupled to build multiple constraint items for factor graph optimization, ensuring the consistency of the global map. Through experiments on real vehicles and the M2DGR public dataset, this system achieves good localization and mapping results in both indoor long corridors and outdoor large scenes. Compared with the well-performing open-source frameworks Lego-LOAM and LIO-SAM, the absolute pose errors obtained by this algorithm are reduced by 4.6%, 24%, respectively , and the relative pose errors are reduced by 48% and 0.3%, respectively. This fully demonstrating that proposed system has better localization accuracy and can solve the problem of poor localization and mapping robustness of unmanned vehicles in environments with lack of geometric features and easy degradation.