Multi-machine distributed collaborative mapping method and system based on laser radar and IMU (Inertial Measurement Unit)

Abstract

The invention provides a multi-machine distributed collaborative mapping method and system based on a laser radar and an IMU (Inertial Measurement Unit), relates to the technical field of automatic driving, and aims to solve the problem that in the prior art, dynamic point cloud can influence the accuracy of measured data; the head carrier is high in information processing capacity and high in calculation difficulty; communication and mapping of other single machines can be influenced by communication disconnection or load collapse of the single machines; the sensor is high in cost and is interfered by a complex closed environment. According to the method, after laser point cloud data and IMU information are received, key frame point cloud data and pose data are generated through first-time loop-back matching of single machines, messages are forwarded to a back-end server through a communication module based on ROS + UDP, second-time loop-back matching between different single machines is carried out through a server side, and a global map is constructed and optimized. According to the method, the problems in the prior art are solved, and the accurate global pose estimation and the optimized global map are obtained.

Description

Technical Field

[0001] The present invention belongs to the technical field of autonomous driving, and particularly relates to a multi-robot distributed collaborative mapping method and system based on lidar and IMU. Background Art

[0002] The statements in this section merely provide background technical information related to the present invention and do not necessarily constitute prior art.

[0003] Simultaneous Localization and Mapping (SLAM) is a technology that estimates the position of a robot or device in real time using sensors (such as lidar, cameras, IMUs, etc.) in an unknown environment while constructing a map of that environment. SLAM technology enables robots to navigate autonomously without relying on pre-constructed maps, effectively cope with dynamic and complex environmental changes, and is widely used in fields such as driverless vehicles, drones, and service robots.

[0004] In the fields of autonomous driving and robotics, based on lidar, by fusing multi-sensor information such as inertial measurement units (IMUs) and wheel speed encoders, the Simultaneous Localization and Mapping (SLAM) technology can be used to estimate the pose of a moving vehicle during its movement and construct a scene point cloud map, thereby realizing the survey and modeling of the actual scene environment.

[0005] However, in the process of point cloud mapping and localization based on lidar, the presence of dynamic point clouds has many negative impacts. On the one hand, point cloud mapping algorithms are based on static environmental features, and dynamic point clouds will seriously interfere with the accuracy of lidar measurement data. In an actual scene, when the lidar scans, it may encounter dynamic objects such as vehicles and pedestrians. The point cloud information reflected by these dynamic objects will change as their positions change. The algorithm is prone to misclassifying the point clouds of dynamic objects as part of the static environmental features, thereby introducing a large amount of error information and seriously reducing the accuracy of odometry, localization, and mapping.

[0006] Meanwhile, for large-scale and extreme scenarios, multi-robot collaborative mapping is often adopted. For example, Chinese Patent CN117289298B discloses a multi-robot collaborative online mapping method, system and terminal device based on lidar. This method often involves the concepts of a master carrier and slave carriers. The master carrier serves as the core of the algorithm and communication, and all the information of the slave carriers is transmitted to the master carrier. However, the master carrier can only be equipped with a development board or an industrial computer, and its data processing ability cannot reach the computing level of a dedicated PC host or server. Moreover, as the master carrier is the core of the algorithm, the stability of the system will be reduced. Once the computing load of the master carrier exceeds its capacity, resulting in downtime or disconnection due to communication problems, the entire system will stop working. Existing solutions often communicate based on the communication framework of ROS itself. The communication framework of ROS itself requires a Master node as the core. Once the Master crashes, the entire multi-robot communication system will collapse, causing all nodes relying on it for communication to malfunction. These two reasons lead to poor overall robustness of the system.

[0007] In addition, existing technical solutions often use a large amount of multi-sensor information. On the one hand, the cost is relatively high. On the other hand, sensors similar to GPS require remote information, or the communication method depends on the cloud, making it vulnerable to interference in complex and enclosed environments. Summary of the Invention

[0008] To overcome the above-mentioned deficiencies of the prior art, the present invention provides a multi-robot distributed collaborative mapping method and system based on lidar and IMU. This method is based on the distributed deployment of lidar and IMU. Each robot adopts the ROS CORE core, and through two-loop matching, point cloud registration, and pose optimization, it overcomes the impact of a single robot's downtime or disconnection on the real-time mapping of other robots, improves the accuracy of positioning and mapping, and thus realizes the construction and optimization of the overall global map.

[0009] To achieve the above object, one or more embodiments of the present invention provide the following technical solutions:

[0010] The first aspect of the present invention provides a multi-robot distributed collaborative mapping method based on lidar and IMU, including:

[0011] S1. Each single robot acquires point cloud data and IMU data;

[0012] S2. Input the point cloud data and IMU data into the front-end odometer to obtain real-time point cloud frame data and initial pose estimation;

[0013] S3. Based on the real-time point cloud frame data and initial pose estimation, perform the first loop matching between the data of each single robot itself to obtain the local map point cloud frame data and optimized pose data of each single robot;

[0014] S4. Input the point cloud frame data of each single - machine local map and the optimized pose data into the server - side for data parsing;

[0015] S5. Based on the data parsing results, adopt the method of first performing the second loop - closure matching between different single - machines, and then performing point cloud registration and pose optimization to construct and optimize the global map.

[0016] As an implementation, input the point cloud data and IMU data into the front - end odometer to obtain real - time point cloud frame data and initial pose estimation. The specific process is as follows:

