A multi-machine 3D laser SLAM method and system for dynamic merging of connected components

Abstract

This invention discloses a multi-machine 3D laser SLAM method and system with dynamic merging of connected components. It involves each agent acquiring sensory data and performing single-machine SLAM to obtain a SLAM map and laser features; transmitting the data to other agents; and optimizing and fusing the features of each agent's SLAM map with those of other agents to achieve multi-agent laser SLAM. This invention utilizes a dynamic connected graph construction and merging algorithm based on matching similarity constraints to dynamically and progressively merge the subgraphs of multiple agents, solving the problem of result confusion when one agent overlaps with multiple other agents during actual multi-machine SLAM operation. It introduces centroid feature analysis and principal component analysis for laser point cloud features, improving loop closure detection efficiency and accuracy. The introduction of a ROS1-ROS2 communication system solves the distributed communication problem among multiple agents, improving the system's applicability.

Description

Technical Field

[0001] This invention belongs to the field of multi-robot 3D laser SLAM, and relates to a multi-robot 3D laser SLAM method and system for dynamic merging of connected components. Background Technology

[0002] With the development of LiDAR technology, Simultaneous Localization and Mapping (SLAM) has been widely applied in autonomous driving and environmental measurement. In large-scale scenarios requiring high time efficiency, such as exploring unknown environments, mines, battlefields, and rescue operations, using a single agent (e.g., a ground-based unmanned vehicle) for mapping increases storage pressure and fails to meet time efficiency requirements. Therefore, collaborative mapping by multiple agents is needed to address these issues. Traditional centralized multi-agent SLAM systems require a central server, making it difficult to guarantee robustness in complex, large-scale scenarios. The development of 5G communication technology and the improvement of agent computing and perception capabilities have made distributed multi-agent 3D LiDAR SLAM possible. Existing DiSCo-SLAM algorithms provide stable localization results through a two-stage factor graph optimization method. However, because the matching order cannot be guaranteed during actual multi-agent SLAM operation, coordinate transfer confusion occurs when one agent overlaps with two or more other agents, resulting in disordered final global map results. The original algorithm uses Scan Context features for loop closure detection. Due to the lack of unified observation coordinates, the Scan Context features at the same location will differ when the robot's perspective changes, leading to reduced search efficiency. At the same time, the traditional ROS1 communication adopts a master-slave communication mechanism, which cannot meet the needs of scenarios where a single intelligent agent operates independently. Summary of the Invention

[0003] The purpose of this invention is to solve the problem that in the actual operation of multi-machine SLAM, the order of matching cannot be guaranteed, which leads to coordinate transfer confusion when one agent overlaps with two or more other agents, resulting in disorder of the final global map. The invention provides a multi-machine 3D laser SLAM method and system with dynamic merging of connected components.

[0004] To achieve the above objectives, the present invention employs the following technical solution:

[0005] This invention proposes a multi-machine 3D laser SLAM method for dynamic merging of connected components, comprising the following steps:

[0006] Loop closure detection is performed based on laser odometry data from multiple agents and point cloud maps to obtain the relative pose transformation matrix between any two agents.

[0007] The relative pose transformation matrix is ​​processed to obtain the factor graphs of each agent;

[0008] A dynamic connectivity graph construction and merging algorithm is used to process the factor graphs of each agent to obtain a multi-agent overall laser SLAM map.

[0009] Preferably, the laser odometry data of the intelligent agent is acquired based on the inertial measurement unit;

[0010] Obtain a point cloud map of the intelligent agent based on LiDAR data.

[0011] Preferably, an improved Scan Context feature method is used to perform loop closure detection on the laser odometry data and point cloud map of multiple agents;

[0012] The improved Scan Context feature method has the following specific steps:

[0013] 1) Obtain the current point cloud data of the lidar at the front end, project the lidar data along the Z-axis direction to obtain a two-dimensional point cloud from the bird's-eye view of the lidar.

[0014] 2) Translate the origin of the two-dimensional point cloud coordinate system to the centroid;

[0015] 3) Perform principal component analysis on the two-dimensional point cloud to obtain three feature vectors representing the three principal axis directions respectively; wherein, in order to ensure that the Z-axis is perpendicular and the three axes are perpendicular to each other, the Y-axis is artificially specified by taking the cross product of the Z-axis and the X-axis, and the side with more point cloud on the X-axis is specified as the positive direction.

[0016] 4) Divide the point cloud of the LiDAR bird's-eye view into N×M sector grids at equal angles and distances starting from the selected positive X principal axis according to polar coordinates. Take the point with the highest Z-axis height in each grid to obtain the improved Scan Context feature of the LiDAR point cloud in the current frame. Store the feature matrix point by point in an N×M kd tree for maintenance.

