Multi-Sensor Fusion Autonomous Driving Localization System in Mining Environments

1. Introduction

Minerals are a crucial material foundation, and heavy trucks are the primary carriers for mineral transportation. Due to the harsh working conditions in mines, frequent traffic accidents, and common issues such as driver fatigue, combined with the rapid advancements in information and communication technology, the transportation technology based on autonomous heavy trucks has emerged.

Currently, positioning in mining areas primarily relies on Global Positioning System (GPS), Remote Sensing (RS), and Geographic Information System (GIS) integration. Large open-pit mining areas and major port terminals mainly use a combination of Global Navigation Satellite System (GNSS) and Inertial Navigation System (INS) technologies [12]. There are also systems that use SLAM for positioning. Presently, SLAM systems can be categorized into three types: laser SLAM [9,13,14,15], visual SLAM [1,2,3,28], and multi-sensor fusion SLAM [24,26,27,29].

While these methods can accomplish vehicle localization and map construction tasks, they often encounter drift or instability issues when facing complex scenarios. Particularly in mining environments, the open space makes it difficult for visual SLAM to capture the necessary features, and the bumps from heavy truck movement can cause sensor data drift, negatively impacting the state estimation of the entire SLAM system and hindering long-term positioning. Furthermore, due to various challenges, the output speed of odometry data is also critical. To address these challenges, we propose iLM, a multi-sensor fusion localization framework suitable for mining scenarios—specifically a SLAM system designed for high accuracy, speed, and robustness in environments characterized by sparse features and bumpy roads.The main contributions of this work include:

• We integrate multiple sensors, including cameras, LiDAR, and IMUs, to develop a multi-sensor fusion localization algorithm. By closely integrating GPS information with the SLAM system, we effectively eliminate initial biases and long-term drift in the localization system.
• We employed the IKD-tree for fast point cloud registration, which enhances the real-time performance of the localization algorithm, and incorporated AprilTag for auxiliary positioning, using factor graphs to fuse various information and produce precise localization results.
• We validated the algorithm using both public datasets and real mining datasets, demonstrating its applicability in practical engineering environments.

2. Related Work

2.1. Vision-Based SLAM

MonoSLAM [1] is indeed one of the earliest and pioneering works in visual SLAM,which is a pioneering algorithm for point-feature-based visual SLAM. In Mono-SLAM, the front end extracts sparse corner points or keypoints from images captured by a monocular camera as feature points, and uses active tracking technology to match these feature points across consecutive image frames. In the back-end motion estimation, the matched feature points from the image frames, along with the camera pose information, are used to derive the state variables at each timestamp. An Extended Kalman Filter (EKF) is employed to update the mean information and covariance matrix of the camera’s state variables based on the pose transformations between consecutive timestamps, while simultaneously constructing a sparse real-world map using the matched feature points.

Since the number of landmarks is limited in Mono-SLAM’s application scenarios, the sparse feature points are prone to loss. In larger work environments, the issue of feature point loss arises due to the increased computational load. To address this, Klein and others proposed the PTAM [2] visual SLAM solution, which was the first algorithm to use nonlinear optimization in place of traditional filters for processing images in the back-end. The front-end camera establishes an initial map based on the current frame image, extracts FAST features from the image frames, and predicts the camera pose of the current frame using a motion model and a RANSAC-based tracking and matching algorithm. The implementation of real-time response to front-end image data and the parallelization of the back-end camera pose estimation and mapping process enables PTAM to operate in real-time.

Richard and others proposed DTAM [3], an algorithm for building dense 3D maps using a monocular camera. DTAM uses a direct method to estimate the depth of each pixel in the image frames, constructing a dense depth map, and continuously updates the depth estimation of each pixel based on the camera’s motion. Compared to PTAM, DTAM does not require sparse feature extraction from the front-end images. Instead, it constructs high-precision, high-density environmental maps through depth estimation of each pixel, improving the tracking performance of targets moving within the scene. J. Engel and others proposed a new monocular camera algorithm called LSD-SLAM [28], which applies the direct method to semi-dense monocular SLAM. Using optical flow to optimize pixel brightness, LSD-SLAM estimates the monocular camera’s motion and builds a semi-dense map. Compared to the sparse maps constructed by earlier visual SLAM systems, the semi-dense maps built by LSD-SLAM provide more texture information on object surfaces and edges. However, LSD-SLAM still relies on feature-point methods for loop closure detection.