[0017] Parse the point cloud data and IMU data through the development board, and input the parsed data into the front - end odometer module;

[0018] The front - end odometer adopts a front - end odometer framework algorithm that combines dynamic point cloud removal and iterative error - state Kalman filtering to obtain real - time point cloud frame data and initial pose estimation.

[0019] As an implementation, the front - end odometer adopts a front - end odometer framework algorithm that combines dynamic point cloud removal and iterative error - state Kalman filtering to obtain real - time point cloud frame data and initial pose estimation. The process is as follows:

[0020] Adopt dynamic point cloud removal technology for the parsed data to obtain the point cloud data after dynamic point cloud processing, that is, real - time point cloud frame data;

[0021] Fuse the IMU data and the point cloud data after dynamic point cloud processing through an iterative extended Kalman filter for pose estimation to obtain the initial pose estimation.

[0022] As an implementation, adopt dynamic point cloud removal technology for the parsed data to obtain the point cloud data after dynamic point cloud processing. The specific process is as follows:

[0023] Divide the three - dimensional space into multiple voxels, and each voxel is a small three - dimensional unit;

[0024] Map each point in the parsed point cloud data to the corresponding voxel according to its spatial position to obtain the mapped voxel point cloud;

[0025] Based on the restriction - promotion criterion, divide the mapped voxel point cloud into several objects;

[0026] Merge adjacent points that belong to different objects simultaneously to obtain the segmented objects;

[0027] Based on the point cloud frames at adjacent moments, calculate the overlap rate of the segmented object point cloud between the point cloud frame at the previous moment and the point cloud frame at the next moment;

[0028] Reject the dynamic point clouds with an overlap rate less than the first specific threshold to obtain the point cloud data after processing the dynamic point clouds.

[0029] As an implementation, the IMU data and the point cloud data after processing the dynamic point clouds are fused through an iterative extended Kalman filter for pose estimation to obtain an initial pose estimation. The specific process is as follows:

[0030] According to the point cloud data after processing the dynamic point clouds, calculate the Jacobian matrix, and combine the Jacobian matrices of all feature points to obtain a large matrix;

[0031] Based on the large matrix, calculate the Kalman gain;

[0032] According to the Kalman gain and the residual, iteratively update the pose state estimation until the pose state estimation is less than the second specific threshold, which is the initial pose estimation.

[0033] As an implementation, based on the real-time point cloud frame data and the initial pose estimation, perform the first loop matching between the own data of each single machine to obtain the local map point cloud frame data and the optimized pose data of each single machine. The specific process is as follows:

[0034] Adopt the Scan Context algorithm to extract the descriptors of each frame;

[0035] Use the feature matching between the descriptors to determine whether the single machine returns to the area it has passed. If so, construct a pose graph structure;

[0036] According to the pose graph structure, where the nodes represent the poses of the single machine at different times, the edges represent the constraint relationships between the nodes, and the adjacent edges represent the pose relationships between the initially estimated adjacent times;

[0037] In the pose graph structure, based on the pose constraints between the loop frames, generate the local map point cloud frame data and the optimized pose data of each single machine by adjusting the poses of the nodes.

[0038] As an implementation, input the local map point cloud frame data and the optimized pose data of each single machine into the server side. The specific process is as follows:

[0039] Combine the point cloud frame data and the optimized pose data into a real-time message, and define the message format through Protobuf;

[0040] Each single machine uses the UDP method in ROS to send the real-time message to the server side.

[0041] As an implementation, based on data parsing, perform the second loop matching and point cloud registration between different single machines. The specific process is as follows:

[0042] Extract descriptors for each individual machine from the local map point cloud frame data of each individual machine, and construct a set of descriptors for each individual machine;

[0043] Traverse the set of descriptors of non-self individual machines, and calculate the relative translation and rotation of non-self individual machines;

[0044] Perform point cloud registration on the current frame descriptor with the smallest relative translation and the historical frame descriptor, and calculate the similarity;

[0045] If the similarity reaches the third specific threshold, the current frame and the historical frame match, then parse the relative offset and rotation, and put them into the factor graph for pose optimization;

[0046] If the similarity does not reach the third specific threshold, repeat the second loop matching, point cloud registration and pose optimization steps between different individual machines until an optimized global map is generated.

[0047] The second aspect of the present invention provides a multi-machine distributed collaborative mapping system based on lidar and IMU, including:

[0048] A data acquisition module for each individual machine to acquire point cloud data and IMU data;

[0049] A first loop matching module between self-data for inputting the point cloud data and IMU data into the front-end odometer to obtain real-time point cloud frame data and initial pose estimation;

[0050] Based on the real-time point cloud frame data and initial pose estimation, perform the first loop matching between self-data for each individual machine to obtain local map point cloud frame data and optimized pose data for each individual machine;

[0051] A global map optimization module for inputting the local map point cloud frame data and optimized pose data of each individual machine into the server side for data parsing;

[0052] Based on the data parsing result, construct and optimize the global map by first performing the second loop matching between different individual machines, then performing point cloud registration and pose optimization.

[0053] The third aspect of the present invention provides a terminal device, the terminal device is a computer, an unmanned vehicle, a drone, an unmanned driving device or a mobile robot, the terminal device includes a memory, a processor and a program stored on the memory and executable on the processor, and when the processor executes the program, it implements the steps in the method described in the first aspect of the present invention.

[0054] The above one or more technical solutions have the following beneficial effects:

[0055] In this embodiment, a distributed deployment solution is adopted, and there is no longer a distinction between the head carrier and the sub-carriers. All the robotic dogs process their respective sensor information, construct local maps, and then send the information to the server side respectively. The more powerful computing power of the server side is utilized to construct the global map, solving the problems of large amount of head carrier information processing and difficult to reach the required computing height.

[0056] In this embodiment, all the robotic dogs have a ROS CORE core. The communication disconnection or load crash of a single robotic dog will not affect the communication and mapping of other dogs, solving the problem that the downtime of the head carrier is likely to cause the entire system to crash; The ROS+UDP method is used to replace the native ROS communication method, getting rid of the drawback that ROS itself requires a Master node and improving the overall robustness of the system.

[0057] In this embodiment, the front-end odometer adopts a front-end odometer framework algorithm that combines dynamic point cloud removal and iterative error state Kalman filtering. It can identify potential dynamic objects in the front and rear frame point clouds, calculate the position overlap rate of the objects in the front and rear frame point clouds. If the overlap rate is too low, the object is considered a dynamic object and the dynamic point cloud data is removed, overcoming the interference of dynamic point clouds on the accuracy of lidar measurement data in the actual scenario, avoiding the introduction of a large amount of error information, and improving the accuracy of positioning and mapping.

[0058] In this embodiment, the loop closure matching is divided into the loop closure matching of the robotic dog's own data and the loop closure matching between different robotic dogs. Among them, the loop closure matching of the robotic dog itself is deployed on each front-end robotic dog, and the loop closure matching between different robotic dogs is deployed on the server. It can not only establish reliable single-machine self-loop constraints to obtain accurate self-pose estimation and single-machine local maps, but also establish multi-machine loop constraints to obtain accurate global pose estimation and global maps.

[0059] In this embodiment, the sensor is only based on the information of 3D lidar and IMU, builds its own local area network, and can be offline remotely throughout the process, and can be used for tasks such as multi-robotic dog formation and navigation.

[0060] Advantages of additional aspects of the present invention will be partly given in the following description, partly will become obvious from the following description, or will be learned through the practice of the present invention. BRIEF DESCRIPTION OF THE DRAWINGS

[0061] The specification drawings constituting a part of the present invention are used to provide a further understanding of the present invention. The schematic embodiments of the present invention and their descriptions are used to explain the present invention and do not constitute an improper limitation to the present invention.

[0062] Figure 1This is the overall scheme flowchart of the multi-robot distributed collaborative mapping method based on lidar and IMU in the first embodiment;

[0063] Figure 2 This is the flowchart of the single-robot mapping method in the first embodiment;

[0064] Figure 3 This is the flowchart of the multi-robot distributed collaborative mapping method based on lidar and IMU in the first embodiment. Detailed implementation manners

[0065] It should be noted that the following detailed description is exemplary and is intended to provide further explanation of the present invention. Unless otherwise specified, all technical and scientific terms used herein have the same meaning as commonly understood by those of ordinary skill in the technical field to which the present invention belongs.

[0066] It should be noted that the terms used herein are only for describing specific implementation manners and are not intended to limit the exemplary embodiments according to the present invention.

[0067] In the case of no conflict, the embodiments in the present invention and the features in the embodiments may be combined with each other.

[0068] First embodiment

[0069] This embodiment discloses a multi-robot distributed collaborative mapping method based on lidar and IMU.

[0070] To more clearly illustrate this embodiment, the implementation process of a multi-robot distributed collaborative mapping based on lidar and IMU can be specifically described as follows:

[0071] A multi-robot distributed collaborative mapping method based on lidar and IMU includes:

[0072] S1. Each single robot acquires point cloud data and IMU data;

[0073] S2. Input the point cloud data and IMU data into the front-end odometer to obtain real-time point cloud frame data and initial pose estimation;

[0074] S3. Based on the real-time point cloud frame data and initial pose estimation, perform the first loop matching between the data of each single robot itself to obtain the local map point cloud frame data and optimized pose data of each single robot;

[0075] S4. Input the local map point cloud frame data and optimized pose data of each single robot into the server for data parsing;

[0076] S5. Based on the data parsing result, first perform the second loop matching between different single robots, and then perform point cloud registration and pose optimization to construct and optimize the global map.

[0077] In this embodiment, a distributed deployment technical solution is adopted. The front-end module is mounted on each robotic dog. The software part includes: a front-end odometry calculation method module, an odometer loop closure module, a local pose optimization module, and a communication module. The hardware part includes a 3D lidar, an IMU, a development board, and a power supply. Among them, the 3D lidar acquires 3D lidar point cloud data, the IMU acquires preliminary pose data, and the development board runs the front-end algorithm to construct their respective local maps. The back-end module is mounted on the server and runs the algorithm part including the communication module, the global loop closure module, and the global pose optimizer module. In addition, there is also a WIFI router part to build a local area network.

[0078] In this embodiment, after the 3D lidar and IMU carried by the robotic dog receive the lidar point cloud data and IMU information, the front-end odometry calculation method module, the odometer loop closure module, and the local pose optimization module are used to achieve a relatively accurate pose estimation in the local coordinate system, generate key frame point cloud data and pose data, subscribe to the topics of the two data, process the messages, fuse the two data into a message format, and publish it to the communication module based on ROS+UDP. This communication module forwards the message to the back-end server via the local area network built on WIFI.