[0017] 5) When a new and improved Scan Context feature is received, the nearest neighbor algorithm is used in the kd-tree to retrieve several features that are adjacent to the current feature, and then the cosine similarity between the retrieved features and the current feature is calculated.

[0018] 6) After detecting potential loop closures using the improved Scan Context feature method, ICP matching and registration are performed between the point cloud at the potential loop closure time and the point cloud at the current time, and the transition matrix T is obtained. * And similarity S * If the similarity S * If the given acceptance threshold τ is met, the loop closure detection is considered successful, and the historical frame information, current frame information, and transition matrix T are processed. * And similarity S *Send it to other intelligent agents.

[0019] Preferably, the cosine similarity is calculated as follows:

[0020]

[0021] Among them, I q Let I be the current feature matrix. c For candidate feature matrices, For I q In the vector of row j, ||·|| represents the magnitude of the orientation quantity, N s d(I) represents the number of vectors. q ,I c The cosine similarity (c) represents the cosine similarity between two vectors; the index c of the historical feature with the highest similarity to the current feature is obtained from the cosine similarity. * and cosine similarity d * If d * When <τ, then the cth * There is a potential loop relationship between the historical pose and the current pose. τ is a given acceptance threshold. Based on the acceptance threshold τ, the data of each agent passing through the loop region is obtained.

[0022] Preferably, the Fast-DDS communication middleware based on ROS2-foxy is used to build a communication architecture based on ROS topic communication through ros1_bridge to realize communication between multiple agents.

[0023] Preferably, the relative pose transformation matrix is ​​processed using a pairwise consistent measurement set maximization detection method;

[0024] The consistency check satisfies the loop closure condition:

[0025]

[0026] in, It represents the pose transformation of agent β within the intelligent body between timestamps l and j. It represents the pose transformation of agent β at timestamp j and agent α at timestamp i. It represents the pose transformation of agent α within the intelligent body between timestamps i and k. It is the inverse pose transformation between agent β at timestamp l and agent α at timestamp k, where ∈ is a given threshold.

[0027] All overlapping observation locations are used as nodes in the global factor graph. For processed loop closures, each loop closure is used as a variable node in the global factor graph. The relative pose transformation between two loop closures detected by loop closure is used as a binary factor between the corresponding two variable nodes. Finally, a factor graph containing global transformation information between each agent is obtained.

[0028] Preferably, the method for processing the factor graphs of each agent using a dynamic connected graph construction and merging algorithm based on matching similarity constraints is as follows:

[0029] 1) Initialize the graph structure where nodes are not connected according to the number of agents, with each agent using its own initial coordinates as the origin.

[0030] 2) When the pose transformation results of the loop closure points are received, connect the graph nodes corresponding to the agents where each loop closure point is located to obtain the connected components;

[0031] 3) Use the depth-first search algorithm to detect connectivity in the graph structure. For each connected component, take the first matched agent as the global coordinate origin and transfer the coordinates of the remaining agents in the connected component to the global coordinate origin according to the transition matrix, which serves as the approximate initial value for subsequent factor graph optimization.

[0032] This invention proposes a multi-machine 3D laser SLAM system with dynamic merging of connected components, characterized in that it includes:

[0033] The pose transformation matrix acquisition module is used to perform loop closure detection based on the laser odometry data of multiple intelligent agents and point cloud map, and to obtain the relative pose transformation matrix between any two intelligent agents.

[0034] A factor graph acquisition module is used to process the relative pose transformation matrix to obtain the factor graphs of each agent.

[0035] The laser SLAM map acquisition module is used to process the factor graphs of each agent using a dynamic connected graph construction and merging algorithm to obtain a multi-agent overall laser SLAM map.

[0036] A computer device includes a memory and a processor, the memory storing a computer program, and the processor executing the computer program to implement the steps of a multi-machine 3D laser SLAM method for dynamic merging of connected components.

[0037] A computer-readable storage medium storing a computer program that, when executed by a processor, implements the steps of a multi-machine 3D laser SLAM method for dynamic merging of connected components.

[0038] Compared with the prior art, the present invention has the following beneficial effects:

[0039] This invention proposes a multi-machine 3D laser SLAM method with dynamic merging of connected components. In existing technologies, the order of matching cannot be guaranteed during the actual operation of multi-machine SLAM, leading to coordinate transfer confusion when one agent overlaps with two or more other agents. This invention introduces a dynamic connected graph construction and merging algorithm based on matching similarity constraints, thereby dynamically and gradually merging the global coordinate transformations of multiple agents. It maintains a global factor graph of the loop closure detection regions between each agent and uses the coordinate transformation results obtained by dynamic merging as initial values ​​to optimize the factor graph and obtain the relative poses between agents. At the same time, it supports the addition of new agents with overlapping regions at any time, and has high stability and adaptability when there are a large number of agents.