Based on the theoretical foundation of PTAM, Raul and others developed and proposed the ORB-SLAM [4]. ORB-SLAM introduces a three-thread structure, innovating on PTAM’s dual-thread system by adding real-time feature point tracking, local map optimization, and global pose loop closure detection threads. The feature point tracking part employs the ORB feature tracking method, where the ORB features, which are highly invariant to rotation and scaling, ensure real-time efficiency for the tracking thread. Additionally, the loop closure detection thread uses ORB descriptors to build a dictionary, providing globally consistent trajectories and maps across different work scenarios while maintaining robustness.

Raúl Mur-Artal and others proposed ORB-SLAM2 [6], which extends ORB-SLAM by adding support for stereo and RGB-D sensors. This approach uses a map dataset to build a bag-of-words (BoW) tree for the loop closure detection thread, and adds a new global bundle adjustment (BA) optimization thread after the loop closure detection to compute the system’s optimal motion results. Campos and others built on ORB-SLAM2 to propose ORB-SLAM3 [7], which supports monocular, stereo, and RGB-D cameras and provides both pinhole and fisheye models. ORB-SLAM3 is the first feature-based tightly-coupled visual-inertial odometry (VIO) system that improves system initialization efficiency by fully relying on maximum a posteriori (MAP) estimation. It introduces a multi-map data fusion technique for location recognition, camera relocalization, loop closure, and precise map merging, ensuring high-precision mapping, usability, and robustness in complex work environments.

At the same time, with the development of new visual rendering methods such as NeRF [16] and 3DGS [17], many SLAM algorithms based on these techniques have emerged. FMapping [18] and iMap [31] are dense SLAM methods based on NeRF, while GO-SLAM [21] is a deep learning-based dense visual SLAM framework that supports global pose optimization and real-time 3D reconstruction based on NeRF, accommodating monocular, stereo, and RGB-D inputs. Co-SLAM [30] combines coordinates and sparse parameter encoding with global bundle adjustment (BA) for online real-time tracking and mapping in NeRF RGB-D SLAM. SplaTAM [32] and Gaussian-SLAM [19] utilize 3D Gaussian map representations for dense SLAM, achieving fast and highly realistic rendering.

While visual-based real-time localization and mapping technology has seen significant development, the 2D image data output by camera sensors cannot fully capture the spatial features of objects. Furthermore, image acquisition by camera sensors is easily affected by changes in lighting within work environments. When faced with extreme lighting changes or rapid equipment movement, the resulting image data may be unclear, making feature recognition difficult and limiting the ability to rely solely on visual data for real-time localization and mapping.

2.2. Lidar-Based SLAM

A representative real-time localization and mapping method based on LiDAR was proposed by Grisetti et al. in the GMapping [13]. This system uses a particle filter algorithm as its foundation, dividing localization and mapping into two separate threads. In the mapping aspect, it introduces an adaptive resampling technique to solve the issue of particle depletion during the mapping process. However, in large-scale environments, the number of particles required for GMapping to recognize the environment increases significantly, leading to higher computational demands and memory usage, which limits its ability to meet real-time performance requirements. Kohlbrecher et al. proposed the HectorSLAM [14] method, which utilizes a multi-resolution approach to process LiDAR point cloud data and align consecutive LiDAR scan beams for motion estimation, while using a grid map to represent the actual mapping results. Compared to GMapping, the HectorSLAM system reduces hardware costs but requires a high-frequency LiDAR to support the computation demands. LOAM [15] based on 3D LiDAR. This approach segments the LiDAR point cloud by resolution, using low-resolution point clouds for vehicle motion estimation and high-resolution point clouds to construct a real-time localization map. The method runs in multiple threads, combining vehicle motion estimation with real-time mapping to form a complete localization system. Tixiao Shan et al. developed the LeGo-LOAM [9] algorithm, which builds on LOAM by adding loop closure detection. Using timestamps as the criterion, it detects loop closures by calculating the Euclidean distance between the poses of two LiDAR point cloud frames, reducing pose errors in large-scale environments. Park et al.proposed [22] that combines LiDAR signal intensity with feature matching based on the LOAM algorithm. This method effectively improves the mapping and localization accuracy by addressing recognition errors in planar structures in the environment. Wang et al. proposed the F-LOAM [25] algorithm based on the LOAM framework. This algorithm introduces a step-by-step undistortion process for distorted point clouds, compensating for point cloud distortion during both pose estimation with low-resolution point clouds and mapping with high-resolution point clouds. Additionally, the algorithm removes the map optimization step from LOAM, directly generating maps from point cloud data, improving computational speed and reducing overall computational cost.