[0079] After the back-end server receives the messages of all robots, it generates corresponding geometric descriptor data for each frame of data, and then retrieves and matches the latest frame of descriptor data received by the current robot with the historical descriptor data of other robots. When the similarity of the descriptor data of different robots is relatively large, a loop closure frame is generated. By calculating the rotation amount and translation amount of the two frames of descriptors, the relative pose transformation of the two frames of point cloud frames can be determined, and then the local map data of the robots corresponding to the point cloud frames can be corrected, and the local maps between the robots can be fused to generate a multi-robot corrected global map.

[0080] As Figure 1 、 Figure 2 shown, in step S1, each single machine acquires point cloud data and IMU data.

[0081] In this embodiment, the 3D lidar carried by each single machine acquires 3D lidar point cloud data, and the inertial measurement unit IMU acquires instantaneous attitude and acceleration data.

[0082] At the same time, the sensors 3D lidar and IMU transmit the data to the development board on the robotic dog in the form of a flight path and publish it to relevant topics.

[0083] The development board runs the front-end algorithm to construct their respective local maps.

[0084] After the above steps, each robot will obtain the unprocessed raw point cloud data and IMU data based on its respective sensors, which facilitates subsequent point cloud preprocessing and the construction of a single robot map by the front-end algorithm. Among them, the input of data to the development board based on the flight path can ensure the stability of data transmission in complex environments.

[0085] As Figure 1 、 Figure 2 shown, in step S2, the point cloud data and IMU data are input into the front-end odometer to obtain real-time point cloud frame data and an initial pose estimate.

[0086] In this embodiment, the specific process is as follows:

[0087] S2-1. Parse the point cloud data and IMU data through the development board, and input the parsed data into the front-end odometer module.

[0088] The development board driver subscribes to relevant topics to parse the sensor data, and uses the parsed data as the input to the front-end odometer module.

[0089] S2-2. The front-end odometer adopts a front-end odometer framework algorithm that combines dynamic point cloud removal and iterative error state Kalman filtering to obtain real-time point cloud frame data and an initial pose estimate.

[0090] S2-2-1. Apply the dynamic point cloud removal technology to the parsed data to obtain the point cloud data after dynamic point cloud processing, that is, real-time point cloud frame data.

[0091] In this embodiment, during the LiDAR mapping process, the presence of dynamic objects (such as vehicles and pedestrians) will interfere with the measurement results, thereby reducing the accuracy of the odometer, positioning, and mapping. Specifically, when the LiDAR scans a moving vehicle, the dynamic point cloud generated by it may be misinterpreted as part of the static environment. When performing positioning and mapping based on the point cloud data subsequently, this misjudgment will introduce significant errors, thereby affecting the accuracy and reliability of the entire mapping system. To improve the mapping accuracy, it is necessary to perform preprocessing of dynamic point cloud removal on the LiDAR data to eliminate the interference caused by dynamic objects.

[0092] First, perform object segmentation, specifically:

[0093] (1) Perform voxel segmentation to divide the three-dimensional space into multiple voxels. Each voxel is a small three-dimensional unit that can contain one or more points. Each point in the point cloud is mapped to the corresponding voxel according to its spatial position.

[0094] (2) Based on the restriction-promotion criterion, divide the mapped voxel points into several objects.

[0095] Perform intensity voxel clustering based on the restriction-promotion criterion to segment the point cloud into multiple objects.

[0096] The materials and surface characteristics of different objects are different, and the intensities of the reflected lasers also vary. Through intensity voxel clustering, the point cloud can be segmented into multiple objects based on this difference.

[0097] (3) Merge adjacent points that belong to different objects simultaneously to obtain the segmented objects.

[0098] In this embodiment, when merging adjacent points, for each point in a voxel, find the adjacent voxels of the voxel where it is located. The points in the adjacent voxels are considered potential neighbor points because they are spatially adjacent. If the adjacent points already belong to different objects, then merge these objects. This is because adjacent points are very likely to belong to the same object, and merging the objects can more completely describe the shape of the object. If a point is unclassified, then assign it to the object of the adjacent points. In this way, the existing classification information can be used to reasonably classify new points. If both adjacent points are unclassified, then create a new object.

[0099] Secondly, for the segmented objects, based on the point cloud frames at adjacent moments, calculate the overlap rate of the position of each object point cloud between the point cloud frame at the previous moment and the point cloud frame at the next moment. If the overlap rate is low, then it is considered to be the point cloud of a high-dynamic object and is separated and removed.

[0100] The process of calculating the overlap rate is as follows:

[0101] (1) Place the two frames of point clouds in the same coordinate system for comparison.

[0102] Transform the point cloud of the current frame to the coordinate system of the previous frame, T pre ,T next are the IMU poses of the two frames, and the formula is:

[0103]

[0104] (2) Calculate the overlap rate.

[0105]

[0106] Among them, matched_voxels are the same voxels of the two frames of point clouds, and #total_voxels is the total number of voxels.

[0107] (3) Remove the dynamic point cloud with an overlap rate less than the first specific threshold to obtain the point cloud data after processing the dynamic point cloud.

[0108] If the overlap rate is too low, i.e., below the first specific threshold, it is considered as dynamic point cloud and is removed to obtain the point cloud data after dynamic point cloud processing and the real-time point cloud frame.

[0109] Among them, the first specific threshold is set according to the point cloud data of the actual scene.

