Multi-device collaborative surveying and mapping method based on collaborative SLAM (Simultaneous Localization and Mapping)

Abstract

The invention provides a multi-device collaborative surveying and mapping method based on collaborative SLAM (Simultaneous Localization and Mapping), which relates to the technical field of computer vision, and comprises the following steps: in any regional scene, each device establishes an initial point cloud map by collecting point cloud data of the regional scene; each device determines a key frame and encodes point cloud data of the key frame into a binary feature map; each device obtains the binary feature map transmitted by the other device, and performs feature matching to obtain a feature matching result; based on a feature matching result, each device sends the distortion-removed point cloud data of the key frame to another device to verify that the point cloud data, corresponding to the key frame scene, of the two devices are consistent, correction of an initial point cloud map is completed, and a target point cloud map of each device is obtained; fusing the target point cloud maps of all devices in the regional scene to obtain a global map corresponding to the regional scene; and converting the global map into a three-dimensional voxel occupation grid based on a three-dimensional encoder so as to complete coordinated surveying and mapping.

Description

Technical Field

[0001] The present invention relates to the field of computer vision technology, and particularly relates to a multi-device collaborative mapping method based on cooperative SLAM. Background Art

[0002] Cutting-edge applications such as robot navigation, augmented reality, and driverless driving are all inseparable from the construction of scene maps. Simultaneous Localization and Mapping (SLAM) technology is a typical mapping technology, and its core is to obtain environmental information through sensors to generate a point cloud map. The point cloud map consists of a set of discrete points, representing the outer surface of an object or a scene. The generation and processing of the point cloud map are relatively simple and are suitable for rapid scanning and visualization of scenes.

[0003] However, since the points in the point cloud map are sparse and can only roughly outline the contours of the objects in the scene, people who are not familiar with the scene cannot have an intuitive understanding of the scene through the point cloud map. Summary of the Invention

[0004] The present invention provides a multi-device collaborative mapping method based on cooperative SLAM, including: in any regional scene, each device establishes an initial point cloud map by collecting the point cloud data of the regional scene; each device determines key frames and encodes the point cloud data of the key frames into binary feature maps; each device obtains the binary feature maps transmitted by another device and performs feature matching to obtain the results of feature matching; based on the results of feature matching, each device sends the undistorted point cloud data of the key frames to another device to verify that the point cloud data of the two devices corresponding to the key frame scene is consistent, and completes the correction of the initial point cloud map to obtain the target point cloud map of each device; fuses the target point cloud maps of all devices in the regional scene to obtain a global map corresponding to the regional scene; based on a three-dimensional encoder, converts the global map into a three-dimensional voxel occupancy grid to complete coordinated mapping.

[0005] In the above solution, converting the global map into a three-dimensional voxel occupancy grid based on a three-dimensional encoder includes: performing voxelization processing on each target point cloud map in the global map to obtain a plurality of voxel grids, where each voxel grid includes a plurality of points; inputting the points of the plurality of voxel grids into a three-dimensional encoder to obtain a plurality of voxel features; inputting the plurality of voxel features into a three-dimensional decoder to obtain a plurality of decoded voxel features; and generating a three-dimensional voxel occupancy grid through an occupancy head from the plurality of decoded voxel features.

[0006] In the above solution, each device determines a key frame, including: when the position or attitude of the device changes beyond a preset threshold, determining that the point cloud data collected at this time is a key frame.

[0007] In the above solution, based on the result of feature matching, each device sends the undistorted point cloud data of the key frame to another device to verify that the point cloud data of the two devices corresponding to the key frame scene is consistent, including: when the feature matching result is consistent, each device selects the key frame represented by the feature map with the closest Hamming distance as the candidate point cloud data for loop closure detection and sends it to another device to verify that the point cloud data of the two devices corresponding to the key frame scene is consistent.

[0008] In the above solution, the method further includes: when the point cloud data of the two devices corresponding to the key frame scene is consistent, the two devices complete the distributed loop closure corresponding to the key frame scene, so that both devices complete the correction of the initial point cloud map and obtain the target point cloud map of each device.

