A method for simultaneous localization and mapping of multiple robots in a wide range of environments

Abstract

The application relates to a multi-robot cooperative simultaneous localization and mapping method in a wide range environment, belonging to the field of simultaneous localization and mapping, comprising initial pose calibration of a plurality of single-robot front ends, and generation of a key pose graph and a key frame point cloud under a wide range environment after initial pose calibration of each single-robot front end; loop detection is performed based on the key pose graph to find a loop candidate pair, according to a priority of the calculated loop candidate pair, two key frame point clouds corresponding to two nodes of the loop candidate pair are used, two-stage coarse-to-fine loop confirmation is used, and an accurate relative pose transformation between the two nodes is obtained as a loop detection result; and based on the key pose graph, the key frame point cloud and the loop detection result, key pose graph aggregation and optimization are performed to obtain a multi-robot trajectory and a global point cloud map. The problems of poor fault tolerance of single-robot mapping precision limitation and improvement of multi-robot SLAM robustness fault tolerance are solved.

Description

Technical Field

[0001] This invention relates to the field of simultaneous localization and mapping (SLAM) technology, specifically to a multi-robot collaborative SLAM method for large-scale environments. Background Technology

[0002] Robot SLAM technology estimates its own motion and builds incremental maps by acquiring pose and map information, enabling automatic robot localization and environmental perception. It has become a key component in addressing the bottlenecks in intelligent robot technology. LiDAR can acquire precise 3D point clouds, is less affected by lighting conditions, and has strong data anti-interference capabilities, making it more suitable for outdoor scenarios. It is an important sensor used in SLAM technology. LiDAR SLAM is widely used in many fields such as robot navigation, autonomous driving, virtual reality, special operations, industrial production, and smart homes.

[0003] However, with the continuous development of robotics technology and increasingly complex task requirements, it has become extremely difficult for a single robot to complete complex tasks in a large-scale outdoor environment. When faced with the requirements of large-scale environmental mapping, the accuracy of single-robot laser SLAM is affected by the limited processing power of its controller and the continuous accumulation of global errors. It may even fail to map due to excessive computational load. Furthermore, in certain specific scenarios, a single robot may experience unexpected malfunctions and be unable to continue the mapping task.

[0004] Limitations of Single-Robot Laser SLAM: A single robot faces significant challenges in completing complex tasks in large-scale outdoor environments. When dealing with large-scale environmental mapping requirements, the accuracy of map construction by a single robot is affected by the limited processing power of its controller and the continuous accumulation of global errors. In some cases, excessive computation can even lead to mapping failure. Furthermore, in certain specific scenarios, a single robot may experience unexpected malfunctions, rendering it unable to continue the mapping task.

[0005] To address the problems inherent in single-robot systems, some research has begun to focus on multi-robot laser SLAM systems. During the mapping process, multiple robots can simultaneously explore the environment from different locations, acquiring more environmental information compared to a single robot. However, current research on multi-robot laser SLAM is still in the exploratory stage. Mature and effective methods for handling information interaction, coordination, and parallel problem-solving among multiple robots have not yet emerged; the anti-interference and fault tolerance of multi-robot laser SLAM also need improvement.

[0006] The collaborative issues of multi-robot laser SLAM: Multi-robot SLAM involves a large amount of data, and the simultaneous scanning and matching, pose graph construction, loop closure detection, and pose graph optimization of multiple ends place extremely high demands on computing power. A reasonable system architecture is needed to allocate various tasks. The robustness and fault tolerance of multi-robot SLAM need to be improved. The pose graphs from multiple robots will form a large number of loop closure candidate pairs, and it is necessary to avoid the impact of erroneous loops on the system.

[0007] In summary, there is an urgent need for a method for simultaneous localization and mapping of multiple robots in a large-scale environment. Summary of the Invention

[0008] Based on the above analysis, this invention proposes a multi-robot collaborative simultaneous localization and mapping method for large-scale environments. This addresses the technical problems in existing technologies, such as limited mapping accuracy and poor fault tolerance of single robots, as well as the need to improve the robustness and fault tolerance of multi-robot SLAM.

[0009] To achieve the above objectives, the present invention provides a method for simultaneous localization and mapping of multiple robots in a large-scale environment, comprising the following steps:

[0010] Multiple single robot front ends perform initial pose calibration. After the initial pose calibration, each single robot front end will generate key pose maps and key frame point clouds in a wide range of environments.

[0011] Based on the key pose map, loop closure detection is performed to find loop closure candidate pairs. According to the priority of the calculated loop closure candidate pairs, the two keyframe point clouds corresponding to the two nodes of the loop closure candidate pairs are used to perform loop closure confirmation in a two-stage coarse-to-fine manner to obtain the precise relative pose transformation between the two nodes, which is used as the loop closure detection result.