[0040] Furthermore, based on the traditional Scan Context algorithm for loop closure detection, a particle point determination algorithm based on laser point cloud topological features and a principal component analysis algorithm are introduced to determine the main viewpoint, thereby improving the efficiency and accuracy of loop closure detection.

[0041] Furthermore, by introducing a communication system based on ros1-ros1_bridge-ros2, multiple agents can operate independently and support exiting and entering the system due to communication problems, thereby improving the robustness of the system.

[0042] This invention proposes a multi-machine 3D laser SLAM system with dynamic merging of connected components. The system is divided into a pose transformation matrix acquisition module, a factor graph acquisition module, and a laser SLAM map acquisition module. This modular approach ensures that each module is independent, facilitating unified management. Attached Figure Description

[0043] To more clearly illustrate the technical solutions of the embodiments of the present invention, the accompanying drawings used in the embodiments will be briefly introduced below. It should be understood that the following drawings only show some embodiments of the present invention and should not be regarded as a limitation on the scope. For those skilled in the art, other related drawings can be obtained based on these drawings without creative effort.

[0044] Figure 1 This is a flowchart of the multi-machine 3D laser SLAM method for dynamic merging of connected components according to the present invention.

[0045] Figure 2 This is a system framework diagram of the method of the present invention.

[0046] Figure 3This is a schematic diagram illustrating the use of a dynamic connected graph construction and merging algorithm based on matching similarity constraints to merge factor graphs and optimize them, as described in this invention.

[0047] Figure 4 This is the communication system framework of the present invention.

[0048] Figure 5 This is the visualization result of the three-dimensional mapping of the present invention.

[0049] Figure 6 This is a diagram of a multi-machine 3D laser SLAM system for dynamic merging of connected components according to the present invention. Detailed Implementation

[0050] To make the objectives, technical solutions, and advantages of the embodiments of the present invention clearer, the technical solutions of the embodiments of the present invention will be clearly and completely described below with reference to the accompanying drawings. Obviously, the described embodiments are only some embodiments of the present invention, and not all embodiments. The components of the embodiments of the present invention described and shown in the accompanying drawings can generally be arranged and designed in various different configurations.

[0051] Therefore, the following detailed description of the embodiments of the invention provided in the accompanying drawings is not intended to limit the scope of the claimed invention, but merely to illustrate selected embodiments of the invention. All other embodiments obtained by those skilled in the art based on the embodiments of the invention without inventive effort are within the scope of protection of the invention.

[0052] It should be noted that similar labels and letters in the following figures indicate similar items. Therefore, once an item is defined in one figure, it does not need to be further defined and explained in subsequent figures.

[0053] In the description of the embodiments of the present invention, it should be noted that if terms such as "upper," "lower," "horizontal," or "inner" indicate the orientation or positional relationship based on the orientation or positional relationship shown in the accompanying drawings, or the orientation or positional relationship commonly used when the product of the invention is in use, they are only for the convenience of describing the present invention and simplifying the description, and do not indicate or imply that the device or element referred to must have a specific orientation, or be constructed and operated in a specific orientation, and therefore should not be construed as a limitation of the present invention. Furthermore, terms such as "first" and "second" are only used to distinguish descriptions and should not be construed as indicating or implying relative importance.

[0054] Furthermore, the use of the term "horizontal" does not imply that the component must be absolutely horizontal, but rather that it can be slightly tilted. For example, "horizontal" simply means that its direction is more horizontal than "vertical," and does not mean that the structure must be completely horizontal, but can be slightly tilted.

[0055] In the description of the embodiments of the present invention, it should also be noted that, unless otherwise explicitly specified and limited, the terms "set," "install," "connect," and "link" should be interpreted broadly. For example, they can refer to a fixed connection, a detachable connection, or an integral connection; they can refer to a mechanical connection or an electrical connection; they can refer to a direct connection or an indirect connection through an intermediate medium; and they can refer to the internal connection of two components. Those skilled in the art can understand the specific meaning of the above terms in the present invention according to the specific circumstances.

[0056] The present invention will now be described in further detail with reference to the accompanying drawings:

[0057] This invention proposes a multi-machine 3D laser SLAM method for dynamic merging of connected components, such as... Figure 1 As shown, it includes the following steps:

[0058] S1. Based on the laser odometry data of multiple agents and the point cloud map, perform loop closure detection to obtain the relative pose transformation matrix between any two agents.

[0059] The laser odometry data and point cloud map data of the intelligent agent are obtained based on the inertial measurement unit;

[0060] Obtain a point cloud map of the intelligent agent based on LiDAR data.