[0110] S2-2-2. Perform pose estimation by fusing IMU data and the point cloud data after dynamic point cloud processing through an iterative extended Kalman filter to obtain the initial pose estimation.

[0111] (1) Calculate the Jacobian matrix according to the point cloud data after dynamic point cloud processing.

[0112] The Jacobian matrix is the partial derivative of the measurement model with respect to the state estimate, reflecting how the residual changes with the state estimate. Since the measurement model is non-linear, it is linearized at the current state estimate and the Jacobian matrix The calculation formula is:

[0113]

[0114] Update the pose state estimate according to the Jacobian matrix.

[0115] Combine the Jacobian matrices of all feature points into a large matrix H for subsequent calculations.

[0116] (2) Calculate the Kalman gain K based on the large matrix.

[0117] The calculation formula is:

[0118] K = (H T R -1 H + P -1 ) -1 H T R -1 (4)

[0119] Among them, R is the covariance matrix of the measurement noise, and P is the covariance matrix of the state prediction.

[0120] (3) Iteratively update the pose state estimate according to the Kalman gain and the residual until the pose state estimate is less than the second specific threshold, which is the final initial pose estimate. Among them, the second specific threshold is set according to the actual point cloud data situation.

[0121] The formula is:

[0122]

[0123] Among them, is the residual vector, J κare the relevant partial derivatives.

[0124] Repeat steps (1), (2), and (3) to calculate the Jacobian matrix, Kalman gain, and update the state estimate until the update amount of the state estimate is less than a preset second specific threshold ∈. The formula is:

[0125]

[0126] It is considered that the state estimate has stabilized. After convergence, the final pose state estimate, that is, the initial pose estimate, can be obtained.

[0127] (4) Use the obtained result to update the current frame of point cloud to the global coordinate system and perform the final update of the local point cloud map.

[0128] After the above steps, each robot will construct a single-robot local map based on its respective sensor data. Among them, Kalman filtering is a recursive least squares algorithm that can dynamically estimate the state of the system according to the IMU data and radar data of the system. Through two steps of prediction and update, the estimate of the robot's pose state is continuously corrected to reduce errors.

[0129] Such as Figure 1 , Figure 2 As shown, in step S3, based on the real-time point cloud frame data and the initial pose estimate, the first loop matching between its own data of each single robot is performed to obtain the point cloud frame data and the optimized pose data of each single-robot local map.

[0130] In this embodiment, the first loop matching between the data of the robot itself is performed. Subscribe to the point cloud frame data and the preliminary pose data, and use the feature extraction method to generate descriptors for loop matching. The specific process is as follows:

[0131] (1) Use the Scan Context algorithm to extract the descriptors of each frame.

[0132] The generation of descriptors uses the Scan Context algorithm to generate the descriptors of each frame of point cloud in real time.

[0133] The specific process is as follows:

[0134] 1) It is necessary to initialize the descriptor matrix with a size of (max_ring, max_sector), and the initial value of each position is set to -1000, which is expressed as:

[0135]

[0136] 2) Traverse each point in the point cloud and fill the descriptor matrix. Extract the coordinates (x, y, z) of the point and calculate the polar coordinates of the point on the 2D plane. The formula is:

[0137]

[0138]

[0139] 3) In polar coordinates, calculate the grid index to which the point belongs. The ring index formula and the sector index formula are respectively:

[0140]

[0141] 4) For each point, compare its height z with the current maximum value in the corresponding grid and take the maximum value. The formula is:

[0142] desc(r, s) = max(desc(r, s), z) (10).

[0143] Traverse the descriptor matrix. If the value in a certain grid is still NO_POINT, set it to 0, which is expressed as:

[0144]

[0145] Finally, output and return the generated descriptor matrix desc. Its size is (max_ring, max_sector). Each element desc(r, s) in the matrix represents the maximum height in the corresponding ring and sector.

[0146] (2) Use the feature matching between descriptors to determine whether the single machine returns to the area where it has been. If so, construct a pose graph structure.

[0147] Use the feature matching between descriptors to determine whether the robot returns to the area it has visited before. The specific process is as follows:

[0148] 1) Calculate the mean vector of the maximum heights Sector-Key.

[0149] For each matrix sc1 and sc2 of the Scan Context descriptors, calculate their Sector-Key for quickly aligning the two Scan Context descriptors. Sector-Key is a vector composed of the mean of the maximum heights of each sector. Its size is 1×N, where N is the number of sectors. Let the mean vector of the maximum heights of each sector of the Sector-Key of sc be vkey Sc , the formula is:

[0150]

[0151] Among them, max_ring is the number of rings, and sc[ring][sector] is the height value of the sector in the ring numbered ring in the Scan Context descriptor.

[0152] 2) Cyclically shift vkey sc2 to the right and compare it with vkey sc1 to find a right shift amount argmin_vkey_shift that minimizes the difference between the two. The formula is:

[0153]

[0154] 3) Expand the search space.

[0155] Based on argmin_vkey_shift, expand a search space to the left and right with a search radius of SEARCH_RADIUS. Calculate the closest Scan Context distance. For each offset num_shift in the search space, cyclically shift sc2 to the right by num_shift columns to generate a new descriptor sc2_shifted.

[0156] 4) Distance calculation.

[0157] Calculate the distance between sc1 and sc2_shifted. If cur_sc_dist < min_sc_dist, then update min_sc_dist and argmin_shift.

[0158] sc2_shifted = circshift(sc2, num_shift)

