A robot repositioning method and system based on multi-line laser radar

Abstract

The invention discloses a robot repositioning method and system based on multi-line laser radar, belonging to the field of robot positioning technology, and the implementation process of the method is as follows: input key frames and key postures; data preprocessing of the input key frames, including voxel filtering down sampling and extraction of regions of interest; point cloud segmentation, dividing the point cloud data into ground and non-ground parts; generating search features, converting the three-dimensional point cloud into a feature matrix on a plane according to the point cloud label; traversing all key frames; inputting real-time radar and nine-axis IMU data to obtain a matrix; two-step search, first preliminarily screening the loaded key frame feature set by yaw angle, narrowing the search range, and then performing a second search by calculating the matrix similarity method; verification; posture transformation; converting the repositioning transformation matrix into a six-dimensional posture as a repositioning posture output. The invention can complete the autonomous repositioning of the robot without external and manual intervention.

Description

Technical Field

[0001] The present invention relates to the field of robot positioning technology, and in particular to a robot repositioning method and system based on a multi-line laser radar. Background Art

[0002] At present, multi-line laser radar has been widely used in robots, mainly in the fields of mapping, positioning, environmental perception, etc. Among them, positioning is the link between maps and navigation, solving the problem of "where am I" for robots. However, when the robot is initially turned on and the positioning is lost, it often requires external factors or manual intervention to complete repositioning. The external factors here mainly refer to the use of GPS to provide global posture. GPS access often means higher price costs, and when the GPS is blocked, it cannot provide a reliable global posture for the robot. Summary of the invention

[0003] The technical task of the present invention is to address the above shortcomings and provide a robot repositioning method and system based on multi-line laser radar, which can complete the autonomous repositioning of the robot without external and manual intervention.

[0004] The technical solution adopted by the present invention to solve its technical problem is:

[0005] A robot relocalization method based on multi-line laser radar, the implementation process of the method is as follows:

[0006] 1) Input key frames and key poses;

[0007] 2) Data preprocessing of the input keyframes, including voxel filtering downsampling and extraction of regions of interest;

[0008] 3) Point cloud segmentation: dividing the point cloud data into ground and non-ground parts;

[0009] 4) Generate search features, extract non-ground point search features of point cloud, and convert the 3D point cloud into a feature matrix on the XY plane according to the point cloud label;

[0010] 5) Traverse all key frames;

[0011] 6) Input real-time radar and nine-axis IMU data, repeat steps 2), 3), and 4) to get the matrix R c ;

[0012] 7) Two-step search: first, preliminarily screen the loaded key frame feature set by yaw angle to narrow the search range, and then perform a second search by calculating matrix similarity;

[0013] 8) Verification: perform NDT point cloud registration on the candidate key frame obtained in step 7) and the current input key frame; set a score threshold, and if the NDT point cloud registration score is less than the score threshold, the verification is successful;

[0014] 9) Posture transformation: convert the key pose corresponding to the successfully verified candidate key frame into a 4×4 matrix and calculate the relocation transformation matrix;

[0015] 10) Convert the relocation transformation matrix into a six-dimensional pose [x, y, z, roll, pitch, yaw] as the relocation pose output.

[0016] The method first loads the pre-saved keyframes and key poses to generate a keyframe search feature set; secondly, the real-time point cloud frame and nine-axis IMU data are input, and a two-step search method is used to obtain the candidate keyframe that is most similar to the current frame; the candidate keyframe is verified again, and if the verification passes, the coordinate transformation is performed; finally, the relocation result is output. The robot can be autonomously relocated without external GPS and human intervention.

[0017] Preferably, the key frames and key postures are saved when the robot constructs an environment map, usually once every 1 second in a moving state.

[0018] Preferably, the voxel filtering adopts the method provided by the PCL point cloud processing library;

[0019] Extract the area of ​​interest and process it from the distance aspect, setting the minimum radius R min and the maximum radius R max , remove the near and far point clouds, and let the points in the point cloud be represented by P(x, y, z). Then the points in the region of interest satisfy the following conditions:

[0020]

[0021] Preferably, the point cloud is segmented using the ray method, and labels are added to the segmented point clouds, with the ground point cloud being labeled as label1 and the non-ground point cloud being labeled as label2.

[0022] Preferably, the three-dimensional point cloud is converted into a feature matrix on the XY plane according to the point cloud label, and the sector angle resolution to be divided is set. angle , then the number of sectors that divide the entire 3D point cloud can be obtained num , sector angle The values ​​are in radians:

[0023]

[0024] In the formula, π represents pi;

[0025] Traverse the 3D point cloud and calculate the sector to which the non-ground point belongs id First, calculate the angle between the point cloud and the origin of the coordinate system relative to the positive direction of the X-axis. The value of angle is in radians:

[0026]

[0027] Then calculate the sector id :

[0028]

[0029] Count the non-ground points with the smallest radius r in each sector:

[0030]

[0031] And reorganize the data of the statistical point set to construct a row of sectors num Column matrix R p , R p Each column represents the non-ground point with the smallest radius r in each sector, and each column contains four features, namely radius, X-axis coordinate, Y-axis coordinate, and label.

[0032] Preferably, the two-step search,

[0033] First, set the initial search range according to the input yaw angle of the nine-axis IMU c , find the key pose yaw angle corresponding to the key frame k With yaw c The candidate keyframe set with the phase difference angle Δyaw, where the yaw angle is in radians:

[0034] -Δyaw <yaw c -yaw k <Δyaw

[0035] Then, traverse the candidate keyframe set, set the sector rotation step size step, and set R according to the rotation step size. p Perform column translation to obtain R' p , the R' obtained by each column shift p With R c Compare and calculate the similarity, and compare to get the R with the smallest similarity p The corresponding candidate key frame and R p With R c The relative transformation matrix R between Δ :

[0036] R c =R Δ*R p .

[0037] Furthermore, during the verification, the candidate key frame obtained in step 7) is aligned with the current input key frame by NDT point cloud registration. Δ as a priori transformation matrix.

[0038] Furthermore, let the 4×4 matrix be R k , the relocation transformation matrix is ​​R final ,but

[0039] R final =R Δ *R k .

[0040] The present invention also claims a robot repositioning system based on a multi-line laser radar, comprising a key frame and key posture input module, a real-time frame input module, a preprocessing module, a point cloud segmentation module, a generation search feature module, a real-time IMU input module, a two-part search module, a verification module, a posture transformation module and an output repositioning posture module,

[0041] The system realizes robot repositioning through the above-mentioned robot repositioning method based on multi-line laser radar.

[0042] The key frame and key pose input module is used to input key frames and key poses;

[0043] The real-time frame input module and the real-time IMU input module are used for real-time frame input and real-time radar and nine-axis IMU data input;

[0044] The preprocessing module is used to perform data preprocessing on the input key frames, including voxel filtering downsampling and extracting the region of interest;

[0045] The point cloud segmentation module divides the point cloud data into two parts: ground and non-ground;

[0046] The search feature generation module extracts the non-ground point search features of the point cloud and converts the three-dimensional point cloud into a feature matrix on the XY plane according to the point cloud label;

[0047] The two-part search module first preliminarily screens the loaded key frame feature set by yaw angle to narrow the search range, and then performs a second search by calculating matrix similarity;

[0048] The verification module is used to perform NDT point cloud registration on the candidate key frames obtained by the two search modules and the current input key frame; a score threshold is set, and if the NDT point cloud registration score is less than the score threshold, the verification is successful;

[0049] The pose transformation module converts the key pose corresponding to the successfully verified candidate key frame into a 4×4 matrix and calculates the relocation transformation matrix;

[0050] The output relocation pose module is used to convert the relocation transformation matrix into a six-dimensional pose [x, y, z, roll, pitch, yaw] as the relocation pose output.