[0061] An improved Scan Context feature method is used to perform loop closure detection on laser odometry data and point cloud maps from multiple agents;

[0062] The improved Scan Context feature method has the following specific steps:

[0063] 1) Obtain the current point cloud data of the lidar at the front end, project the lidar data along the Z-axis direction to obtain a two-dimensional point cloud from the bird's-eye view of the lidar.

[0064] 2) Translate the origin of the two-dimensional point cloud coordinate system to the centroid;

[0065] 3) Perform principal component analysis on the two-dimensional point cloud to obtain three feature vectors representing the three principal axis directions respectively; wherein, in order to ensure that the Z-axis is perpendicular and the three axes are perpendicular to each other, the Y-axis is artificially specified by taking the cross product of the Z-axis and the X-axis, and the side with more point cloud on the X-axis is specified as the positive direction.

[0066] 4) Divide the point cloud of the LiDAR bird's-eye view into N×M sector grids at equal angles and distances starting from the selected positive X principal axis according to polar coordinates. Take the point with the highest Z-axis height in each grid to obtain the improved Scan Context feature of the LiDAR point cloud in the current frame. Store the feature matrix point by point in an N×M kd tree for maintenance.

[0067] 5) When a new and improved Scan Context feature is received, the nearest neighbor algorithm is used in the kd-tree to retrieve several features that are adjacent to the current feature, and then the cosine similarity between the retrieved features and the current feature is calculated.

[0068] 6) After detecting potential loop closures using the improved Scan Context feature method, ICP matching and registration are performed between the point cloud at the potential loop closure time and the point cloud at the current time, and the transition matrix T is obtained. * And similarity S * If the similarity S * If the given acceptance threshold τ is met, the loop closure detection is considered successful, and the historical frame information, current frame information, and transition matrix T are processed. * And similarity S * Send it to other intelligent agents.

[0069] The method for calculating cosine similarity is as follows:

[0070]

[0071] Among them, I q Let I be the current feature matrix. c For candidate feature matrices, For I q In the vector of row j, ||·|| represents the magnitude of the orientation quantity, N s d(I) represents the number of vectors. q ,I c The cosine similarity (c) represents the cosine similarity between two vectors; the index c of the historical feature with the highest similarity to the current feature is obtained from the cosine similarity. * and cosine similarity d * If d * When <τ, then the cth * There is a potential loop relationship between the historical pose and the current pose. τ is a given acceptance threshold. Based on the acceptance threshold τ, the data of each agent passing through the loop region is obtained.

[0072] The Fast-DDS communication middleware based on ROS2-foxy uses ros1_bridge to build a communication architecture based on ROS topic communication, enabling communication between multiple agents and transmitting loop closure detection data.

[0073] S2. Process the relative pose transformation matrix to obtain the factor graphs of each agent;

[0074] The relative pose transformation matrix is ​​processed using a pairwise consistent measurement set maximization detection method;

[0075] The consistency check satisfies the loop closure condition:

[0076]

[0077] in, It represents the pose transformation of agent β within the intelligent body between timestamps l and j. It represents the pose transformation of agent β at timestamp j and agent α at timestamp i. It represents the pose transformation of agent α within the intelligent body between timestamps i and k. It is the inverse pose transformation between agent β at timestamp l and agent α at timestamp k, where ∈ is a given threshold.

[0078] All overlapping observation locations are used as nodes in the global factor graph. For processed loop closures, each loop closure is used as a variable node in the global factor graph. The relative pose transformation between two loop closures detected by loop closure is used as a binary factor between the corresponding two variable nodes. Finally, a factor graph containing global transformation information between each agent is obtained.

[0079] S3. A dynamic connected graph construction and merging algorithm is used to process the factor graphs of each agent to obtain a multi-agent overall laser SLAM map.

[0080] The method for processing the factor graphs of each agent using a dynamic connected graph construction and merging algorithm based on matching similarity constraints is as follows:

[0081] 1) Initialize the graph structure where nodes are not connected according to the number of agents, with each agent using its own initial coordinates as the origin.

[0082] 2) When the pose transformation results of the loop closure points are received, connect the graph nodes corresponding to the agents where each loop closure point is located to obtain the connected components;

[0083] 3) Use the depth-first search algorithm to detect connectivity in the graph structure. For each connected component, take the first matched agent as the global coordinate origin and transfer the coordinates of the remaining agents in the connected component to the global coordinate origin according to the transition matrix, which serves as the approximate initial value for subsequent factor graph optimization.