[0159] cur_sc_dist = distDirectSC(sc1, sc2_shifted)

[0160]

[0161] Return the minimum distance min_sc_dist and the optimal offset argmin_shift. If the minimum distance min_sc_dist is less than the threshold 0.3, it is considered to match the historical scene.

[0162] (3) Construct a pose graph structure, where nodes represent the poses of a single machine at different times, edges represent the constraint relationships between nodes, and adjacent edges represent the pose relationships between initially estimated adjacent times.

[0163] Among them, nodes represent the poses of the robot at different moments, and edges represent the constraint relationships between nodes. The pose relationships between adjacent moments are initially estimated for adjacent edges, and at the same time, the pose constraints between loop frames in the above text are added.

[0164] (4) In the pose graph structure, based on the pose constraints between loop frames, by adjusting the poses of the nodes, the point cloud frame data of each single-robot local map and the optimized pose data are generated.

[0165] The loop constraint is to query the current location of the robot and determine whether the current location is similar to a certain location passed through in history. If it is similar, it is considered the same location, and the pose data of the historical location is used to optimize the current location data to eliminate odometry errors. The specific judgment method is as follows

[0166] Suppose the query frame point cloud is S, the loop-matched historical frame point cloud is T, and the points in the point cloud are respectively represented as s i ∈S and t j ∈T. Based on the iterative closest point algorithm, its goal is to find a transformation matrix, and the formula is:

[0167]

[0168] Among them, R is a 3×3 rotation matrix, and t is a 3×1 translation vector.

[0169] According to formula (15), minimize the objective function E(T) based on the sum of Euclidean distances, and the formula is:

[0170]

[0171] Solve the objective function E(T) in formula (10) through an iterative optimization method to obtain the transformation matrix T that minimizes E(T). This T is the transformation matrix between the two frames of point clouds sought. Represent this transformation matrix T as a six-degree-of-freedom relative pose Represent the loop-matched historical frame as the map scan P n , corresponding pose T n , and obtain the pose estimate value of the query scan P Q in the entire map coordinate system through the following transformation Add this pose transformation relationship as a loop constraint to the factor graph for pose optimization. The pose formula is as follows:

[0172]

[0173] The factor graph is generally optimized using the odometry constraints between adjacent frames and the loop closure constraints between the current frame and the loop closure frame. The optimization goal is to adjust the poses of the nodes so that the entire graph satisfies all the constraints, thereby minimizing the error of the entire system and generating relatively accurate single-robot local map point cloud frame data and optimized pose data.

[0174] Due to the pose constraints of adjacent frames generated only by odometry data, when the radar carrier robot is working over a long distance, the odometry data will continuously form cumulative errors, which will cause the point cloud map to deform. After the above steps, through loop closure matching between the current frame and historical frames of the robot, loop closure constraints are established in the factor graph to optimize the cumulative error data of the IMU.

[0175] As Figure 1 、 Figure 3 shown, in step S4, each single-robot local map point cloud frame data and the optimized pose data are input to the server side for data parsing.

[0176] In this embodiment, the ROS+UDP method is used to forward the above-mentioned point cloud frame data and optimized pose data, and each robot sends real-time information to the server side.

[0177] The specific process is as follows:

[0178] (1) Combine the point cloud frame data and the optimized pose data into a real-time message, and define the message format through Protobuf.

[0179] First, subscribe to the ROS topics of both data, combine the two messages into one message, and define the message format through Protobuf.

[0180] (2) Each single robot uses the UDP method in ROS to send the real-time message to the server side.

[0181] Send it to the server through the UDP protocol based on the established local area network.

[0182] After the above steps, define a message format, combine the point cloud frame data and pose data generated by each robot at the same time into one message and send it to the server, ensuring the one-to-one relationship between the two at the same time.

[0183] As Figure 1 、 Figure 3 shown, in step S5, based on the data parsing results, first perform loop closure matching between different single robots for the second time, and then perform point cloud registration and pose optimization to construct and optimize the global map.

[0184] In this embodiment, based on data parsing, a global map is constructed and optimized. Specifically, based on data parsing, a method is adopted in which loop closure matching between different single machines is first performed, and then point cloud registration and pose optimization are carried out.

[0185] Based on data parsing, loop closure matching between different single machines and point cloud registration are carried out. The specific process is as follows:

[0186] (1) From the local map point cloud frame data of each single machine, descriptors of each single machine are extracted, and a descriptor set of each single machine is constructed.

[0187] In this embodiment, when robot 1 receives a new point cloud frame, based on the Ring algorithm, a rotation and translation invariant descriptor Ring1 is generated. The set of historical descriptors stored by robot i is Ringlist_i.

[0188] (2) Traverse the descriptor sets of non-self single machines, and calculate the relative translation amount and rotation amount of non-self single machines.

[0189] In this embodiment, traverse the descriptor sets of non-self single machines Ringlist_i, and match Ring1 with each historical descriptor in each Ringlist_i (i is not 1). For example, in the case of three robots, match the descriptors of robot 1 at each moment with the descriptor sets of robot 2 and robot 3; match the descriptors of robot 2 at each moment with the descriptor sets of robot 1 and robot 3; match the descriptors of robot 3 at each moment with the descriptor sets of robot 1 and robot 2.