[0051] The present invention also claims a robot repositioning device based on a multi-line laser radar, comprising: at least one memory and at least one processor;

[0052] The at least one memory is used to store a machine-readable program;

[0053] The at least one processor is used to call the machine-readable program to execute the above method.

[0054] Compared with the prior art, the robot repositioning method and system based on multi-line laser radar of the present invention has the following beneficial effects:

[0055] The method or system first loads the pre-saved keyframes and key poses to generate a keyframe search feature set; secondly, the real-time point cloud frame and nine-axis IMU data are input, and a two-step search method is used to obtain the candidate keyframe that is most similar to the current frame; the candidate keyframe is verified again, and if the verification passes, the coordinate transformation is performed; finally, the relocation result is output. Autonomous relocation of the robot can be achieved without external GPS and human intervention. BRIEF DESCRIPTION OF THE DRAWINGS

[0056] Figure 1 It is a diagram illustrating the implementation process of the robot relocalization method based on multi-line laser radar provided in an embodiment of the present invention. DETAILED DESCRIPTION

[0057] The present invention will be further described below in conjunction with specific embodiments.

[0058] The embodiment of the present invention provides a robot relocation method based on a multi-line laser radar, and the implementation process of the method is as follows:

[0059] 1) Input key frames and key poses. The key frames and key poses are saved when the robot builds an environment map, usually once every 1 second in motion.

[0060] 2) Perform data preprocessing on the input key frames, including voxel filtering downsampling and extracting the region of interest.

[0061] The voxel filtering adopts the method provided by the PCL point cloud processing library, and the voxel size is set to (0.25, 0.25, 0.25) cm;

[0062] The extraction of the region of interest is mainly processed from the distance aspect, setting the minimum radius R min , maximum radius R max The points in the area of ​​interest are 5m and 85m respectively. The near and far point clouds are removed. If the points in the point cloud are represented by P(x, y, z), then the points in the area of ​​interest meet the following conditions:

[0063]

[0064] 3) Point cloud segmentation: divide the point cloud data into two parts: ground and non-ground. The method used is the ray method, and labels are added to the segmented point cloud. The ground point cloud is labeled as label1, and the value of label1 is a constant 5; the non-ground point cloud is labeled as label2, and the value of label2 is a constant 10.

[0065] 4) Generate search features. This part converts the 3D point cloud into a feature matrix on the XY plane according to the point cloud label. The details are as follows:

[0066] Set the sector angle resolution to be divided into sectors angle , here sector angle If set to 0.01745, the number of sectors into which the entire 3D point cloud can be divided is sector num for:

[0067]

[0068] In the formula, π represents pi;

[0069] Traverse the 3D point cloud and calculate the sector to which the non-ground point belongs id , first calculate the angle between the point cloud and the coordinate origin relative to the positive direction of the X-axis:

[0070]

[0071] Then calculate the sector id :

[0072]

[0073] Count the non-ground points with the smallest radius r in each sector:

[0074]

[0075] And reorganize the data of the statistical point set to construct a row of sectors num Column matrix R p , R pEach column represents the non-ground point with the smallest radius r in each sector, and each column contains four features, namely radius, X-axis coordinate, Y-axis coordinate, and label.

[0076] 5) Traverse all key frames.

[0077] 6) Input real-time radar and nine-axis IMU data, repeat steps 2), 3), and 4) to get the matrix R c .

[0078] 7) Two-step search: first, preliminarily screen the loaded key frame feature set by yaw angle to narrow the search range, and then perform a second search by calculating matrix similarity:

[0079] First, set the initial search range according to the input yaw angle of the nine-axis IMU c , find the key pose yaw angle corresponding to the key frame k With yaw c The candidate keyframe set with the phase difference angle Δyaw, where Δyaw is set to 1.57:

[0080] -Δyaw <yaw c -yaw k <Δyaw

