Solid-state line laser external reference pose calibration method

Abstract

The invention relates to the technical field of robot visual perception and calibration, in particular to a solid-state line laser external reference pose calibration method, which omits the cost and trouble of carrying, setting and maintaining a special calibration object by using natural reference planes such as a wall surface in a working environment of a cleaning robot, is more convenient and efficient in calibration process, is easy to implement on site, and is suitable for popularization and application. The method does not need to depend on a large number of observation point clouds and accurate relative motion measurement, the robustness and applicability of the method on a low-cost or non-ideal motion platform are improved, complete six-degree-of-freedom pose parameters are directly solved, the problems of error accumulation and coupling possibly caused by parameter step-by-step calibration are avoided, and the method is suitable for large-scale popularization and application. The invention provides a solid-state line laser external parameter calibration solution which is more convenient, robust, automatic and high in environmental adaptability, effectively overcomes many limitations in the prior art, and has important value for improving the sensing precision and application convenience of the line laser carried by the mobile robot.

Description

TECHNICAL FIELD

[0001] The present application relates to the technical field of robot vision perception and calibration, and particularly relates to a solid-state line laser external parameter pose calibration method. BACKGROUND

[0002] Sensor external parameter calibration is a key technology in multi-sensor systems (such as autonomous vehicles, robots, drones, etc.), aiming to determine the relative pose relationship (such as translation and rotation parameters) between different sensors, and the accuracy of external parameter calibration directly affects the effect of multi-source data fusion, and then determines the reliability of system perception and decision-making, but the existing calibration technology faces the following challenges in actual application.

[0003] At present, the mainstream method of line laser external parameter calibration on mobile robots mainly includes the following three kinds:

[0004] (1) The robot carries a line laser to perform a specific motion (such as translation, rotation) in front of the calibration object, and the feature lines (such as edges, corner points, plane intersection lines) formed by the line laser scanning the surface of the calibration object are used to solve the external parameters by using geometric constraint relationships (such as point-line constraint, line-plane constraint, coplanar constraint) or optimization algorithms, but it needs to carry, place and maintain a special high-precision calibration object during calibration, which is cumbersome and inconvenient to implement quickly in the work site, and the calibration object is also easy to wear, damage or lose, affecting the precision, and manual intervention is required, and a large amount of data is collected through multiple specific motions, which has low automation degree;

[0005] (2) The data obtained by the robot body line laser scanning the same scene (or fixed feature points / lines) at different poses is combined with the accurate motion information of the robot itself (usually from the encoder or laser SLAM, etc.), and the kinematic constraint equation is established to solve the external parameters of the line laser relative to the robot base, but the calibration accuracy is highly dependent on the measurement accuracy of the robot motion, and when the robot slips or the ground is uneven, the motion state is poor, the calibration error will increase significantly, and the motion environment and positioning system accuracy of the robot during operation are required;

[0006] (3) In the presence of walls, floors, regular object structured or semi-structured environments, the constraint relationship between the contour lines obtained by line laser scanning, such as ground intersection lines and wall intersection lines, and the known or assumed geometric structures in the environment, such as horizontal ground and vertical wall, is used for calibration, but it needs to have clear horizontal and vertical surfaces in the environment, and it is difficult to find reliable and sufficient geometric constraints in a chaotic environment, which may lead to calibration failure or precision decline;

[0007] Moreover, the partial optimization of the external parameter pose calibration method is sensitive to the initial guess value of the pose, and if the initial value deviates greatly, it is easy to lead to the convergence of the optimization process to the wrong local optimal solution, and some calibration methods may exist coupling (such as translation and rotation) when solving 6-DoF (six degrees of freedom) parameters, leading to unstable solution or uneven precision. Therefore, a solid-state linear laser external parameter pose calibration method is proposed. SUMMARY

[0008] In order to solve the technical problems existing in the prior art, the present application provides a solid-state linear laser external parameter pose calibration method.

[0009] In order to solve the above technical problems, the present application provides the following technical scheme: a solid-state linear laser external parameter pose calibration method, the pose calibration method specifically comprises the following steps:

[0010] S1: converting the data of the solid-state linear laser and the single linear laser radar from polar coordinate form to point cloud form, and using the nearest corner point segmentation method and the maximum change angle segmentation method to segment the given point cloud according to the requirements;

[0011] S2: after performing coordinate rotation transformation on each point in the point cloud segment according to the calibration requirements, for the point cloud, using the RANSAC algorithm, fitting a straight line by randomly sampling two points and selecting inliers based on a distance threshold, and iteratively selecting the straight line with the most inliers as the optimal fitting result; for the point clouds formed by two segments of solid-state linear laser, the above process is performed respectively to obtain the optimal fitting straight line of each segment, and the included angle between the two straight lines is calculated, and the calibration of the roll angle, the pitch angle and the yaw angle is completed in turn using the calculated included angle information;

