Three-dimensional laser radar repositioning method based on key frame map

CN119001664APending Publication Date: 2024-11-22ROSIWIT TECHNOLOGY CO LTD
View PDF 0 Cites 0 Cited by

Patent Information

Application Number
CN202411096474.0
Authority / Receiving Office
CN · China
Patent Type
Applications(China)
Current Assignee / Owner
Filing Date
2024-08-12
Publication Date
2024-11-22

AI Technical Summary

Technical Problem

[0003]本发明提出的一种基于关键帧地图的三维激光雷达重定位方法,解决了现有的问题

Benefits of technology

[0022] The beneficial effects of the present invention are: allowing the robot to be moved to another place in the shutdown state without causing initial relocation failure. The relocation method based on a priori maps and a more efficient automatic relocation process can be achieved after two screenings. Afterwards, a more accurate initialization repositioning pose is obtained, no manual intervention is required, ensuring the accuracy and speed of repositioning, and greatly improving the efficiency of the robot's automatic work.

✦ Generated by Eureka AI based on patent content.

Smart Images

  • Figure CN119001664A_ABST
    Figure CN119001664A_ABST
Patent Text Reader

Abstract

The invention discloses a three-dimensional laser radar relocation method based on a key frame map, relates to the technical field of robot slam, and adopts a self-developed map format, a calculation mode of a matching degree between frames of a 3D laser radar is firstly adopted, and then a relocation method of secondary screening of a 2D binary grid map is adopted. The method specifically comprises the following steps: step 1, loading a map; 2, analyzing a map; step 3, inputting a laser frame; step 4, traversing all key frames; 5, constructing a binary grid map; step 6, point cloud conversion; 7, calculating a matching degree; and step 8, selecting the pose with the highest matching degree. The method has the beneficial effects that based on a relocation mode of a prior map and a more efficient automatic relocation process, a more accurate initialized relocation pose is obtained after two times of screening, manual intervention is not needed any more, the relocation accuracy is ensured, and the automatic working efficiency of the robot is improved.
Need to check novelty before this filing date? Find Prior Art

Description

Technical Field

[0001] The invention relates to the field of robot slam technology, and in particular to a three-dimensional laser radar repositioning method based on a key frame map. Background Art

[0002] In the current robotics field, when performing positioning based on a priori maps, an initial approximate position needs to be given manually, which is also called the relocalization process. Currently, most of the industry uses manual operations or records the current posture in real time during the operation of the robot. In this way, when the robot is turned on again, the last saved pose is loaded for initialization relocalization. If the robot is moved to another place while it is turned off, the initialization relocalization will fail. At this time, manual intervention is required, which cannot guarantee the accuracy and speed of relocalization, and greatly affects the efficiency of the robot's automatic work. Summary of the invention

[0003] The present invention proposes a three-dimensional laser radar repositioning method based on a key frame map, which solves the existing problems.

[0004] In order to achieve the above purpose, the present invention adopts the following technical scheme: a three-dimensional laser radar relocation method based on a key frame map, using a self-developed map format, first adopting a method of calculating the matching degree between 3D laser radar frames, and then adopting a relocation method of secondary screening of a 2D binary grid map to obtain a more accurate initialization relocation posture, specifically comprising the following steps:

[0005] Step 1: Load the map: Load the established map. The map is built using our company's self-developed map building algorithm and saved in our company's self-developed map format.

[0006] Step 2: Analyze the map: extract the information required by the key frames and stitch them together to form a global point cloud map;

[0007] Step 3: Input laser frame lasersacn: Input the laser frame lasersacn scanned by the current laser radar;

[0008] Step 4: Traverse all key frames: filter and downsample the voxel of the laser point cloud of the current frame, and then match the laser point cloud of the current frame with each key frame, and finally obtain multiple post-selected poses;

[0009] Step 5: Construct a binary grid map: Construct a binary grid map with a grid size of 0.1m, that is, there are only occupied points and idle points in the grid map, and the initial grid map is full of idle points;

[0010] Step 6: Point cloud conversion: convert the current frame point cloud from 3D point cloud to 2D point cloud according to the method in step 5;

[0011] Step 7: Calculate the matching degree: transform the 2D point cloud into the grid map coordinate system and calculate the matching degree between the two.

