A multi-robot collaborative mapping system

Abstract

The application relates to a multi-robot cooperative mapping system in the field of simultaneous localization and mapping, which comprises a plurality of mobile platforms and a central control center; each mobile platform is loaded with a laser radar, a laser odometer and a computing host; the laser radar is used for acquiring point cloud data of a surrounding environment; the laser odometer is used for acquiring pose data of the mobile platform; the computing host processes the point cloud data and the pose data to obtain key frame point cloud and a key pose graph, and sends the key frame point cloud and the key pose graph to the central control center through a wireless network card; the central control center performs loop detection based on the key frame point cloud and the key pose graph to find a loop candidate pair, performs pose graph aggregation and optimization based on a loop detection result, the key frame point cloud and the key frame point cloud to obtain a global point cloud map of a region to be mapped. The application solves the problems of low mapping efficiency and long mapping time consumption in the prior art.

Description

Technical Field

[0001] This invention relates to the field of simultaneous localization and mapping technology, specifically to a multi-robot collaborative mapping system. Background Technology

[0002] Simultaneous Localization and Mapping (SLAM) is a key technology that uses sensors on the robot itself to acquire information about the surrounding environment and estimate its own position and orientation, thereby building a map. This technology provides important support for robots to achieve autonomous localization and navigation in unknown environments. Currently, single-robot SLAM technology has achieved considerable robustness and has been widely used in many fields.

[0003] However, single-robot SLAM technology faces some challenges when dealing with map building in large-scale environments. Single-robot SLAM is inefficient, with increased estimation time and decreased estimation accuracy, often making it difficult to achieve good results.

[0004] In conclusion, there is an urgent need for a multi-robot collaborative mapping system. Summary of the Invention

[0005] Based on the above analysis, this invention proposes a multi-robot collaborative mapping system to solve the technical problems of low mapping efficiency and long mapping time in the prior art.

[0006] To achieve the above objectives, the present invention provides a multi-robot collaborative mapping system.

[0007] Includes multiple mobile platforms and a central control center;

[0008] Each mobile platform is equipped with a lidar, a laser odometer, and a computing host;

[0009] The lidar is used to acquire point cloud data of the surrounding environment;

[0010] The laser odometer is used to acquire the pose data of the mobile platform;

[0011] The computing host processes the point cloud data and pose data to obtain key frame point cloud and key pose map, and sends the key frame point cloud and key pose map to the central control center through the wireless network card.

[0012] The central control center performs loop closure detection based on the keyframe point cloud and the key pose graph to find candidate loop closure pairs. Based on the loop closure detection results, the keyframe point cloud, and the keyframe point cloud, the pose graph is aggregated and optimized to obtain a global point cloud map of the region to be mapped.

[0013] Furthermore, the central control center issues a synchronization time synchronization command to the computing host of each mobile platform, thereby providing synchronization time synchronization services to all devices on each mobile platform;

[0014] After all devices are synchronized, the central control center issues movement commands to the computing host to control each mobile platform. The computing host controls each mobile platform to move within the area to be mapped. The movement path of each mobile platform covers the entire area to be mapped. After the movement is completed, each mobile platform returns to its starting position.

[0015] Each of the aforementioned computing hosts sends the keyframe point cloud and key pose map to the central control center in real time.

[0016] Furthermore, the lidar is connected to the computing host via the switch;

[0017] Both the wireless network card and the laser odometer are connected to the computing host via a USB interface;

[0018] The wireless network card is used for communication between the computer host and the central control center.

[0019] The central control center includes high-performance computing hosts and storage devices;

[0020] The high-performance computing host is used for loop closure detection and generating the global point cloud map;

[0021] The storage device is used to store the key point cloud frame data, key pose map data, loop closure detection results, and global point cloud map.

[0022] Furthermore, each computing host has two network ports: one for data transmission within the mobile platform and the other for data transmission with the central control center.

[0023] Further, the computing host processes the point cloud data and pose data to obtain keyframe point clouds and key pose maps, including:

[0024] The computing host preprocesses the point cloud data, removes motion distortion from the point cloud data using the uniform motion assumption, 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.

[0025] Each mobile platform operates an onboard radar odometry system, which uses point cloud frame-local map matching to estimate the pose of the mobile platform and construct the current local pose map of the mobile platform.

[0026] 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 moving platform at the moment of point cloud frame scanning.

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

[0028] The computing host selects key pose nodes from the local pose graph and constructs a key pose graph.

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

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

[0031] The pose of the mobile platform after it exceeds the motion threshold is added as a new key pose node;

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

[0033] Furthermore, the central control center performs loop closure detection based on the keyframe point cloud and the key pose map to find loop closure candidate pairs, including:

[0034] The high-performance computing host takes the key pose node in the key pose graph received from each computing host as the current node and calculates the Euclidean distance between the current node and other nodes.

[0035] 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;

[0036] Among them, the other nodes are all historical key pose nodes in the key pose graph generated for each mobile platform.

[0037] Furthermore, the high-performance computing host utilizes the point clouds of two keyframes corresponding to the two nodes of the loop closure candidate pair to perform a two-stage coarse-to-fine loop closure confirmation to obtain the precise relative pose transformation between the two nodes, including:

[0038] The high-performance computing host uses the TEASER++ algorithm to extract local feature descriptors based on the two keyframe point clouds of the loop closure candidate pair in the computing queue, matches the features of the two keyframe point clouds, establishes the correspondence between the two keyframe point clouds, and estimates the relative pose transformation between the two keyframe point clouds through the minimum error function.

[0039] 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.

[0040] Furthermore, the high-performance computing host performs key pose graph aggregation and optimization to obtain multi-mobile platform trajectories and a global point cloud map, including:

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

[0042] 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.

[0043] 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 and removing noise and outliers, the movement trajectory of multiple mobile platforms and the global point cloud map are obtained.

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

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

[0046] 1. Advantages of low computing power requirements and memory usage: By running the laser odometry independently on the mobile platform, 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 of the base station.

[0047] 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.

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

[0049] In summary, the present 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 multi-robot collaborative mapping system.

[0050] 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

[0051] 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.

[0052] Figure 1 This is a schematic diagram of a multi-robot collaborative mapping system;

[0053] Figure 2 A schematic diagram of a global point cloud map obtained by performing loop closure detection, pose graph aggregation and optimization on a high-performance computer. Detailed Implementation

[0054] 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.

[0055] like Figure 1 As shown, a multi-robot collaborative mapping system includes multiple mobile platforms and a central control center;

[0056] Each mobile platform is equipped with a lidar, a laser odometer, and a computing host;

[0057] The lidar is used to acquire point cloud data of the surrounding environment;

[0058] The laser odometer is used to acquire the pose data of the mobile platform;

[0059] The computing host processes the point cloud data and pose data to obtain key frame point cloud and key pose map, and sends the key frame point cloud and key pose map to the central control center through the wireless network card.

[0060] The central control center performs loop closure detection based on the keyframe point cloud and the key pose graph to find candidate loop closure pairs. Based on the loop closure detection results, the keyframe point cloud, and the keyframe point cloud, the pose graph is aggregated and optimized to obtain a global point cloud map of the region to be mapped.

[0061] The central control center issues a synchronization time synchronization command to the computing host of each mobile platform, and performs synchronization time synchronization service for all devices on each mobile platform.

[0062] After all devices are synchronized, the central control center issues movement commands to the computing host to control each mobile platform. The computing host controls each mobile platform to move within the area to be mapped. The movement path of each mobile platform covers the entire area to be mapped. After the movement is completed, each mobile platform returns to its starting position.

[0063] Each of the aforementioned computing hosts sends the keyframe point cloud and key pose map to the central control center in real time.

[0064] The lidar is connected to the computing host via the switch;

[0065] Both the wireless network card and the laser odometer are connected to the computing host via a USB interface;