[0084] Specifically: The flowchart of the specific implementation framework of this invention is as follows: Figure 2As shown, it consists of a single-machine section and a multi-machine section. Each section comprises a front-end and a back-end. In the single-machine section, the front-end is responsible for transmitting environmental information data collected by sensors to the laser odometry and estimating the pose of adjacent point cloud data to form a local map. The back-end is responsible for optimizing the pose measured by the odometry at different times to obtain a global map, and can also optimize the single-machine SLAM results based on information related to itself from other agents. The multi-machine section's front-end function is to perform loop closure detection based on data from each agent and obtain relative poses; the back-end function is to remove outliers from the front-end results and optimize the global poses between different agents. The main implementation process includes the following steps:

[0085] Step 1: Deploy a real-time localization and mapping framework based on 3D LiDAR and IMU.

[0086] Each vehicle executes an independent laser SLAM system, using inertial measurement units (IMUs) and 3D LiDAR data from the agent to perform real-time localization and mapping, obtaining laser odometry data and point cloud maps of the corresponding point clouds.

[0087] Step 2: The real-time acquired LiDAR data is transmitted to the front end of the multi-machine SLAM system. The front end is responsible for loop closure detection. When different agents are detected to have potential overlap, ICP point cloud matching is performed to obtain the relative pose transformation matrix between the two agents at this time.

[0088] The keyframe LiDAR point cloud data of the current moment obtained from the individual SLAM systems of each agent, along with the odometry data obtained from the SLAM systems, are input into the front end of the multi-agent SLAM system. The multi-agent SLAM system runs distributed across the industrial control computers of each agent vehicle. The front end is responsible for detecting whether the agents have traversed overlapping areas, specifically using an improved Scan Context feature method for loop closure detection.

[0089] The improved Scan Context feature method has the following specific steps:

[0090] 1) Obtain the current point cloud data of the lidar at the front end, project the lidar data along the Z-axis direction to obtain a two-dimensional point cloud from the perspective of the lidar's bird's-eye view (BEV).

[0091] 2) Translate the origin of the two-dimensional point cloud coordinate system to the centroid.

[0092] 3) Principal component analysis (PCA) is performed on the two-dimensional point cloud to obtain three eigenvectors representing the three principal axes. To ensure that the Z-axis is perpendicular and the three axes are mutually perpendicular, the Y-axis can be manually specified by taking the cross product of the Z-axis and the X-axis. The side with a larger number of points on the X-axis is designated as the positive direction.

[0093] 4) Divide the point cloud from the BEV perspective into N×M sector grids at equal angles and distances, starting from the selected positive X-axis direction using polar coordinates. Take the point with the highest Z-axis height in each grid to obtain the improved ScanContext feature of the current frame's LiDAR point cloud. Store the feature matrix point-by-point in an N×M kd-tree for maintenance.

[0094] 5) When new and improved Scan Context features are received, the nearest neighbor algorithm (KNN) is used in the kd-tree to retrieve several features that are adjacent to the current feature. The cosine similarity between these retrieved features and the current feature is calculated using the following formula:

[0095]

[0096] Among them, I q Let I be the current feature matrix. c For candidate feature matrices, For I q In the vector of row j, ||·|| represents the magnitude of the orientation quantity, N s d(I) represents the number of vectors. q ,I c The cosine similarity (c) represents the cosine similarity between two vectors. The index c of the historical feature with the highest similarity to the current feature is obtained using the cosine similarity. * and cosine similarity d * If d * When <τ, then the c-th... * There is a potential loop relationship between the historical pose and the current pose, where τ is a given acceptance threshold. The data of each agent passing through the loop region is obtained based on the acceptance threshold.

[0097] 6) After detecting potential loop closures using the improved Scan Context feature method, ICP matching and registration are performed between the point cloud at the potential loop closure time and the point cloud at the current time, and the transition matrix T is obtained. * And similarity S * If S * If the given acceptance threshold is met, the loop closure detection is considered successful, and the historical frame information, current frame information, and transition matrix T are processed. * And similarity S * Send it to other intelligent agents.

[0098] Step 3: Based on the information obtained from the front end of the multi-machine SLAM system, perform incremental consistency detection using the back end of the multi-machine SLAM system.

[0099] The backend of the multi-machine SLAM system receives loop closure information detected by the frontend and performs Pairwise Consistent Measurement Set Maximization (PCM) detection on the loop closure information. PCM checks the consistency of constraints between agents. If the constraints between any two agents are consistent... and A closed loop is acceptable if the following conditions are met:

[0100]

[0101] in, It represents the pose transformation of agent β within the intelligent body between timestamps l and j. It represents the pose transformation of agent β at timestamp j and agent α at timestamp i. It represents the pose transformation of agent α within the intelligent body between timestamps i and k. is the inverse pose transformation between agent β at timestamp l and agent α at timestamp k, where ∈ is a given threshold. To ensure robust real-time localization, a delayed initialization is used: incremental PCM is not performed until a specified number of cyclic closure candidates are reached.