[0009] In the above solution, when the two devices complete the distributed loop closure corresponding to the key frame scene, so that both devices complete the correction of the initial point cloud map and obtain the target point cloud map of each device, it further includes: obtaining the relative pose transformation transmitted by another device, so that both devices complete the correction of the initial point cloud map.

[0010] In the above solution, the 3D encoder is preset, and the 3D encoder includes multiple encoding layers, and each encoding layer includes a 3D convolutional layer, a batch normalization layer, and a ReLU layer.

[0011] In the above solution, inputting the points of multiple voxel grids into the 3D encoder to obtain multiple voxel features includes: after the points in each voxel grid are encoded by multiple encoding layers, the voxel features are obtained through a pooling layer.

[0012] In the above solution, the 3D decoder realizes feature decoding through transposed convolution and splicing.

[0013] In the above solution, the occupancy head includes a fully connected layer, a pooling layer, and a softmax activation function layer.

[0014] The technical solution of the embodiment of the present invention has at least the following beneficial effects:

[0015] (1) When dealing with a scene with a large area, multiple devices cooperate to build a map, significantly improving the surveying and mapping efficiency.

[0016] (2) This method can intuitively understand what objects exist in the scene. By converting the point cloud map into an occupancy grid and combining semantic information, direct observation surveying and mapping of the environment can be realized. BRIEF DESCRIPTION OF THE DRAWINGS

[0017] Figure 1 Schematically shows a flowchart of a multi-device collaborative surveying and mapping method based on cooperative SLAM according to an embodiment of the present invention;

[0018] Figure 2Schematically shows a flowchart of distributed loop detection according to an embodiment of the present invention;

[0019] Figure 3 Schematically shows a flowchart of converting a global map into a three - dimensional voxel occupancy grid based on a three - dimensional encoder according to an embodiment of the present invention;

[0020] Figure 4 Schematically shows a structural diagram of an encoding layer according to an embodiment of the present invention;

[0021] Figure 5 Schematically shows a structural diagram of a three - dimensional encoder according to an embodiment of the present invention;

[0022] Figure 6 Schematically shows a structural diagram of a three - dimensional decoder according to an embodiment of the present invention. Detailed implementation manners

[0023] To make the objectives, technical solutions, and advantages of the present invention clearer and more understandable, the present invention will be further described in detail below with reference to specific embodiments and the accompanying drawings.

[0024] Figure 1 Schematically shows a flowchart of a multi - device collaborative mapping method based on cooperative SLAM according to an embodiment of the present invention.

[0025] Please specifically refer to Figure 1 , the specific process of the multi - device collaborative mapping method based on cooperative SLAM in the embodiment of the present invention includes operations S110 to S160.

[0026] In operation S110, in any regional scene, each device establishes an initial point cloud map by collecting the point cloud data of the regional scene.

[0027] In an embodiment of the present invention, for any regional scene where a scene map needs to be constructed, multiple devices are set in this region. The device can be a terminal device with functions such as shooting, data transmission, and analysis, such as a robot or a drone, etc. In an embodiment of the present invention, no special restrictions are imposed on the specific form of the device.

[0028] Exemplarily, in a region of a fixed size, there are multiple robots. Each robot collects multi - frame point cloud data within the regional scene through sensors carried by itself, such as lidar, cameras, etc., and establishes an initial point cloud map.

[0029] Specifically, each robot extracts environmental feature points from the point cloud data of the sensor, matches the feature points of the current frame with those of the previous frame, estimates the current position and pose of the robot based on the matching result, then updates the environmental map according to the new pose information, and finally performs loop detection to detect whether the robot returns to the previous position and optimize the map. After the above steps, each robot generates an initial point cloud map.

[0030] It should be noted that at this time, within this area, each robot can only obtain the local map collected by itself and determine its own position on the local map, and cannot obtain the global map of exploring this area, nor determine its exact position in the global map.

[0031] In operation S120, each device determines a key frame and encodes the point cloud data of the key frame into a binary feature map.

[0032] In an embodiment of the present invention, each device determining a key frame includes: when the position or pose of the device changes by more than a preset threshold, determining the point cloud data collected at this time as the key frame.