LiDAR-based SLAM can obtain precise 3D depth data of object features, allowing for excellent feature extraction in environments rich in features and with smaller scanning ranges. However, in open environments with repetitive features, LiDAR SLAM may suffer from real-time trajectory drift and mapping errors due to incorrect point cloud matching. Moreover, when a vehicle is traveling in poor road conditions, resonance caused by bumps may lead to repeated scanning of surrounding objects by the LiDAR beam, resulting in incorrect overlaps in the mapping output.

2.3. Multi-Sensor Fusion SLAM

To overcome the limitations of single sensors in various complex environments, real-time state estimation and map construction using multi-sensor fusion has been introduced. Multi-sensor fusion refers to the synchronized processing of information collected by different sensors at different times and locations using algorithms, and optimizing the synchronized data to obtain corresponding pose estimation information. The Aerial Robotics Group proposed the VINS-Mono [20] algorithm, which introduces tightly coupled monocular visual-inertial odometry. It fuses monocular camera data with IMU (Inertial Measurement Unit) data for localization and mapping, enabling robust system initialization in unknown initial states, as well as online relocalization and 4-DOF global pose graph optimization. PL-VIO [23] uses point and line features to recover texture features in the environment, improving localization accuracy in low-texture environments.

Zuo et al.proposed LIC-FUSION [27], a tightly coupled fusion localization method combining LiDAR, visual, and inertial data. This method uses a Multi-State Constraint Kalman Filter (MSCKF) to estimate poses based on IMU motion information, while feature extraction is performed on LiDAR and camera data to update the real-time motion estimation.

LIO-SAM [24] is a tightly coupled fusion of LiDAR and IMU for localization. The method performs IMU motion information pre-integration and uses this pre-integration to compensate for distortion in the LiDAR point cloud. It extracts features from the undistorted point cloud and combines IMU motion information with point cloud data to form the initial values of the LiDAR-inertial odometry. In the mapping optimization part, the method introduces a sliding window keyframe technique, applying graph optimization to IMU pre-integration factors, LiDAR-inertial odometry factors, and loop closure detection factors within the sliding window to estimate the vehicle’s pose.FAST-LIO [11] proposes a tightly coupled iterative Kalman filtering framework for LiDAR and IMU. The main contribution of the paper is the provision of a new formula for calculating the Kalman gain. FAST-LIO2 [8] implements efficient mapping based on an efficient tightly coupled iterative Kalman filter.

R3LIVE [29] uses LiDAR, IMU, and visual data for fusion-based localization. The method tightly couples the LiDAR-Inertial Odometry (LIO) and Visual-Inertial Odometry (VIO) subsystems. The LIO system constructs a global map’s geometric structure using point cloud data and minimizes the distance between each scanned point cloud and the corresponding map point plane to iteratively estimate motion. The VIO system projects map tracking points onto the current image frame and minimizes the photometric error of these map tracking points to iteratively estimate motion.Based on the R3live framework, SR-LIVO [5] utilizes 3D LiDAR-Inertial-Vision scanning for reconstruction SLAM, with scan data and image timestamps synchronized.

With the advancement of multimodal approaches, several excellent SLAM works combining text and images have also emerged. TextSLAM [34] uses texture-rich blocks as semantic features of text objects for image matching and data association, resulting in more precise and robust performance. SNI-SLAM [33] employs a semantic SLAM system using neural implicit representations, enabling accurate semantic mapping, high-quality surface reconstruction, and robust camera tracking simultaneously.

Fusing IMU, LiDAR, and camera sensors for real-time localization and mapping effectively combines the advantages of multiple sensors. Compared to the limited output of single sensors, multi-sensor fusion addresses the problem of acquiring multi-source localization information at its root, providing greater adaptability to complex work environments. Multi-sensor fusion-based localization methods significantly improve the accuracy and real-time performance of simultaneous localization and mapping, ensuring the smooth operation of autonomous heavy trucks.

3. Materials and Methods

To address the challenges faced by autonomous vehicles in complex environments, we proposes a multi-sensor fusion-based localization method using three types of sensors: IMU , LiDAR , and a monocular camera. The design integrates data from these sensors to enhance real-time performance and improve localization accuracy. Various optimization techniques are applied to refine the system’s efficiency in real-world scenarios. The overview of the method is illustrated in Figure 1.

3.1. LiDAR-Inertial Odometry

A.Motion Compensation