[0012] Based on the key pose graph, the key frame point cloud, and the loop closure detection results, the key pose graph is aggregated and optimized to obtain multi-robot trajectories and a global point cloud map.

[0013] Furthermore, the multiple single robot front ends perform initial pose calibration, including:

[0014] Each individual robot uses its onboard LiDAR to scan the surrounding environment and obtain point cloud data;

[0015] Based on the point cloud data obtained by the LiDAR scanning of each single robot, each single robot front end executes the point cloud registration ICP algorithm to calculate the relative pose transformation between the point cloud data scanned by the LiDAR of each robot, and obtains the initial relative pose between the coordinate systems of each robot.

[0016] Select one single robot front end from multiple single robot front ends as robot number one, define the initial time coordinate system of robot number one as the world coordinate system, and calculate the absolute pose of each of the other single robot front ends in the world coordinate system at the initial time based on the initial relative pose of robot number one with other robots.

[0017] Furthermore, the generation of key pose maps and key frame point clouds in a large-scale environment includes:

[0018] Each single robot front end preprocesses the point cloud data, uses the uniform motion assumption to remove motion distortion from the point cloud data, and applies the adaptive voxelization filter in the PCL point cloud library to perform voxel downsampling to remove noise, thus obtaining a preprocessed point cloud frame.

[0019] Each single robot front end operates an onboard radar odometry system. The radar odometry system uses point cloud frame-local map matching to estimate the pose of the single robot front end and construct the current local pose map of the single robot front end.

[0020] The preprocessed LiDAR point cloud frame is used as input, and the point cloud frame is matched with the current local pose map using the GICP algorithm to output the pose of the single robot front end at the point cloud frame scanning time.

[0021] Then, the pose is added to the current local pose graph to incrementally construct the local pose graph.

[0022] The single robot front end selects key pose nodes from the local pose graph to construct a key pose graph.

[0023] Furthermore, each node in the key pose graph corresponds to a pose estimated by the radar odometry;

[0024] The edge connecting two nodes represents the relative pose transformation between these two nodes;

[0025] The pose of the single robot front end after exceeding the motion threshold is added as a new key pose node;

[0026] The point cloud frame corresponding to each key pose node is selected as the key frame point cloud.

[0027] Further, the step of performing loop closure detection based on the key pose graph to find loop closure candidate pairs includes:

[0028] The key pose nodes in the key pose graphs received from the front end of each single robot are taken as the current node, and the Euclidean distance between the current node and other nodes is calculated.

[0029] The Euclidean distance from the current node is less than the threshold d. maxNode pairs consisting of nodes are considered as loopback candidate pairs;

[0030] Among them, the other nodes are all the historical key pose nodes in the key pose graph generated by each single robot front end.

[0031] Further, calculating the priority of the candidate loopback pairs includes:

[0032] Calculate the degradation factor of the two nodes in the candidate loop closure pair;

[0033] Calculate the degradation factor of one of the nodes a. as follows:

[0034]

[0035] Where, λ minA =min(eig(A) T A)), λ minA For A T The smallest eigenvalue of A, where A is the information matrix of node a;

[0036] Calculate the degradation factor D of another node b. B ,as follows:

[0037]

[0038] Where, λ minB =min(eig(B) T B)), λ minB For B T The smallest eigenvalue of B, where B is the information matrix of node b;

[0039] right and D B Find the minimum value as the degeneration factor D for candidate loop pairs, as follows:

[0040]

[0041] Candidate loop-loop candidates are sorted according to the magnitude of their degradation factors, and the loop-loop candidates with larger degradation factors are given higher priority.

[0042] The candidate loop closure pairs are added to the calculation queue in descending order of priority.

[0043] Furthermore, the step of using the two keyframe point clouds corresponding to the two nodes of the loop closure candidate pair to perform loop closure confirmation in a two-stage coarse-to-fine manner to obtain the precise relative pose transformation between the two nodes includes:

[0044] Based on the two keyframe point clouds of the loop closure candidate pair in the computation queue, the TEASER++ algorithm is used to extract local feature descriptors, match the features of the two keyframe point clouds, establish the correspondence between the two keyframe point clouds, and estimate the relative pose transformation between the two keyframe point clouds through the minimum error function.

[0045] Based on the relative pose transformation between the two keyframe point clouds after a rough estimate by the TEASER++ algorithm, the GICP algorithm is used to minimize the distance between the two keyframe point clouds through iteration. The GICP algorithm iteratively optimizes the process to find the optimal relative pose transformation between the two keyframe point clouds, thus obtaining the accurate relative pose transformation of the loop closure candidate pair.

[0046] Furthermore, the step of performing key pose graph aggregation and optimization to obtain multi-robot trajectories and a global point cloud map includes:

[0047] Obtain either two key pose maps or one key pose map corresponding to each loop closure candidate pair;

