Map updating method for high-resolution radar point cloud reconstruction

Abstract

The invention relates to the field of robot point cloud map management, in particular to a map updating method for high-resolution radar point cloud reconstruction. The method comprises the following steps: firstly, aligning point cloud maps under different sessions by adopting a two-step point cloud map alignment mode; then performing high-resolution point cloud reconstruction on the sparse point cloud frame to obtain a pixel-level point cloud frame; performing bidirectional inter-frame change detection by using a high-resolution point cloud frame, and dividing the point cloud; and finally, superposing the divided point clouds to obtain an updated map. According to the method, after point cloud maps under different sessions are aligned through a two-step alignment method, high-resolution point cloud frames are reconstructed for sparse point clouds, description of the point cloud frames on the world environment is approximate to a pixel level, then the point cloud frames obtained through reconstruction are used for inter-frame change detection, bidirectional inter-frame change detection is used, and the inter-frame change detection accuracy is improved. Dividing the point clouds under different sessions, and finally performing map updating by using the divided point clouds. The method not only can solve the cost problem caused by the hardware problem, but also can effectively reduce the operation complexity.

Description

Technical Field

[0001] The present invention relates to the field of robot point cloud map management, and specifically to a map update method for high-resolution radar point cloud reconstruction. Background Art

[0002] Robots are widely used in life, such as: floor-sweeping robots, logistics robots, etc. In laser SLAM (Simultaneous Localization and Mapping), map update is a key step, which involves updating and maintaining the map created by the robot to reflect environmental changes. In practical applications, the environment may change, such as the appearance of moving obstacles, the opening or closing of doors, etc. These changes need to be reflected in the map in a timely manner to ensure that the robot's navigation and decision-making can adapt to environmental changes. Map update can also be used to improve the robot's self-localization by comparing measurement data with a known map to reduce positioning errors. However, in currently open-source algorithms, generally only single mapping is considered, without considering the maintenance of the point cloud map. When the robot performs positioning and navigation on a changed point cloud map, errors will occur, resulting in positioning errors and even serious accidents.

[0003] Incremental SLAM (Incremental Simultaneous Localization and Mapping) is a method for constructing and maintaining maps in SLAM. It gradually updates the map, fuses new sensor observation data in real time, and is used for robot positioning.

[0004] First, the robot uses a laser sensor or other sensors to collect a point cloud of data in the environment. These point clouds of data contain landmarks or feature points around the robot. Second, the robot will match the new point cloud of data with the previous one. This matching process involves feature extraction to extract landmarks or feature points from the data, and then feature matching to estimate the pose of the robot. Next, using an incremental optimization algorithm, the current pose and map of the robot are updated to minimize the matching error as much as possible. This usually involves non-linear optimization, such as graph optimization or extended Kalman filtering. Then, the robot fuses the new pose and map information with the previous pose and map to update the global pose and map. Finally, this process loops continuously to instantaneously update the pose and map of the robot to adapt to the dynamics of the environment. Although incremental SLAM can have the effect of map updating, there are also some technical drawbacks: First, the problem of computational complexity. Over time, the number of maps and data points increases, resulting in high computational complexity. This may lead to latency and real-time issues, especially in large-scale environments. Second, the handling of environmental dynamics. Incremental SLAM is usually based on the assumption of a static environment, and the handling of objects in a dynamic environment (such as moving obstacles) is still a challenge. In addition, sensor noise and errors have a negative impact on map quality and positioning accuracy. Improving sensor calibration and filtering techniques remains a research direction. Moreover, the data association problem. When matching point clouds of data, there is a data association problem, that is, how to associate new data points with landmarks in the previous point cloud of data. Incorrect associations may lead to positioning errors. Summary of the Invention

[0005] The present invention aims to be able to effectively solve the map update problem, and further achieve the accurate positioning and navigation of the robot, and provides a map update method based on low-resolution lidar point clouds, which is a point cloud map update method under different sessions.

[0006] The technical solution adopted by the present invention to achieve the above object is:

[0007] A map update method for high-resolution radar point cloud reconstruction, comprising the following steps:

[0008] 1) Adopt a two-step point cloud map alignment method to align the point cloud maps under different sessions;

[0009] 2) Based on the aligned point cloud map, perform high-resolution point cloud reconstruction on the sparse point cloud frames to obtain pixel-level point cloud frames, that is, high-resolution point cloud frames;

[0010] 3) Perform two-way inter-frame change detection on the high-resolution point cloud frames and divide the point clouds;

[0011] 4) Overlay the segmented point cloud to obtain an updated map.

