Dual 2D LiDAR relative pose calibration method and system

Abstract

The present invention discloses a relative pose calibration method for dual 2D laser radars, comprising the following steps: S1, simultaneously acquiring laser frames of two 2D laser radars; S2, forming candidate poses within a pose window range based on position step length and angle step length. Pose_m , determine the candidate pose with the highest matching score between the two laser frames and the occupancy grid map M, which is the local optimal pose; S3, form a smaller pose window centered on the current local optimal pose, reduce the position step size and angle step size, and execute step S3 until the score of the current optimal pose is no greater than the score of the previous optimal pose, then output the global optimal pose; S4, calculate the relative pose between the two 2D lidars based on their global optimal pose in the map coordinate system. This solves the calibration problem of dual 2D lidars requiring overlapping areas or initial calibration values, reduces dependence on the environment and radar installation, avoids falling into local extremes, and improves calibration accuracy, stability, and efficiency.

Description

Technical Field

[0001] The present invention belongs to the technical field of radar pose calibration, and more specifically, the present invention relates to a dual 2D laser radar relative pose calibration method and system. Background Art

[0002] As laser robots become increasingly widely used in modern logistics and warehousing, the requirements for their positioning and navigation accuracy are becoming increasingly critical across various application scenarios. LiDAR positioning and navigation accuracy depends not only on algorithm accuracy but also on the accuracy of the radar's pose calibration. A single LiDAR robot's field of view is limited, leading many robots to use dual LiDARs for positioning and navigation. This pose calibration issue is becoming increasingly important.

[0003] Application publication number CN112305521B, application publication date 2021.02.02, patent name: Dual laser radar relative position calibration method, device, equipment and storage medium, which first obtains the first scanning point set of the first laser radar for the preset scene and the second scanning point set of the second laser radar for the preset scene; secondly, determines the position conversion matrix based on the preset relative position of the second laser radar to the first laser radar; finally, determines the actual relative position of the second laser radar to the first laser radar based on the position conversion matrix, the first scanning point set and the second scanning point set. The technical problems existing in this technical solution are as follows:

[0004] Using Scan_to_Scan matching makes the algorithm highly dependent on initial values, so its solution uses a preset relative pose to determine the position transformation matrix. If the preset pose differs from the true value by a certain distance, it is easy to fall into a local extreme value during iterative optimization, making the optimization result not the optimal pose and resulting in large calibration errors. Summary of the Invention

[0005] The present invention provides a dual 2D laser radar relative pose calibration method, aiming to improve the above problems.

[0006] The present invention is implemented as follows: a dual 2D laser radar relative pose calibration method, the method comprising the following steps:

[0007] S1. Acquire laser frames from two 2D laser radars simultaneously.

[0008] S2. Based on the position step and angle step, a candidate pose Pose_m is formed within the pose window. The candidate pose with the highest matching score between the two laser frames and the occupancy grid map M is determined as the local optimal pose.

[0009] S3, forming a smaller pose window with the current local optimal pose as the center, reducing the position step and angle step, and executing step S2 until the score of the current optimal pose is no greater than the score of the previous optimal pose, then outputting the global optimal pose;

[0010] S4. Calculate the relative pose between the two 2D lidars based on the global optimal pose of the two 2D lidars in the map coordinate system.

[0011] Furthermore, after step S4, the method further includes:

[0012] S5. Change the current posture of the robot within the maximum posture window, execute steps (1) to (4), and put the calculated relative posture Pose_relative into the initial relative posture set Pose_relative_raw in sequence;

[0013] S6. Calculate the mean and covariance matrix of the relative poses in the initial relative pose set;

[0014] S7. Calculate the standard deviation of the relative poses in the initial relative pose set, eliminate relative poses with large deviations, and calculate the accurate relative poses of the two 2D lidars.

[0015] Furthermore, when the deviation Pose_err of the current relative pose exceeds the standard std_err, the corresponding relative pose is deleted from the initial relative pose set to obtain a new relative pose set. The pose mean of the new relative pose set is the accurate relative pose of the 2D lidar.

[0016] Furthermore, before step S1, the method further includes:

[0017] S0. Build a high-resolution occupancy grid map M using a single 2D lidar.

[0018] Furthermore, the maximum position step value Step_lin=3Res, where Res is the resolution of the occupied grid map M;

[0019] The maximum angle step value is the angle Step_Ang when the farthest measuring point rotates 3 grids. Its calculation formula is as follows:

[0020] Step_Ang=arc(1-3*Res / (2*Range_max 2 ));

[0021] Among them, Range_max is the maximum ranging value of the 2D lidar.