[0081] Next, traverse the candidate keyframe set, set the sector rotation step size step, and set R according to the rotation step size. p Shift the columns to get R ′ p , the R obtained by each column shift ′ p With R c Compare and calculate the similarity, and compare to get the R with the smallest similarity p The corresponding candidate key frame and R p With R c The relative transformation matrix R between Δ :

[0082] R c =R Δ *R p .

[0083] 8) Verify, perform NDT point cloud registration on the candidate keyframe obtained in step 7) and the current input keyframe, and Δ As a priori transformation matrix; set the score threshold score th , score th The value is 0.5. If the NDT point cloud registration score is ndt Less than score th , the verification is successful.

[0084] 9) Posture transformation: transform the key pose corresponding to the successfully verified candidate key frame into a 4×4 matrix R k , calculate the relocation transformation matrix R final :

[0085] R final =R Δ *R k .

[0086] 10) Reposition the transformation matrix R final Converted into a six-dimensional pose [x, y, z, roll, pitch, yaw] as the repositioning pose output.

[0087] The method first loads the pre-saved keyframes and key poses to generate a keyframe search feature set; secondly, the real-time point cloud frame and nine-axis IMU data are input, and a two-step search method is used to obtain the candidate keyframe that is most similar to the current frame; the candidate keyframe is verified again, and if the verification passes, the coordinate transformation is performed; finally, the relocation result is output. The robot can be autonomously relocated without external GPS and human intervention.

[0088] The embodiment of the present invention also provides a robot repositioning system based on a multi-line laser radar, including a key frame and key posture input module, a real-time frame input module, a preprocessing module, a point cloud segmentation module, a generation search feature module, a real-time IMU input module, a two-part search module, a verification module, a posture transformation module and an output repositioning posture module.

[0089] The key frame and key pose input module is used to input key frames and key poses;

[0090] The real-time frame input module and the real-time IMU input module are used for real-time frame input and real-time radar and nine-axis IMU data input;

[0091] The preprocessing module is used to perform data preprocessing on the input key frames, including voxel filtering downsampling and extracting the region of interest;

[0092] The point cloud segmentation module divides the point cloud data into two parts: ground and non-ground;

[0093] The search feature generation module extracts the non-ground point search features of the point cloud and converts the three-dimensional point cloud into a feature matrix on the XY plane according to the point cloud label;

[0094] The two-part search module first preliminarily screens the loaded key frame feature set by yaw angle to narrow the search range, and then performs a second search by calculating matrix similarity;

[0095] The verification module is used to perform NDT point cloud registration on the candidate key frames obtained by the two search modules and the current input key frame; a score threshold is set, and if the NDT point cloud registration score is less than the score threshold, the verification is successful;

[0096] The pose transformation module converts the key pose corresponding to the successfully verified candidate key frame into a 4×4 matrix and calculates the relocation transformation matrix;

[0097] The output relocation pose module is used to convert the relocation transformation matrix into a six-dimensional pose [x, y, z, roll, pitch, yaw] as the relocation pose output.

[0098] The specific implementation process of the system to achieve robot repositioning is as follows:

[0099] 1) Input key frames and key poses. The key frames and key poses are saved when the robot builds an environment map, usually once every 1 second in motion.

[0100] 2) Perform data preprocessing on the input key frames, including voxel filtering downsampling and extracting the region of interest.

[0101] The voxel filtering adopts the method provided by the PCL point cloud processing library, and the voxel size is set to (0.25, 0.25, 0.25) cm;

[0102] The extraction of the region of interest is mainly processed from the distance aspect, setting the minimum radius R min , maximum radius R max The points in the area of ​​interest are 5m and 85m respectively. The near and far point clouds are removed. If the points in the point cloud are represented by P(x, y, z), then the points in the area of ​​interest meet the following conditions:

[0103]

[0104] 3) Point cloud segmentation: divide the point cloud data into two parts: ground and non-ground. The method used is the ray method, and labels are added to the segmented point cloud. The ground point cloud is labeled as label1, and the value of label1 is a constant 5; the non-ground point cloud is labeled as label2, and the value of label2 is a constant 10.