[0033] Exemplarily, each robot first selects a key frame. When the position or pose of the robot changes by more than a preset threshold, it indicates that the motion state of the robot has changed greatly and it may encounter other robots. At this moment, the point cloud data collected by the robot is the point cloud data of the key frame.

[0034] Furthermore, the LiDAR-Iris algorithm is used to encode the point cloud data of the key frame into a binary feature map.

[0035] In an embodiment of the present invention, the robot realizes distributed loop detection through feature exchange. Loop detection refers to the ability of the robot to recognize that it has reached a certain scene and close the map. The distributed loop detection will be described in detail below.

[0036] In operation S130, each device obtains the binary feature map transmitted by another device and performs feature matching to obtain the result of feature matching.

[0037] In operation S140, based on the result of feature matching, each device sends the undistorted point cloud data of the key frame to another device to verify that the point cloud data corresponding to the key frame scene of the two devices is consistent, and completes the correction of the initial point cloud map to obtain the target point cloud map of each device.

[0038] In an embodiment of the present invention, based on the result of feature matching, each device sends the undistorted point cloud data of the key frame to another device to verify that the point cloud data of the two devices corresponding to the key frame scene is consistent, including: when the feature matching result is consistent, each device selects the key frame represented by the feature map with the closest Hamming distance as the candidate point cloud data for loop closure detection and sends it to another device to verify that the point cloud data of the two devices corresponding to the key frame scene is consistent.

[0039] Further, when the point cloud data of the two devices corresponding to the key frame scene is consistent, the two devices complete the distributed loop closure corresponding to the key frame scene, so that both devices complete the correction of the initial point cloud map and obtain the target point cloud map of each device.

[0040] It should be noted that when the two devices complete the distributed loop closure, each device will also obtain the relative pose transformation transmitted by the other device, so that both devices complete the correction of the initial point cloud map and obtain the target point cloud map.

[0041] Figure 2 Schematically shows a flowchart of distributed loop closure detection according to an embodiment of the present invention.

[0042] Exemplarily, as Figure 2 shown, taking two robots (robot a and robot b) as an example, the process of distributed loop closure detection is described in detail. Robot b obtains the binary feature map transmitted by robot a and performs feature matching. If robot b can recognize that it has reached a certain scene based on the feature map, that is, when the feature matching result is consistent, robot b selects the key frame represented by the feature map with the closest Hamming distance as the candidate point cloud data for loop closure detection and sends it to robot a. Then robot a uses the random sample consensus algorithm to verify whether the point clouds collected by robot a and robot b corresponding to the key frame scene are consistent enough. If they are consistent, it means that robot a and b have reached the same scene, and the point cloud maps constructed by the two can complete the distributed loop closure according to this common scene. Robot a will complete the loop closure through verification and send the relative pose transformation of robot a to robot b for robot b to further correct the constructed map, thus completing the distributed loop closure detection.

[0043] In operation S150, fuse the target point cloud maps of all devices in the fusion area scene to obtain the global map corresponding to the area scene.

[0044] In an embodiment of the present invention, in order to obtain the global map corresponding to the area scene, the target point cloud maps of all devices in the area scene are fused.

[0045] Exemplarily, after distributed loop detection, the robot can determine its position in the global map. After further correcting the map through outlier removal and pose graph optimization, the target point cloud maps collected by each robot are superimposed to obtain a fused global map.

[0046] In operation S160, based on the 3D encoder, the global map is converted into a 3D voxel occupancy grid to complete collaborative mapping.

[0047] Based on the above, the global map generated by superimposing the target point cloud maps of multiple robots is relatively sparse and cannot intuitively represent the regional scene. Therefore, in the embodiments of the present invention, a 3D encoder is proposed to convert the point cloud data of the global map into a 3D voxel occupancy grid, that is, the 3D space is divided into a number of square voxel grids. If there is an object in the grid, it is filled with different colors according to the object category, while the unoccupied grid remains blank. The 3D voxel occupancy grid can intuitively represent which objects and their corresponding positions are in the regional scene, and is highly understandable.

[0048] The process of converting the global map into a 3D voxel occupancy grid based on the 3D encoder will be described in detail below.