[0048] For each loop candidate pair, connect the two key pose graphs or the two candidate nodes in a key pose graph with an edge, where the edge represents the precise relative pose transformation of the candidate pair, thereby aggregating multiple key pose graphs to obtain a complete pose graph.

[0049] The GNC algorithm is used to optimize the complete pose graph. By iteratively adjusting the position of each pose node in the complete pose graph, noise and outliers are removed, resulting in multi-robot trajectories and a global point cloud map.

[0050] Furthermore, the Euclidean distance threshold d max It is 2-5 meters or 30°-40°.

[0051] Furthermore, each single robot's front end is located on its respective robot platform;

[0052] Each single robot is equipped with a lidar and a laser odometer at its front end;

[0053] The lidar at the front end of each robot uses different specifications.

[0054] Compared with the prior art, the present invention can achieve at least one of the following beneficial effects:

[0055] 1. Advantages of low computing power requirements and memory usage: By independently running a laser odometry at the front end of a single robot, sparse keyframe point clouds and pose nodes are selected according to certain distance and angle thresholds and a sparse pose graph is constructed, which reduces the computing load and memory usage at the base station.

[0056] 2. Advantages in robustness and fault tolerance: Based on the richness of environmental features, candidate loop closure pairs in the pose graph are screened and prioritized, eliminating candidate loop closure pairs with high degradation and prioritizing the computation of candidate loop closure pairs with better observability; robust pose graph optimization is performed through the progressive non-convex (GNC) algorithm, further enhancing the robustness and fault tolerance of the system.

[0057] 3. Provides more stable and efficient positioning and mapping in a wide range of environments.

[0058] In summary, this invention aims to solve the problems of high computing power and memory requirements and robustness and fault tolerance in the prior art, thereby providing a more efficient and reliable method for simultaneous localization and mapping of multiple robots in a wide range of environments.

[0059] In this invention, the above-described technical solutions can be combined with each other to achieve more preferred combinations. Other features and advantages of this invention will be set forth in the following description, and some advantages may become apparent from the description or be learned by practicing the invention. The objects and other advantages of this invention can be realized and obtained from what is particularly pointed out in the description and drawings. Attached Figure Description

[0060] The accompanying drawings are for illustrative purposes only and are not intended to limit the invention. Throughout the drawings, the same reference numerals denote the same parts.

[0061] Figure 1 This is a schematic diagram of a multi-robot collaborative simultaneous localization and mapping method in a large-scale environment.

[0062] Figure 2 Flowchart for obtaining key pose maps and keyframe point clouds for the front end of multiple robots;

[0063] Figure 3 Flowchart for performing loop closure detection;

[0064] Figure 4 A flowchart for obtaining a global point cloud map by aggregating and optimizing key pose graphs. Detailed Implementation

[0065] Preferred embodiments of the present invention will now be described in detail with reference to the accompanying drawings, which form part of this application and are used together with the embodiments of the present invention to illustrate the principles of the present invention, but are not intended to limit the scope of the present invention.

[0066] This invention proposes a method for simultaneous localization and mapping of multiple robots in a large-scale environment, such as... Figure 1 As shown, it includes the following steps:

[0067] Step S1: Multiple single robot front ends perform initial pose calibration. After the initial pose calibration, each single robot front end will generate key pose maps and key frame point clouds in a large-scale environment.

[0068] Step S2: Based on the key pose map, perform loop closure detection to find loop closure candidate pairs. According to the priority of the calculated loop closure candidate pairs, use the two keyframe point clouds corresponding to the two nodes of the loop closure candidate pairs to perform loop closure confirmation in a two-stage coarse-to-fine manner to obtain the precise relative pose transformation between the two nodes, which is used as the loop closure detection result.

[0069] Step S3: Based on the key pose graph, the key frame point cloud, and the loop closure detection results, perform key pose graph aggregation and optimization to obtain multi-robot trajectories and a global point cloud map.

[0070] Step S1, as follows Figure 2 As shown, steps S11-S12 are included, specifically.

[0071] Step S11, multiple single robot front ends perform initial pose calibration, including:

[0072] Each individual robot uses its onboard LiDAR to scan the surrounding environment and obtain point cloud data;

[0073] Based on the point cloud data obtained from different LiDAR scans, each single robot front end executes the point cloud registration ICP algorithm to calculate the relative pose transformation between the LiDAR scan point cloud data of each robot, and obtains the initial relative pose between the coordinate systems of each robot.

[0074] Select one single robot front end from multiple single robot front ends as robot number one, define the initial time coordinate system of robot number one as the world coordinate system, and calculate the absolute pose of each of the other single robot front ends in the world coordinate system at the initial time based on the initial relative pose of robot number one.

[0075] For example, a single robot has three front-end units.

[0076] Multiple single-robot front-ends use their respective onboard LiDAR to scan the surrounding environment, acquiring point cloud and point timestamp data.