[0012] Step 1) includes the following steps:

[0013] 1.1) Coarse registration: Obtain the spatial boundary values of the point cloud, divide the point cloud map based on the spatial boundary values, retain a set number of point cloud data in the central area of the map, and use the retained point cloud data to obtain a prior pose of the point cloud map in different sessions.

[0014] 1.2) Fine registration: Use the global point cloud data for registration, and correct the registration result using the prior pose to obtain a transformation pose. Transform one point cloud map to another point cloud map according to the transformation pose to complete the alignment of the point cloud maps in different sessions.

[0015] Step 2) includes the following steps:

[0016] 2.1) Sample the poses corresponding to the frames in the global map to obtain multiple pose data.

[0017] 2.2) Simulate the characteristics of lidar mapping to project the point cloud under the selected pose to complete point cloud reconstruction.

[0018] Step 2.2) includes the following steps:

[0019] 2.2.1) Search in the radius domain according to the effective reflection distance of the lidar, and filter the point cloud that meets the radius projection range into the candidate point cloud set.

[0020] 2.2.2) Number the point cloud in the point cloud set according to the vertical resolution and horizontal resolution to convert the unordered point cloud into an ordered point cloud.

[0021] Step 2.2.2) is specifically:

[0022] Divide the spatial area according to the horizontal angle and vertical angle, and select the point cloud closest to the lidar in each divided grid as the valid point at that position, and discard the remaining points.

[0023] Step 3) includes the following steps:

[0024] 3.1) Obtain the depth matrix corresponding to each frame of the point cloud, and perform differential processing on it to obtain a depth difference matrix.

[0025] 3.2) Use the method of bidirectional mapping to judge whether each element in the depth difference matrix obtained by differentiation is a changing point cloud using an adaptive threshold based on the depth of the corresponding point cloud. Regard the point cloud exceeding the threshold as a changing point cloud, and then divide the point cloud into a changing point cloud and an unchanged point cloud.

[0026] The method of two-way mapping is specifically as follows:

[0027] Using the point cloud frame generated from the prior map raw as the reference frame, perform change detection on the point cloud frame generated from the current map new, and extract the point cloud new(add) that has changed relative to the prior map. The remaining unchanged point cloud is denoted as new(res):

[0028] new = new(add) + new(res)

[0029] Using the point cloud frame generated from the current map as the reference frame, perform change detection on the point cloud frame generated from the prior map, and extract the point cloud raw(keep) that has not changed relative to the current map. The remaining changed point cloud is denoted as raw(res):

[0030] raw = raw(keep) + raw(res).

[0031] The specific content of step 4) is as follows:

[0032] Add the point cloud newly added relative to the prior map and the point cloud of the unchanged part relative to the current map as the new map New_map, that is

[0033] New_map = new(add) + raw(keep).

[0034] The present invention has the following beneficial effects and advantages:

[0035] The present invention aims at low-resolution lidar, and then simulates the effect of high-resolution lidar to realize point cloud change detection between frames and then perform map update. This method can not only solve the cost problem brought by hardware problems, but also effectively reduce the computational complexity. Brief Description of the Drawings

[0036] Figure 1 Flowchart of the invention;

[0037] Figure 2 Schematic diagram of point cloud alignment results, (a) Alignment effect of other methods, (b) Alignment effect of the method proposed by the present invention;

[0038] Figure 3 Comparison diagram of the original point cloud frame and the new frame after reconstruction, (a) Original lidar point cloud frame, (b) Lidar point cloud frame after reconstruction;

[0039] Figure 4 Detection result map of the current map;

[0040] Figure 5 Detection result map of the prior map;

[0041] Figure 6Result map of the new map. Detailed implementation mode

[0042] The present invention will be further described in detail below with reference to the accompanying drawings and embodiments.

[0043] A method for high-resolution radar point cloud reconstruction, which uses a low-resolution lidar to simulate the performance of a high-resolution lidar. Specifically, it includes the following aspects: First, using a two-step point cloud map alignment can improve the accuracy of point cloud map alignment. Second, using this method can use a low-resolution lidar (such as: 16 lines) to reconstruct high-resolution point cloud frames, which has an obvious effect improvement during map update, and at the same time greatly reduces the hardware cost. Third, when using this method to reconstruct high-resolution point clouds, the point cloud can be reconstructed according to actual needs, without being limited to the number of lidar lines on the market, and at the same time, the resolution of the ROI area can be improved to achieve better effects according to requirements. Finally, using two-way change detection enables more reasonable data selection when updating the point cloud map. Specifically:

[0044] (1) System structure

[0045] A flowchart of a high-resolution point cloud reconstruction method is as Figure 1 shown:

[0046] First, adopt a two-step point cloud map alignment method to align the point cloud maps in different sessions; then perform high-resolution point cloud reconstruction on the sparse point cloud frames to obtain pixel-level point cloud frames; then use the high-resolution point cloud frames to perform two-way inter-frame change detection to divide the point cloud; finally, superimpose the divided point clouds to obtain an updated map. After aligning the point cloud maps in different sessions through the two-step alignment method of the present invention, for sparse point clouds, high-resolution point cloud frames are reconstructed, so that the description of the point cloud frames for the world environment is approximately pixel-level. Then, use the reconstructed point cloud frames to perform inter-frame change detection, and use a two-way inter-frame change detection to divide the point clouds in different sessions. Finally, use the divided point clouds to update the map.

[0047] (2) Specific steps

[0048] 1. First, adopt a two-step point cloud map alignment method to align the point cloud maps in different sessions;

[0049] This method is based on map updates in different sessions. Therefore, the maps in different sessions are first aligned. Due to the large amount of data in the point cloud map, a two-step alignment method is used for point cloud alignment: When performing rough alignment, the spatial boundary values of the point cloud are first obtained, and the point cloud map is divided based on these spatial boundary values, retaining a small part of the point cloud data in the central area of the map. Then, a prior pose of the point cloud map in different sessions is obtained using this part of the data. Based on this pose, the global map is finely aligned. The fine alignment uses the global point cloud data, and the transformation pose is obtained after registration. One point cloud map is transformed to another point cloud map according to the pose transformation, that is, the alignment of the point cloud maps in different sessions is completed.

[0050] 2. Then, high-resolution point cloud reconstruction is performed on the sparse point cloud frames to obtain pixel-level point cloud frames;

[0051] The high-resolution point cloud frames are generated using the global map generated by the low-resolution lidar. Since the data of the global map has been obtained, it is not necessary to generate the point cloud frames very densely when generating the data. Therefore, when performing high-resolution point cloud frame reconstruction, first, the pose corresponding to the frame when obtaining the global map is sampled, and a corresponding pose data is sampled every five meters. Then, the characteristics of lidar mapping are simulated to project the point cloud to the selected pose. When projecting, first, a radius domain search is performed according to the effective reflection distance of the lidar, and the point clouds that meet the radius projection range are screened into the candidate point cloud set. The point clouds are numbered according to the vertical resolution and horizontal resolution to convert the unordered point cloud into an ordered point cloud. The specific operation is to divide the spatial area according to the horizontal angle and vertical angle. For each grid after division, the point cloud closest to the lidar is selected as the valid point at that position, and the rest of the points are discarded. When performing high-resolution point cloud frame reconstruction, since the lidar point cloud is manually numbered, its resolution can be freely selected without being generated according to the fixed number of lines of the lidar on the market. Moreover, in order to better utilize the complete data characteristics of the global point cloud, the point cloud resolution in the corresponding area can be increased according to the ROI (region of interesting) to achieve better data effects.

[0052] 3. Then, two-way inter-frame change detection is performed using the high-resolution point cloud frames to divide the point cloud;

[0053] Based on point cloud map alignment and high-resolution point cloud reconstruction, frame-by-frame change detection is performed on the corresponding high-resolution point cloud frames one by one. When performing frame-by-frame change detection, a depth difference matrix is used to screen the point cloud of the changed part. First, the depth matrix corresponding to each frame of point cloud is obtained, and then the depth matrices of the corresponding frames are differentiated to obtain the depth difference matrix. Then, considering the characteristic that the greater the depth, the greater the grid distance, an adaptive dynamic threshold is set to determine whether the point cloud belongs to the changed point cloud. When the data in the depth difference matrix is detected to exceed this threshold, the point will be extracted, and then the changed point cloud of each frame is obtained. At the same time, considering the complete detection of map changes and the non-real-time nature of map updates, two-way mapping change detection is used during change detection: First, the point cloud frame generated from the prior map (raw) is used as the reference frame to perform change detection on the point cloud frame generated from the current map (new). The depth difference matrix is used to detect and extract the point cloud that is the near view and the far view relative to the reference point cloud from the current point cloud frame. This part of the point cloud includes two parts: one part is the newly added near-view points relative to the prior map, and the other part is the newly added far-view points relative to the prior map. However, both parts of the point cloud need to be added to the prior map, denoted as: new(add). Therefore, when using the prior map as the reference, the current map can be represented by the following formula (1):