[0012] Step 8. Select the pose with the highest match: Select the pose with the highest match in step 7 as the current relocalization pose.

[0013] Preferably, the map is based on a map format developed by the company, a priori map has been established, and point clouds and poses of key frames are saved in the map.

[0014] Preferably, the specific steps of step 2 are: extracting the key frame point clouds and the key frame poses, and splicing all the key frame point clouds together to form a large global point cloud map.

[0015] Preferably, the laser frame lasersacn is one of the commonly used information types in the robot operating system, and is used to transmit and process laser radar data.

[0016] Preferably, the specific calculation method of step 4 is as follows: assuming that there are a points in the current frame, of which there are b points on the ground and c points on the ceiling, then the points currently involved in calculating the matching degree are abc points in total, and the pose P is obtained after matching. The point cloud of the current frame is aligned with the key frame through pose conversion, and then the matching points are found among the points involved in the matching degree calculation. If a point in the key frame can be found within a distance of l around a point in the current frame, it is recorded as a valid point, and the distance l is the size of the voxel filtering area. In this way, a total of d valid points can be selected, and the final matching degree between the two frames is

[0017] Preferably, first determine whether the matching degree is greater than a threshold, set the threshold to 0.7 in advance, and then compare the pose P obtained by matching the current frame and the key frame with the pose P of the key frame. t The pose P of the current frame in the global map coordinate system can be obtained t ′=P t +P, that is, the coordinates of the point cloud of the current frame under the global map, that is, the candidate pose of the current positioning. By traversing all key frames, multiple candidate poses can be obtained.

[0018] Preferably, the specific steps of step five are: filtering according to the z-axis of the point cloud coordinates, using only points between -0.5<z<0.5, and then setting the z-axis of the point cloud coordinates to 0, converting the 3D point cloud into a 2D point cloud, and the initial grid map is full of idle points. When the point cloud hits the grid, the grid state is set to occupied.

[0019] Preferably, the binary grid map does not have grids in unknown states and ray casting processes.

[0020] Preferably, the specific steps of step seven are: traversing all candidate poses obtained in step four, converting the 2D point cloud in step six to the grid map coordinate system in step five, and calculating the matching degree between the 2D point cloud and the grid map.

[0021] Preferably, the specific calculation method of the matching degree is: assuming that there are a points in the 2D point cloud, if a point hits the occupied grid of the grid map, it is recorded as a valid point, and there are b valid points in total, then the matching degree is

[0022] The beneficial effects of the present invention are: even if the robot is moved to another place in the shutdown state, it will not cause the initialization repositioning failure. The repositioning method based on the prior map and the more efficient automatic repositioning process can obtain a more accurate initialization repositioning posture after two screenings, and no longer require manual intervention, thereby ensuring the accuracy and speed of repositioning and greatly improving the efficiency of the robot's automatic work. BRIEF DESCRIPTION OF THE DRAWINGS

[0023] Figure 1 It is a flow chart of the relocation system of the present invention. DETAILED DESCRIPTION

[0024] The technical solutions in the embodiments of the present invention will be described clearly and completely below in conjunction with the drawings in the embodiments of the present invention. Obviously, the described embodiments are only part of the embodiments of the present invention, rather than all the embodiments.

[0025] Reference Figure 1 As shown in the figure, a 3D laser radar relocalization method based on key frame map uses a self-developed map format. First, the matching degree between 3D laser radar frames is calculated, and then a relocalization method of secondary screening of 2D binary grid map is used to obtain a more accurate initial relocalization posture. Specifically, the following steps are included:

[0026] Step 1: Load the map: Load the established map. The map is built using our company's self-developed map building algorithm and saved in our company's self-developed map format.

[0027] Step 2: Analyze the map: extract the information required by the key frames and stitch them together to form a global point cloud map;

[0028] Step 3: Input laser frame lasersacn: Input the laser frame lasersacn scanned by the current laser radar;

[0029] Step 4: Traverse all key frames: filter and downsample the voxel of the laser point cloud of the current frame, and then match the laser point cloud of the current frame with each key frame, and finally obtain multiple post-selected poses;

[0030] Step 5: Construct a binary grid map: Construct a binary grid map with a grid size of 0.1m, that is, there are only occupied points and idle points in the grid map, and the initial grid map is full of idle points;