[0102] Global factor graph optimization is performed: A global factor graph of the loop closure detection regions between each agent is maintained and optimized to obtain the relative poses between agents. All overlapping observation locations are used as nodes in the global factor graph. For loop closures detected by PCM, each loop closure is used as a variable node in the global factor graph. The relative pose transformation between two loop closures is used as a binary factor between the corresponding two variable nodes, ultimately resulting in a factor graph containing global transformation information between each agent. The GTSAM library is used for optimization to obtain the relative poses between agents.

[0103] Step 4: Introduce a dynamic connected graph construction and merging algorithm based on matching similarity constraints.

[0104] Since the matching order cannot be guaranteed during multi-machine SLAM operation, when an agent overlaps with two or more other agents, coordinate transformation confusion occurs, leading to disorder in the final global map. To address this, a dynamic connectivity graph construction and merging algorithm based on matching similarity constraints is introduced. This algorithm progressively merges the factor graphs of multiple agents, maintaining a global factor graph of loop closure detection regions among the agents. The coordinate transformation results obtained from the dynamic merging are used as initial values ​​for subsequent optimization. The specific steps are as follows:

[0105] 1) Initialize the graph structure where each node is not connected according to the number of agents, and use the initial coordinates of each car as the origin of the coordinate system;

[0106] 2) When the pose transformation results of the loop closure points detected by PCM are received, the graph nodes corresponding to the agents where each loop closure point is located are connected to form connected components.

[0107] 3) Use a depth-first search algorithm to detect connectivity in the graph structure. For each connected component, take the first matched agent as the global coordinate origin, and transfer the coordinates of the remaining agents in the connected component to the global coordinate origin according to the transition matrix obtained in step 2, as approximate initial values ​​for subsequent factor graph optimization.

[0108] Preferably, single-machine-to-multi-machine factor graph optimization is performed: the results from the multi-machine model are used to optimize the single-machine SLAM factor graph, achieving global consistency in the coordinate relationships and pose trajectory estimations of each agent in the multi-machine SLAM system. For each agent, nodes and factors related to itself from the multi-machine factor graph are added to its own maintained factor graph, and then factor graph optimization is performed to obtain a globally consistent result, i.e., the overall multi-agent laser SLAM map. The nodes and factors related to itself are... Figure 3 The nodes and factors shown are those whose observations overlap with those of other agents that have inter-agent measurements with themselves.

[0109] The aforementioned 3D LiDAR SLAM system based on a dynamic connected component merging method uses a dynamic connected graph construction and merging algorithm based on matching similarity constraints to merge factor graphs and perform optimization. (See the schematic diagram below.) Figure 3 .

[0110] Preferably, a distributed communication system is built: the communication system framework is as follows Figure 4 As shown, the hardware utilizes the PGY 5G router to meet the requirements of large-scale multi-machine geographically dispersed networking and high bandwidth in the environment. On the software side, the ROS2-foxy Fast-DDS communication middleware is used, and a communication architecture based on ROS topic communication is built through ros1_bridge to realize the distributed transmission of data received at the front end and pose transformation information obtained at the back end in the multi-machine SLAM system.

[0111] The mapping results of the multi-machine 3D laser SLAM method with dynamic merging of connected components proposed in this invention are shown in the figure. Figure 5 .like Figure 5 As shown, this is a point cloud map constructed by four agents using a multi-machine SLAM system.

[0112] This invention proposes a multi-machine 3D laser SLAM system with dynamic merging of connected components, such as... Figure 6 As shown, it includes:

[0113] The pose transformation matrix acquisition module is used to perform loop closure detection based on the laser odometry data of multiple intelligent agents and point cloud map, and to obtain the relative pose transformation matrix between any two intelligent agents.

[0114] A factor graph acquisition module is used to process the relative pose transformation matrix to obtain the factor graphs of each agent.

[0115] The laser SLAM map acquisition module is used to process the factor graphs of each agent using a dynamic connected graph construction and merging algorithm to obtain a multi-agent overall laser SLAM map.

[0116] An embodiment of the present invention provides a terminal device comprising: a processor, a memory, and a computer program stored in the memory and executable on the processor. When the processor executes the computer program, it implements the steps in the various method embodiments described above. Alternatively, when the processor executes the computer program, it implements the functions of each module / unit in the various device embodiments described above.

[0117] The computer program can be divided into one or more modules / units, which are stored in the memory and executed by the processor to complete the present invention.

[0118] The terminal device may be a desktop computer, laptop, handheld computer, or cloud server, etc. The terminal device may include, but is not limited to, a processor and a memory.