[0054] new= new(add) + new(res) (1)

[0055] Similarly, using the point cloud frame generated from the current map as the reference, change detection is performed on the point cloud frame generated from the prior map, and the part that has not changed relative to the reference frame is screened out. This part is the point cloud part that should be retained, denoted as: raw(keep). Therefore, when using the current map as the reference, the representation form of the prior map is as shown in formula (2):

[0056] raw = raw(keep) + raw(res) (2)

[0057] 4. Finally, the divided point clouds are superimposed to obtain the updated map.

[0058] The representation of the new map is: The sum of the point cloud newly added relative to the prior map and the point cloud that has not changed relative to the current map is the new map, as shown in formula (3):

[0059] New_map = new(add) + raw(keep) (3)

[0060] Embodiment

[0061] 1. First, a two-step point cloud map alignment method is adopted to align the point cloud maps in different sessions;

[0062] This method is based on map updates in different sessions. Therefore, the maps in different sessions are first aligned. Due to the large amount of data in the point cloud map, a two-step alignment method is used for point cloud alignment: When performing rough alignment, the spatial boundary values of the point cloud are first obtained, and the point cloud map is divided based on these spatial boundary values, retaining a small part of the point cloud data in the central area of the map. Then, a prior pose of the point cloud map in different sessions is obtained using this part of the data. Based on this pose, the global map is finely aligned. The fine alignment uses the global point cloud data, and the transformation pose is obtained after registration. One point cloud map is transformed to another point cloud map according to the pose transformation, that is, the alignment of the point cloud maps in different sessions is completed. The result of the point cloud alignment is as Figure 2 shown:

[0063] 2. Then, high-resolution point cloud reconstruction is performed on the sparse point cloud frames to obtain pixel-level point cloud frames;

[0064] The high-resolution point cloud frames are generated using the global map generated by the low-resolution lidar. Since the data of the global map has been obtained, it is not necessary to generate the point cloud frames very densely when generating the data. Therefore, when performing high-resolution point cloud frame reconstruction, the poses corresponding to the frames when obtaining the global map are first sampled, and a pose data is sampled every five meters; then, the characteristics of lidar mapping are simulated to project the point cloud to the selected pose. When projecting, a radius domain search is first performed according to the effective reflection distance of the lidar, and the point clouds that meet the radius projection range are screened into the candidate point cloud set. The point clouds are numbered according to the vertical resolution and horizontal resolution, and the unordered point clouds are converted into ordered point clouds. The specific operation is to divide the spatial area according to the horizontal angle and vertical angle, and the point cloud closest to the lidar in each divided grid is selected as the effective point at that position, and the rest of the points are discarded. When performing high-resolution point cloud frame reconstruction, since the lidar point cloud is numbered manually, its resolution can be freely selected without having to generate it according to the fixed number of lines of the lidar on the market. Moreover, in order to better utilize the complete data characteristics of the global point cloud, the point cloud resolution in the corresponding area can be increased according to the ROI (region of interesting) to achieve better data effects. The original frame and the reconstructed effect diagram are as Figure 3 shown:

[0065] 3. Then, two-way inter-frame change detection is performed using the high-resolution point cloud frames to divide the point cloud;

[0066] Based on point cloud map alignment and high-resolution point cloud reconstruction, frame-by-frame change detection is performed on corresponding high-resolution point cloud frames one by one. When performing frame-by-frame change detection, a depth difference matrix is used to screen the point cloud of the changed part. First, the depth matrix corresponding to each frame of point cloud is obtained, and then the depth matrices of the corresponding frames are differentiated to obtain the depth difference matrix. Then, considering the characteristic that the grid distance is larger for larger depths, an adaptive dynamic threshold is set to determine whether the point cloud belongs to the changed point cloud. The setting of this threshold is shown in Equation (4):

[0067]

[0068] When it is detected that the data in the depth difference matrix exceeds this threshold, the point will be extracted, and then the changed point cloud of each frame is obtained.

[0069] At the same time, considering the complete detection of map changes and the non-real-time nature of map updates, two-way mapping change detection is used during change detection: First, the point cloud frame generated from the prior map (raw) is used as the reference frame to perform change detection on the point cloud frame generated from the current map (new). The depth difference matrix is used to detect and extract the point cloud that is the near view and the far view relative to the reference point cloud from the current point cloud frame. This part of the point cloud includes two parts: one part is the newly added near-view points relative to the prior map, and the other part is the newly added far-view points relative to the prior map. However, both parts of the point cloud need to be added to the prior map, denoted as: new(add). Therefore, when using the prior map as the reference, the result obtained according to Equation (1) is as Figure 4 shown:[[]]