[0077] Point cloud data reflects the environmental information surrounding the location of a single robot's front end.

[0078] The relative pose transformation between point clouds obtained from different LiDAR scans is calculated using a point cloud registration algorithm to obtain the initial pose of the front end of each single robot.

[0079] For point cloud data obtained from different LiDAR scans, each pair of single robot front ends executes the ICP (Iterative Closet Point) algorithm to calculate the relative pose transformation between point cloud data.

[0080] Determination of initial relative pose: The ICP algorithm is used to calculate the relative pose transformation between the point cloud data scanned by the LiDAR at the front end of each single robot. After the ICP algorithm is iterated, the initial relative pose of each single robot front end relative to the other single robot front ends is obtained.

[0081] Among multiple single robot front-ends, one is randomly selected as robot number one, and its initial coordinate system is defined as the world coordinate system. The poses of the other single robot front-ends are described relative to this world coordinate system.

[0082] Absolute pose calculation: Based on the initial relative pose, the absolute pose of each single robot front end in the world coordinate system at the initial moment is calculated, and the initial pose calibration of multiple single robot front ends is completed.

[0083] Initial pose calibration ensures that the initial pose of each single robot front end is calibrated in the same coordinate system, thus ensuring that multiple single robot front ends work collaboratively in the same coordinate system.

[0084] In step S12, after the initial pose calibration, each single robot front end will generate a key pose map and key frame point cloud in a wide range of environments.

[0085] After initial calibration, each single robot front end will generate key pose maps and key frame point clouds in a wide range of environments.

[0086] The generation of key pose maps and key frame point clouds in a large-scale environment includes:

[0087] Each single robot front end preprocesses the point cloud data, uses the uniform motion assumption to remove motion distortion from the point cloud data, and applies the adaptive voxelization filter in the PCL point cloud library to perform voxel downsampling to remove noise, thus obtaining a preprocessed point cloud frame.

[0088] Each single robot front end operates an onboard radar odometry system. The radar odometry system uses point cloud frame-local map matching to estimate the pose of the single robot front end and construct the current local pose map of the single robot front end.

[0089] The preprocessed LiDAR point cloud frame is used as input, and the point cloud frame is matched with the current local pose map using the GICP (Generalized Iterative Closest Point) algorithm to output the pose of the single robot front end at the point cloud frame scanning time.

[0090] Then, the pose is added to the current local pose graph to incrementally construct the local pose graph.

[0091] The single robot front end selects key pose nodes from the local pose graph to construct a key pose graph.

[0092] The key pose map and key frame point cloud are generated by a single robot front end located on the robot platform that is flexibly adapted to different specifications of LiDAR.

[0093] Each single robot's front end is located on its respective robot platform;

[0094] Each single robot is equipped with a lidar and a laser odometer at its front end;

[0095] The lidar at the front end of each robot uses different specifications.

[0096] The ratio of the robot platform to the single robot front end is 1:1.

[0097] The front end of a single robot performs preprocessing of the LiDAR point cloud, removes motion distortion, and performs voxel downsampling to denoise, resulting in a preprocessed point cloud frame.

[0098] It takes a certain amount of time, such as 0.1 seconds, for a LiDAR to scan each frame of point cloud data. During this time, the LiDAR also moves with the front end of the single robot. Therefore, the position coordinate reference of each point in the original point cloud generated by the LiDAR is different, resulting in motion distortion.

[0099] The distortion of lidar point cloud data is corrected using the assumption of uniform motion.

[0100] Since the radar scan of one frame takes only 1.0 second, which is very short, the process of the lidar completing a frame of point cloud data scanning can be regarded as uniform motion. Based on the timestamp of the points in the lidar point cloud data, the relative pose of each point at the end of the scan is calculated, and the point cloud data is uniformly converted to the lidar coordinate system at the end of the frame scan, thereby eliminating motion distortion.

[0101] The point cloud is downsampled using an adaptive voxelization filter from the PCL (Point Cloud Library) to remove noise and ensure that point clouds from different LiDAR configurations are similar in size and density.

[0102] Preprocessing the lidar point cloud reduces the computational load and memory consumption of subsequent loop closure detection.

[0103] Each node in the key pose graph corresponds to a pose estimated by the radar odometry.

[0104] The edge connecting two nodes represents the relative pose transformation between these two nodes;

[0105] The pose of the single robot front end after exceeding the motion threshold is added as a new key pose node;

[0106] The point cloud frame corresponding to each key pose node is selected as the key frame point cloud.

[0107] To avoid introducing too many poses and increasing the computational load, preferably, the poses of the single robot front end that exceed the motion threshold are added as new key pose nodes.

[0108] To reduce computational and memory overhead, this invention selects key frames and key pose nodes to construct a sparse key pose graph.

[0109] The selection method for key pose nodes and key frames is as follows:

[0110] Set motion threshold: Add new key pose nodes only after the front end of a single robot exceeds a certain motion threshold.

[0111] The motion threshold includes translation and / or rotation.

[0112] Example motion threshold settings: a translation of 2m and a rotation of 30° at the front end of a single robot.

[0113] Keyframe selection: The LiDAR point cloud corresponding to each key pose node is selected as the keyframe point cloud.

[0114] Relative pose calculation: Calculate the relative pose of adjacent key pose nodes in the key pose graph based on the pose of the key pose nodes, and construct the edges of the key pose graph.

[0115] Key pose graphs and keyframe point clouds are used for subsequent loop closure detection, pose graph optimization, and global point cloud map generation.

[0116] When the front end of a single robot moves out of the communication range, the key pose map and key frame point cloud wait in the queue and are sent in batches when communication is re-established.

[0117] The purpose of this step is to preprocess the LiDAR point cloud acquired by the single robot front end, estimate the pose of the single robot front end, and select key frames and key pose nodes to construct a key pose map.

[0118] Step S2, as follows Figure 3 As shown, steps S21-S23 are included, specifically.

[0119] Based on the key pose maps and keyframe point clouds of each single robot's front end, loop closure detection is performed on multiple robots.

[0120] Loop closure detection includes loop closure detection within a single robot front end and loop closure detection between single robot front ends.

[0121] Loop closure detection can determine whether a single robot's front end has returned to a previously visited historical position, which is key to reducing the cumulative error of SLAM trajectory.

[0122] In addition, loop closure detection can detect overlapping portions between local pose maps created by individual robot front-ends in a team, in order to aggregate them into a geometrically consistent global map.

[0123] Step S21, performing loop closure detection based on the key pose graph to find candidate loop closure pairs, including:

[0124] The step of performing loop closure detection based on the key pose graph to find loop closure candidate pairs includes:

[0125] The key pose nodes in the key pose graphs received from the front end of each single robot are taken as the current node, and the Euclidean distance between the current node and other nodes is calculated.

[0126] The Euclidean distance from the current node is less than the threshold d. max Node pairs consisting of nodes are considered as loopback candidate pairs;

[0127] Among them, the other nodes are all the historical key pose nodes in the key pose graph generated by each single robot front end.

[0128] (1) Calculate Euclidean distance: For the current node, calculate the Euclidean distance to other nodes;

[0129] (2) Generate candidate node pairs: select nodes whose Euclidean distance to the current node is less than the threshold d. max Node pairs consisting of nodes are used as candidate pairs for loop matching.

[0130] (3) Adaptive adjustment of Euclidean distance threshold: Euclidean distance threshold d max The value is determined using an adaptive method.

[0131] Determine d based on specific needs and environment. max The threshold is dynamically adjusted according to changes in the environment to adapt to loop closure detection under different conditions.

[0132] The Euclidean distance threshold d max It is 2-5 meters or 30°-40°.

[0133] A qualifying node pair is a combination of nodes that satisfy the Euclidean distance threshold with respect to the current node. These node pairs are considered candidate loop closure pairs.

[0134] Step S22: Based on the calculated priority of the loop closure candidate pair, the two keyframe point clouds corresponding to the two nodes of the loop closure candidate pair are used to perform loop closure confirmation in a two-stage coarse-to-fine manner to obtain the precise relative pose transformation of the two. Specifically.

[0135] The calculation of the priority of the loopback candidate pairs includes:

[0136] Calculate the degradation factor of the two nodes in the candidate loop closure pair;

[0137] Calculate the degradation factor of one of the nodes a. as follows:

[0138]

[0139] Where, λ minA =min(eig(A) T A)), λ minA For A T The smallest eigenvalue of A, where A is the information matrix of node a; the eig function is the eigenvalue calculation function.

[0140] Calculate the degradation factor D of another node b. B ,as follows:

[0141]

[0142] Where, λ minB =min(eig(B) T B)), λ minB For B T The smallest eigenvalue of B, where B is the information matrix of node b;

[0143] right and D B Find the minimum value as the degeneration factor D for candidate loop pairs, as follows:

[0144]

[0145] Candidate loop-loop candidates are sorted according to the magnitude of their degradation factors, and the loop-loop candidates with larger degradation factors are given higher priority.

[0146] The candidate loop closure pairs are added to the calculation queue in descending order of priority.

[0147] λ minA , λ minB The larger the value of λ, the lower the degree of node degradation and the richer the scene features; minA , λ minBThe smaller the value, the higher the degree of degradation of the node and the more blurred the scene features. The richer the scene features of the loop closure node, the easier it is to generate accurate loop closure pose estimation.

[0148] Priority sorting: The candidate loop candidates are sorted according to the magnitude of the degradation factor D of the loop candidate pairs, and the loop candidate pairs with larger degradation factors are given higher priority.