[0022] Furthermore, the maximum attitude window range is expressed as follows:

[0023] The maximum posture window range is defined as: twice the robot size and 360-degree posture, which is expressed as follows:

[0024] x∈[-Dis_max,+Dis_max];

[0025] y∈[-Dis_max,+Dis_max];

[0026] theta∈[-π,+π];

[0027] Dis_max is the maximum diagonal distance, Length and Width represent the length and width of the robot respectively.

[0028] Furthermore, the method for obtaining the local optimal pose of the 2D lidar A is as follows;

[0029] S21, searching for candidate poses in the pose window using the position step value and the angle step value;

[0030] S22. Based on the candidate poses, project the point cloud of laser frame Scan_A onto the occupancy grid map M for scoring, and return to step S21 until all candidate poses in the pose window are traversed. The pose with the highest score is the local optimal pose of 2D laser radar A.

[0031] Furthermore, the scoring calculation formula is as follows:

[0032] Score i =∑(M(S i (Pose_m))) / N

[0033] Among them, S i (Pose_m) represents the two-dimensional grid coordinate (r, c) obtained by projecting the i-th laser point in the laser point cloud onto the grid map through the candidate pose Pose_m, and M(r, c) represents the occupancy probability of the two-dimensional grid coordinate (r, c) in the grid map.

[0034] The present invention is implemented as follows: a 2D laser radar relative posture calibration system, the system comprising:

[0035] Two 2D laser radars are installed on the robot, and the two 2D laser radars are communicated with the processor; the processor calibrates the relative posture between the two 2D laser radars based on the above-mentioned dual 2D laser radar relative posture calibration method.

[0036] The present invention solves the calibration problem of dual 2D laser radars requiring overlapping areas, special walls or calibration initial values, reduces dependence on the environment and radar installation, avoids falling into local extreme values, and improves the accuracy, stability and efficiency of calibration. BRIEF DESCRIPTION OF THE DRAWINGS

[0037] Figure 1 A schematic diagram of the structure of a dual 2D lidar relative pose calibration system provided in an embodiment of the present invention;

[0038] Figure 2 Flowchart of the relative pose calibration method for dual 2D lidars provided in an embodiment of the present invention;

[0039] Figure 3 Schematic diagram of the relative pose calibration of two 2D lidars provided in an embodiment of the present invention. DETAILED DESCRIPTION

[0040] The specific implementation methods of the present invention will be further explained in detail below by describing the embodiments with reference to the accompanying drawings, so as to help those skilled in the art to have a more complete, accurate and in-depth understanding of the inventive concept and technical solution of the present invention.

[0041] Figure 1 This is a schematic diagram of the structure of the dual 2D lidar relative pose calibration system provided by an embodiment of the present invention. For ease of explanation, only the parts related to the embodiment of the present invention are shown. The system includes:

[0042] The 2D laser radar A and 2D laser radar B are installed on the robot. The 2D laser radar A and 2D laser radar B are connected to the processor. The processor determines the relative posture of 2D laser radar B under 2D laser radar A based on the following dual 2D laser radar relative posture calibration method, as follows: Figure 3 shown.

[0043] Figure 2 A flow chart of a dual 2D lidar relative pose calibration method provided in an embodiment of the present invention, the method comprising the following steps:

[0044] (1) Building a high-resolution occupancy grid map M using a single 2D lidar;

[0045] First, a high-resolution occupancy grid map M is established by the 2D laser radar A or 2D laser radar B on the robot. The occupancy grid map is constructed using the general SLAM method.

[0046] (2) Determine the maximum pose window range based on the robot size;

[0047] Given the robot dimensions (Length, Width), where Length and Width represent the length and width of the robot respectively, calculate the maximum diagonal distance.

[0048] The maximum posture window range is defined as: twice the robot size and 360-degree posture, which is expressed as follows:

[0049] x∈[-Dis_max,+Dis_max];

[0050] y∈[-Dis_max,+Dis_max];

[0051] theta∈[-π,+π];

[0052] (3) Calculate the maximum position step value and the maximum angle step value;

[0053] The maximum range value of the 2D lidar is Range_max. Through bicubic interpolation, the maximum position step value Step_lin = 3Res, where Res is the resolution of the grid map M. The maximum angle step value is the angle Step_Ang when the farthest ranging point is rotated by 3 grids. The calculation formula is as follows:

[0054] Step_Ang=arc(1-3*Res / (2*Range_max 2 ))

[0055] (4) Obtain the candidate pose Pose_m under the maximum position step value and the maximum angle step value;