[0190] First, convert the candidate loop feature list Ringlist_i into a KDTree data structure for quickly finding the candidate points closest to the current loop feature. Calculate the minimum distance and optimal offset between the candidate point descriptor and the current frame descriptor. The process here is the same as the descriptor feature matching process of a single machine. Calculate the relative translation amount and relative rotation amount between two frames of descriptors of different single machines according to formulas (12)(13)(14).

[0191] (3) Perform point cloud registration on the current frame descriptor and the historical frame descriptor with the minimum relative translation amount, and calculate the similarity.

[0192] If the descriptor of a certain historical frame differs from the descriptor of the current scan frame by the minimum distance min_sc_dist, then the two are the most similar.

[0193] (4) Perform point cloud registration on the historical descriptor obtained in step (3) and the current scan, and calculate the point cloud registration as the similarity. If the similarity reaches the third specific threshold, it is determined that there is a frame match between the two robots, that is, the two robots have passed through the same location. Use this location to optimize the pose relationship between the two robots and their respective local maps, and then combine their respective local maps into a global map.

[0194] When the similarity reaches the third specific threshold, the specific value varies depending on the environment and is generally 0.2, then it is considered that the two match. Analyze the relative offset between the two frames of point clouds, perform pose optimization, reconstruct the positions of their local maps in the global coordinate system, and combine them into an optimized global map.

[0195] The point cloud registration process is carried out in an iterative closest point manner. According to formulas (15) and (16), minimize the objective function E(T) based on the minimum sum of Euclidean distances to calculate the point cloud registration score, the similarity.

[0196] If the registration score reaches the specific threshold, the current frame matches the historical frame. Analyze the relative offset and rotation amount according to formula (17) to obtain the relative pose transformation relationship, and put it into the factor graph for pose optimization.

[0197] (5) If the registration score does not reach the specific threshold, repeat the second loop matching, point cloud registration, and pose optimization steps between different single machines until an optimized global map is generated.

[0198] After the above steps, find the same scenes between the local maps of different robots, and based on this, complete the fusion between the local maps, and then complete the construction of the global map.

[0199] Add the current descriptor to the descriptor set Ringlist_1 of the current robot, and then each robot repeats the process of steps S1 to S5.

[0200] Embodiment 2

[0201] The purpose of this embodiment is to provide a multi-robot distributed collaborative mapping system based on lidar and IMU, including:

[0202] A data acquisition module for each single machine to acquire point cloud data and IMU data;

[0203] A first loop matching module for its own data, which inputs the point cloud data and IMU data into the front-end odometer to obtain real-time point cloud frame data and initial pose estimation;

[0204] Based on real-time point cloud frame data and initial pose estimation, perform the first loop matching between the data of each single machine itself to obtain the local map point cloud frame data and optimized pose data of each single machine.

[0205] A global map optimization module for inputting the local map point cloud frame data and optimized pose data of each single machine to the server side for data parsing.

[0206] Based on the data parsing results, construct and optimize the global map by first performing the second loop matching between different single machines, and then performing point cloud registration and pose optimization.

[0207] Based on a multi-machine distributed collaborative mapping system based on lidar and IMU, implement the method steps in Embodiment 1.

[0208] Embodiment 3

[0209] The purpose of this embodiment is to provide a terminal device, which is a computer, an unmanned vehicle, a drone, an unmanned driving device or a mobile robot. The terminal device includes a memory, a processor, and a program stored on the memory and executable on the processor. When the processor executes the program, the steps in the method described in the first aspect of the present invention are implemented.

[0210] The steps involved in the devices in the above embodiments correspond to those in Method Embodiment 1. For specific implementation manners, reference may be made to the relevant description part of Embodiment 1. The term "computer-readable storage medium" should be understood to include a single medium or multiple media including one or more instruction sets; it should also be understood to include any medium that can store, encode, or carry an instruction set for execution by a processor and enable the processor to execute any method in the present invention.

[0211] Those skilled in the art should understand that the above-mentioned modules or steps of the present invention can be implemented by a general computer device. Optionally, they can be implemented by program codes executable by a computing device, so that they can be stored in a storage device and executed by the computing device, or they can be separately fabricated into individual integrated circuit modules, or multiple modules or steps among them can be fabricated into a single integrated circuit module for implementation. The present invention is not limited to any specific combination of hardware and software.

[0212] Although the specific implementation manners of the present invention have been described above in conjunction with the accompanying drawings, it is not a limitation to the protection scope of the present invention. Those skilled in the art should understand that based on the technical solutions of the present invention, various modifications or deformations that do not require creative labor by those skilled in the art are still within the protection scope of the present invention.

Claims

1. A multi-robot distributed collaborative mapping method based on lidar and IMU, characterized in that, It includes: S1. Each single device acquires point cloud data and IMU data; S2. Input the point cloud data and IMU data into the front-end odometer to obtain real-time point cloud frame data and initial pose estimation; S3. Based on the real-time point cloud frame data and initial pose estimation, perform the first loop matching between the self-data of each single device to obtain the local map point cloud frame data and optimized pose data of each single device; S4. Input the local map point cloud frame data and optimized pose data of each single device into the server for data parsing; S5. Based on the data parsing result, first perform the second loop matching between different single devices, and then perform point cloud registration and pose optimization to construct and optimize the global map.

2. The multi-robot distributed collaborative mapping method based on lidar and IMU according to claim 1, wherein, Input the point cloud data and IMU data into the front-end odometer to obtain real-time point cloud frame data and initial pose estimation. The specific process is as follows: The development board parses the point cloud data and IMU data and inputs the parsed data into the front-end odometer module; The front-end odometer adopts a front-end odometer framework algorithm combining dynamic point cloud removal and iterative error state Kalman filtering to obtain real-time point cloud frame data and initial pose estimation.