[0049] Figure 3 FIG. schematically shows a flowchart of converting a global map into a 3D voxel occupancy grid based on a 3D encoder according to an embodiment of the present invention. Figure 4 FIG. schematically shows a structural diagram of an encoding layer according to an embodiment of the present invention. Figure 5 FIG. schematically shows a structural diagram of a 3D encoder according to an embodiment of the present invention.

[0050] Please refer specifically to Figure 3 , the specific process of converting the global map into a 3D voxel occupancy grid based on the 3D encoder in the embodiments of the present invention includes operations S310 to S340.

[0051] In operation S310, each target point cloud map in the global map is voxelized to obtain a plurality of voxel grids, where each voxel grid includes a plurality of points.

[0052] In operation S320, the points of the plurality of voxel grids are input into the 3D encoder to obtain a plurality of voxel features.

[0053] It can be understood that each target point cloud map in the global map is voxelized to obtain a plurality of voxel grids. For example, the point cloud in each target point cloud map includes a 3D space with ranges of W, D, and H along the X, Y, and Z axes respectively. Correspondingly, for voxelization, a size of V D 、V H 、V Wfor each voxel grid, thus dividing the three-dimensional space into several voxel grids, and each voxel grid will contain a different number of points, that is, each voxel grid contains a different number of point clouds.

[0054] Further, input the points of multiple voxel grids into a preset three-dimensional encoder, such as Figure 4 and Figure 5 As shown, in the embodiment of the present invention, the three-dimensional encoder is preset. The three-dimensional encoder includes multiple encoding layers, and each encoding layer includes a three-dimensional convolutional layer, a batch normalization layer, and a ReLU layer. After the points of the voxel grid pass through the three-dimensional convolutional layer, the batch normalization layer, and the ReLU layer of the encoding layer, initial point features are obtained, and then the features are aggregated once through a max pooling layer, and the aggregated features are concatenated with the initial point features to form the output target point features. The output target point features combine point-by-point features and local aggregation features and can describe the shape information of the point cloud.

[0055] Further, input the points of multiple voxel grids into the three-dimensional encoder to obtain multiple voxel features, including: after the points in each voxel grid are encoded by multiple encoding layers, voxel features are obtained through a pooling layer.

[0056] It can be understood that, as Figure 5 shown, the target point features output by each encoding layer are input into another encoding layer, that is, the points in the voxel grid are continuously encoded by stacking the feature encoding layers to deepen the understanding of the shape information of the point cloud. Subsequently, voxel features are obtained through a max pooling layer.

[0057] In operation S330, input multiple voxel features into the three-dimensional decoder to obtain multiple decoded voxel features.

[0058] In operation S340, through the occupancy head, generate a three-dimensional voxel occupancy grid from multiple decoded voxel features.

[0059] Figure 6 Schematically shows the structural diagram of the three-dimensional decoder according to the embodiment of the present invention.

[0060] As Figure 6 shown, in the embodiment of the present invention, the decoder structure is designed based on three-dimensional convolution, and feature decoding is realized through transposed convolution and concatenation. For example, input multiple voxel features into the three-dimensional decoder, and after passing through a fully connected layer, a three-dimensional transposed convolutional layer, a batch normalization layer, and a ReLU layer, multiple decoded voxel features are obtained.

[0061] Further, multiple decoded voxel features enter the occupancy head. In this embodiment, the occupancy head is composed of a fully connected layer, a pooling layer, and a softmax activation function layer. The occupancy head is used to generate the semantic probability of the voxel grid (i.e., the probability that the object in the grid belongs to a certain class), select the class with the highest probability as the object type of the grid and assign the corresponding color. Through the occupancy head, multiple decoded voxel features are used to generate a three-dimensional voxel occupancy grid.

[0062] Through the embodiments of the present invention, a collaborative mapping method for multiple robots based on cooperative SLAM is provided. First, the global point map is collaboratively constructed by the robot interaction key frame features, and then the global map is converted into a three-dimensional voxel occupancy grid through an autoencoder to achieve efficient collaborative mapping.