[0056] A series of candidate poses Pose_m are formed through the maximum position step value 3Res and the maximum angle step value Step_Ang;

[0057] Pose_m.x∈[-Dis_max,-Dis_max+Setp_lin,-Dis_max+2Step_lin,....,Dis_max];

[0058] Pose_m.y∈[-Dis_max,-Dis_max+Setp_lin,-Dis_max+2Step_lin,....,Dis_max];

[0059] Pose_m.theta∈[-π,-π+Step_Ang,-Dis_max+2Step_Ang,....,π];

[0060] (5) Simultaneously obtain the laser frame Scan_A of 2D laser radar A and the laser frame Scan_B of 2D laser radar B, and determine the candidate pose with the highest matching score between the laser frames Scan_A and Scan_B and the occupancy grid map M, which is the local optimal pose;

[0061] The same processing is performed on the laser frame Scan_A and the laser frame Scan_B. The present invention takes the laser frame Scan_A as an example for detailed description, and the specific description is as follows:

[0062] Project the point cloud in the laser frame Scan_A to the occupancy grid map M through the candidate pose Pose_m, calculate the matching score between the laser frame Scan_A and the occupancy grid map M, and take the candidate pose with the highest score as the local optimal pose of the 2D laser radar A under the maximum step;

[0063] The candidate poses are searched with the maximum position step value and the maximum angle step value. Based on the candidate poses, the point cloud is projected onto the occupancy grid map M (also known as the probability map) for scoring. The calculation formula is as follows:

[0064] Score i =∑(M(S i (Pose_m))) / N

[0065] Among them, S i (Pose_m) represents the two-dimensional grid coordinates (r, c) obtained by projecting the i-th laser point in the laser point cloud onto the grid map through the candidate pose Pose_m, and M(r, c) represents the occupancy probability of the two-dimensional grid coordinates (r, c) in the grid map. The above method can also be used to obtain the local optimal pose Pose of the 2D laser radar B under the maximum step value. Bt1 .

[0066] (6) A pose window is formed with the local optimal pose as the center, and its range value is half of the current range value. The position step value and the angle step value are halved. Steps (4) and (5) are executed to obtain the local optimal pose of 2D lidar A and 2D lidar B at the current step value.

[0067] (7) Loop through step (6) until the current maximum score is no greater than the previous maximum score. At this point, obtain the global optimal pose Pose of the 2D laser radar A. A and the optimal pose Pose of the 2D lidar B in the global map coordinate system B .

[0068] (8) Calculate the relative pose Pose_relative of 2D laser radar B under 2D laser radar A, or the relative pose Pose_relative of 2D laser radar A under 2D laser radar B, and put it into the initial relative pose set Pose_relative_raw;

[0069] 2D laser radar A under the relative position of radar B under 2D laser radar A Pose_relative is the pose of 2D laser radar B in the coordinate system,

[0070] Assume Pose A=(px,py,ptheta), convert Pose_A to pose matrix T_PA:

[0071]

[0072] Similarly, convert Pose_B into the pose matrix T_PB, then we can get the pose matrix TBA of B in the A radar coordinate system:

[0073] T_BA=T_PA -1 *T_PB;

[0074] Then convert the relative pose matrix into a relative pose vector:

[0075] Pose_relative = T2V(T_BA);

[0076] T2V is the basic formula for converting a 3D transformation matrix into a 3D vector.

[0077] Continuously move the robot within the window pose range, that is, change the current pose of the robot, execute steps (5) to (8), and put the calculated relative pose Pose_relative into the initial relative pose set Pose_relative_raw in sequence;

[0078] (9) Calculate the mean and covariance matrix of the initial relative pose;

[0079] The mean E_raw and covariance matrix Cov of the initial relative pose in the initial relative pose set Pose_relative_raw can be calculated. The calculation formulas of the mean E_raw and covariance matrix Cov are as follows:

[0080]

[0081]

[0082] Among them, Pose_relative_raw i Represents the i-th relative pose Pose_relative in the initial relative pose set;

[0083] (10) Calculate the standard deviation, eliminate the postures with large deviations, and calculate the accurate relative posture of the dual radars;

[0084] The standard deviation std_err and deviation Pose_err of the initial relative pose set Pose_relative_raw are calculated as follows:

[0085] std_err=sqrt(Cov [0,0] ,Cov [1,1] ,Cov[2,2] );

[0086] Pose_err=pose_relative_raw i -E_raw.

[0087] Cov [0,0] 、Cov [1,1] 、Cov [2,2] Represents the three elements on the diagonal of the covariance matrix Cov, namely the first row and first column Cov [0,0] , second row, second column Cov [1,1] And the third row of the third category Cov [2,2] The element value on .