[0012] S3: first, rotating the ground point cloud data radar_ground scanned by the solid-state linear laser according to the pre-calibrated roll angle θ roll , pitch angle θ pitch and yaw angle θ yaw in turn (correcting the attitude), then extracting the Z coordinate values (height information) of all points from the transformed point cloud, then sorting and filtering these height values using the IQR (interquartile range) method, and finally calculating the average value of the remaining valid values, which is the final calibrated Z axis coordinate.

[0013] S4: aligning the corner point positions of the laser radar point cloud laser_cloud and the solid-state linear laser point cloud radar_cloud, calculating the offset offset x and offset y between the corner points, which are the required x-axis and y-axis coordinates to be calibrated.

[0014] Preferably, the step S1 specifically comprises the following steps:

[0015] S11: Convert the polar coordinates of the laser point to a point cloud. The data conversion expressions for solid-state line lasers and single-line lidar are as follows:

[0016]

[0017] Where, θ i =θ min +i·Δθ (i=0,1,...,N-1), r is the distance measured by the laser point, θ min The minimum offset starting angle of the sensor is Δθ, where Δθ is the angle increment.

[0018] After conversion, the solid-state line laser point cloud information is denoted as radar_cloud, and the lidar point cloud information is denoted as laser_cloud;

[0019] S12: Nearest Corner Segmentation Method: Taking the robot's geometric center as the origin (0,0), traverse the given point cloud dataset, calculate the distance from each point to the origin, and record the index of the nearest point; then, using this index as the boundary, divide the point cloud data into two parts. Through the above process, the given solid-state laser point cloud and lidar point cloud are divided into left and right parts respectively. The specific formula for the distance function dist from the origin is:

[0020]

[0021] Iterate through the dist corresponding to each point in the point cloud dataset, find the smallest dist, and this point is the nearest corner point. Then, use its corresponding index... min The point cloud is divided into left and right parts;

[0022] Among them, after solid-state line laser point cloud segmentation, they are denoted as radar_left and radar_right respectively;

[0023] After the LiDAR point cloud is segmented, it is denoted as laser_left and laser_right respectively;

[0024] S13: Maximum Change Angle Segmentation Method: Calculates the angle formed by each point in a given point cloud dataset with points spaced windowSize before and after it. The specific formula for calculating the angle is as follows:

[0025]

[0026] Where A, B, and C are three points, (BA)·(CA) represents the dot product, and |BA| and |CA| are the magnitudes of the vectors;

[0027] Calculate the index of the point in the radar_left and radar_right point clouds where the angle change is the largest. angle The points on the right-angled walls are divided into radar_left_1 and radar_right_1, and the points on the ground are divided into radar_left_2 and radar_right_2.

[0028] Superimpose radar_left_2 and radar_right_2 to obtain the complete ground point cloud, denoted as radar_ground, and rename radar_left_1 as radar_left and radar_right_1 as radar_right.

[0029] Preferably, step S2 specifically includes the following steps:

[0030] S21: Calibrate roll angle θ roll ;

[0031] S22: Calibrate the pitch angle θ pitch ;

[0032] S23: Calibrate yaw angle θ yaw .

[0033] Preferably, step S21 specifically includes the following steps:

[0034] S211: Rotate both the radar_left and radar_right point cloud fragments around the X-axis by an angle θ. Calculate the spatial angle formed by radar_left' and radar_right' after each rotation, iterate through all θ, and find the rotation angle that maximizes the angle; this is the desired calibration angle θ. roll Wherein, the rotation transformation matrix T is a 4×4 homogeneous transformation matrix, and the specific formula for calculating T is:

[0035]

[0036] Where θ is the roll angle, in radians, and R... 3x3 It is a 3x3 rotation matrix representing the rotation about the X-axis, t is the displacement vector, which is set to zero in this formula, so that there is no translation, and the lower half [0 1] represents the extended part in homogeneous coordinates;

[0037] Select a point from the original point cloud fragment and denote it as P, then P = (x, y, z). Transform it using matrix T to obtain P' = (x', y', z'), which can be expressed as P' = T·P. h , where Ph =(x,y,z,1) T This is the homogeneous coordinate form of point P. The specific transformation operations are as follows:

[0038]

[0039] S212: The standard form of the general linear equation expression fitted by the RANSAC algorithm is:

[0040] ax + by + c = 0

[0041] Where a, b, and c are the coefficients of the line, and x and y are the coordinates of a point on a two-dimensional plane;

[0042] Randomly select two points from the point cloud fragment, namely (x1, y1) and (x2, y2), and calculate the slope m of the straight line connecting the two points. The specific calculation method for m is as follows:

[0043]

[0044] The equation of the line is:

[0045] y-y1=m(x-x1);

[0046] Converting the equations of the above lines into standard form, we get: a = m, b = -1, c = y1 - m·x1;

[0047] The distance d from each point in the point cloud segment to the aforementioned line is calculated by iterating through the points. When d ≤ threshold, the point is considered an interior point. The specific formula for calculating the distance d from a point to the line is as follows:

[0048]

[0049] Among them, (x i ,y i () represents a point in a given point cloud segment. It repeatedly selects two different points to fit a straight line, counts the number of interior points on each fitted line, and selects the line with the most interior points as the optimal fitting result. The more interior points, the closer the fitted line is to the real line.

[0050] S213: Fit the general linear equations of the point clouds radar_left' and radar_right' using the RANSAC algorithm to obtain the direction vectors in the xoy coordinate system. and For vectors and Perform the following calculations: dot product, norm of each vector, and angle θ between the vectors.

[0051] The specific calculation process is as follows:

[0052]

[0053] Preferably, step S22 specifically includes the following steps:

[0054] Rotate both the radar_left and radar_right point cloud fragments around the X-axis by an angle θ. roll Rotation angle θ around the Y-axis ), calculate the spatial angle formed by radar_left and radar_right after each rotation, iterate through all θ, and the angle closest to 90° is the corresponding pitch angle calibration angle θ. pitch Wherein, the rotation transformation matrix T is a 4×4 homogeneous transformation matrix, and the specific formula for calculating T is:

[0055]

[0056] Where θ is the pitch angle, R pitch ×R roll It is a 3x3 rotation matrix, representing a rotation θ about the X-axis. roll 1 degree, then rotate θ degrees around the Y-axis;

[0057] Using the aforementioned matrix, the point clouds of radar_left and radar_right are rotated. A straight line is fitted to the transformed point clouds radar_left and radar_right using the RANSAC algorithm, calculated in the same way as in S212. Then, the direction vectors in the xoy coordinate system are obtained, and the angle between the two vectors is calculated in the same way as in S213. After iterating through the pitch angles θ, the angle closest to 90° is taken as the calibrated angle θ. pitch .

[0058] Preferably, step S23 specifically includes the following steps:

[0059] Solid-state line laser point cloud: The radar_left and radar_right point clouds are rotated and transformed using the calculation formula in S22, where R pitch ×R roll Represents the rotation θ about the X-axis roll Degrees, then rotate θ around the Y-axis. pitch For the transformed point cloud radar_left"' and radar_right"', the direction vector in the xoy coordinate system is obtained by straight line fitting in S212. and After normalization, we can obtain the unit vector. and Calculate the angle bisector vector

[0060] Single-line LiDAR point cloud: The laser_left and laser_right point clouds are fitted with straight lines in S212 to obtain the direction vector in the xoy coordinate system. After normalization, the unit vector can be obtained. and Calculate the angle bisector vector

[0061] Then, the angle θ between the angle bisector vector of the solid-state line laser and the single-line lidar is calculated. The specific calculation formula is as follows:

[0062] cosθ=v l ·n l

[0063]

[0064] Where θ is the final calibration angle θ. yaw .

[0065] Preferably, step S3 specifically includes the following steps:

[0066] S31: When processing solid-state line laser ground point clouds (radar_ground), a pose transformation must first be performed on them. The transformed point cloud is denoted as radar_ground'. The rotation transformation matrix T is a 4×4 homogeneous transformation matrix. The specific formula for calculating T is as follows:

[0067]

[0068] Among them, R yaw ×R pitch ×R roll It is a 3x3 rotation matrix representing a rotation θ about the X-axis. roll degrees, and then rotate θ around Y. pitch degrees, and finally the rotation θ around the Z-axis. yaw Spend.

[0069] S32: Traverse all points in the point cloud radar_ground', store the Z-value information of each point in the vector list z_data, sort the elements of z_data in ascending order from smallest to largest, and denote it as z_data'. Use the IQR method to obtain the range of the middle 50% of the data in z_data' and find outliers. The specific calculation formula is as follows:

[0070] Select the 25th percentile Q1 and the 75th percentile Q3;

[0071] IQR = Q3 - Q1;

[0072] Therefore: Low bound: low_bound = Q1 - 1.5 × IQR

[0073] High boundary: up_bound = Q3 + 1.5 × IQR;

[0074] Then, use binary search to find the valid Z values ​​within the high and low boundary range of the elements in z_data', and obtain the starting index of the valid element and the ending index of the valid element in z_data'. The optimal Z value is then:

[0075]

[0076] Preferably, step S4 specifically includes the following steps:

[0077] The point cloud radar_cloud of the solid-state line laser is rotated and transformed using the transformation method in S31. Then, the coordinates of the nearest corner point radar_point of the point cloud are calculated by corner point segmentation in S12.

[0078] The laser radar point cloud (laser_cloud) is directly segmented by the corner points in S12, and the coordinates of the nearest corner point (laser_point) of the point cloud are calculated.

[0079] Then, calculate the offsets between the coordinates radar_point and laser_point, which are the required x-axis and y-axis coordinates for calibration. The specific calculation method is as follows:

[0080]

[0081] Compared with the prior art, the beneficial effects of the present invention are as follows:

[0082] 1. This invention utilizes natural reference planes such as walls in the robot's working environment, eliminating the cost and hassle of carrying, setting up, and maintaining dedicated calibration objects. The calibration process is more convenient and efficient, and easy to implement on-site. It does not rely on a large number of observation point clouds and precise relative motion measurements, thus improving the robustness and applicability of the method on low-cost or non-ideal motion platforms. It directly solves for the complete six-degree-of-freedom pose parameters, avoiding the error accumulation and coupling problems that may be introduced by step-by-step parameter calibration.

[0083] 2. The calibration method in this invention mainly relies on a stable reference plane that is commonly found in the environment, which greatly reduces the requirements for the geometric structure of the environment. It is applicable to a wider range of unstructured or semi-structured scenarios such as warehouses, factory workshops, and flat outdoor areas. It overcomes the problem of traditional methods failing in complex environments and provides a more convenient, robust, automated and environmentally adaptable solid-state line laser extrinsic parameter calibration solution. It effectively overcomes many limitations of existing technologies and has important value for improving the sensing accuracy and application convenience of mobile robots equipped with line lasers. Attached Figure Description

[0084] Figure 1 This is a schematic diagram of the technical solution of the present invention. Detailed Implementation

[0085] The present invention will be further described below with reference to the accompanying drawings and embodiments, which illustrate the above and other technical features and advantages of the present invention. However, the following embodiments are merely preferred embodiments of the present invention and are not exhaustive.

[0086] Example:

[0087] like Figure 1 As shown, the present invention provides a method for extrinsic parameter pose calibration of a solid-state line laser, the pose calibration method specifically including the following steps:

[0088] S1: Convert the data of solid-state line laser and single-line lidar from polar coordinates to point cloud form, and use the nearest corner segmentation method and the maximum change angle segmentation method to segment the given point cloud as required;

[0089] S2: After performing coordinate rotation transformation on each point in the point cloud segment according to the calibration requirements, the RANSAC algorithm is used for the point cloud. By randomly sampling two points to fit a straight line and filtering the interior points based on a distance threshold, this process is iterated to select the straight line with the most interior points as the optimal fitting result. The above process is performed on the point clouds formed by two solid-state line lasers to obtain their respective optimal fitting straight lines, and the angle between these two straight lines is calculated. Using the calculated angle information, the roll angle, pitch angle, and yaw angle are calibrated in sequence.

[0090] S3: First, the ground point cloud data radar_ground obtained by solid-state line laser scanning is processed according to a pre-calibrated roll angle θ. roll Pitch angle θ pitch and yaw angle θ yaw The rotation transformation (attitude correction) is performed sequentially. Then, the Z coordinate values ​​(height information) of all points are extracted from the transformed point cloud. Next, the IQR (interquartile range) method is used to sort and filter these height values ​​to remove statistical outliers. Finally, the average of the remaining valid values ​​is calculated, which is the final calibrated Z-axis coordinate.

[0091] S4: Align the corner positions of the lidar point cloud (laser_cloud) with the solid-state linear lidar point cloud (radar_cloud), and calculate the offset between the corner points. x and offset y , which are the x-axis and y-axis coordinates that need to be calibrated.

[0092] In this embodiment, step S1 specifically includes the following steps:

[0093] S11: Convert the polar coordinates of the laser point to a point cloud. The data conversion expressions for solid-state line lasers and single-line lidar are as follows:

[0094]

[0095] Where, θ i =θ min +i·Δθ (i=0,1,...,N-1), r is the distance measured by the laser point, θ min The minimum offset starting angle of the sensor is Δθ, where Δθ is the angle increment.

[0096] After conversion, the solid-state line laser point cloud information is denoted as radar_cloud, and the lidar point cloud information is denoted as laser_cloud.

[0097] S12: Nearest Corner Segmentation Method: Taking the robot's geometric center as the origin (0,0), traverse the given point cloud dataset, calculate the distance from each point to the origin, and record the index of the nearest point; then, using this index as the boundary, divide the point cloud data into two parts. Through the above process, the given solid-state laser point cloud and lidar point cloud are respectively divided into left and right parts. The specific formula for the distance function dist from the origin is:

[0098]

[0099] Iterate through the dist corresponding to each point in the point cloud dataset, find the smallest dist, and this point is the nearest corner point. Then, use its corresponding index... min The point cloud is divided into left and right parts;

[0100] Among them, after solid-state line laser point cloud segmentation, they are denoted as radar_left and radar_right respectively;

[0101] After the LiDAR point cloud is segmented, it is denoted as laser_left and laser_right, respectively.

[0102] S13: Calculate the angle between each point in the given point cloud dataset and the points preceding and following it at intervals of windowSize. The specific formula for calculating the angle is as follows:

[0103]

[0104] Where A, B, and C are three points, (BA)·(CA) represents the dot product, and |BA| and |CA| are the magnitudes of the vectors;

[0105] Calculate the index of the point in the radar_left and radar_right point clouds where the angle change is the largest. angle The points on the right-angled walls are divided into radar_left_1 and radar_right_1, and the points on the ground are divided into radar_left_2 and radar_right_2.

[0106] Superimpose radar_left_2 and radar_right_2 to obtain the complete ground point cloud, denoted as radar_ground, and rename radar_left_1 as radar_left and radar_right_1 as radar_right.

[0107] In this embodiment, step S2 specifically includes the following steps:

[0108] S21: Calibrate roll angle θ roll ;

[0109] S22: Calibrate the pitch angle θ pitch ;

[0110] S23: Calibrate yaw angle θ yaw .

[0111] In this embodiment, step S21 specifically includes the following steps:

[0112] S211: Rotate both the radar_left and radar_right point cloud fragments around the X-axis by an angle θ. ), calculate the spatial angle formed by radar_left' and radar_right' after each rotation, iterate through all θ, and find the rotation angle that maximizes the angle, which is the desired calibration angle θ. roll Wherein, the rotation transformation matrix T is a 4×4 homogeneous transformation matrix, and the specific formula for calculating T is:

[0113]

[0114] Where θ is the roll angle, in radians, and R... 3x3 It is a 3x3 rotation matrix representing the rotation about the X-axis, t is the displacement vector, which is set to zero in this formula, so that there is no translation, and the lower half [0 1] represents the extended part in homogeneous coordinates;

[0115] Select a point from the original point cloud fragment and denote it as P, then P = (x, y, z). Transform it using matrix T to obtain P' = (x', y', z'), which can be expressed as P' = T·P. h , where Ph =(x,y,z,1) T This is the homogeneous coordinate form of point P. The specific transformation operations are as follows:

[0116]

[0117] S212: The standard form of the general linear equation expression fitted by the RANSAC algorithm is:

[0118] ax + by + c = 0

[0119] Where a, b, and c are the coefficients of the line, and x and y are the coordinates of a point on a two-dimensional plane;

[0120] Randomly select two points from the point cloud fragment, namely (x1, y1) and (x2, y2), and calculate the slope m of the straight line connecting the two points. The specific calculation method for m is as follows:

[0121]

[0122] The equation of the line is:

[0123] y-y1=m(x-x1);

[0124] That is: m·x-y+y1-m·x1=0;

[0125] Converting the equations of the above lines into standard form, we get: a = m, b = -1, c = y1 - m·x1.

[0126] The distance d from each point in the point cloud segment to the aforementioned line is calculated by iterating through the points. When d ≤ threshold, the point is considered an interior point. The specific formula for calculating the distance d from a point to the line is as follows:

[0127]

[0128] Among them, (x i ,y i ) represents a point in a given point cloud segment. We repeatedly select two different points to fit a straight line, count the number of interior points on each fitted line, and select the line with the most interior points as the optimal fitting result.

[0129] S213: Fit the general linear equations of the point clouds radar_left' and radar_right' using the RANSAC algorithm to obtain the direction vectors in the xoy coordinate system. and For vectors and Perform the following calculations: dot product, norm of each vector, and angle θ between the vectors:

[0130]

[0131] In this embodiment, step S22 specifically includes the following steps:

[0132] Rotate both the radar_left and radar_right point cloud fragments around the X-axis by an angle θ. roll Rotation angle θ around the Y-axis Calculate the spatial angle formed by "radar_left" and "radar_right" after each rotation. Iterate through all θ values, and the angle closest to 90° is the corresponding pitch calibration angle θ. pitch Wherein, the rotation transformation matrix T is a 4×4 homogeneous transformation matrix, and the specific formula for calculating T is:

[0133]

[0134]

[0135] Where θ is the pitch angle, R pitch ×R roll It is a 3x3 rotation matrix, representing a rotation θ about the X-axis. roll 1 degree, then rotate θ degrees around the Y-axis;