[0031] Step 6: Point cloud conversion: convert the current frame point cloud from 3D point cloud to 2D point cloud according to the method in step 5;

[0032] Step 7: Calculate the matching degree: transform the 2D point cloud into the grid map coordinate system and calculate the matching degree between the two.

[0033] Step 8. Select the pose with the highest match: Select the pose with the highest match in step 7 as the current relocalization pose.

[0034] The map is based on the company's self-developed map format. A priori maps have been established, and the point clouds and poses of key frames are saved in the map. The specific steps of step 2 are: extract the key frame point clouds and the poses of key frames, and stitch all the key frame point clouds together to form a large global point cloud map. Laser frames lasersacn are one of the commonly used information types in robot operating systems, which are used to transmit and process lidar data.

[0035] This scheme adopts GICP proposed in the paper "Generalized-ICP" as the matching algorithm. The traditional matching degree calculation is based on the average distance from each point in the current frame to the nearest point in the key frame after matching as the basis for matching degree calculation, and sets an empirical value to obtain the normalized matching degree. This scheme proposes a new matching degree calculation method. The specific calculation method of step four is: assuming that there are a points in the current frame, including b points on the ground and c points on the ceiling, then the points currently involved in the matching degree calculation are abc points in total. After matching, the pose P is obtained, and the point cloud of the current frame is aligned with the key frame through pose conversion, and then the matching points are found among the points involved in the matching degree calculation. If a point in the key frame can be found within a distance of l around a point in the current frame, it is recorded as a valid point. The distance l is the size of the voxel filtering area. In this way, a total of d valid points can be selected, and the final matching degree between the two frames is First, determine whether the matching degree is greater than the threshold. Set the threshold to 0.7 in advance. According to the pose P matched between the current frame and the key frame and the pose P of the key frame, t The pose P of the current frame in the global map coordinate system can be obtained t ′=P t +P, that is, the coordinates of the point cloud of the current frame under the global map, that is, the candidate pose of the current positioning. By traversing all key frames, multiple candidate poses can be obtained.

[0036] The specific steps of step five are as follows: filter according to the z-axis of the point cloud coordinates, only use points between -0.5<z<0.5, then set the z-axis of the point cloud coordinates to 0, convert the 3D point cloud into a 2D point cloud, and the initial grid map is full of idle points. When the point cloud is hit in the grid, the grid state is set to occupied. Compared with the traditional grid map, the binary grid map does not have unknown grid states and Ray casting processes, so the construction process is faster and more efficient.

[0037] The specific steps of step 7 are: traverse all the candidate poses obtained in step 4, transform the 2D point cloud in step 6 into the grid map coordinate system in step 5, and calculate the matching degree between the 2D point cloud and the grid map. The specific calculation method of the matching degree is: assuming that there are a points in the 2D point cloud, if a point hits the occupied grid of the grid map, it is recorded as a valid point. There are b valid points in total, then the matching degree is

[0038] When the present invention is used, a keyframe map is input on an established map. After traversing all keyframes, the current laser frame is matched with the keyframe to determine whether the matching degree is greater than a threshold. If the matching degree is greater than the threshold, it is recorded as a candidate pose, and all candidate poses obtained by traversing are recorded. At the same time, all keyframe point clouds are spliced ​​to form a global large map, a 2D grid map is constructed, and the laser frame is compressed into a 2D point cloud. Finally, the point cloud and the grid map under the current pose are matched to determine whether the traversal is completed. If the traversal is not completed, it returns to traverse all candidate poses again. If the traversal is completed, the pose with the highest matching degree is selected as the current repositioning pose, thereby obtaining the initial positioning pose;

[0039] Even if the robot is moved to another place in the off state, it will not cause the initialization repositioning to fail. The repositioning method based on the prior map and the more efficient automatic repositioning process can obtain a more accurate initialization repositioning posture after two screenings. Human intervention is no longer required, ensuring the accuracy and speed of repositioning, greatly improving the efficiency of the robot's automatic work.

[0040] The above description is only a preferred specific implementation manner of the present invention, but the protection scope of the present invention is not limited thereto. Any technician familiar with the technical field can make equivalent replacements or changes according to the technical scheme and inventive concept of the present invention within the technical scope disclosed by the present invention, which should be covered by the protection scope of the present invention.