[0066] The wireless network card is used for communication between the computer host and the central control center.

[0067] The central control center includes high-performance computing hosts and storage devices;

[0068] The high-performance computing host is used for loop closure detection and generating the global point cloud map;

[0069] The storage device is used to store the key point cloud frame data, key pose map data, loop closure detection results, and global point cloud map.

[0070] Each computing host has two network ports: one for data transmission within the mobile platform and the other for data transmission with the central control center.

[0071] For example, there are 5 front-ends for the mobile platform.

[0072] Multiple motion platforms use their respective onboard LiDAR to scan the surrounding environment, acquiring point cloud data and timestamp data of the points.

[0073] Point cloud data reflects the environmental information surrounding the location of the motion platform.

[0074] The computing host processes the point cloud data and pose data to obtain keyframe point clouds and key pose maps, including:

[0075] The computing host processes the point cloud data and pose data to obtain keyframe point clouds and key pose maps, including:

[0076] The computing host preprocesses the point cloud data, removes motion distortion from the point cloud data using the uniform motion assumption, 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.

[0077] Each mobile platform operates an onboard radar odometry system, which uses point cloud frame-local map matching to estimate the pose of the mobile platform and construct the current local pose map of the mobile platform.

[0078] 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 moving platform at the moment of point cloud frame scanning.

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

[0080] The computing host selects key pose nodes from the local pose graph and constructs a key pose graph.

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

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

[0083] The pose of the mobile platform after it exceeds the motion threshold is added as a new key pose node;

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

[0085] The motion platform and the computing host are in a 1:1 ratio.

[0086] To avoid introducing too many poses and increasing the computational load, preferably, the poses of the mobile platform after exceeding the motion threshold are added as new key pose nodes.

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

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

[0089] Set motion threshold: Add new key pose nodes only after the moving platform exceeds a certain motion threshold.

[0090] Examples of motion threshold settings: a translation of 2m or a rotation of 30° for the moving platform.

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

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

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

[0094] When the mobile platform moves out of the communication range, the pose map and keyframe point cloud wait in the queue of the computing host and are sent to the central control center in batches when communication is re-established.

[0095] The central control center provides a centralized control and coordination hub, enabling multiple motion platforms to coordinate and simultaneously locate and map in a wide range of environments.

[0096] like Figure 2 As shown, the high-performance computing host in the central control center performs loop closure detection based on the keyframe point cloud and the key pose map to find loop closure candidate pairs, including:

[0097] The high-performance computing host takes the key pose node in the key pose graph received from each computing host as the current node and calculates the Euclidean distance between the current node and other nodes.

[0098] 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;

[0099] Among them, the other nodes are all historical key pose nodes in the key pose graph generated for each mobile platform.

[0100] Loop closure detection can determine whether a mobile platform has returned to a previously visited historical location, which is key to reducing the cumulative error of SLAM trajectory.

[0101] In addition, loopbacks can detect overlapping portions between local maps created by mobile platforms within a team, allowing them to be aggregated into a geometrically consistent global map.

[0102] The high-performance computing host receives key pose maps and key frame point clouds sent by various computing hosts.

[0103] The key pose nodes in the pose graph recently received by the high-performance computing host from each mobile platform are used as the current node, and the Euclidean distance between the current node and other nodes is calculated.

[0104] 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;

[0105] Among them, the other nodes are all the historical key pose nodes in the pose graph generated by each computing host and received by the high-computing-power computing host.

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