[0149] Finally, the candidate loopback pairs are added to the calculation queue in descending order of priority.

[0150] Step S23: Using the two keyframe point clouds corresponding to the two nodes of the loop closure candidate pair, a two-stage coarse-to-fine loop closure confirmation is performed to obtain the precise relative pose transformation between the two, including:

[0151] The step of using the two keyframe point clouds corresponding to the two nodes of the loop closure candidate pair, and performing a two-stage coarse-to-fine loop closure confirmation to obtain the precise relative pose transformation between the two nodes includes:

[0152] Based on the two keyframe point clouds of the loop closure candidate pair in the computation queue, the TEASER++ algorithm is used to extract local feature descriptors, match the features of the two keyframe point clouds, establish the correspondence between the two keyframe point clouds, and estimate the relative pose transformation between the two keyframe point clouds through the minimum error function.

[0153] Based on the relative pose transformation between the two keyframe point clouds after a rough estimate by the TEASER++ algorithm, the GICP algorithm is used to minimize the distance between the two keyframe point clouds through iteration. The GICP algorithm iteratively optimizes the process to find the optimal relative pose transformation between the two keyframe point clouds, thus obtaining the accurate relative pose transformation of the loop closure candidate pair.

[0154] Specifically, loop candidate pairs in the computation queue are computed in descending order of their priority.

[0155] The calculation steps for each candidate loop closure pair are as follows:

[0156] (1) TEASER++ rough estimate.

[0157] The TEASER++ algorithm is used to find a rough estimate of the relative pose transformation between two frames of point clouds for loop closure candidate pairs.

[0158] (2) Use the GICP (Generalized Iterative Closet Point) algorithm for precise matching.

[0159] The point clouds of the two keyframes of the loop closure candidate pairs, which were roughly estimated by the TEASER++ algorithm, were then finely registered using the GICP algorithm.

[0160] The GICP algorithm uses the coarse estimation result (rough relative pose transformation) obtained from TEASER++ as the initial value. Based on the point clouds of two keyframes of the loop closure candidate pair, the initial value is optimized to obtain the accurate relative pose transformation, including the following steps:

[0161] Point cloud registration: The GICP algorithm is used, which iteratively minimizes the distance between point clouds in two keyframes to find the optimal relative pose transformation between them. The GICP iterative process progressively optimizes the relative pose transformation relationship;

[0162] Parameter tuning: Adjust the parameters of the GICP algorithm, such as the maximum number of iterations and the convergence threshold, to obtain better matching results;

[0163] Precise relative pose: The GICP algorithm outputs the precise relative pose transformation between the point clouds of two keyframes for the loop closure candidate pair, thus obtaining the precise relative pose transformation of the loop closure candidate pair.

[0164] The precise relative pose transformation of loop closure candidate pairs is used as the loop closure detection result for subsequent joint pose graph optimization and global point cloud map generation.

[0165] The goal of this step is to perform loop closure detection. This involves calculating the relative pose transformation to determine if loops exist, and then using the loop closure detection results for joint pose graph optimization and global point cloud map generation. The entire process includes generating candidate node pairs for loop closure detection, priority calculation, and a two-stage, coarse-to-fine loop closure confirmation.

[0166] Step S3, as follows Figure 4 As shown, steps S31-S33 are included, specifically.

[0167] Based on the key pose graph, keyframes, and loop closure detection results, joint pose graph optimization is performed to obtain a globally consistent and drift-free global point cloud map.

[0168] The process of aggregating and optimizing key pose graphs to obtain multi-robot trajectories and a global point cloud map includes:

[0169] Step S31: Obtain two key pose maps or one key pose map corresponding to each loop closure candidate pair;

[0170] Step S32: For each loop candidate pair, connect the two key pose graphs or the two candidate nodes in a key pose graph with an edge, where the edge represents the precise relative pose transformation of the candidate pair, thereby aggregating multiple key pose graphs to obtain a complete pose graph.

[0171] Step S33: Optimize the complete pose graph using the GNC algorithm. By iteratively adjusting the position of each pose node in the complete pose graph, noise and outliers are removed to obtain the multi-robot trajectory and global point cloud map.

[0172] Step S31,

[0173] If a loop closure candidate pair is in a key pose graph of a single robot front end, a corresponding key pose graph is obtained.

[0174] If the loop closure candidate pair is in the key pose maps of two single robot front ends, then two key pose maps are obtained.

[0175] Step S32,

[0176] Based on the loop closure detection results, the key pose graphs sent by the front end of each single robot are connected together, and the edges corresponding to the loop closure candidate pairs are added to the key pose graphs to form the complete pose graph of the multi-robot.

[0177] (1) Adding loop closure edges: Add the edges corresponding to the loop closure candidate pairs in the loop closure detection results to the key pose graph of each single robot front end;