[0063] The above specific embodiments further elaborate on the purpose, technical solution, and beneficial effects of the present invention. It should be understood that the above are only specific embodiments of the present invention and are not used to limit the present invention. Any modifications, equivalent replacements, improvements, etc. made within the spirit and principle of the present invention shall be included within the protection scope of the present invention.

Claims

1. A multi-device collaborative mapping method based on collaborative SLAM, characterized in that: The method comprises: In any regional scene, each device establishes an initial point cloud map by collecting point cloud data of the regional scene; Each of the devices determines a key frame, and encodes the point cloud data of the key frame into a binary feature map; Each of the devices obtains the binary feature map transmitted by another device, and performs feature matching to obtain a feature matching result; Based on the result of the feature matching, each device sends the dedistorted point cloud data of the key frame to another device to verify that the point cloud data of the two devices corresponding to the key frame scene are consistent, and completes the correction of the initial point cloud map to obtain the target point cloud map of each device; Fusing the target point cloud maps of all devices in the regional scene to obtain a global map corresponding to the regional scene; Based on a 3D encoder, the global map is converted into a 3D voxel occupancy grid to complete coordinated mapping.

2. The multi-device collaborative mapping method based on collaborative SLAM according to claim 1, characterized in that: The converting the global map into a three-dimensional voxel occupancy grid based on a three-dimensional encoder comprises: voxelize each target point cloud map in the global map to obtain a plurality of voxel grids, wherein each voxel grid includes a plurality of points; Inputting the points of the plurality of voxel grids into a three-dimensional encoder to obtain a plurality of voxel features; Inputting the plurality of voxel features into a three-dimensional decoder to obtain a plurality of decoded voxel features; The plurality of decoded voxel features are used to generate a three-dimensional voxel occupancy grid through an occupancy head.

3. The multi-device collaborative mapping method based on collaborative SLAM according to claim 1, characterized in that: Each of the devices determines a key frame, including: When the position or posture change of the device exceeds a preset threshold, the point cloud data collected at this time is determined to be a key frame.

4. The multi-device collaborative mapping method based on collaborative SLAM according to claim 1, characterized in that: Based on the result of the feature matching, each device sends the dedistorted point cloud data of the key frame to another device to verify that the point cloud data of the two devices corresponding to the key frame scene are consistent, including: When the feature matching results are consistent, each device selects the key frame represented by the feature map with the closest Hamming distance as the candidate point cloud data for loop detection and sends it to the other device to verify that the point cloud data corresponding to the key frame scene of the two devices are consistent.

5. The multi-device collaborative mapping method based on collaborative SLAM according to claim 1 or 4, characterized in that: The method further comprises: When the point cloud data of the two devices corresponding to the key frame scene are consistent, the two devices complete the distributed loop corresponding to the key frame scene, so that both devices complete the correction of the initial point cloud map and obtain the target point cloud map of each device.

6. The multi-device collaborative mapping method based on collaborative SLAM according to claim 5, characterized in that: The two devices complete a distributed loop corresponding to the key frame scene, so that both devices complete the correction of the initial point cloud map, and obtain the target point cloud map of each device, and also include: The relative pose transformation transmitted by the other device is obtained so that both devices can complete the correction of the initial point cloud map.

7. The multi-device collaborative mapping method based on collaborative SLAM according to claim 2, characterized in that: The three-dimensional encoder is preset and includes multiple encoding layers, each of which includes a three-dimensional convolutional layer, a batch normalization layer and a ReLU layer.

8. The multi-device collaborative mapping method based on collaborative SLAM according to claim 2 or 7, characterized in that: The step of inputting the points of the plurality of voxel grids into a three-dimensional encoder to obtain a plurality of voxel features comprises: After each point in the voxel grid is encoded by multiple encoding layers, the voxel feature is obtained by a pooling layer.

9. The multi-device collaborative mapping method based on collaborative SLAM according to claim 2, characterized in that: The three-dimensional decoder realizes feature decoding through deconvolution and splicing.

10. The multi-device collaborative mapping method based on collaborative SLAM according to claim 2, characterized in that: The occupancy head includes a fully connected layer, a pooling layer and a softmax activation function layer.

Images