[0105] 4) Generate search features. This part converts the 3D point cloud into a feature matrix on the XY plane according to the point cloud label. The details are as follows:

[0106] Set the sector angle resolution to be divided into sectors angle , here sector angle If set to 0.01745, the number of sectors into which the entire 3D point cloud can be divided is sector numfor:

[0107]

[0108] In the formula, π represents pi;

[0109] Traverse the 3D point cloud and calculate the sector to which the non-ground point belongs id , first calculate the angle between the point cloud and the coordinate origin relative to the positive direction of the X-axis:

[0110]

[0111] Then calculate the sector id :

[0112]

[0113] Count the non-ground points with the smallest radius r in each sector:

[0114]

[0115] And reorganize the data of the statistical point set to construct a row of sectors num Column matrix R p , R p Each column represents the non-ground point with the smallest radius r in each sector, and each column contains four features, namely radius, X-axis coordinate, Y-axis coordinate, and label.

[0116] 5) Traverse all key frames.

[0117] 6) Input real-time radar and nine-axis IMU data, repeat steps 2), 3), and 4) to get the matrix R c .

[0118] 7) Two-step search: first, preliminarily screen the loaded key frame feature set by yaw angle to narrow the search range, and then perform a second search by calculating matrix similarity:

[0119] First, set the initial search range according to the input yaw angle of the nine-axis IMU c , find the key pose yaw angle corresponding to the key frame k With yaw c The candidate keyframe set with the phase difference angle Δyaw, where Δyaw is set to 1.57:

[0120] -Δyaw <yaw c -yaw k <Δyaw

[0121] Next, traverse the candidate keyframe set, set the sector rotation step size step, and set R according to the rotation step size. p Perform column translation to obtain R′ p , the R′ obtained by each column shift p With R c Compare and calculate the similarity, and compare to get the R with the smallest similarity p The corresponding candidate key frames are calculated at the same time. p With R c The relative transformation matrix R between Δ :

[0122] R c =R Δ *R p .

[0123] 8) Verify, perform NDT point cloud registration on the candidate keyframe obtained in step 7) and the current input keyframe, and Δ As a priori transformation matrix; set the score threshold score th , score th The value is 0.5. If the NDT point cloud registration score is ndt Less than score th , the verification is successful.

[0124] 9) Posture transformation: transform the key pose corresponding to the successfully verified candidate key frame into a 4×4 matrix R k , calculate the relocation transformation matrix R final :

[0125] R final =R Δ *R k .

[0126] 10) Reposition the transformation matrix R final Converted into a six-dimensional pose [x, y, z, roll, pitch, yaw] as the repositioning pose output.

[0127] An embodiment of the present invention further provides a robot repositioning device based on a multi-line laser radar, comprising: at least one memory and at least one processor;

[0128] The at least one memory is used to store a machine-readable program;

[0129] The at least one processor is used to call the machine-readable program to execute the robot relocalization method based on multi-line laser radar described in the above embodiment.

[0130] Through the above specific implementations, those skilled in the art can easily implement the present invention. However, it should be understood that the present invention is not limited to the above specific implementations. On the basis of the disclosed implementations, those skilled in the art can arbitrarily combine different technical features to achieve different technical solutions.

[0131] Except for the technical features described in the specification, all other technical features are known technologies to those skilled in the art.

Claims

