1 Introduction
Simultaneous Localization and Mapping (SLAM) technology, proposed in 1988, has been primarily used to study the intelligence of mobile robots. Currently, intelligent vehicles equipped with key sensing devices such as lidar, cameras, and inertial measurement units (IMUs) can already meet the autonomous navigation tasks in indoor scenes with the help of SLAM technology. SLAM technology is a key technology for achieving autonomous localization in unknown environments, and lidar-based simultaneous localization and mapping (laser SLAM) technology is widely used in fields such as autonomous driving, indoor and outdoor robot navigation, and 3D reconstruction. The main reasons why laser SLAM can be an indispensable technology in autonomous driving localization solutions are: (1) laser SLAM can provide localization services in unknown environments without the need for pre-arrangement of the localization scene; (2) lasers can accurately measure the angle and distance of obstacle points; (3) it can work in environments with poor lighting and large changes in illumination; and (4) it can generate grid maps and intuitive point cloud maps that are easy to navigate.
In his classic book "Probabilistic Robotics" published in 2005, former Google self-driving car leader Sebastian Thrun introduced how to use two-dimensional LiDAR for map building and localization based on probabilistic filtering methods. The book elaborates on the process of gradually developing from the classic FastSLAM algorithm based on Rao-Blackwellised Particle Filter (RBPF) to the LiDAR-based mapping standard GMAPping algorithm [4]. In 2016, Google open-sourced its optimized LiDAR SLAM algorithm Cartographer, which improved the shortcomings of GMAPping, such as computational time consumption and inability to effectively handle closed loops, and effectively made up for the shortcomings of GMAPping by using the idea of inter-frame point cloud map matching and sub-map construction.
While laser SLAM technology can meet positioning accuracy requirements in certain scenarios, it struggles to provide continuous and stable high-precision positioning output in full-condition autonomous driving scenarios. Furthermore, due to the structural similarity of the positioning scene and the insufficient environmental description capability of laser point clouds, false positives (false positives) in SLAM loop closure detection are prone to occur, thus reducing the system's positioning and mapping performance. This research aims to propose an innovative solution for laser SLAM in positioning and mapping tasks under scenarios with structural similarity and strong laser interference by fusing geomagnetic signals with the loop closure detection component of laser SLAM. Simultaneously, this research applies this technology to the field of autonomous driving positioning and mapping, achieving accurate matrix completion-based autonomous vehicle positioning in all weather and all terrains, improving the accuracy and stability of autonomous vehicle positioning algorithms.
2. Related Research Progress
2.1 Laser SLAM closed-loop detection
In recent years, some foreign scholars have proposed loop closure verification algorithms to address the impact of false alarms in loop closure detection on SLAM systems. Bosse and Roberts proposed using the maximum correlation sum between entropy sequences and the verification metric of the projected histogram to estimate the correctness of loop closure detection, thereby improving the accuracy of loop closure detection. Granstrom et al. proposed a loop closure verification criterion based on post-transformation scanning and final registration. Corso suggested that when rigorously handling loop closure verification in indoor environments, since loop closure can be detected from multiple sensor data sources, loop closure verification algorithms and loop closure detection algorithms should be studied separately. Furthermore, Gao and Harle, building on existing reports, proposed using a geomagnetic sequence signal matching and localization method to correct the loop closure detection stage of PDR (Pedestrian Dead Reckoning)-based SLAM, thereby improving the accuracy of pedestrian gait detection and localization.
(a)
(c) False detections and error localization in closed loop (d) Error mapping in environmental interference Figure 1 Common false detection interferences in closed loop and error localization and mapping
2.2 Closed-loop detection model
Loop closure detection refers to the process where an autonomous vehicle, after traveling a long distance and returning to a previously visited location, re-observes the features previously seen on the map and adjusts the global map based on the differences between the two observations to reduce the accumulation of errors during localization and mapping. Currently, the main loop closure detection methods include: (1) frame-to-frame loop closure detection; (2) frame-to-sub-map loop closure detection; and (3) sub-map-to-sub-map loop closure detection. All of these loop closure detection algorithms use correlation scanning matching, determining the similarity between two frames of laser point clouds through the rotation matrix R and the translation vector T. When the lidar acquires a new scanning frame, it searches for the optimal matching frame within a certain range. If the optimal matching frame meets the requirements, it is considered a loop closure. This matching problem can be described by the following formula:
(1)
Where ξ is the rigid transformation of the matching solution, ω is the search space, k is the number of poses in the search space, and Mnearest represents the confidence that the product of the observed laser point cloud hk and the position transformation matrix Tξ most closely coincides with the map laser grid point cloud. For the confidence sum when each frame of laser point cloud is inserted into the sub-map, the higher the confidence, the more similar it is considered. To find the matching frame with the largest confidence sum in the ω space, it is necessary to find the optimal solution of the matching result in the ω space. A common method is brute force matching, that is, within the search space, performing the above nonlinear least squares calculation on each frame and the current frame. The rigid transformation ξ calculated by the matching algorithm is multiplied by the initial pose of the intelligent vehicle to obtain the current pose (xi, yi, θi). When there are materials that interfere with the laser beam between the two matched frames (objects with low reflectivity or glass that transmits the beam, etc.), these interfering factors in the scene are the main components of the error accumulation in the laser point cloud matching stage. This accumulated error can be eliminated by the closed-loop detection method. The following scenarios are prone to causing false detections in closed-loop detection of laser SLAM: (1) Indoor corridors, empty indoor lobbies, and rooms with similar structural layouts, as shown in Figure 1(a); (2) Objects with low reflectivity in the scene, such as black light-absorbing walls and furniture; (3) Objects in the scene that are glass or transmit laser beams, as shown in Figure 1(b); (4) Objects in the scene that reflect laser beams, such as mirrors and stainless steel.
As the map expands, the efficiency of matching and searching determines the real-time performance of loop closure detection.
When SLAM deals with large scenes and complex interference environments, real-time localization and mapping place higher demands on the efficiency of loop closure detection algorithms. Furthermore, in environments with high spatial similarity, the insufficient representation of environmental features by low-beam lidar leads to frequent false alarms in loop closure detection. In scenes with interference from the laser beam (glass, mirrors, stainless steel, etc.), some incompletely explored areas will appear when the autonomous vehicle runs its localization and mapping algorithm; these areas are shown in gray, as shown in the red circle in Figure 1(d). Traditional lidar localization methods address these interferences by attaching reflective sheets to the interfering laser scanning plane or through manual verification. Traditional manual post-processing methods use drawing tools in the RoboStudio robot simulation software to erase misjudged areas in the mapping results, making them appear as explored areas. Therefore, this method is neither intelligent nor time-consuming. This paper proposes a loop closure detection method that fuses geomagnetic sequence signals with lidar, based on research on geomagnetic sequence signals, providing a "one-stop" solution to the impact of the aforementioned interferences on the entire localization and mapping system.
3. Design of a fusion algorithm for geomagnetic signals and laser SLAM
This paper, based on geomagnetic navigation technology and laser SLAM algorithm, designs a loop detection algorithm that fuses geomagnetic sequence search and laser point cloud matching for the loop detection stage of localization and mapping of unmanned vehicles. The algorithm framework is shown in Figure 2, where the proposed loop detection algorithm with coarse matching and geomagnetic feature screening is highlighted in the red dashed box.
Figure 2. Framework of the geomagnetic signal and lidar fusion localization and mapping algorithm
3.1 Algorithm Design for Fusion Positioning Based on Geomagnetic Signals and LiDAR
The geomagnetic signal and lidar fusion localization and mapping algorithm is designed based on the lidar SLAM algorithm framework, and therefore, like the lidar SLAM framework, it has front-end scan matching, back-end optimization, and loop closure detection stages. The proposed loop closure coarse matching and geomagnetic feature screening loop closure detection algorithm introduces geomagnetic sequence matching technology into the lidar SLAM loop closure detection stage. The entire algorithm flow for geomagnetic signal and lidar fusion localization and mapping is shown in Figure 3. The loop closure coarse matching and geomagnetic feature screening loop closure detection algorithm is located in the red dashed box on the right side of the entire system frame. The overall algorithm framework parameter settings are divided into sensor parameter settings and pose map parameter settings. The sensor parameter settings are further divided into inputs from the lidar and inputs from the IMU. The lidar parameter settings include: scan angle range, scan angle interval, number of scan points, scan time, and number of beams per scan.
Figure 3. Flowchart of the localization and mapping algorithm based on geomagnetic signal and lidar fusion.
The algorithm first initializes the parameters mentioned above, using the initial position of the moving vehicle as the origin, and defines the search area for laser point cloud matching around the vehicle coordinates estimated by the IMU. Within this radius, the laser SLAM front-end point cloud matching algorithm is called to estimate more refined coordinates of the vehicle. The vehicle's attitude is provided by the IMU at the current moment, at which point the front-end pose estimation is complete. However, during the laser point cloud matching process, due to environmental interference, accumulated errors inevitably occur. Some of these errors are eliminated by the back-end optimization stage, while others are eliminated by the loop closure detection stage. The proposed loop closure coarse matching and geomagnetic feature screening loop closure detection algorithm utilizes the characteristics of fast search and matching of geomagnetic sequences to provide a stable and accurate set of loop closure candidates for laser SLAM.
3.2 Loop Closed-Loop Coarse Matching and Geomagnetic Feature Screening Loop Detection Algorithm
First, acquire the current laser point cloud data and the corresponding three-axis geomagnetic signal, and insert the current frame point cloud into the sub-map. The three-dimensional column vector composed of the three-axis geomagnetic signal is (X,Y,Z). The magnitudes of the geomagnetic sequence in the X, Y, and Z axes are mx, my, and mz, respectively. Calculate the magnitude of the geomagnetic sequence and add it to the geomagnetic sub-sequence Si, where Si represents the magnitude of the i-th 10 s geomagnetic sequence.
(2)
Next, following the laser point cloud matching algorithm, the newly constructed point cloud sub-map is matched with the historical sub-map. This paper adds point cloud sub-maps that meet the sub-map matching score threshold to the candidate sub-map set, and the corresponding geomagnetic sequence signals are added to the candidate sub-sequence set. If no point cloud sub-map meets the conditions, the current pose does not have a loop closure, and the loop closure detection process is exited. Then, the distance between the candidate geomagnetic sub-sequence and the newly added geomagnetic sub-sequence is calculated using the Dynamic Time Warping (DTW) algorithm. Considering that even at the loop closure detection location, the motion trajectory cannot completely overlap, the motion in the vicinity of the loop closure detection area must sometimes approach the previous motion trajectory and sometimes move away from it. For this unconstrained motion (motion without a fixed motion trajectory), setting a fixed DTW distance to constrain the threshold is not applicable. Therefore, the Iterative Closest Point (ICP) algorithm is used to calculate the motion trajectory in the candidate set and match it with the current trajectory, recording the number of steps the algorithm iterates to convergence. Finally, for candidate geomagnetic subsequences that satisfy the iteration step constraint (i.e., similar motion trajectories), the DTW threshold constraint is relaxed. This allows even motions deviating from their original trajectories to meet the DTW geomagnetic matching threshold selection criteria at the location of closed-loop detection.
Figure 4. Loop closure coarse matching and geomagnetic feature screening loop closure detection algorithm
Figure 4 shows the flowchart of the loop detection algorithm using coarse matching and geomagnetic feature screening. The algorithm presented in this paper can further filter accurate loop detection points in the candidate sub-maps detected by laser loop closure using DTW (geometric mean time) based on geomagnetic matching and ICP (internal trajectory matching) algorithms. This solves the problems of false detection in loop closure (caused by similarity in indoor building structures) and location mapping disturbances (caused by the transmission and reflection of laser beams by indoor glass and smooth mirror materials). Specifically, the number of convergence steps in ICP is used as the trajectory similarity evaluation, and the threshold of the matched geomagnetic sequence at the loop detection point is adaptively adjusted to ensure the accuracy and stability of loop detection.
When a new geomagnetic sequence Snew is generated, the Euclidean distance between the already generated geomagnetic sequence Si and Snew is calculated using the DTW algorithm. When the distance DTW(Snew, Si) is less than the loop closure detection threshold DLC, the possible pose node for loop closure detection is added to the loop closure detection candidate set. Table 1 shows the pseudocode of the loop closure detection algorithm for the fusion of geomagnetic signals and lidar. Figure 5 shows the principle of geomagnetic sequence and lidar SLAM fusion. The three-axis geomagnetic signal is used to form a sequence Si by summing the squares, and a new pose node Xi is formed by the schematic diagram corresponding to the pose with the same timestamp, which reflects the fusion principle of geomagnetic sequence signal and lidar calculated pose.
Table 1. Pseudocode of the closed-loop detection algorithm based on the fusion of geomagnetic signals and lidar.
Figure 5. Schematic diagram of the fusion principle of geomagnetic sequence and laser SLAM.
4. Experiment
4.1 Design of Unmanned Driving Hardware Testing Platform
This study primarily focuses on providing high-precision positioning and mapping for heavy-duty Automated Guided Vehicles (AGVs) in dock warehouses. Figure 6 shows the overall block diagram of the geomagnetic and laser fusion SLAM experimental platform. The main sensing devices used for positioning and mapping in this experimental platform include a Hokuyo single-line lidar, a Hesai 40-line lidar, and an Xsens IMU. The Xsens IMU serves as the sensing device for geomagnetic sequence signals, acquiring spatial three-axis geomagnetic signals via a laptop computer running a ROS (Robot Operating System). The industrial control computer of the experimental platform uses an NVIDIA Jetson TX2; the laptop only serves as the terminal for user interaction, sending and receiving commands to the industrial control computer via Wi-Fi. The lidar and the geomagnetic signal-acquiring IMU are connected together to the Jetson TX2 for mapping and positioning. This experimental platform not only meets the verification requirements of using spatial geomagnetic sequences as positioning matching signals but also serves as the main experimental platform for the closed-loop detection method of laser SLAM based on geomagnetic signals.
Figure 6. Overall block diagram of the geomagnetic and laser fusion SLAM experimental platform
4.2 Comparison Experiment of Positioning and Mapping Results
To compare the advantages and disadvantages of Google's Cartographer laser SLAM algorithm [5] and the geomagnetic and laser fusion SLAM algorithm in this paper in terms of mapping and localization effects, a scene that causes a lot of interference to the lidar was selected, as shown in Figure 1(b) (2nd floor of Building D, Shenzhen Institutes of Advanced Technology, Chinese Academy of Sciences). The experimental scene has a large area of floor-to-ceiling windows, and a comparative experiment was conducted. Figure 7 is a plan view of the experimental scene.
Figure 7. Layout plan of the experimental site
The steps of the comparative experiment of localization and mapping algorithms are as follows: (1) The AGV experimental platform is used for the control experiment, and the experimental irrelevant variables are controlled to carry out the experiment (in the same scene, time period, and control the experimental vehicle to run on roughly the same trajectory); (2) Start the experimental vehicle and run the Cartographer SLAM algorithm at the same time, and observe the localization trajectory and the grid map established by the SLAM algorithm in real time; (3) Let the experimental vehicle run a complete cycle, and save the running trajectory and the grid map established after the run is completed; (4) Run the geomagnetic and laser fusion SLAM algorithm proposed in this paper, and repeat steps (2) and (3).
4.3 Experimental Results and Analysis
4.3.1 Comparison Experiment of Positioning and Mapping Effects
Figure 8(a) shows the grid map and calculated trajectory of the AGV equipped with LiDAR after running for one week in an indoor scene. The results show that the largest discrepancy between the actual trajectory and the actual trajectory is the initial erroneous localization trajectory, caused by structural similarity in the scene. This is because the LiDAR observations show significant similarity regardless of whether the vehicle is traveling from left to right or right to left. Only after passing through this environment with high structural similarity does the Cartographer's localization match the actual situation. The trajectory localized by the algorithm in the same interference environment, as shown in Figure 8(b), matches the actual movement of the AGV.
In a comparative experiment of localization and mapping performance, the traditional SLAM algorithm often fails to map and locate correctly when dealing with glass interference in the environment, as shown in Figure 8(c). This is because the lidar's ability to describe the scene is insufficient, the inter-frame matching features are not obvious enough, and the closed-loop detection step is ineffective in dealing with the aforementioned interference. Because smooth mirror materials such as glass transmit and reflect laser beams, this algorithm easily constructs traversable areas that do not exist in reality (areas separated by glass materials), which will lead to collisions for navigation tasks using this map. To address these issues, based on the differences and stability of geomagnetic signals in spatial distribution, the geomagnetic and laser fusion solution proposed in this paper achieves better mapping and localization performance when dealing with the aforementioned interference, as shown in Figures 8(c) and 8(d), where the circled areas correspond to the glass interference areas in Figure 1(b).
Figure 8. Comparison experiment of localization and mapping
4.3.2 Algorithm Execution Efficiency Experiment and Result Analysis
To demonstrate the improvement in laser point cloud matching speed achieved by the closed-loop detection algorithm integrating geomagnetic signals and lidar, 100 frames of laser data were used for comparison in the experiment. The proposed algorithm, the brute-force matching search algorithm, and Cartographer's branch-and-bound accelerated closed-loop detection algorithm were also compared. To ensure fairness, 100 frames of laser point clouds were randomly selected from the laser and geomagnetic data collected during one cycle of AGV operation, and the running time of the three closed-loop detection algorithms after 100 matches was tested. Figure 9 shows that, with the same number of matches, the brute-force matching algorithm takes approximately 13 s/match; the Cartographer algorithm takes approximately 6.7 s/match; while the geomagnetic matching detection algorithm takes only 0.9 s/match. This demonstrates that geomagnetic matching can reduce the generated candidate set. In the 100-frame matching task, the proposed algorithm's matching speed is 10% faster than Cartographer's branch-and-bound accelerated matching algorithm.
Figure 9. Comparison of time consumption of three loop closure detection algorithms
4.3.3 Algorithm Stability Experiment and Result Analysis
The experimental data consisted of a ring of laser point clouds and geomagnetic sequence signals collected from the scene shown in Figure 7. Let N be the total number of laser point cloud frames to be matched in the experimental data, M be the score threshold for loop closure detection, and S be the number of candidate frames after algorithm filtering. Since only one correct loop closure detection result exists in one ring of experimental data, the recall rate of loop closure detection can be defined as...
If the false positive rate is , then the accuracy rate of loop closure detection is . To verify the stability of the algorithm in loop closure detection, experiments were conducted to adjust the score threshold M of the matching algorithm and obtain the false positive rate of loop closure detection under different recall rates. Then, comparative experiments and analyses were performed on the brute-force matching algorithm, the Cartographer branch-and-bound accelerated matching algorithm, and the algorithm in this paper.
The distribution of false positive rate and recall rate for loop closure detection is shown in Figure 10. After the proposed algorithm uses the geomagnetic matching algorithm to filter candidate loop closure detection pose nodes, the false positive rate of the proposed algorithm remains consistently low compared to the other two algorithms because the geomagnetic matching algorithm can quickly filter out many results that could easily lead to false positives. As can be seen from Figure 10, regardless of the score threshold setting for matching the laser point cloud frame with the submap, the false positive rate of the proposed algorithm for loop closure detection is lower than that of the branch-and-bound accelerated matching algorithm and the brute-force matching algorithm at the same recall rate; the false positive rate of loop closure detection is reduced by 23% at a recall rate of 0.8, significantly improving the stability of the loop closure detection algorithm.
Figure 10 Comparison of false detection rate and recall rate of three loop closure detection algorithms
5. Summary and Outlook
This paper focuses on the false detection phenomenon in the loop closure detection stage of laser SLAM, and the loop closure false detection and mapping distortion caused by interference in the localization and mapping environment. First, after investigating relevant research on laser SLAM loop closure detection, the paper combines geomagnetic matching in geomagnetic navigation technology with the loop closure detection stage of laser SLAM. This is achieved by analyzing the necessary IMU data in the laser SLAM algorithm...
This paper extracts geomagnetic signals from sensing devices as the basis for loop closure judgment, effectively solving the problems of slow loop closure detection matching speed and false loop closure detection in mapping and localization technologies. Then, based on the proposed loop closure detection algorithm that fuses geomagnetic sequence search and laser point cloud matching, and the design of a geomagnetic and laser fusion SLAM experimental platform, the traditional SLAM algorithm is compared with the proposed algorithm. Finally, the reasons for the failure of traditional SLAM algorithm in localization and mapping are analyzed in detail, and solutions are proposed for the above-mentioned SLAM failure scenarios.
Experimental results show that, compared with the industry benchmark Cartographer algorithm for laser SLAM technology, the proposed algorithm reduces false detections caused by high local similarity; simultaneously, in the localization and mapping environment, it improves the loop closure false detections and mapping distortion caused by laser beam reflection and transmission interference. In terms of stability, compared with the Cartographer algorithm, the proposed algorithm significantly improves both matching speed and loop closure detection stability in laser SLAM.
Traditional filter-based laser SLAM algorithms, such as CoreSLAM, GMAPping, and basal-optimized HectorSLAM, KartoSLAM, LagoSLAM, and Cartographer, cannot guarantee accurate localization and mapping in laser-interference scenarios. However, the algorithm presented in this paper utilizes geomagnetic matching technology to improve the laser loop closure detection process, enabling it to consistently output accurate localization and mapping results in all of the aforementioned interference scenarios. Multiple experimental tests demonstrate that while improving loop closure detection speed by 31%, the localization accuracy of the proposed algorithm remains comparable to the Cartographer algorithm in laser-free scenarios, thus improving the efficiency of localization and mapping. Furthermore, in laser-interference scenarios, the localization accuracy and mapping performance of the proposed algorithm outperform all the aforementioned laser SLAM algorithms.
The complex outdoor environment presents greater challenges to precise positioning and mapping for autonomous driving. Furthermore, outdoor conditions introduce more interference to LiDAR sensing and IMU geomagnetic signal measurements. Our future research direction is to explore how to extend geomagnetic and LiDAR fusion SLAM technology to outdoor driving environments while ensuring positioning accuracy and mapping quality.