[0070] Similarly, using the point cloud frame generated from the current map as the reference, change detection is performed on the point cloud frame generated from the prior map, and the part that has not changed relative to the reference frame is screened out. This part is the point cloud part that should be retained, denoted as: raw(keep). Therefore, when using the current map as the reference, the result obtained according to Equation (2) is as Figure 5 shown:[[]]

[0071] 4. Finally, the divided point clouds are superimposed to obtain the updated map.

[0072] The new map is represented as: the sum of the point cloud newly added relative to the prior map and the point cloud that has not changed relative to the current map is the new map. The result obtained according to Equation (3) is as Figure 6 shown.

Claims

1. A map update method for high-resolution radar point cloud reconstruction, characterized in that, It includes the following steps: 1) Adopt a two-step point cloud map alignment method to align point cloud maps under different sessions; 2) Based on the aligned point cloud map, perform high-resolution point cloud reconstruction on sparse point cloud frames to obtain pixel-level point cloud frames, i.e., high-resolution point cloud frames; 3) Perform bidirectional inter-frame change detection on high-resolution point cloud frames and divide the point clouds; 4) Superimpose the divided point clouds to obtain an updated map.

2. The method for updating a map for high-resolution radar point cloud reconstruction according to claim 1, wherein, The step 1) includes the following steps: 1.1) Coarse registration: Obtain the spatial boundary values of the point cloud, divide the point cloud map based on the spatial boundary values, retain a set number of point cloud data in the central area of the map, and use the retained point cloud data to obtain a prior pose of the point cloud map under different sessions; 1.2) Fine registration: Use the global point cloud data for registration, and use the prior pose to correct the registration result to obtain a transformation pose. Transform one point cloud map to another point cloud map according to the transformation pose to complete the alignment of point cloud maps under different sessions.

3. A method for updating a map for high-resolution radar point cloud reconstruction according to claim 2, characterized in that, The step 2) includes the following steps: 2.1) Sample the poses corresponding to the frames in the global map to obtain multiple pose data; 2.2) Simulate the characteristics of lidar mapping to project the point cloud to the selected poses to complete point cloud reconstruction.

4. A method for updating a map for high-resolution radar point cloud reconstruction according to claim 3, characterized in that, The step 2.2) includes the following steps: 2.2.1) Search in the radius domain according to the effective reflection distance of the lidar, and filter the point clouds that meet the radius projection range into the candidate point cloud set; 2.2.2) Number the point clouds in the point cloud set according to the vertical resolution and horizontal resolution to convert the unordered point clouds into ordered point clouds.

5. A method for updating a map for high-resolution radar point cloud reconstruction according to claim 4, characterized in that, The step 2.2.2) is specifically: Divide the spatial region according to the horizontal angle and vertical angle, and select the point cloud closest to the lidar in each divided grid as the valid point at that position, and discard the rest of the points.

6. A method for updating a map for high-resolution radar point cloud reconstruction according to claim 1, characterized in that, The step 3) includes the following steps: 3.1) Obtain the depth matrix corresponding to each frame of the point cloud and perform differential processing on it to obtain a depth difference matrix; 3.2) Use the bidirectional mapping method to use an adaptive threshold based on the depth of the corresponding point cloud to judge whether each element in the depth difference matrix obtained by the difference is a changing point cloud. Regard the point clouds exceeding the threshold as changing point clouds, and then divide the point clouds into changing point clouds and unchanged point clouds.

7. A method for updating a map in high-resolution radar point cloud reconstruction according to claim 6, characterized in that, The bidirectional mapping method is specifically: Use the point cloud frame generated from the prior map raw as the reference frame to perform change detection on the point cloud frame generated from the current map new, and extract the point cloud new(add) that has changed relative to the prior map. The remaining unchanged point clouds are denoted as new(res): new = new(add) + new(res) Use the point cloud frame generated from the current map as the reference frame to perform change detection on the point cloud frame generated from the prior map, and extract the point cloud raw(keep) that has not changed relative to the current map. The remaining changing point clouds are denoted as raw(res): raw = raw(keep) + raw(res).

8. A method for updating a map for high-resolution radar point cloud reconstruction according to claim 1 or 7, characterized in that, The step 4) is specifically: Add the point cloud newly added relative to the prior map to the point cloud that has not changed relative to the current map, and use it as the new map New_map, that is New_map = new(add) + raw(keep).

Images