In a stationary state of the carrier, the starting and ending points of the LiDAR scan coincide, allowing the LiDAR to obtain ideal point clouds. However, when the carrier is in motion, the relative displacement caused by the movement results in different coordinates for the same laser points within a single frame, as shown in Figure 2. We utilize IMU motion information to process the LiDAR point clouds based on the premise of achieving time synchronization between multiple sensors. By using interpolation methods, the pre-integrated IMU information is interpolated into the adjacent frames of the LiDAR samples. The external parameter matrix is used to convert the vehicle motion information obtained from the IMU into point cloud motion information in the LiDAR coordinate system of the adjacent frames.

Based on the initial position information of the LiDAR point clouds and the position information after motion, the point cloud compensation change matrix between the two frames before and after the laser sampling is obtained. Using this point cloud compensation change matrix, the initial coordinates of the point clouds in the current frame are transformed into the ideal point cloud coordinates of the next frame. Point clouds with coordinate deviations are replaced, completing the motion distortion compensation for the point clouds in the current frame. The point cloud de-distortion process is illustrated in Figure 3. Finally, the pose of each laser point in the k-th frame of the point cloud is corrected to obtain the de-distorted point cloud in the LiDAR coordinate system at the end of that frame.

B.Feature Extraction and Matching

The LiDAR points are divided into ground points and non-ground points based on their spatial distribution in the point cloud, and downsampling is performed on the ground points. Local curvature is calculated for both types of points using the formula :

$$c={\displaystyle \frac{1}{\left|S\right|∥X_{(k,i)}^L∥}}·∥\sum _{j\in S,j\ne i}\left(X_{(k,i)}^L-X_{(k,j)}^L\right)∥$$

(1)

where S represent the down-sampled subset of the de-distorted point cloud set, $X_{\left(k,i\right)}^L$ is the laser coordinates of point $\mathbf{i}$ at time $\mathbf{k}$ in the LiDAR coordinate system, where $\mathbf{c}$ represents the curvature value of the local point. The curvature values for all points in the point cloud are calculated and then sorted to obtain the set of feature points.

For the extracted feature point set, the keyframes are first selected from consecutive LiDAR point clouds. A threshold is set for the vehicle’s pose change during driving. At a certain time $\mathbf{t}$, the vehicle’s pose is denoted as $X_i$. As the vehicle continues to move, when the pose change exceeds the threshold, the current LiDAR frame $F_{(i+1)}$ is selected as a keyframe, and the point cloud frames between the initial LiDAR frame at pose $X_i$ and keyframe $F_{(i+1)}$ are discarded. In the practical application of this study, the threshold for pose change during vehicle movement is set as a displacement of 1 meter or a rotation angle change of 15°.

Based on the keyframe set, a local map is constructed. A sliding window with a scale of n frames is used to filter the LiDAR keyframes, and the merged local point clouds form the local point cloud map $M_i$ corresponding to the keyframe set. The local point cloud map can be represented based on feature point classification and the keyframe set:

$$M_i=\left\{M_i^e,M_i^p\right\}=\left\{\begin{array}{c}{M}_i^e=F_i^e\cup F_{i-1}^e\cup \dots \cup F_{i-n}^e\hfill \\ M_i^p=F_i^p\cup F_{i-1}^p\cup \dots \cup F_{i-n}^p\hfill \end{array}\right.$$

(2)

where $X_i^p$ denotes planar feature points and $X_i^e$ denotes edge feature points.

Let $X_i$denote the pose state at a given moment, with the corresponding keyframe $F_{(i+1)}$ containing edge feature points and local feature points. In the local point cloud, these correspond to lines and planes, respectively. The distance between edge feature points and planar feature points to the local point cloud map can be calculated using the following formula:

$$d_e={\displaystyle \frac{\left|\left(P_{i+1}^e-P_i^e\right.)\times \right.\left.\left(P_{i+1}^e-P_i^e\right)\right|}{\left|P_i^e-P_i^e\right|}}\mathrm{and}{d}_p={\displaystyle \frac{\left|\begin{array}{c}\left(P_{i+1}^p-P_i^p\right)\\ (P_i^p-P_i^p)\times (P_i^p-P_i^p)\end{array}\right|}{\left|(P_i^p-P_i^p)\times (P_i^p-P_i^p)\right|}}$$

(3)

C.Mapping

In the process of multi-sensor fusion localization, we introduced a monocular camera sensor to expand the information acquisition range, which significantly improved localization accuracy. However, there was no substantial increase in operational speed. To achieve faster and more accurate high-definition mapping, we propose using an ikd-tree [8] to replace the kd-tree typically used for laser point cloud registration in multi-sensor fusion localization methods, thereby enhancing the operational speed and real-time performance of the fusion localization approach.The iKD-Tree construction process is illustrated in Algorithm 1.

Algorithm 1 iKD-Tree Construction

Require: Point set V, Point count per group N    Ensure: Constructed iKD-Tree  1: Select the root and initialize subtree  2: while Subtree is not empty do  3:    Sort points by covariance  4:    Set median point as split value and add to tree node T  5:    Assign points left of median to left child, right to right child  6: end while  7: Label node T with attribute LazyLabelIntPullup  8: Complete iKD-Tree construction

When the ikd-tree updates the tree with new point increments only, the computation time is shorter compared to the existing static kd-tree. In addition to point-wise operations, the ikd-tree also supports various functionalities such as box-wise operations showed in Algorithm 2 . During real-time mapping, the ikd-tree allows the map to be updated as the sensor poses move, ensuring that the tree structure can efficiently accommodate dynamic insertions and deletions. While performing incremental operations (i.e., insertion, reinsertion, and deletion), the ikd-tree actively monitors the tree structure and rebalances parts of the tree. This dynamic balancing guarantees efficient nearest neighbor searches in later stages.

Algorithm 2 iKD-Tree Box Update

Require: Search box $C_0$, tree node T, containing range $C_T$    Ensure: Updated iKD-Tree  1: if $C_T\cap C_0=\varnothing$ then  2:    return No update needed  3: end if  4: if $C_T\subseteq C_0$ then  5:    Mark subtree and node as deleted  6:    Set  $T.\mathrm{Pushdown}\leftarrow \mathrm{true}$ 7:   return   8: end if  9: for each point P in node T do   10:    if $P\in C_0$ then   11:      Remove P from node and add to update list   12:    end if  13: end for  14: Recursively update child nodes   15: Update root node properties   16: return Complete box update operation

D.LiDAR-Inertial Motion Estimation

Using the initial pose estimation from the IMU as the initial pose value for the laser-inertial odometry, the initial keyframe is set based on this initial pose value. The rotation matrix $R_{L_{k+1}}^{L_k}$and the translation vector $t_{L_{k+1}}^{L_k}$ are calculated for the transformation between the k keyframe and the k+1 keyframe. The frame transformation matrix $T_{L_{k+1}}^{L_k}$is composed of $R_{L_{k+1}}^{L_k}$and $t_{L_{k+1}}^{L_k}$. A schematic diagram showing the distribution between the laser keyframe and the IMU pre-integration is illustrated in Figure 4.

The z-axis optimization is based on the distance from laser plane points within the keyframe to the corresponding point cloud planes, aiming to minimize the distance from the laser plane points to the corresponding point cloud planes in the z-axis direction:

$$argmin\left\{T_{L_{k+1}}^{L_k}-{\left[J_p^T{J}_p+\lambda diag\left(J_p^T{J}_p\right)\right]}^{-1}{J}_p^T{d}_p\right\}$$

(4)

where $J_P$ represents the Jacobian matrix associated with the plane features, and $d_P$ denotes the residual or distance between the laser plane points and the corresponding points in the point cloud:

$$d_p=f_p\left(P_i^{L_i},T_{L_{k+1}}^{L_k}\right)J_p={\displaystyle \frac{\partial f_p}{\partial T_{L_{k+1}}^{L_k}}}$$

(5)

Similarly, the optimization for the x and y axes is performed based on the distance from the laser edge points within the keyframe to the corresponding edge lines in the point cloud:

$$argmin\left\{T_{L_{k+1}}^{L_k}-{\left[J_e^T{J}_e+\lambda dig\left(J_e^T{J}_e\right)\right]}^{-1}{J}_e^T{d}_e\right\}$$

(6)

3.2. Visual-Inertial Odometry

A.Visual-Inertial Motion Estimation

In the preprocessing of monocular camera information, only two-dimensional visual features can be obtained. Therefore, we use the depth information output from the LiDAR point clouds to recover the spatial depth information of the two-dimensional visual feature points. The laser point cloud features are aligned with the visual features based on the same timestamp for each frame of data, as shown in Figure 5. After alignment, the spatial information from the laser point cloud features is associated with the nearest visual feature points to obtain three-dimensional visual feature points.

The three-dimensional spatial coordinates of the projection point of the visual feature point onto the laser feature plane are calculated using the normal vector of the laser feature plane and the two-dimensional coordinates of the visual feature point:

$$X_{3D}^C=\left|{\displaystyle \frac{d}{n^T{X}_{2D}^C}}\right|X_{2D}^C$$

(7)

Where d represents the distance from the laser feature plane to the origin, n represents the normal vector of the laser feature plane, $X_{2D}^C$ denotes the two-dimensional coordinates of the visual feature point in the image plane, and $X_{3D}^C$ represents the three-dimensional spatial coordinates of the projection point of the visual feature point onto the laser feature plane.

For the spatial visual features with recovered depth, the PnP method is used for visual-inertial pose estimation, resulting in the optimized pose estimation of the visual-inertial odometry.

B.(Optional) AprilTag-Assisted Localization

AprilTag is a visual fiducial system popular in robotics research. Based on different QR code tags, the AprilTag algorithm can obtain the coordinates and orientation of each tag in the camera coordinate system. In open environments, due to the sparsity of features, it is challenging to extract features for localization solely from images captured by the camera. To address this application scenario, we propose an optional method for auxiliary calculation using AprilTag markers. By placing AprilTag markers in the scene, the camera can receive information from the QR code tags. When a tag is detected, the AprilTag algorithm can compute the position coordinates of the tag relative to the camera.

We set up to recognize four AprilTag markers simultaneously to calculate the camera’s pose.By utilizing the relative position information between four April tags and the camera, we can represent the position of each tag using its three-dimensional coordinates $P_t^{\left(i\right)}$,for $i=1,2,3,4$ while the camera’s pose is expressed in homogeneous coordinates $T_c$. The equation for solving the camera pose can be expressed as:

$$P_c=T_c·P_t^{\left(i\right)}$$

(8)

Combining the distance relationships between the camera and each tag, we can derive the following equation:

$$d_c^{\left(i\right)}=\left|p_c-p_t^{\left(i\right)}\right|=\sqrt{{(p_{c_x}-p_{t_x}^{\left(i\right)})}^2+{(p_{c_y}-p_{t_y}^{\left(i\right)})}^2+{(p_{c_z}-p_{t_z}^{\left(i\right)})}^2}$$

(9)

By minimizing the reprojection error, we define the optimization objective function as:

$$\mathsf{\Phi }\left(T_c\right)=\sum _{i=1}^4{(d_c^{\left(i\right)}-{\left|T_c·p_t^{\left(i\right)}\right|}^2)}^2$$

(10)

After obtaining the AprilTag-assisted pose information, we perform error-state extended Kalman filter (EKF) optimization with the IMU data to achieve a smoother trajectory.

3.3. Back-end Optimization

The global pose estimation employs a factor graph approach, where the pose estimates from various components are divided into individual factor graphs. Due to the characteristics of the real-world environment, under stable GPS signals, we have incorporated a GPS factor to assist in positioning correction, preventing pose drift during long-term operation. Additionally, we have included optional AprilTag-assisted positioning for joint optimization. A schematic diagram of the principles of global pose estimation based on the factor graph method is shown in Figure 6.

The sensor information within each factor graph is deeply fused, allowing for real-time updates of the local map for newly added pose estimates. The optimization variables for each pose estimate are input as factors within the factor graph. By estimating the pose between consecutive keyframes n and $n+1$,the pose transformation formula under no error influence can be obtained:

$$\delta T_n^{n+1}=T_n^{-1}{T}_{n+1},\delta \xi ={ln\left(T_n^{-1}{T}_{n+1}\right)}^{\vee }$$

(11)

Let the actual pose estimation error within consecutive frames be represented as $e_n^{n+1}$:

$$e_n^{n+1}=\delta {\xi }_n^{n+1}{ln\left[{\left(T_n^{n+1}\right)}^{-1}{T}_n^{-1}{T}_{n+1}\right]}^{\vee }$$

(12)

This error is minimized using the Gauss-Newton method for linear computation:

$$C=min{\displaystyle \frac{1}{2}}\sum _{i,j\in \epsilon }{e}_{ij}^T{\Sigma }_{ij}^{-1}{e}_{ij}$$

(13)

After multiple iterations and convergence, when the error objective function converges to the designed threshold or the maximum number of iterations is reached, the global optimal pose estimation can be obtained.

4. Experiment

We validated the effectiveness of our localization method using both public and field datasets, with GPS ground truth as the benchmark. We conducted comparative tests on localization accuracy between our method and the mainstream localization algorithm LIO-SAM [24], followed by a quantitative analysis and comparison of trajectory errors.

4.1. Public Dataset

We utilized several publicly available datasets that are widely recognized in the robotics and computer vision community.The Garden dataset [] is primarily collected from the playground and surrounding areas of a campus, where the lighting conditions remain relatively stable, and pedestrian and vehicle traffic is minimal. Some paths within this dataset pass through wooded areas and other obstructed environments, effectively testing the feasibility of the fusion localization method. Handheld dataset [] is collected using a handheld multi-sensor device in a more dynamic environment with higher pedestrian and vehicle traffic. This scenario presents numerous obstacles, providing a robust context for evaluating the applicability of the localization method.

The mapping results of the proposed multi-sensor fusion localization method on the Garden and Handheld datasets are shown in Figure 7. As observed, the fusion localization method proposed in this paper does not exhibit map drift during testing on public datasets and demonstrates clear scanning accuracy of surrounding features. Therefore, this fusion localization method shows a certain degree of applicability on public datasets.

Using the proposed multi-sensor fusion localization method on public datasets, we first analyzed the tree-building time and search speed of the ikd-tree, as shown in Table 1. Under identical poses, box-by-box search takes 50 less time than point-by-point search, indicating the speed advantage of ikd-tree’s box-by-box search. A comprehensive time comparison with the LIO-SAM method was also conducted, with the analysis results presented in Table 2.

4.2. Mine Self-Collected Dataset

This section introduces the testing of the multi-sensor fusion localization method in real-world scenarios. Multi-sensor brackets were installed on both a test cart and a heavy truck for the experiments. Data was collected from locations with different characteristics, followed by localization evaluation and visualization of the mapping results to analyze the proposed method.

A.Test Cart Positioning Test and Analysis

The experimental test cart used in the experiment is shown in Figure 8. The specifications of the components are as follows:

• Monocular Camera: The model is SENSING GSML2, with a focal length of 3 mm.
• LiDAR: The model is HeSai PandaXT32. Its measurement range is 120 m, with a measurement accuracy of ±2 cm. The horizontal field of view is 360°, with a horizontal angular resolution of 0.18° (10 Hz). The vertical angle ranges from -16° to +15°, with a vertical angular resolution of 1°.
• IMU Gyroscope:The monocular camera is equipped with aYESENSE YIS500-A IMU gyroscope, which has an output frequency of 200 Hz.

We conducted localization and mapping of the experimental cart along different paths within the industrial park. By leveraging the trajectory output from the fusion positioning method, we compared it with the GPS ground truth data of the experimental cart to analyze the localization errors. Using the EVO [35] (Evaluation of Odometry and SLAM) tool, we calculated the absolute pose error (APE) between the estimated trajectory and the ground truth. In Figure 9, (a) represents the trajectory error for the dataset, while (b) illustrates the trajectory error along the x, y, and z axes for the same dataset.

B.Field Work Scenario Positioning Test and Analysis

In this section, we use a self-collected dataset from a mining site, with the satellite image of the mining test area shown in Figure 10. Our fusion localization method is applied to the self-collected dataset.

The autonomous driving positioning test utilizes an 8×4 Dongfeng dump heavy truck, which can be applied in working environments such as mines, ports, and industrial parks, capable of unloading cargo. This heavy truck is designed to be equipped with three types of sensors for multi-sensor fusion positioning, including one monocular camera, one LiDAR, and one IMU inertial navigation gyroscope, as shown in the left of Figure 11. The model of the monocular camera is SENSING GSML2, the LiDAR model is HESAI XT32, and the IMU gyroscope model is YESENSE YIS500-A.

In harsh working environments, adverse factors such as flying debris from blasting, rain erosion, and other environmental impacts pose challenges. Additionally, issues related to sensor heat dissipation, blind spots in data collection, and equipment stability must be considered. Therefore, this paper presents a multi-sensor support frame designed with an integrated safety enclosure to ensure the structural stability of the multi-sensor fusion setup. The structural design is illustrated in the right of Figure 11.

We conducted tests on the proposed positioning algorithm , the LVI-SAM [26] and the LIO-SAM [24] algorithm using datasets collected from the field, including both circling and turning maneuvers around the mining site. The positioning trajectories were outputted, and we utilized the EVO [35] tool to compare these trajectories with the GPS ground truth data from the datasets. The EVO tool also provided insights into the position errors along the x, y, and z axes for both algorithms across the two datasets. In Figure 12, (a) illustrates the trajectory errors of different algorithms for the heavy truck circling dataset, while (b) displays the xyz axis trajectory errors of different algorithms for the same dataset.

At the same time, we compared the running speeds of the LIO-SAM algorithm, LVI-SAM algorithm, and our proposed algorithm on the mining self-collected dataset, as shown in Table 3. The table shows that the fusion localization method incorporating the iKD-Tree has a significant improvement in runtime speed compared to LIO-SAM, with an increase of over 33 percentage.

The quantitative analysis of the test results for the proposed positioning method and the LIO-SAM algorithm on the heavy truck’s surrounding driving dataset, performed using EVO, is summarized in Table 4, which presents four types of errors. Figure 13 illustrates the statistical results of the quantitative analysis, where (a) compares the absolute pose trajectory errors of the two algorithms in the heavy truck surrounding driving scenario. The errors for both algorithms are relatively stable, indicating that the multi-sensor fusion positioning with the addition of a monocular camera shows superior performance. (b) displays the box plot of the absolute errors. From the figure, it is evident that for the heavy truck surrounding driving scenario, the absolute errors of the proposed positioning method are generally lower than those of the LIO-SAM algorithm and are significantly more stable.

For the U-turn driving dataset, we performed trajectory evaluations using both algorithms. In Figure 14, (a) shows the trajectory error for the U-turn dataset, while (b) illustrates the trajectory errors along the x, y, and z axes. Table 5 provides a quantitative analysis of the trajectory error. Figure 15 presents statistical graphs of the quantitative analysis: (a) compares the absolute pose trajectory errors for the two algorithms in the U-turn scenario; (b) shows box plots of absolute trajectory errors for each algorithm in the same scenario.

In the heavy truck circular driving dataset, there is some path drift in the GPS ground truth trajectory, mainly caused by signal fluctuation due to occlusions from mining site buildings and surrounding vegetation. In contrast, the path output by our fusion localization algorithm is nearly unaffected by such environmental obstructions.

When evaluating accuracy in the two scenarios—circular and turning driving—the root mean square error (RMSE) of absolute pose trajectory for our localization algorithm is reduced by 61.84 percentage and 34.05 percentage, respectively, compared to LIO-SAM. This improvement is attributed to the noise filtering method in our approach, which applies EMD and fractal Gaussian noise filtering to the IMU output. This process effectively reduces low-frequency colored noise in the IMU signal, enhances the signal-to-noise ratio, and improves the accuracy of both the LiDAR-inertial odometry and visual-inertial odometry in our method.

At the same time, we also tested the beneficial effects of incorporating AprilTag-assisted localization in sections with significant bumps and without GPS signals, with the results shown in Figure 16. After adding the AprilTag localization factor, our algorithm demonstrated significant improvements in positioning accuracy and final error during long-duration travel without GPS.

Figure 17 shows the test results of our algorithm compared to LIO-SAM in a complete mining scene. It is evident that our algorithm is smoother and more accurate, exhibiting less drift.

To demonstrate the effectiveness of our algorithm during long-duration travel, we tested our algorithm at different driving distances in a mining scenario (with a maximum speed of 30 km/h for the truck). We recorded the maximum error between the odometry produced by our algorithm and the actual GPS across varying distances, and compared it with LIO-SAM and LVI-SAM. The results are shown in Table 6.The results from the table indicate that the odometry information produced by our algorithm has the smallest error compared to the actual GPS values. Additionally, due to the scarcity of visual information in the mining site, the LVI-SAM algorithm was significantly affected during actual use.

The experimental results above indicate that our localization method produces lower errors, resulting in a more stable positioning path with minimal drift. The integration of monocular cameras for multi-sensor fusion localization contributes to achieving a more stable error pattern, while GPS signals and AprilTag-assisted localization help our system operate reliably in long-distance travel scenarios and bumpy road segments. This supports the advantages and adaptability of our method based on real-world testing in various environments.

5. Discussion and Conclusions

We aim to achieve stable and accurate localization for autonomous heavy-duty trucks and have conducted research on simultaneous localization and mapping (SLAM) technology based on multi-sensor fusion. Using IMU, monocular cameras, and LiDAR as sensors, we address the issue of localization information loss encountered by autonomous heavy-duty trucks in mining environments by proposing a multi-sensor data fusion localization method. This work primarily covers the processing of multi-sensor information and the study of multi-sensor fusion localization methods.

To address the problem of slow odometry publishing frequency, we propose using an iKD-Tree to organize the point cloud data structure, which effectively enhances the speed of odometry output. Additionally, we introduced GPS factors and AprilTag-assisted localization factors in graph optimization to tackle issues arising from bumpy road segments and long-distance travel in mining scenarios. We conducted validation tests on the effectiveness of our localization method using both public datasets and field data, employing GPS ground truth for accuracy assessment.

The experimental results indicate that our localization algorithm offers superior accuracy compared to current mainstream localization methods, and it is also more stable and robust.