1. A robot relocation method based on multi-line laser radar, characterized in that: The implementation process of this method is as follows: 1) Input key frames and key poses; 2) Data preprocessing of the input keyframes, including voxel filtering downsampling and extraction of regions of interest; 3) Point cloud segmentation: dividing the point cloud data into ground and non-ground parts; 4) Generate search features, extract non-ground point search features of point cloud, and convert the 3D point cloud into a feature matrix on the XY plane according to the point cloud label; 5) Traverse all key frames; 6) Input real-time radar and nine-axis IMU data, repeat steps 2), 3), and 4) to obtain the feature matrix R c ; 7) Two-step search: first, preliminarily screen the loaded key frame feature set by yaw angle to narrow the search range, and then perform a second search by calculating matrix similarity; 8) Verify, and perform NDT point cloud registration between the candidate key frame obtained in step 7) and the current input key frame; Set a score threshold. If the NDT point cloud registration score is less than the score threshold, the verification is successful. 9) Posture transformation: convert the key pose corresponding to the successfully verified candidate key frame into a 4×4 matrix and calculate the relocation transformation matrix; 10) Convert the relocation transformation matrix into a six-dimensional pose [x, y, z, roll, pitch, yaw] as the relocation pose output; The two-step search, First, set the initial search range according to the input yaw angle of the nine-axis IMU c , find the key pose yaw angle corresponding to the key frame k With yaw c The candidate keyframe set with the phase difference angle Δyaw, where the yaw angle is in radians: -Δyaw<yaw c -yaw k <Δyaw Then, traverse the candidate keyframe set, set the sector rotation step size step, and set R according to the rotation step size. p Shift the columns to get R , p , the R obtained by each column shift , p With R c Compare and calculate the similarity, and compare to get the R with the smallest similarity p The corresponding candidate key frame and R p With R c The relative transformation matrix R between Δ : R c =R Δ *R p ; During the verification, the candidate key frame obtained in step 7) is aligned with the current input key frame by NDT point cloud registration. Δ as a priori transformation matrix.

2. The robot relocation method based on multi-line laser radar according to claim 1, characterized in that: The key frames and key poses are saved when the robot constructs an environment map.

3. A robot repositioning method based on multi-line laser radar according to claim 1 or 2, characterized in that: The voxel filtering adopts the method provided by the PCL point cloud processing library; Extract the area of ​​interest and process it from the distance aspect, setting the minimum radius R min and the maximum radius R max , remove the near and far point clouds, and let the points in the point cloud be represented by P(x, y, z). Then the points in the region of interest satisfy the following conditions:

4. The robot relocation method based on multi-line laser radar according to claim 1, characterized in that: The point cloud is segmented using the ray method, and labels are added to the segmented point clouds, where the ground point cloud is labeled as label1 and the non-ground point cloud is labeled as label2.

5. A robot repositioning method based on multi-line laser radar according to claim 1 or 4, characterized in that: The three-dimensional point cloud is converted into a feature matrix on the XY plane according to the point cloud label, and the sector angle resolution to be divided is set. angle , then the number of sectors that divide the entire 3D point cloud can be obtained num , sector angle The values ​​are in radians: In the formula, π represents pi; Traverse the 3D point cloud and calculate the sector to which the non-ground point belongs id First, calculate the angle between the point cloud and the origin of the coordinate system relative to the positive direction of the X-axis. The value of angle is in radians: Then calculate the sector id : Count the non-ground points with the smallest radius r in each sector: And reorganize the data of the statistical point set to construct a row of sectors num Column matrix R p , R p Each column represents the non-ground point with the smallest radius r in each sector, and each column contains four features, namely radius, X-axis coordinate, Y-axis coordinate, and label.

6. The robot relocation method based on multi-line laser radar according to claim 1, characterized in that: Let the 4×4 matrix be R k , the relocation transformation matrix is ​​R final ,but R final =R Δ *R k 。 7. A robot relocation system based on multi-line laser radar, characterized in that: It includes key frame and key pose input module, real-time frame input module, preprocessing module, point cloud segmentation module, generation search feature module, real-time IMU input module, two-part search module, verification module, pose transformation module and output relocation pose module. The system realizes robot repositioning through the robot repositioning method based on multi-line laser radar described in any one of claims 1 to 6.

8. A robot repositioning device based on multi-line laser radar, characterized in that: include: at least one memory and at least one processor; The at least one memory is used to store a machine-readable program; The at least one processor is configured to call the machine-readable program to execute the method according to any one of claims 1 to 6.

Images