[0088] When an element of Pose_err exceeds the corresponding element of std_err, delete the corresponding pose pair in the initial set to obtain a new relative pose set with K relative poses. Calculate the pose mean E, which is the final calibrated relative pose of the dual lidar.

[0089] E = ∑Pose_relative / K.

[0090] The present invention solves the calibration problem of dual 2D laser radars requiring overlapping areas, special walls or calibration initial values, reduces dependence on the environment and radar installation, avoids falling into local extreme values, and improves the accuracy, stability and efficiency of calibration.

[0091] The present invention has been described exemplarily. Obviously, the specific implementation of the present invention is not limited to the above-mentioned method. As long as various non-substantial improvements are made using the method concept and technical solution of the present invention, or the concept and technical solution of the present invention are directly applied to other occasions without improvement, they are all within the scope of protection of the present invention.

Claims

1. A dual 2D laser radar relative pose calibration method, characterized in that: The method comprises the following steps: S1. Acquire laser frames from two 2D laser radars simultaneously. S2. Based on the position step and angle step, a candidate pose Pose_m is formed within the pose window. The candidate pose with the highest matching score between the two laser frames and the occupancy grid map M is determined as the local optimal pose. S3, forming a smaller pose window with the current local optimal pose as the center, reducing the position step and angle step, and executing step S2 until the score of the current optimal pose is no greater than the score of the previous optimal pose, then outputting the global optimal pose; S4, calculating the relative pose between the two 2D laser radars based on the global optimal pose of the two 2D laser radars in the map coordinate system; After step S4, the method further includes: S5. Change the current posture of the robot within the maximum posture window, execute steps (1) to (4), and put the calculated relative posture Pose_relative into the initial relative posture set Pose_relative_raw in sequence; S6. Calculate the mean and covariance matrix of the relative poses in the initial relative pose set; S7, calculating the standard deviation of the relative poses in the initial relative pose set, eliminating relative poses with large deviations, and calculating the accurate relative poses of the two 2D lidars; The maximum position step value Step_lin=3Res, where Res is the resolution of the occupied grid map M; The maximum angle step value is the angle Step_Ang when the farthest measuring point rotates 3 grids. Its calculation formula is as follows: Step_Ang=arccos(1-3*Res / (2*Range_max) 2 )); Among them, Range_max is the maximum ranging value of the 2D lidar.

2. The dual 2D laser radar relative pose calibration method according to claim 1, wherein: When the deviation Pose_err of the current relative pose exceeds the standard std_err, the corresponding relative pose is deleted from the initial relative pose set to obtain a new relative pose set. The pose mean of the new relative pose set is the accurate relative pose of the 2D lidar.

3. The dual 2D laser radar relative pose calibration method according to claim 1, wherein: Before step S1, the method further includes: S0. Build a high-resolution occupancy grid map M using a single 2D lidar.

4. The dual 2D laser radar relative pose calibration method according to claim 1, wherein: The maximum attitude window range is expressed as follows: The maximum posture window range is defined as: twice the robot size and 360-degree posture, which is expressed as follows: x∈[-Dis_max,+Dis_max]; y∈[-Dis_max,+Dis_max]; theta∈[-π,+π]; Dis_max is the maximum diagonal distance, Length and Width represent the length and width of the robot respectively.

5. The dual 2D laser radar relative pose calibration method according to claim 1, wherein: The method for obtaining the local optimal pose of 2D lidar A is as follows; S21, searching for candidate poses in the pose window using the position step value and the angle step value; S22. Based on the candidate poses, project the point cloud of laser frame Scan_A onto the occupancy grid map M for scoring, and return to step S21 until all candidate poses in the pose window are traversed. The pose with the highest score is the local optimal pose of 2D laser radar A.

6. The dual 2D laser radar relative pose calibration method according to claim 5, characterized in that: The scoring calculation formula is as follows: Score i =∑(M(S i (Pose_m))) / N Among them, S i (Pose_m) represents the two-dimensional grid coordinate (r, c) obtained by projecting the i-th laser point in the laser point cloud onto the grid map through the candidate pose Pose_m, and M(r, c) represents the occupancy probability of the two-dimensional grid coordinate (r, c) in the grid map.

7. A dual 2D laser radar relative pose calibration system, characterized in that: The system comprises: Two 2D laser radars are installed on the robot, and the two 2D laser radars are communicatively connected to the processor; The processor calibrates the relative pose between two 2D lidars based on the dual 2D lidar relative pose calibration method described in any one of claims 1 to 6.

Images