[0136] Using the aforementioned matrix, the point clouds of radar_left and radar_right are rotated. A straight line is fitted to the transformed point clouds radar_left and radar_right using the RANSAC algorithm, calculated in the same way as in S212. Then, the direction vectors in the xoy coordinate system are obtained, and the angle between the two vectors is calculated in the same way as in S213. After iterating through the pitch angles θ, the angle closest to 90° is taken as the calibrated angle θ. pitch .

[0137] In this embodiment, step S23 specifically includes the following steps:

[0138] Solid-state line laser point cloud: The radar_left and radar_right point clouds are rotated and transformed using the calculation formula in S22, where R pitch ×R roll Represents the rotation θ about the X-axis roll Degrees, then rotate θ around the Y-axis. pitch For the transformed point cloud radar_left"' and radar_right"', the direction vector in the xoy coordinate system is obtained by straight line fitting in S212. and After normalization, we can obtain the unit vector. and Calculate the angle bisector vector

[0139] Single-line LiDAR point cloud: The laser_left and laser_right point clouds are fitted with straight lines in S212 to obtain the direction vector in the xoy coordinate system. After normalization, the unit vector can be obtained. and Calculate the angle bisector vector

[0140] Then, the angle θ between the angle bisector vector of the solid-state line laser and the single-line lidar is calculated. The specific calculation formula is as follows:

[0141] cosθ=v l ·n l

[0142]

[0143] Where θ is the final calibration angle θ. yaw .

[0144] In this embodiment, step S3 specifically includes the following steps:

[0145] S31: When processing solid-state line laser ground point clouds (radar_ground), a pose transformation must first be performed. The transformed point cloud is denoted as radar_ground'. The rotation transformation matrix T is a 4×4 homogeneous transformation matrix, and the specific formula for calculating T is as follows:

[0146]

[0147] Among them, R yaw ×R pitch ×R roll It is a 3x3 rotation matrix representing a rotation θ about the X-axis. roll degrees, and then rotate θ around Y. pitch degrees, and finally the rotation θ around the Z-axis. yaw Spend.

[0148] S32: Traverse all points in the point cloud radar_ground', store the Z-value information of each point in the vector list z_data, sort the elements of z_data in ascending order from smallest to largest, and denote it as z_data'. Use the IQR method to obtain the range of the middle 50% of the data in z_data' and find outliers. The specific calculation formula is as follows:

[0149] Select the 25th percentile Q1 and the 75th percentile Q3;

[0150] IQR = Q3 - Q1;

[0151] Therefore: Low bound: low_bound = Q1 - 1.5 × IQR

[0152] High boundary: up_bound = Q3 + 1.5 × IQR;

[0153] Then, use binary search to find the valid Z values ​​within the high and low boundary range of the elements in z_data', and obtain the starting index of the valid element and the ending index of the valid element in z_data'. The optimal Z value is then:

[0154]

[0155] In this embodiment, step S4 specifically includes the following steps:

[0156] The point cloud radar_cloud of the solid-state line laser is rotated and transformed using the transformation method in S31. Then, the coordinates of the nearest corner point radar_point of the point cloud are calculated by corner point segmentation in S12.

[0157] The laser radar point cloud (laser_cloud) is directly segmented using the corner points in S12, and the coordinates of the nearest corner point (laser_point) are calculated. The offset between the coordinates (radar_point) and (laser_point) is then calculated, which gives the required x-axis and y-axis coordinates for calibration. The specific calculation method is as follows:

[0158]

[0159] The above description is merely a preferred embodiment of the present invention and is illustrative rather than restrictive. Those skilled in the art will understand that many changes, modifications, and even equivalents can be made within the spirit and scope defined by the claims of the present invention, all of which will fall within the protection scope of the present invention.

Claims

1. A method for extrinsic parameter pose calibration of a solid-state line laser, characterized in that, The pose calibration method specifically includes the following steps: S1: Convert the data of solid-state line laser and single-line lidar from polar coordinates to point cloud form, and use the nearest corner segmentation method and the maximum change angle segmentation method to segment the given point cloud as required; S2: After performing coordinate rotation transformation on each point in the point cloud segment according to the calibration requirements, the RANSAC algorithm is used for the point cloud. By randomly sampling two points to fit a straight line and filtering the interior points based on a distance threshold, this process is iterated to select the straight line with the most interior points as the optimal fitting result. The above process is performed on the point clouds formed by two solid-state line lasers to obtain their respective optimal fitting straight lines, and the angle between these two straight lines is calculated. Using the calculated angle information, the roll angle, pitch angle, and yaw angle are calibrated in sequence. S3: First, the ground point cloud data radar_ground obtained by solid-state line laser scanning is processed according to a pre-calibrated roll angle θ. roll Pitch angle θ pitch and yaw angle θ yaw The rotation transformation (attitude correction) is performed sequentially. Then, the Z coordinate values ​​(height information) of all points are extracted from the transformed point cloud. Next, the IQR (interquartile range) method is used to sort and filter these height values ​​to remove statistical outliers. Finally, the average of the remaining valid values ​​is calculated, which is the final calibrated Z-axis coordinate. S4: Align the corner positions of the lidar point cloud (laser_cloud) with the solid-state linear lidar point cloud (radar_cloud), and calculate the offset between the corner points. x and offset y , which are the x-axis and y-axis coordinates that need to be calibrated.