[0178] (2) Pose graph connection: Connect two nodes in the same single robot front end key pose graph, or connect two nodes in the two key pose graphs of two single robots front ends to realize the aggregation of key pose graphs of multiple robots and obtain a complete pose graph.

[0179] Step S33,

[0180] The complete pose graph is optimized by performing a progressive non-convex GNC (Guidance Navigation and Control) algorithm.

[0181] Robust pose graph optimization is performed using an incremental non-convex GNC algorithm to obtain optimized multi-robot trajectories.

[0182] (1) GNC algorithm selection: The progressive non-convex GNC algorithm is adopted. This algorithm is a robust optimization algorithm for nonlinear optimization.

[0183] (2) Application of GTSAM library: Use the GTSAM library (GeorgiaTech Smoothing and Mapping, a C++ library based on factor graphs) to implement complete pose graph optimization;

[0184] (3) Optimization objective: To obtain globally consistent and drift-free multi-robot trajectories through the optimized complete pose graph;

[0185] (4) Iterative optimization: The GNC algorithm is an iterative optimization process. It adjusts the position of each pose node through multiple iterations to minimize the error.

[0186] (5) Robustness: The GNC algorithm is robust, handles noise and outliers, and ensures the robustness of the optimization results.

[0187] Both pose graph optimization and GNC algorithm are implemented using the GTSAM library.

[0188] For example, when performing full pose graph optimization using the GNC algorithm:

[0189] For example, in a complete pose graph optimization, there are three points A, B, and C in sequence. The relative pose transformation from A to B is 1 meter, and the relative pose transformation from B to C is 0.8 meters. The optimal relative pose transformation between A and C is not 1 plus 0.8 equals 1.8 meters. A, B, and C may form a small triangle, with AB being 1 meter and BC being 0.8 meters. The optimal relative pose transformation between A and C may satisfy AC < AB + BC, for example, 1.6 meters.

[0190] In SLAM, for example, the position between two nodes measured by laser odometry may differ from the position between two nodes detected by loop closure because both measurements are noisy. By using the GNC algorithm to optimize and average the measurements, noise and outliers can be removed, resulting in a more realistic result.

[0191] This invention addresses the technical problems of limited mapping accuracy and poor fault tolerance in single-robot SLAM, as well as the need to improve the robustness and fault tolerance of multi-robot SLAM, and brings the following effects and advantages:

[0192] 1. Advantages of low computing power requirements and memory usage: By independently running a laser odometry at the front end of a single robot, sparse keyframe point clouds and pose nodes are selected according to certain distance and angle thresholds and a sparse pose graph is constructed, which reduces the computing load and memory usage at the base station.

[0193] 2. Advantages in robustness and fault tolerance: Based on the richness of environmental features, candidate loop closure pairs in the pose graph are screened and prioritized, eliminating candidate loop closure pairs with high degradation and prioritizing the computation of candidate loop closure pairs with better observability; robust pose graph optimization is performed through the progressive non-convex (GNC) algorithm, further enhancing robustness and fault tolerance.

[0194] 3. Provides more stable and efficient positioning and mapping in a wide range of environments.

[0195] In summary, this invention aims to solve the problems of high computing power and memory requirements, as well as robustness and fault tolerance in the prior art, thereby providing a more efficient and reliable multi-robot collaborative laser SLAM method in large-scale environments.

[0196] Those skilled in the art will understand that all or part of the processes of the methods described in the above embodiments can be implemented by a computer program instructing related hardware, and the program can be stored in a computer-readable storage medium. The computer-readable storage medium may be a disk, optical disk, read-only memory, or random access memory, etc.

[0197] The above description is only a preferred embodiment of the present invention, but the scope of protection of the present invention is not limited thereto. Any changes or substitutions that can be easily conceived by those skilled in the art within the scope of the technology disclosed in the present invention should be included within the scope of protection of the present invention.

Claims