[0107] (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.

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

[0109] 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.

[0110] The Euclidean distance threshold d ma x is 2-5 meters or 30°-40°.

[0111] 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.

[0112] 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.

[0113] The high-performance computing host utilizes the point clouds of two keyframes corresponding to the two nodes of the loop closure candidate pair, and performs a two-stage coarse-to-fine loop closure confirmation to obtain the precise relative pose transformation between the two nodes, including:

[0114] The high-performance computing host uses the TEASER++ algorithm to extract local feature descriptors based on the two keyframe point clouds of the loop closure candidate pair in the computing queue, matches the features of the two keyframe point clouds, establishes the correspondence between the two keyframe point clouds, and estimates the relative pose transformation between the two keyframe point clouds through the minimum error function.

[0115] 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.

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

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

[0118] 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.

[0119] Input: The TEASER++ algorithm takes two frames of point cloud data as input, which are two frames of point cloud data for loop closure candidate pairs respectively;

[0120] Feature extraction: Extracting features from point cloud data, such as extracting local feature descriptors;

[0121] Feature matching: Perform feature matching on the features of two point clouds to establish the correspondence between the two point clouds;

[0122] Coarse estimation of relative pose transformation: The relative pose transformation between two frames of point clouds is estimated by using the minimum error function.

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

[0124] 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.

[0125] Input: Use the coarse estimation results obtained from TEASER++ as the initial values ​​for the GICP algorithm, and the point clouds of two keyframes for the loop closure candidate pair;

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

[0127] 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;

[0128] 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.

[0129] The goal of this step is to perform loop closure detection on a high-performance computing host, determine the existence of loops by calculating relative pose transformations, and perform joint pose graph optimization and global point cloud map generation.

[0130] The high-performance computing host located in the central control center receives the pose graph, keyframes and loop closure detection calculation results generated by the computing host, performs joint pose graph optimization, and finally obtains a globally consistent and drift-free global point cloud map.

[0131] This invention employs a centralized multi-robot architecture, in which a central control center receives key pose graphs and keyframe data from each computing host, as well as loop closure calculation results from a high-performance computing host. It then performs pose graph optimization to obtain an optimized trajectory. The optimized trajectory is used to transform the keyframes to a global coordinate system, thereby generating an optimized global point cloud map.

[0132] The high-performance computing host performs key pose graph aggregation and optimization to obtain multi-mobile platform trajectories and a global point cloud map, including:

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

[0134] 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.

[0135] 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 and removing noise and outliers, the movement trajectory of multiple mobile platforms and the global point cloud map are obtained.

[0136] The high-performance computing host receives key pose maps and key frames sent by the computing host, as well as the loop closure detection results calculated by itself.

[0137] Based on the received loop closure detection results, the high-performance computing host connects the key pose graphs sent by each computing host, adds edges corresponding to loop closure candidate pairs in the key pose graphs, and forms a complete pose graph for the multi-robot.

[0138] (1) Adding loop closure edges: Add the edges corresponding to the loop closure candidate pairs in the loop closure detection results to the pose graph of each mobile platform;

[0139] (2) Pose graph connection: Connect two nodes in the key pose graph of the same mobile platform, or connect two nodes in the two key pose graphs of two mobile platforms to realize the aggregation of key pose graphs of multiple motion platforms and obtain a complete pose graph.

[0140] The high-performance computing host executes the progressive non-convex GNC algorithm (Guidance Navigation and Control) to optimize the pose graph.

[0141] The high-performance computing host uses an incremental non-convex GNC algorithm to perform robust pose graph optimization, resulting in optimized multi-robot trajectories.

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

[0143] (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;

[0144] (3) Optimization goal: To obtain globally consistent and drift-free multi-mobile platform trajectories through the optimized complete pose graph;

[0145] (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.

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

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

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

[0149] 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.

[0150] 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.

[0151] The high-performance computing host receives and integrates key pose maps, key frames, and calculated loop closure detection results from various mobile platforms. It then performs robust joint pose map optimization using the GNC algorithm to ultimately obtain a globally consistent and drift-free global point cloud map.

[0152] By design including, Figure 1 The system architecture shown, which involves a mobile platform and a central control center, offers the following effects and advantages:

[0153] 1. Advantages of low computing power requirements and memory usage: By running the laser odometry independently on the mobile platform, 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 of the base station.

[0154] 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.

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

[0156] 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.

[0157] 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 multi-robot collaborative mapping system, characterized in that, Includes multiple mobile platforms and a central control center; Each mobile platform is equipped with a lidar, a laser odometer, and a computing host; The lidar is used to acquire point cloud data of the surrounding environment; The laser odometer is used to acquire the pose data of the mobile platform; The computing host processes the point cloud data and pose data to obtain key frame point cloud and key pose map, and sends the key frame point cloud and key pose map to the central control center through the wireless network card. The central control center includes high-performance computing hosts and storage devices; The high-performance computing host is used for loop closure detection and generation of a global point cloud map; The central control center performs loop closure detection based on the key frame point cloud and the key pose map to find loop closure candidate pairs. The high-performance computing host utilizes the point clouds of two keyframes corresponding to the two nodes of the loop closure candidate pair, and performs a two-stage coarse-to-fine loop closure confirmation to obtain the precise relative pose transformation between the two nodes, including: The high-performance computing host uses the TEASER++ algorithm to extract local feature descriptors based on two keyframe point clouds of loop closure candidate pairs in the computing queue, matches the features of the two keyframe point clouds, establishes the correspondence between the two keyframe point clouds, and estimates 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 richness of environmental features, candidate loop closure pairs in the key pose graph are screened and prioritized, eliminating candidate loop closure pairs with high degradation and prioritizing the calculation of candidate loop closure pairs with better observability. Based on the loop closure detection results and the keyframe point cloud, pose graph aggregation and optimization are performed to obtain a global point cloud map of the region to be mapped. The high-performance computing host performs key pose graph aggregation and optimization to obtain multi-mobile platform trajectories and a global point cloud map, including: 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 GNC algorithm is used to optimize the complete pose graph. By iteratively adjusting the position of each pose node in the complete pose graph and removing noise and outliers, the movement trajectory of multiple mobile platforms and the global point cloud map are obtained.

2. The system according to claim 1, characterized in that, The central control center issues a synchronization time synchronization command to the computing host of each mobile platform to provide synchronization time synchronization services to all devices on each mobile platform. After all devices are synchronized, the central control center issues movement commands to the computing host to control each mobile platform. The computing host controls each mobile platform to move within the area to be mapped. The movement path of each mobile platform covers the entire area to be mapped. After the movement is completed, each mobile platform returns to its starting position. Each of the aforementioned computing hosts sends the keyframe point cloud and key pose map to the central control center in real time.

3. The system according to claim 2, characterized in that... The lidar is connected to the computing host via a switch; Both the wireless network card and the laser odometer are connected to the computing host via a USB interface; The wireless network card is used for communication between the computer host and the central control center. The storage device is used to store the keyframe point cloud data, key pose map data, loop closure detection results, and global point cloud map.

4. The system according to claim 3, characterized in that, Each computing host has two network ports: one for data transmission within the mobile platform and the other for data transmission with the central control center.

5. The system according to claim 4, characterized in that, The computing host processes the point cloud data and pose data to obtain keyframe point clouds and key pose maps, including: The computing host preprocesses the point cloud data, removes motion distortion from the point cloud data using the uniform motion assumption, 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 mobile platform operates an onboard radar odometry system, which uses point cloud frame-local map matching to estimate the pose of the mobile platform and construct the current local pose map of the mobile platform. 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 moving platform at the moment of point cloud frame scanning. Then, the pose is added to the current local pose graph to incrementally construct the local pose graph. The computing host selects key pose nodes from the local pose graph and constructs a key pose graph.

6. The system according to claim 5, 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 mobile platform after it exceeds 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.

7. The system according to claim 6, characterized in that, The central control center performs loop closure detection based on the keyframe point cloud and the key pose map to find loop closure candidate pairs, including: The high-performance computing host takes the key pose node in the key pose graph received from each computing host as the current node and calculates the Euclidean distance between the current node and other nodes. 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 historical key pose nodes in the key pose graph generated for each mobile platform.

8. The system according to claim 7, characterized in that, Euclidean distance threshold It is 2-5 meters.

Images