2. The solid-state line laser extrinsic parameter pose calibration method as described in claim 1, characterized in that, Step S1 specifically includes the following steps: S11: Convert the polar coordinates of the laser point to a point cloud. The data conversion expressions for solid-state line lasers and single-line lidar are as follows: Where, θ i =θ min +i·Δθ (i=0,1,...,N-1), r is the distance measured by the laser point, θ min The minimum offset starting angle of the sensor is Δθ, where Δθ is the angle increment. After conversion, the solid-state line laser point cloud information is denoted as radar_cloud, and the lidar point cloud information is denoted as laser_cloud; S12: Nearest Corner Segmentation Method: Taking the robot's geometric center as the origin (0,0), traverse the given point cloud dataset, calculate the distance from each point to the origin, and record the index of the nearest point; then, using this index as the boundary, divide the point cloud data into two parts. Through the above process, the given solid-state laser point cloud and lidar point cloud are divided into left and right parts respectively. The specific formula for the distance function dist from the origin is: Iterate through the dist corresponding to each point in the point cloud dataset, find the smallest dist, and this point is the nearest corner point. Then, use its corresponding index... min The point cloud is divided into left and right parts; Among them, after solid-state line laser point cloud segmentation, they are denoted as radar_left and radar_right respectively; After the LiDAR point cloud is segmented, it is denoted as laser_left and laser_right respectively; S13: Maximum Change Angle Segmentation Method: Calculates the angle formed by each point in a given point cloud dataset with points spaced windowSize before and after it. The specific formula for calculating the angle is as follows: Where A, B, and C are three points, (BA)·(CA) represents the dot product, and |BA| and |CA| are the magnitudes of the vectors; Calculate the index of the point in the radar_left and radar_right point clouds where the angle change is the largest. angle The points on the right-angled walls are divided into radar_left_1 and radar_right_1, and the points on the ground are divided into radar_left_2 and radar_right_2. Superimpose radar_left_2 and radar_right_2 to obtain the complete ground point cloud, denoted as radar_ground, and rename radar_left_1 as radar_left and radar_right_1 as radar_right.

3. The solid-state line laser extrinsic parameter pose calibration method as described in claim 1, characterized in that, S2 specifically includes the following steps: S21: Calibrate roll angle θ roll ; S22: Calibrate the pitch angle θ pitch ; S23: Calibrate yaw angle θ yaw .

4. The solid-state line laser extrinsic parameter pose calibration method as described in claim 1, characterized in that, S21 specifically includes the following steps: S211: Rotate both the radar_left and radar_right point cloud fragments around the X-axis by an angle. Calculate the spatial angle formed by radar_left' and radar_right' after each rotation, iterate through all θ, and find the rotation angle that maximizes the angle; this is the desired calibration angle θ. roll Wherein, the rotation transformation matrix T is a 4×4 homogeneous transformation matrix, and the specific formula for calculating T is: Where θ is the roll angle, in radians, and R... 3x3 It is a 3x3 rotation matrix representing the rotation about the X-axis, t is the displacement vector, which is set to zero in this formula, so it means there is no translation, and the lower half [0 1] represents the extended part in homogeneous coordinates; Select a point from the original point cloud fragment and denote it as P, then P = (x, y, z). Transform it using matrix T to obtain P' = (x', y', z'), which can be expressed as P' = T·P. h , where P h =(x,y,z,1) T This is the homogeneous coordinate form of point P. The specific transformation operations are as follows: S212: The standard form of the general linear equation expression fitted by the RANSAC algorithm is: ax + by + c = 0 Where a, b, and c are the coefficients of the line, and x and y are the coordinates of a point on a two-dimensional plane; Randomly select two points from the point cloud fragment, namely (x1, y1) and (x2, y2), and calculate the slope m of the straight line connecting the two points. The specific calculation method for m is as follows: The equation of the line is: y-y1=m(x-x1); Converting the equations of the above lines into standard form, we get: a = m, b = -1, c = y1 - m·x1; The distance d from each point in the point cloud segment to the aforementioned line is calculated by iterating through the points. When d ≤ threshold, the point is considered an interior point. The specific formula for calculating the distance d from a point to the line is as follows: Among them, (x i ,y i () represents a point in a given point cloud segment; Repeatedly select two different points to fit a straight line, count the number of interior points on each fitted line, and select the line with the most interior points as the optimal fitting result. S213: Fit the general line equations of the point cloud radar_left' and radar_right' using the RANSAC algorithm to obtain the direction vectors in the xoy coordinate system. and For vectors and Perform the following calculations: dot product, norm of each vector, and angle θ between the vectors; The specific calculation process is as follows:

5. The solid-state line laser extrinsic parameter pose calibration method as described in claim 1, characterized in that, S22 specifically includes the following steps: Rotate both the radar_left and radar_right point cloud fragments around the X-axis by an angle θ. roll Rotation angle θ around the Y-axis Calculate the spatial angle formed by radar_left and radar_right after each rotation, iterate through all θ, and the angle closest to 90° is the corresponding pitch calibration angle θ. pitch Wherein, the rotation transformation matrix T is a 4×4 homogeneous transformation matrix, and the specific formula for calculating T is: Where θ is the pitch angle, R pitch ×R roll It is a 3x3 rotation matrix, representing a rotation θ about the X-axis. roll 1 degree, then rotate θ degrees around the Y-axis; Using the aforementioned matrix, the point clouds of radar_left and radar_right are rotated. A straight line is fitted to the transformed point clouds radar_left and radar_right using the RANSAC algorithm, calculated in the same way as in S212. Then, the direction vectors in the xoy coordinate system are obtained, and the angle between the two vectors is calculated in the same way as in S213. After iterating through the pitch angles θ, the angle closest to 90° is taken as the calibrated angle θ. pitch .

6. The solid-state line laser extrinsic parameter pose calibration method as described in claim 1, characterized in that, S23 specifically includes the following steps: Solid-state line laser point cloud: The radar_left and radar_right point clouds are rotated and transformed using the calculation formula in S22, where R pitch ×R roll Represents the rotation θ about the X-axis roll Degrees, then rotate θ around the Y-axis. pitch For the transformed point cloud radar_left"' and radar_right"', the direction vector in the xoy coordinate system is obtained by straight line fitting in S212. and After normalization, we can obtain the unit vector. and Calculate the angle bisector vector Single-line LiDAR point cloud: The laser_left and laser_right point clouds are fitted with straight lines in S212 to obtain the direction vector in the xoy coordinate system. After normalization, the unit vector can be obtained. and Calculate the angle bisector vector Then, the angle θ between the angle bisector vector of the solid-state line laser and the single-line lidar is calculated. The specific calculation formula is as follows: Where θ is the final calibration angle θ. yaw .

7. The solid-state line laser extrinsic parameter pose calibration method as described in claim 1, characterized in that, S3 specifically includes the following steps: S31: When processing solid-state line laser ground point clouds (radar_ground), a pose transformation must first be performed on them. The transformed point cloud is denoted as radar_ground'. The rotation transformation matrix T is a 4×4 homogeneous transformation matrix. The specific formula for calculating T is as follows: Among them, R yaw ×R pitch ×R roll It is a 3x3 rotation matrix representing a rotation θ about the X-axis. roll degrees, and then rotate θ around Y. pitch degrees, and finally the rotation θ around the Z-axis. yaw Spend; S32: Traverse all points in the point cloud radar_ground', store the Z-value information of each point in the vector list z_data, sort the elements of z_data in ascending order from smallest to largest, and denote it as z_data'. Use the IQR method to obtain the range of the middle 50% of the data in z_data' and find outliers. The specific calculation formula is as follows: Select the 25th percentile Q1 and the 75th percentile Q3; IQR = Q3 - Q1; Therefore: Low bound: low_bound = Q1 - 1.5 × IQR High boundary: up_bound = Q3 + 1.5 × IQR; Then, use binary search to find the valid Z values ​​within the high and low boundary range of the elements in z_data', and obtain the starting index of the valid element and the ending index of the valid element in z_data'. The optimal Z value is then:

8. The solid-state line laser extrinsic parameter pose calibration method as described in claim 1, characterized in that, S4 specifically includes the following steps: The point cloud radar_cloud of the solid-state line laser is rotated and transformed using the transformation method in S31. Then, the coordinates of the nearest corner point radar_point of the point cloud are calculated by corner point segmentation in S12. The laser radar point cloud (laser_cloud) is directly segmented by the corner points in S12, and the coordinates of the nearest corner point (laser_point) of the point cloud are calculated. Then, calculate the position offsets of the coordinates radar_point and laser_point, which are the required x-axis and y-axis coordinates for calibration. The specific calculation method is as follows:

Images