[0119] The processor may be a central processing unit (CPU), or other general-purpose processors, digital signal processors (DSPs), application-specific integrated circuits (ASICs), field-programmable gate arrays (FPGAs), or other programmable logic devices, discrete gate or transistor logic devices, discrete hardware components, etc.

[0120] The memory can be used to store the computer program and / or module. The processor implements various functions of the terminal device by running or executing the computer program and / or module stored in the memory and calling the data stored in the memory.

[0121] If the modules / units integrated into the terminal device are implemented as software functional units and sold or used as independent products, they can be stored in a computer-readable storage medium. Based on this understanding, all or part of the processes in the methods of the above embodiments can also be implemented by a computer program instructing related hardware. The computer program can be stored in a computer-readable storage medium, and when executed by a processor, it can implement the steps of the various method embodiments described above. The computer program includes computer program code, which can be in the form of source code, object code, executable files, or certain intermediate forms. The computer-readable medium can include: any entity or device capable of carrying the computer program code, recording media, USB flash drives, portable hard drives, magnetic disks, optical disks, computer memory, read-only memory (ROM), random access memory (RAM), electrical carrier signals, telecommunication signals, and software distribution media, etc. It should be noted that the content included in the computer-readable medium can be appropriately added or removed according to the requirements of legislation and patent practice in the jurisdiction. For example, in some jurisdictions, according to legislation and patent practice, computer-readable media do not include electrical carrier signals and telecommunication signals.

[0122] This invention introduces a dynamic connected graph construction and merging algorithm based on matching similarity constraints to dynamically and progressively merge the global coordinate transformations of multiple agents. It maintains a global factor graph of the loop closure detection regions among the agents and uses the dynamically merged coordinate transformation results as initial values ​​for factor graph optimization to obtain the relative poses between agents. It also supports the addition of new agents with overlapping regions at any time. Principal component analysis based on laser point cloud topology features is introduced to obtain the principal direction, improving the efficiency and accuracy of loop closure detection. Furthermore, by introducing a communication system based on ros1-ros1_bridge-ros2, multiple agents can operate independently and support entry and exit from the system due to communication problems, improving the system's robustness. It has the following advantages: 1) By introducing a dynamic connected graph construction and merging algorithm based on matching similarity constraints, it solves the problem of coordinate transfer confusion when an agent overlaps with two or more other agents, as the matching order cannot be guaranteed during multi-machine SLAM operation. 1) Achieved global consistency of the global map under the condition that any number of agents enter or leave the system and that they are successfully matched with each other, thus improving the adaptability and stability of the entire system when there are a large number of agents; 2) Based on the traditional loop closure detection Scan Context algorithm, a principal component analysis algorithm based on laser point cloud topology features was introduced to determine the main viewpoint, which improved the efficiency and accuracy of the system's loop closure detection; 3) The introduction of a communication system based on ros1-ros1_bridge-ros2 and a 5G communication hardware module enabled multiple agents to exist independently, supported exiting and entering the system due to communication problems, and met the system's communication bandwidth requirements, thus improving the robustness of the system.

[0123] The above are merely preferred embodiments of the present invention and are not intended to limit the present invention. Various modifications and variations can be made to the present invention by those skilled in the art. Any modifications, equivalent substitutions, improvements, etc., made within the spirit and principles of the present invention should be included within the scope of protection of the present invention.

Claims

1. A multi-machine 3D laser SLAM method for dynamic merging of connected components, characterized in that, Includes the following steps: Loop closure detection is performed based on laser odometry data from multiple agents and point cloud maps to obtain the relative pose transformation matrix between any two agents. The relative pose transformation matrix is ​​processed to obtain the factor graphs of each agent; A dynamic connected graph construction and merging algorithm is used to process the factor graphs of each agent to obtain a multi-agent overall laser SLAM map. An improved Scan Context feature method is used to perform loop closure detection on laser odometry data and point cloud maps from multiple agents; The improved Scan Context feature method has the following specific steps: 1) Obtain the current point cloud data of the lidar at the front end, project the lidar data along the Z-axis direction to obtain a two-dimensional point cloud from the bird's-eye view of the lidar. 2) Translate the origin of the two-dimensional point cloud coordinates to the centroid; 3) Perform principal component analysis on the two-dimensional point cloud to obtain three feature vectors representing the three principal axis directions respectively; wherein, in order to ensure that the Z-axis is perpendicular and the three axes are perpendicular to each other, the Y-axis is artificially specified by taking the cross product of the Z-axis and the X-axis, and the side with more point cloud on the X-axis is specified as the positive direction. 4) Divide the point cloud of the LiDAR bird's-eye view into N×M sector grids at equal angles and distances starting from the selected positive X principal axis in polar coordinates. Take the point with the highest Z-axis height in each grid to obtain the improved Scan Context feature of the LiDAR point cloud in the current frame. Store the feature matrix point by point in an N×M kd tree for maintenance. 5) When a new and improved Scan Context feature is received, the nearest neighbor algorithm is used in the kd-tree to retrieve several features that are adjacent to the current feature, and then the cosine similarity between the retrieved features and the current feature is calculated. 6) After detecting potential loop closures using the improved Scan Context feature method, ICP matching and registration are performed between the point cloud at the potential loop closure time and the point cloud at the current time, and the transition matrix is ​​obtained. and similarity If similarity Meet the given acceptance threshold If the loop closure detection is successful, the historical frame information, current frame information, and transition matrix will be stored. and similarity Send it to other intelligent agents.