1. A method for simultaneous localization and mapping of multiple robots in a large-scale environment, characterized in that, Includes the following steps: Multiple single robot front ends perform initial pose calibration. After the initial pose calibration, each single robot front end will generate key pose maps and key frame point clouds in a large-scale environment. Based on the key pose map, loop closure detection is performed to find loop closure candidate pairs. According to the priority of the calculated loop closure candidate pairs, the two keyframe point clouds corresponding to the two nodes of the loop closure candidate pairs are used to perform loop closure confirmation in a two-stage coarse-to-fine manner to obtain the precise relative pose transformation between the two nodes, which is used as the loop closure detection result. Calculating the priority of the candidate loopback pairs includes: Calculate the degradation factor of the two nodes in the candidate loop closure pair; Calculate the degradation factor of one of the nodes a. as follows: in, , for The minimum eigenvalue, where A is the information matrix of node a; Calculate the degradation factor of another node b as follows: in, , for The minimum eigenvalue of node b, where B is the information matrix of node b; Minimum eigenvalue or The larger the value, the lower the degree of degradation of the corresponding node; right and Find the minimum value as the degeneration factor D for candidate loop pairs, as follows: ) The candidate loop pairs are sorted according to the magnitude of their degradation factors, and the candidate loop pairs with larger degradation factors are given higher priority. The loop closure candidate pairs are added to the calculation queue in descending order of priority; The step of using the two keyframe point clouds corresponding to the two nodes of the loop closure candidate pair, and performing a two-stage coarse-to-fine loop closure confirmation to obtain the precise relative pose transformation between the two nodes includes: Based on the two keyframe point clouds of the loop closure candidate pair in the computation queue, the TEASER++ algorithm is used to extract local feature descriptors, match the features of the two keyframe point clouds, establish the correspondence between the two keyframe point clouds, and estimate the relative pose transformation between the two keyframe point clouds through the minimum error function. Based on the relative pose transformation between the two keyframe point clouds after rough estimation by the TEASER++ algorithm, the GICP algorithm is used to minimize the distance between the two keyframe point clouds through iteration. The GICP algorithm iteratively optimizes the process to find the optimal relative pose transformation between the two keyframe point clouds, thus obtaining the accurate relative pose transformation of the loop closure candidate pair. Based on the key pose graph, the key frame point cloud, and the loop closure detection results, key pose graph aggregation and optimization are performed to obtain multi-robot trajectories and a global point cloud map. The process of aggregating and optimizing key pose graphs to obtain multi-robot trajectories and a global point cloud map includes: Obtain either two key pose maps or one key pose map corresponding to each loop closure candidate pair; For each loop candidate pair, connect the two key pose graphs or the two candidate nodes in a key pose graph with an edge, where the edge represents the precise relative pose transformation of the candidate pair, thereby aggregating multiple key pose graphs to obtain a complete pose graph. The complete pose graph is optimized using the GNC algorithm. By iteratively adjusting the position of each pose node in the complete pose graph and removing noise and outliers, a multi-robot trajectory and a global point cloud map are obtained.

2. The method according to claim 1, characterized in that, The multiple single robot front ends perform initial pose calibration, including: Each individual robot uses its onboard LiDAR to scan the surrounding environment and obtain point cloud data; Based on the point cloud data obtained by the LiDAR scanning of each single robot, each single robot front end executes the point cloud registration ICP algorithm to calculate the relative pose transformation between the point cloud data scanned by the LiDAR of each robot, and obtains the initial relative pose between the coordinate systems of each robot. Select one single robot front end from multiple single robot front ends as robot number one, define the initial time coordinate system of robot number one as the world coordinate system, and calculate the absolute pose of each of the other single robot front ends in the world coordinate system at the initial time based on the initial relative pose of robot number one with other robots.

3. The method according to claim 2, characterized in that, The generation of key pose maps and key frame point clouds in a large-scale environment includes: Each single robot front end preprocesses the point cloud data, uses the uniform motion assumption to remove motion distortion from the point cloud data, and applies the adaptive voxelization filter in the PCL point cloud library to perform voxel downsampling to remove noise, thus obtaining a preprocessed point cloud frame. Each single robot front end operates an onboard radar odometry system. The radar odometry system uses point cloud frame-local map matching to estimate the pose of the single robot front end and construct the current local pose map of the single robot front end. The preprocessed LiDAR point cloud frame is used as input, and the point cloud frame is matched with the current local pose map using the GICP algorithm to output the pose of the single robot front end at the point cloud frame scanning time. Then, the pose is added to the current local pose graph to incrementally construct the local pose graph. The single robot front end selects key pose nodes from the local pose graph to construct a key pose graph.

4. The method according to claim 3, characterized in that, Each node in the key pose graph corresponds to a pose estimated by the radar odometry. The edge connecting two nodes represents the relative pose transformation between these two nodes; The pose of the single robot front end after exceeding the motion threshold is added as a new key pose node; The point cloud frame corresponding to each key pose node is selected as the key frame point cloud.

5. The method according to claim 4, characterized in that, The step of performing loop closure detection based on the key pose graph to find loop closure candidate pairs includes: The key pose nodes in the key pose graphs received from the front end of each single robot are taken as the current node, and the Euclidean distance between the current node and other nodes is calculated. The Euclidean distance from the current node is less than the threshold. Node pairs consisting of nodes are considered as loopback candidate pairs; Among them, the other nodes are all the historical key pose nodes in the key pose graph generated by each single robot front end.

6. The method according to claim 5, characterized in that, Euclidean distance threshold It is 2-5 meters.

7. The method according to any one of claims 1-5, characterized in that, Each single robot's front end is located on its respective robot platform; Each single robot is equipped with a lidar and a laser odometer at its front end; The lidar at the front end of each robot uses different specifications.

Images