Claims

1. A three-dimensional laser radar relocalization method based on key frame map, characterized in that: First, the matching degree between 3D lidar frames is calculated, and then the relocation method of secondary screening of 2D binary grid map is adopted to obtain a more accurate initial relocation pose, which specifically includes the following steps: Step 1: Load the map: Load the established map. The map is built using the self-developed map building algorithm and saved in the self-developed map format. Step 2: Analyze the map: extract the information required by the key frames and stitch them together to form a global point cloud map; Step 3: Input laser frame lasersacn: Input the laser frame lasersacn scanned by the current laser radar; Step 4: Traverse all key frames: filter and downsample the voxel of the laser point cloud of the current frame, and then match the laser point cloud of the current frame with each key frame, and finally obtain multiple post-selected poses; Step 5: Construct a binary grid map: Construct a binary grid map with a grid size of 0.1m, that is, there are only occupied points and idle points in the grid map, and the initial grid map is full of idle points; Step 6: Point cloud conversion: convert the current frame point cloud from 3D point cloud to 2D point cloud according to the method in step 5; Step 7: Calculate the matching degree: transform the 2D point cloud into the grid map coordinate system and calculate the matching degree between the two. Step 8. Select the pose with the highest match: Select the pose with the highest match in step 7 as the current relocalization pose.

2. The three-dimensional laser radar repositioning method based on key frame map according to claim 1 is characterized in that: The map is based on a self-developed map format, a priori maps have been established, and point clouds and poses of key frames are saved in the map.

3. The three-dimensional laser radar repositioning method based on key frame map according to claim 1 is characterized in that: The specific steps of step 2 are: extracting the key frame point clouds and the key frame poses, and splicing all the key frame point clouds together to form a large global point cloud map.

4. The three-dimensional laser radar repositioning method based on key frame map according to claim 1 is characterized in that: The laser frame lasersacn is one of the commonly used information types in the robot operating system, and is used to transmit and process laser radar data.

5. The three-dimensional laser radar repositioning method based on key frame map according to claim 1 is characterized in that: The specific calculation method of step 4 is as follows: assuming that there are a points in the current frame, including b points on the ground and c points on the ceiling, then the points currently involved in the calculation of the matching degree are abc points in total. After matching, the pose P is obtained. The point cloud of the current frame is aligned with the key frame through pose conversion, and then the matching points are found among the points involved in the matching degree calculation. If a point in the key frame can be found within a distance of l around a point in the current frame, it is recorded as a valid point. The distance l is the size of the voxel filtering area. In this way, a total of d valid points can be selected, and the final matching degree between the two frames is 6. The three-dimensional laser radar repositioning method based on key frame map according to claim 5 is characterized in that: First, determine whether the matching degree is greater than the threshold. Set the threshold to 0.7 in advance. According to the pose P matched between the current frame and the key frame and the pose P of the key frame, t The pose P of the current frame in the global map coordinate system can be obtained t ′=P t +P, that is, the coordinates of the point cloud of the current frame under the global map, that is, the candidate pose of the current positioning. By traversing all key frames, multiple candidate poses can be obtained.

7. The three-dimensional laser radar repositioning method based on key frame map according to claim 1 is characterized in that: The specific steps of step five are: filtering according to the z-axis of the point cloud coordinates, only using points between -0.5<z<0.5, and then setting the z-axis of the point cloud coordinates to 0, converting the 3D point cloud into a 2D point cloud, and the initial grid map is full of idle points. When the point cloud hits the grid, the grid state is set to occupied.

8. The three-dimensional laser radar repositioning method based on key frame map according to claim 1 is characterized in that: The binary grid map does not have a grid with an unknown state and a Ray casting process.

9. The three-dimensional laser radar repositioning method based on key frame map according to claim 1, characterized in that: The specific steps of step seven are: traversing all candidate poses obtained in step four, converting the 2D point cloud in step six into the grid map coordinate system in step five, and calculating the matching degree between the 2D point cloud and the grid map.

10. The three-dimensional laser radar repositioning method based on key frame map according to claim 9, characterized in that: The specific calculation method of the matching degree is: assuming that there are a points in the 2D point cloud, if a point hits the occupied grid of the grid map, it is recorded as a valid point. There are b valid points in total, then the matching degree is