2. The multi-machine 3D laser SLAM method for dynamic merging of connected components according to claim 1, characterized in that, The laser odometry data of the intelligent agent is obtained from the inertial measurement unit; Obtain a point cloud map of the intelligent agent based on LiDAR data.

3. The multi-machine 3D laser SLAM method for dynamic merging of connected components according to claim 1, characterized in that, The method for calculating cosine similarity is as follows: （1） in, For the current feature matrix, For candidate feature matrices, for The j-th row vector in the middle, The modulus representing the orientation quantity Indicates the number of vectors. The cosine similarity represents the cosine similarity between two vectors; the index of the historical feature with the highest similarity to the current feature is obtained from the cosine similarity. and cosine similarity ;like At that time, the first There is a potential cyclic relationship between the historical pose and the current pose. Given an acceptance threshold, based on the acceptance threshold Obtain the data of each agent traversing the loop region.

4. The multi-machine 3D laser SLAM method for dynamic merging of connected components according to claim 1, characterized in that, The Fast-DDS communication middleware based on ROS2-foxy uses ros1_bridge to build a communication architecture based on ROS topic communication, enabling communication between multiple agents.

5. The multi-machine 3D laser SLAM method for dynamic merging of connected components according to claim 1, characterized in that, The relative pose transformation matrix is ​​processed using a pairwise consistent measurement set maximization detection method; The consistency check satisfies the loop closure condition: （2） in, It is an intelligent agent timestamp and pose changes within the intelligent body. It is an intelligent agent timestamp and intelligent agents timestamp pose transformation between intelligent agents It is an intelligent agent timestamp and pose changes within the intelligent body. It is an intelligent agent timestamp and intelligent agents timestamp Inverse pose transformation between intelligent agents It is a given threshold; All overlapping observation locations are used as nodes in the global factor graph. For processed loop closures, each loop closure is used as a variable node in the global factor graph. The relative pose transformation between two loop closures obtained from loop closure detection is used as a binary factor between the corresponding two variable nodes. Finally, a factor graph containing global transformation information between each agent is obtained.

6. The multi-machine 3D laser SLAM method for dynamic merging of connected components according to claim 5, characterized in that, The method for processing the factor graphs of each agent using a dynamic connected graph construction and merging algorithm based on matching similarity constraints is as follows: 1) Initialize the graph structure where each node is not connected according to the number of agents, with each agent using its own initial coordinates as the origin of the coordinate system; 2) When the pose transformation results of the loop closure points are received, connect the graph nodes corresponding to the agents where each loop closure point is located to obtain the connected components; 3) Use the depth-first search algorithm to detect the connectivity of the graph structure. For each connected component, take the first matched agent as the global coordinate origin and transfer the coordinates of the remaining agents on the connected component to the global coordinate origin according to the transition matrix, which serves as the approximate initial value for subsequent factor graph optimization.

7. A multi-machine 3D laser SLAM system for dynamic merging of connected components, characterized in that, The method described by any one of claims 1 to 6 includes: The pose transformation matrix acquisition module is used to perform loop closure detection based on the laser odometry data of multiple intelligent agents and the point cloud map, and to obtain the relative pose transformation matrix between any two intelligent agents. A factor graph acquisition module is used to process the relative pose transformation matrix to obtain the factor graphs of each agent. The laser SLAM map acquisition module is used to process the factor graphs of each agent using a dynamic connected graph construction and merging algorithm to obtain a multi-agent overall laser SLAM map.

8. A computer device comprising a memory and a processor, wherein the memory stores a computer program, characterized in that, When the processor executes the computer program, it implements the steps of the multi-machine 3D laser SLAM method for dynamic merging of connected components as described in any one of claims 1 to 6.

9. A computer-readable storage medium storing a computer program, characterized in that, When the computer program is executed by the processor, it implements the steps of the multi-machine 3D laser SLAM method for dynamic merging of connected components as described in any one of claims 1 to 6.

Images