3. The multi-robot distributed collaborative mapping method based on lidar and IMU according to claim 2, wherein, The front-end odometer adopts a front-end odometer framework algorithm combining dynamic point cloud removal and iterative error state Kalman filtering to obtain real-time point cloud frame data and initial pose estimation. The process is as follows: Adopt dynamic point cloud removal technology for the parsed data to obtain the point cloud data after dynamic point cloud processing, that is, real-time point cloud frame data; Fuse the IMU data and the point cloud data after dynamic point cloud processing through an iterative extended Kalman filter for pose estimation to obtain the initial pose estimation.

4. A multi-robot distributed collaborative mapping method based on lidar and IMU according to claim 1, characterized in that, Adopt dynamic point cloud removal technology for the parsed data to obtain the point cloud data after dynamic point cloud processing. The specific process is as follows: Divide the three-dimensional space into multiple voxels, and each voxel is a small three-dimensional unit; Map each point in the parsed point cloud data to the corresponding voxel according to its spatial position to obtain the mapped voxel point cloud; Based on the restriction-promotion criterion, divide the mapped voxel points into several objects; Merge the adjacent points that belong to different objects at the same time to obtain the segmented objects; Based on the point cloud frames at adjacent moments, calculate the overlap rate of the segmented object point cloud between the previous moment point cloud frame and the next moment point cloud frame; Eliminate the dynamic point clouds with an overlap rate less than the first specific threshold to obtain the point cloud data after dynamic point cloud processing.

5. A multi-robot distributed collaborative mapping method based on lidar and IMU according to claim 1, characterized in that, Fuse the IMU data and the point cloud data after dynamic point cloud processing through an iterative extended Kalman filter for pose estimation to obtain the initial pose estimation. The specific process is as follows: According to the point cloud data after dynamic point cloud processing, calculate the Jacobian matrix, and combine the Jacobian matrices of all feature points to obtain a large matrix; Based on the large matrix, calculate the Kalman gain; Iteratively update the pose state estimation according to the Kalman gain and the residual until the pose state estimation is less than the second specific threshold, which is the final initial pose estimation.

6. The multi-robot distributed collaborative mapping method based on lidar and IMU according to claim 1, characterized in that Based on the real-time point cloud frame data and the initial pose estimation, perform the first loop matching between the data of each individual device to obtain the local map point cloud frame data and the optimized pose data of each individual device. The specific process is as follows: Adopt the Scan Context algorithm to extract the descriptors of each frame; Use the feature matching between the descriptors to determine whether the individual device returns to the area it has passed. If so, construct a pose graph structure; According to the pose graph structure, where the nodes represent the poses of the individual device at different times, the edges represent the constraint relationships between the nodes, and the adjacent edges represent the pose relationships between the initially estimated adjacent times; In the pose graph structure, based on the pose constraints between the loop frames, generate the local map point cloud frame data and the optimized pose data of each individual device by adjusting the poses of the nodes.

7. A multi-robot distributed collaborative mapping method based on lidar and IMU according to claim 1, characterized in that, Input the local map point cloud frame data and the optimized pose data of each individual device into the server side. The specific process is as follows: Combine the point cloud frame data and the optimized pose data into a real-time message and define the message format through Protobuf; Each individual device uses the UDP method in ROS to send the real-time message to the server side.

8. The multi-robot distributed collaborative mapping method based on lidar and IMU according to claim 1, characterized in that Based on data parsing, perform the second loop matching and point cloud registration between different individual devices. The specific process is as follows: Extract the descriptors of each individual device from the local map point cloud frame data of each individual device and construct a descriptor set for each individual device; Traverse the descriptor sets of non-self individual devices and calculate the relative translation and rotation amounts of non-self individual devices; Perform point cloud registration on the current frame descriptor and the historical frame descriptor with the smallest relative translation amount and calculate the similarity; If the similarity reaches the third specific threshold, the current frame and the historical frame are matched, then parse the relative offset and rotation amounts and put them into the factor graph for pose optimization; If the similarity does not reach the third specific threshold, repeat the second loop matching, point cloud registration, and pose optimization steps between different individual devices until the optimized global map is generated.

9. A multi-robot distributed collaborative mapping system based on lidar and IMU, characterized in that Including: A data acquisition module for each individual device to acquire point cloud data and IMU data; A first loop matching module between the data of the individual device itself, which is used to input the point cloud data and IMU data into the front-end odometer to obtain the real-time point cloud frame data and the initial pose estimation; Based on the real-time point cloud frame data and the initial pose estimation, perform the first loop matching between the data of each individual device itself to obtain the local map point cloud frame data and the optimized pose data of each individual device; A global map optimization module for inputting the local map point cloud frame data and the optimized pose data of each individual device into the server side for data parsing; Based on the data parsing results, adopt the method of first performing the second loop matching between different individual devices, then performing point cloud registration and pose optimization to construct and optimize the global map.

10. A terminal device, where the terminal device is a computer, a driverless vehicle, a drone, a driverless device or a mobile robot, characterized in that, The terminal device includes a memory, a processor, and a program stored on the memory and executable on the processor. The processor executes the steps of the method according to any one of claims 1-8 above.

Images