Multi-radar calibration method and system in automatic loading scene

Abstract

The invention discloses a multi-radar calibration method and system in an automatic loading scene, and the method comprises the steps: sequentially carrying out the preprocessing of the point cloud data of radars through a statistical filtering algorithm and a voxel down-sampling algorithm, and obtaining the preprocessed point cloud of a radar 1 and a radar 2; respectively aligning the radar 1 and the radar 2 with the ground, constraining a solution space range of point cloud alignment, and obtaining point clouds P1 and P2 of the radar 1 and the radar 2 after alignment with the ground normal vector; the point clouds P1 and P2 of the radar 1 and the radar 2 are cut by using a field angle common-view area range cutting method, the point clouds between the radar 1 and the radar 2 are matched and aligned, and matching result rigid body transformation 2T1 is obtained. And finally, respectively aligning the radar 1 and the radar 2 with the working space coordinate system. According to the method, calibration can be carried out under the condition that a calibration reference object is not needed, point cloud matching can be realized under the condition that a radar common-view area is extremely small, the reliability and robustness of calibration are ensured, and the method has relatively good practicability.

Description

Technical Field

[0001] The present invention belongs to the technical field of multi-radar calibration, and particularly relates to a calibration method and system for multi-radars in an automatic loading scenario. Background Art

[0002] In the logistics industry, loading and handling is one of the crucial links. It not only relates to the safe transportation of goods but also directly affects logistics efficiency. However, traditional loading methods often have many problems, such as low efficiency, high labor costs, and irregular loading. These problems seriously restrict the development of the logistics industry. To solve these problems, automatic loading machines have emerged, bringing a revolutionary change to the loading operation.

[0003] Carriage scanning and recognition is a key part of automatic loading, which provides basic data for automatic loading and directly affects the loading quality and final effect. Currently, there are single-radar and multi-radar schemes for carriage scanning and recognition. In the multi-radar carriage scanning and recognition, radar calibration is required. The existing known radar calibration schemes need external specially-made calibration reference objects, obtain point clouds by scanning the known reference objects, and obtain calibration results through point cloud matching. Secondly, the point cloud matching technology ICP (Iterative Closest Point) method used in the existing schemes has very few correct matches due to the extremely small common viewing area when performing corresponding point cloud pairing and matching, resulting in ICP not converging.

[0004] In summary, the existing technology has the following defects:

[0005] ① The existing schemes require specially customized calibration reference objects, making the calibration process troublesome and inefficient. Moreover, it is difficult to guarantee the accuracy during the production process of the calibration reference objects, and the high-precision calibration objects are costly.

[0006] ② When using point cloud matching for external parameter calibration between multiple radars in the traditional scheme, since multiple radars need to comprehensively scan the carriage, the common viewing area between the radars is necessarily too small, and the common viewing area only accounts for a very small part of the entire field of view, resulting in incorrect point cloud matching or even complete inability to match. Eventually, the calibration fails.

[0007] ③ The existing schemes only calibrate the external parameters between the radars, and the external parameters between the radar and the loading work area are not processed. Summary of the Invention

[0008] The purpose of the present invention is to provide a calibration method and system for multi-radars in an automatic loading scenario, aiming to solve the above problems. The present invention can be applicable to the situation of using multiple radars for vehicle scanning in an automatic loading scenario to automatically calibrate the external parameters of the radars.

[0009] The present invention is mainly implemented through the following technical solutions:

[0010] A calibration method for multiple radars in an automatic loading scenario, comprising the following steps:

[0011] Step S1: Sequentially preprocess the point cloud data of the radars using a statistical filtering algorithm and a voxel downsampling algorithm to obtain the preprocessed point clouds of Radar 1 and Radar 2;

[0012] Step S2: Align Radar 1 and Radar 2 with the ground respectively, constrain the solution space range of the point cloud alignment, and obtain the point clouds P 1 and P 2 ;

[0013] Step S3: Use the method of cropping the co-visible area range of the field of view to crop the point clouds P 1 and P 2 of Radar 1 and Radar 2 respectively, match and align the point clouds between Radar 1 and Radar 2, and obtain the rigid body transformation 2 T 1 ;

[0014] Step S4: Align Radar 1 and Radar 2 with the working space coordinate system respectively.

[0015] To better implement the present invention, further, the step S2 includes the following steps:

[0016] Step S21: Use the ransac plane extraction algorithm to extract all planes of the point clouds of Radar 1 and Radar 2 respectively;

[0017] Step S22: Based on all the planes of the point clouds of Radar 1 and Radar 2 respectively, extract the ground where the point clouds are located;

[0018] Step S23: Align the point clouds of Radar 1 and Radar 2 with the ground respectively: Construct a rotation matrix R g to make R g ×N = Z, where N is the original ground normal and Z is the normal to be aligned; Rotate the input point cloud based on the rotation matrix R g to obtain the point clouds P 1 and P 2 .

[0019] To better implement the present invention, further, the step S21 includes the following steps:

[0020] Step S211: Initialize an empty list;

[0021] Step S212: Process the input point cloud using the RANSAC plane extraction algorithm to obtain a plane of this point cloud and the remaining point cloud, and store the plane in a list;

[0022] Step S213: Substitute the remaining point cloud into Step S212 again for recursive processing until the number of remaining point clouds is lower than the threshold and then stop.

[0023] To better implement the present invention, further, the step S22 includes the following steps:

[0024] Step S221: Obtain the list storing the point cloud planes and the preprocessed point cloud;

[0025] Step S222: Sort the point cloud planes in the list in descending order according to the number of point clouds, and only keep the top n planes with the highest number of points;

[0026] Step S223: For each plane, determine whether the input point cloud is on one side of this plane, and discard the planes with point clouds on both sides; then, take the plane with the largest number of point clouds among the remaining planes as the ground.

[0027] To better implement the present invention, further, the step S223 includes the following steps:

[0028] Step A1: Initialize the point cloud count count_plus in the positive direction of the plane and the point cloud count count_minus in the negative direction of the plane;

[0029] Step A2: Traverse each point in the point cloud and calculate its distance to the plane;

[0030] Step A3: If the distance > the threshold, then increment count_plus by 1; if the distance < -the threshold, then decrement count_minus by 1;

[0031] Step A4: Repeat Step A2 and Step A3 until all points in the point cloud are calculated; if count_plus × count_plus < 0, it is considered that there are point clouds on both sides of the plane, otherwise, it is considered that there is only point cloud on one side of the plane.

[0032] To better implement the present invention, further, the step S23 includes the following steps:

[0033] Step S231: Through cross product operation, calculate a vector V that is perpendicular to both the original ground normal N and the normal Z to be aligned, and use it as the rotation axis; where V = N^Z, where N ^ is the ground normal for the anti-symmetric operation;

[0034] Step S232: Calculate the rotation angle α = arccos(N·Z) by which the original ground normal N rotates to the normal Z along the rotation axis V;

[0035] Step S233: Obtain the rotation matrix R that levels the ground of the point cloud from the rotation axis V and the rotation angle α through the Rodriguez formula g :

[0036] R g = cos(α)I + (1 - cos(α))VV T + sin(α)V^.

[0037] To better implement the present invention, further, the step S3 includes the following steps:

[0038] Step S31: Calculate the initial value of the pose transformation between radars 2 T 10 ;

[0039]

[0040] where: α 0 is the initial value of the rotation angle of 180° around the Z axis preset in terms of rotation;

[0041] t x is the translation distance along the X axis preset in terms of translation;

[0042] Step S32: Calculate the rough value of the pose transformation between radars based on the initial value 2 T 10 ;

[0043] Step S321: Use the common view frustum culling method to cull the point clouds P 1 and P 2 of radar 1 and radar 2 respectively, to obtain the point clouds P 1c and P 2c ;

[0044] Step S322: Adopt the point-to-point ICP algorithm for rough matching to obtain the matching result rigid body transformation T tmp ;

[0045] Step S323: Update the initial value 2 T 10 to T tmp ;

[0046] Step S324: Repeat steps S321 - S323 until the set number of iterations is reached, and use the finally obtained 2 T 10 as the rough value of the pose transformation between radars;

[0047] Step S33: Calculate the exact value of the pose transformation between radars based on the rough value in Step S32 2 T 1 :

[0048] Step S331: Use the common view frustum culling method to cull the point clouds P 1 and P 2 of Radar 1 and Radar 2 respectively, and set the culling angle threshold to Fov×1.1 to obtain the common view frustum point clouds P' 1c and P' 2c ;

[0049] Step S332: Adopt the normal estimation algorithm to calculate the normal estimations of the point clouds P' 1c and P' 2c respectively, and obtain the normal point clouds N 1c and N 2c ;

[0050] Step S333: Adopt the point-to-plane ICP point cloud matching algorithm to calculate the exact matching of the two point clouds, and obtain the matching result rigid body transformation 2 T 1 .

[0051] To better implement the present invention, further, in the Step S321, the common view frustum culling method includes the following steps:

[0052] Step C1: Obtain the initial value 2 T 10 and the rotation matrices 1 R g and 2 R g of Radar 1 and Radar 2 aligned with the ground respectively;

[0053] Step C2: Respectively construct the rigid body transformations 1 R g and 2 R g from the rotation matrices of Radar 1 and Radar 2 1 'T g and 2 'T g :

[0054]

[0055] Step C3: Calculate the pose transformations 1 T g and 2 T g , 1 T g = 2T 10 × 1 'T g , 2 T g = 2 T 10 × 2 'T g ;

[0056] Step C4: Calculate the coordinates p 1 and p 2 on the ground for Radar 1 and Radar 2 respectively:

[0057] p 1 = 1 T g [0 0 0 1] T ,P 2 = 2 t G [0 0 0 1] T ;

[0058] Step C5: Calculate the x-axis vectors V 1X and V 2X for Radar 1 and Radar 2 respectively:

[0059] V 1x = g T 1 [1 0 0 0] T ,V 2x = g T 2 [1 0 0 0] T ;

[0060] Step C6: Calculate the vectors V 1j from each point in the point cloud of Radar 1 to Radar 2 2k and the vectors V 1j 1j from each point in the point cloud of Radar 2 to Radar 2 1 respectively: 2k -p 2k ,V 2 =P 1j -p 2k ;

[0062] where: j is the j-th point in the point cloud of Radar 1;

[0063] k is the k-th point in the point cloud of Radar 2;

[0064] Step C7: Calculate the angle α 1j between each point in the point cloud of Radar 1 and the x-axis of Radar 2 2k and the angle α 1j between each point in the point cloud of Radar 2 and the x-axis of Radar 1 1c respectively:

[0065]

[0066] Step C8: Save all the points in the point cloud of Radar 1 that satisfy |α 1j | < Fov × 1.5 to the point cloud P 1c ; Save all the points in the point cloud of Radar 2 that satisfy |α 2k | < Fov × 1.5 to the point cloud P 2c , as the common perspective point cloud; where Fov is the field of view angle of the laser.

[0067] To better implement the present invention, further, the step S4 includes the following steps:

[0068] Step S41: Transform the point cloud of Radar 1 processed in step S2 into P' 1 = 2 T 1 × P 1 , calculate the merged point cloud P merge = P' 1 + P 2 ;

[0069] Step S42: Use the voxel filtering method to downsample the point cloud P merge and obtain P downsample , removing the duplicate point cloud caused by the merger;

[0070] Step S43: According to the preset height of the loading machine, crop the point cloud P within the range where the loading machine guide rail is located cut ;

[0071] Step S44: Use the ICP algorithm to perform point cloud matching between the preset point cloud of the loading machine guide rail P rail and the point cloud P cut , respectively obtaining the rigid body transformation matrices T rail-left and T rail-right for the left and right guide rails;

[0072] Step S45: Calculate the x-axis offset and y-axis offset:

[0073] Bias x = 0.5(T rail-left [0,3] + T rail-right [0,3]), Bias y = 0.5(T rail-left [1,3] + T rail-right [1,3]);

[0074] Step S46: Calculate the yaw-axis offset Angle z :

[0075] Angle z = 0.5(Angle rail-left + Angle rail-right );

[0076] Where: Angle rail-left is the Z-axis component of the Euler angle calculated from the rigid body transformation matrix T rail-left ;

[0077] Angle rail-right is the Z-axis component of the Euler angle calculated from the rigid body transformation matrix T rail-right ;

[0078] Step S47: Calculate the rotation matrix R, translation vector M of the aligned workspace coordinate system, and the rigid body transformation matrix workspace T merge :

[0079]

[0080] M = [Bias x Bias y Bias z T ;

[0081]

[0082] Step S48: Calculate the transformation matrix from Radar 1 to the loading machine workspace:

[0083] workspace T 1 = workspace T merge 2 R 1 1 T g ;

[0084] Calculate the transformation matrix from Radar 2 to the loading machine workspace:

[0085] workspace T 2 = workspace T merge 2 T g .

[0086] The present invention is mainly implemented through the following technical solutions:

[0087] ​An automatic loading scenario multi-radar calibration system, which is based on the above-mentioned multi-radar calibration method in the automatic loading scenario, includes a radar point cloud data preprocessing module, a radar and ground alignment module, a point cloud matching and alignment module between radars, and a radar and working space coordinate system alignment module; the radar and ground alignment module is used to constrain the solution space range of point cloud alignment through ground alignment processing, and the point cloud matching and alignment module between radars is used to crop the point cloud of the radar by the method of cropping the co-visual area range of the field of view angle and realize the matching and alignment of the point cloud between radars.

[0088] The beneficial effects of the present invention are as follows:

[0089] The present invention can perform calibration without a calibration reference object, solves the collateral problems such as the production and carrying of the calibration reference, saves time and resources, and improves the calibration efficiency. Different from the traditional iterative ICP or the global ICP method based on the FPFH descriptor, the present invention avoids obtaining incorrect matching relationships in the corresponding point matching of ICP by the method of field of view angle cropping. The present invention can realize point cloud matching in the case of an extremely small co-visual area of radars, and solves the problem that the traditional method cannot match point clouds with only a small co-visual area. The present invention also calibrates the external parameters of the radar in the loading work area, improving the work efficiency. Aiming at the situation of a small co-visual area of multiple radars in the automatic loading scenario, the present invention can ensure successful calibration, ensuring the reliability and robustness of the calibration, and having good practicability. Description of the Drawings

[0090] Figure 1 It is a distribution schematic diagram of Radar 1 and Radar 2 in Embodiment 1;

[0091] Figure 2 It is a flowchart of the multi-radar calibration method in the automatic loading scenario of the present invention;

[0092] Figure 3 It is a schematic diagram of the preprocessed point cloud of one of the radars;

[0093] Figure 4 It is a schematic diagram of the effect after the point clouds of Radar 1 and Radar 2 are respectively aligned with the ground;

[0094] Figure 5 It is a schematic diagram of the effect after cropping the co-visual area from the point clouds of Radar 1 and Radar 2;

[0095] Figure 6 It is a schematic diagram of the overall effect after rough matching of the co-visual areas of the point clouds of Radar 1 and Radar 2;

[0096] Figure 7 It is a schematic diagram of the local effect after rough matching of the co-visual areas of the point clouds of Radar 1 and Radar 2;

[0097] Figure 8 It is a schematic diagram of the effect after rough and fine matching of the co-visible areas of the point clouds of Radar 1 and Radar 2;

[0098] Figure 9 It is a schematic diagram of the guide rail identified by the present invention;

[0099] Figure 10 It is a schematic diagram of the effect of the final calibration of the present invention. Specific implementation manner

[0100] Example 1:

[0101] A calibration method for multiple radars in an automatic loading scenario restricts the solution space range of point cloud alignment through ground alignment processing; meanwhile, a method of cropping the co-visible area range of the field of view angle is used to avoid incorrect matching relationships when interpolating corresponding points in ICP (Iterative Closest Point) point cloud matching. As Figure 2 shown, it specifically includes the following steps:

[0102]

a

[0103]

b

[0104]

c

[0105]

d

[0106] Preferably, as Figure 3 shown, the specific calculation process of the above-mentioned

a

[0107] (a1) First, through the driver program of the laser device, the original point cloud data of the lidar is obtained, and it is accumulated for a period of time. For Radar 1, the data point cloud 1 is obtained, and for Radar 2, the data point cloud 2 is obtained

[0108] (a2) The statistical filtering algorithm provided by the PCL point cloud algorithm library is called for outlier removal of the original point cloud. When calling, the parameters of the statistical filtering algorithm are set as follows: the standard deviation is set to 2.0, and the number of points for K-nearest neighbor search is set to 10.

[0109] (a3) The voxel downsampling algorithm provided by the PCL point cloud algorithm library is called for the original point cloud to reduce the number of point clouds and thus reduce the subsequent calculation time. Specifically, when calling, the voxel size of the voxel downsampling is set to 0.03.

[0110] (a4) For each radar, the above operation steps of (a1)-(a3) are executed. For Radar 1, the preprocessed point cloud 1 is obtained; for Radar 2, the preprocessed point cloud 2 is obtained.

[0111] Preferably, as Figure 4 shown, where the point clouds of radar 1 and radar 2 are marked with red and green distributions; the specific process of aligning the [b] radar with the ground is as follows:

[0112] (b1) Extract all planes of the point cloud through the plane extraction module;

[0113] (b2) Extract the ground where the point cloud is located through the ground extraction module;

[0114] (b3) Align the point cloud with the ground through the ground alignment module;

[0115] (b4) Perform the above steps from (b1) to (b3) on the point cloud of each radar.

[0116] Furthermore, where (b1) the specific process of the plane extraction module is as follows:

[0117] (b1.1) Initialize an empty list to store the point cloud planes;

[0118] (b1.2) For the input point cloud, call the ransac (random sample consensus) plane extraction algorithm of the pcl point cloud algorithm library to obtain a plane of this point cloud and the remaining point cloud. Put this plane into the list in (b1.1). Where the plane includes the plane equation and the corresponding point cloud.

[0119] (b1.3) Substitute the remaining point cloud produced in step (b1.2) into step (b1.2) again for recursive processing until the number of remaining point clouds is less than 200 and then stop.

[0120] (b1.4) The plane extraction is completed, and the plane list in (b1.1) is the output of this module.

[0121] Furthermore, where (b2) the specific process of the ground extraction module is as follows:

[0122] (b2.1) The input data is the plane list produced by the (b1) module and the preprocessed radar point cloud;

[0123] (b2.2) Sort the plane list by the number of points, only keep the top 10 planes with the highest number of points, and discard the remaining planes. According to experience, the number of points in the ground point cloud is often the largest or the second largest. Therefore, the extra planes are discarded here to reduce the subsequent calculation amount. Since voxel downsampling was performed during the previous point cloud preprocessing, the number of points in the planes here can be considered to be basically proportional to the area of this plane.

[0124] (b2.3) In the above plane list, for each plane, determine whether the input point cloud is on one side of the plane. Discard the planes where point clouds exist on both sides of the plane.

[0125] The principle is as follows:

[0126] The ground is always on one side of the point cloud. Because lidar cannot penetrate the ground. Some long walls also have this property. If the wall is too short or has windows, lidar can scan to the other side of the wall, and at this time, point clouds will appear on both sides of the wall. It can be concluded that if point clouds exist on both sides of a plane, then this plane must not be the ground; if point clouds exist on only one side of a plane, then this plane is very likely to be the ground.

[0127] (b2.4) After the above exclusion steps, the plane with the largest number of points among the remaining planes is the ground, which is used as the output result of this ground module.

[0128] Specifically, the process of determining whether the point cloud is on one side of the plane described in (b2.3) is as follows:

[0129] (b2.3.1) The input data is the plane equation of the plane and the input lidar point cloud described in (b2.1).

[0130] (b2.3.2) Initialize the point cloud count in the positive direction of the plane count_plus = 0, and initialize the point cloud count in the negative direction of the plane count_minus = 0.

[0131] (b2.3.3) Traverse each point in the point cloud and calculate its distance to the plane. Use the following formula: distance = ax + by + cz + d. Where x, y, z are the coordinates of the point; a, b, c, d are the coefficients of the plane equation of the plane. The plane equation is given by the aforementioned step (b1.2).

[0132] (b2.3.4) If distance > 0.5, increment count_plus by one; if distance < -0.5, decrement count_minus by one. If count_plus * count_plus < 0, then there are point clouds on both sides of the plane, and the calculation of this module ends; otherwise, continue to traverse and calculate the next point in the point cloud.

[0133] (b2.3.5) Repeat (b2.3.3) and (b2.3.4) until all points in the point cloud have been calculated. If the situation of count_plus * count_plus < 0 does not occur, then there are point clouds on only one side of the plane.

[0134] Furthermore, the specific process of the ground alignment module described in (b3) is as follows:

[0135] (b3.1) The input data is the ground extracted in step (b2) and the input radar point cloud described in (b2.1).

[0136] (b3.2) From the plane equation of the ground (given by the aforementioned (b1.2)) ax + by + cz + d = 0, the normal vector N(a, b, c) of the ground is obtained. It is preset that the ground is vertically upward, and its normal vector should be Z(0, 0, 1).

[0137] (b3.3) Construct the rotation matrix R g , such that R g N = Z, then this rotation matrix can make the ground of the point cloud horizontal.

[0138] (b3.4) Use the aforementioned rotation matrix R g to rotate the input radar point cloud to obtain the point cloud aligned with the ground normal vector.

[0139] Specifically, among them, the construction method of the rotation matrix in (b3.3) is as follows:

[0140] (b3.3.1) The required inputs are the original ground normal vector N(a, b, c) and the normal vector Z(0, 0, 1) to be aligned to. Among them, N is the normal vector of the plane described in (b3.2); Z is the vertically upward vector.

[0141] (b3.3.2) Through the cross product operation, calculate a vector V that is perpendicular to both the normal vectors N and Z as the rotation axis. That is, the rotation axis V = N^Z, where the ^ symbol is the skew-symmetric operation;

[0142]

[0143] (b3.3.3) Through the cosine theorem, calculate the angle α = arccos(N·Z) that the vector N needs to rotate along the rotation axis V to the vector Z;

[0144] (b3.3.4) From the rotation axis Axis and the rotation angle α, the rotation matrix R can be obtained through the Rodriguez formula g :

[0145] R g = cos(α)I + (1 - cos(α))VV T + sin(α)V^.

[0146] Preferably, for the point cloud matching and alignment between the [c] radars, the process is as follows:

[0147] (c1) The required input data is the point cloud after the alignment processing of the aforementioned [b] radar with the ground. Denote the point cloud of radar 1 as P 1, the point cloud of radar 2 is denoted as P 2 .

[0148] (c2) Use the initial value estimation module to calculate the initial value of the pose transformation between radars 2 T 10 .

[0149] (c3) Use the rough matching module to calculate the rough value of the pose transformation between radars.

[0150] (c4) Use the fine matching module to calculate the exact value of the pose transformation between radars 2 T 1 .

[0151] Furthermore, among them, the calculation method of the (c2) initial value estimation module is as follows:

[0152] (c2.1) Since the radar is already aligned with the ground and it is known that the two preset radars are installed opposite to each other, so an initial value α of a 180° rotation around the z-axis is preset in terms of rotation 0 .

[0153] (c2.2) According to the installation experience, a translation distance t of 8 meters along the x-axis is preset in terms of translation x .

[0154] (c2.3) Calculate the initial value 2 T 10 :

[0155]

[0156] Furthermore, secondly, the process of the (c3) rough matching module is as follows:

[0157] (c3.1) As Figure 5 shown, use the common view frustum culling module to cull the two point clouds respectively. For the point cloud P 1 after culling is denoted as P 1c , for the point cloud P 2 after culling is denoted as P 2c .

[0158] (c3.2) As Figure 6 and Figure 7 shown, call the point-to-point ICP algorithm in the pcl algorithm library for rough matching. When calling, set the source point cloud of the point-to-point ICP as P 1c , set the target point cloud as P 2c ; set the maximum number of iterations of the ICP to 1 time; set the distance of the minimum corresponding points to 6.0 meters. After the call is completed, obtain the matching result rigid body transformation T tmp .

[0159] (c3.3) The aforementioned initial value2 T 10 Replace it with the result T of the above (c3.2). tmp Return to step (c3.1), continue, and iterate 100 times in total.

[0160] (c3.4) Iteration is completed, and the latest 2 T 10 is the rough matching result.

[0161] Specifically, the process of the common perspective cropping module described in (c3.1) is as follows:

[0162] (c3.1.1) The input data is the initial value estimation result T of the above (c2), 2 T 10 and the rotation matrices R and R of the two radars aligned with the ground calculated by the [b] radar - ground alignment module 1 R g and 2 R g .

[0163] (c3.1.2) Construct a rigid - body transformation from the rotation matrix:

[0164]

[0165] (c3.1.3) Calculate 1 T g and 2 T g , 1 T g = 2 T 10 × 1' T g , 2 T g = 2 T 10 × 2' T g ;

[0166] (c3.1.4) Calculate the coordinate p of radar 1 on the ground 1 = 1 T g [1 0 0 0] T ; Similarly, calculate the coordinate p of radar 2 on the ground 2 = 2 T g [0 0 0 1] T .

[0167] (c3.1.5) Calculate the x - axis vector V of radar 1 1x = g T 1[1 0 0 0] T ; The x-axis vector V of the same radar 2 2x = g T 2 [1 0 0 0] T ;

[0168] (c3.1.6) Calculate the vector V from each point in the point cloud of radar 1 to radar 2 1j = P 1j - p 1 ; Similarly, calculate the vector V from each point in the point cloud of radar 2 to radar 2 2k = P 2k - p 2 . Where j is the j-th point in point cloud 1; k is the k-th point in point cloud 2.

[0169] (c3.1.7) Calculate the angle between the x-axis of radar 1 and each point in its point cloud as:

[0170]

[0171] Similarly, calculate the angle between the x-axis of radar 2 and each point in its point cloud as:

[0172]

[0173] Where j is the j-th point in point cloud 1; k is the k-th point in point cloud 2.

[0174] (c3.1.8) Save all points in the point cloud of radar 1 that satisfy |α 1j | < Fov × 1.5 to the point cloud P 1c ; Save all points in the point cloud of radar 2 that satisfy |α 2k | < Fov × 1.5 to the point cloud P 2c . The saved point cloud is the common view point cloud. Where Fov is the field of view angle of the laser, which can be obtained from the laser radar device supplier.

[0175] Furthermore, secondly, as Figure 8 shown, the process of the (c4) fine matching module is as follows:

[0176] (c4.1) The required input data is the point cloud processed by the aforementioned module [b] radar and ground alignment module. Denote the point cloud of radar 1 as P 1 , and denote the point cloud of radar 2 as P 2 . And the relative pose transformation between radars calculated by the aforementioned (c3) rough matching module.

[0177] (c4.2) Call the aforementioned (c3.1) common view clipping module to process the point clouds P 1and P 2 Perform cropping, but set the cropping angle threshold to Fov * 1.1. Obtain the cropped common viewpoint point cloud P' 1c and P' 2c .

[0178] (c4.3) Use the normal estimation algorithm in the pcl point cloud algorithm library to calculate the normal estimation of the point clouds P' 1c and P' 2c respectively, and obtain the normal point cloud N 1c and N 2c .

[0179] (c4.4) Use the point-to-plane ICP point cloud matching algorithm in the pcl point cloud algorithm library to calculate the precise matching of the two point clouds. When calling, set the source point cloud of the point-to-plane ICP to P' 1c , set the target point cloud to P' 2c ; set the normal of the source point cloud N 1c , set the normal of the target point cloud to N 2c ; set the maximum number of iterations of ICP to 1000 times; set the distance of the minimum corresponding points to 0.1 meters. After the call, obtain the final matching result rigid body transformation 2 T 1 .

[0180] Preferably, finally, the [d] radar is aligned with the workspace coordinate system, and the process is as follows:

[0181] (d1) From the transformation matrix obtained by the precise matching, merge the two laser point clouds. The specific operations are as follows:

[0182] ① Use the rigid body transformation to transform the point cloud of radar 1: P' 1 = 2 T 1 ×P 1 ;

[0183] ② Calculate the merged point cloud P merge = P' 1 + P 2 . Where P 1 and P 2 are the point clouds after the two radars have been aligned to the ground respectively, which are completed by the previous step [b]. Where 2 T 1 is the transformation matrix from point cloud 1 to point cloud 2, which is completed by the previous step [c].

[0184] (d2) Voxel filtering of the point cloud. Call the voxel filtering in the pcl point cloud algorithm library to downsample P merge to remove the duplicate point clouds caused by the merger and reduce the subsequent calculation amount. The filtering result is denoted as P downsample .

[0185] (d3) Point cloud clipping. According to the preset height of the loading machine, clip the approximate range where the loading machine guide rail is located, denoted as P cut 。

[0186] (d4) As Figure 9 shown, match the preset point cloud P of the loading machine guide rail rail with P cut to obtain the poses T rail-left and T rail-right of the left and right guide rails. The guide rail point cloud is obtained by scanning the loading machine guide rail in advance. The point cloud matching algorithm directly calls the ICP algorithm in the pcl point cloud library.

[0187] (d5) Since the working space of the loading machine is in the middle of the left and right guide rails of the loading machine, the y-axis offset is calculated as: Bias x = 0.5(T rail-left [0,3] + T rail-right [0,3]); Similarly, the x-axis offset is Bias y = 0.5(T rail-left [1,3] + T rail-right [1,3]).

[0188] (d6) Since the direction of the working space of the loading machine is the same as the direction of the guide rail, the yaw-axis offset is calculated as: Angle z = 0.5(Angle rail-left + Angle rail-right ). Where Angle rail-left is the z-axis component of the Euler angle calculated from the rigid body transformation matrix T rail-left . Where Angle rail-right is the z-axis component of the Euler angle calculated from the rigid body transformation matrix T rail-right .

[0189] (d7) Since the point clouds P 1 and P 2 are the point clouds after two radars have been aligned to the ground respectively, so Bias z = 0, Angle x = 0, Angle y = 0. Then the rotation matrix for aligning the working space coordinate system:

[0190]

[0191] Translation vector M = [Bias x Bias y Biaz z T ;​

[0192] The rigid body transformation matrix that aligns with the workspace coordinate system is as follows:

[0193]

[0194] (d8) Calculate the transformation matrix from Radar 1 to the working space of the loading machine:

[0195] workspace T 1 = workspace T maerge 2 T 1 1 T g ;

[0196] Calculate the transformation matrix from Radar 2 to the working space of the loading machine:

[0197] workspace T 2 = workspace T merge 2 T g .

[0198] That is, the final calibration result is obtained. As Figure 10 shown, the point cloud of the working space is intercepted for display. The blue part is the ground, and the red parts on the upper and lower sides are the guide rails and their attachments. The white part on the ground is the occluded blank area without point cloud. It can be seen that after calibration, the coordinate system is already parallel to the guide rail; the z-axis of the coordinate system is on the ground; and there is no ghosting in the point clouds of the two radars after calibration.

[0199] The present invention can perform calibration without making a calibration reference object, saving time and resources. It can achieve point cloud matching in the case of an extremely small common viewing area of the radars. It can simultaneously perform external parameter calibration of the working area coordinate system, improving work efficiency. When matching and aligning the point clouds between Radar 1 and Radar 2, the present invention adopts a point cloud matching method with a small common viewing area, which is different from the traditional iterative ICP or the global ICP method based on FPFH descriptors. By means of the method of field of view angle clipping, it avoids obtaining incorrect matching relationships in the corresponding point matching of ICP. The present invention solves the problem that the traditional method cannot match the point clouds with only a small common viewing area, making it possible to match the point clouds in such scenarios.

[0200] The above are only the preferred embodiments of the present invention, and do not impose any form of limitation on the present invention. Any simple modification or equivalent change made to the above embodiments based on the technical essence of the present invention shall fall within the protection scope of the present invention.

Claims

1. A multi-radar calibration method in an automatic loading scenario, characterized in that: The following steps are involved: Step S1: Preprocessing the point cloud data of the radar using a statistical filtering algorithm and a voxel downsampling algorithm in sequence to obtain preprocessed point clouds of radar 1 and radar 2; Step S2: Align radar 1 and radar 2 with the ground respectively, constrain the solution space range of point cloud alignment, and obtain point clouds P1 and P2 of radar 1 and radar 2 aligned with the ground normal vector; Step S3: Use the method of cropping the common view area of ​​the field of view to crop the point clouds P1 and P2 of radar 1 and radar 2 respectively, match and align the point clouds between radar 1 and radar 2, and obtain the rigid body transformation of the matching result 2 T1; Step S4: Align radar 1 and radar 2 with the workspace coordinate system respectively.

2. The method for calibrating multiple radars in an automatic loading scenario according to claim 1, characterized in that: The step S2 comprises the following steps: Step S21: using the RANSAC plane extraction algorithm to extract all planes of the point clouds of radar 1 and radar 2 respectively; Step S22: extracting the ground where the point cloud is located based on all planes of the point cloud of radar 1 and radar 2 respectively; Step S23: Align the point clouds of radar 1 and radar 2 with the ground respectively: construct a rotation matrix R that makes the ground of the point cloud horizontal g , so that R g ×N=Z, where N is the original ground normal and Z is the normal to be aligned; based on the rotation matrix R g The input point cloud is rotated to obtain the point clouds P1 and P2 of radar 1 and radar 2 that are aligned with the ground normal vector.

3. The multi-radar calibration method in an automatic loading scenario according to claim 2 is characterized in that: The step S21 comprises the following steps: Step S211: Initialize an empty list; Step S212: using the RANSAC plane extraction algorithm to process the input point cloud, obtaining a plane of the point cloud and the remaining point clouds, and storing the plane in a list; Step S213: Substitute the remaining point clouds into step S212 again for recursive processing until the number of remaining point clouds is lower than a threshold.

4. The multi-radar calibration method in an automatic loading scenario according to claim 3 is characterized in that: The step S22 comprises the following steps: Step S221: obtaining a list of stored point cloud planes and pre-processed point clouds; Step S222: sorting the point cloud planes in the list from largest to smallest according to the number of point clouds, and only retaining the first n planes with the highest number of points; Step S223: For each plane, determine whether the input point cloud is on one side of the plane, and discard the planes with point clouds on both sides; then, the plane with the largest number of point clouds among the remaining planes is taken as the ground.

5. The method for calibrating multiple radars in an automatic loading scenario according to claim 4, characterized in that: The step S223 comprises the following steps: Step A1: Initialize the point cloud count count_plus in the positive direction of the plane and the point cloud count count_minus in the negative direction of the plane; Step A2: traverse each point in the point cloud and calculate its distance to the plane; Step A3: If the distance > threshold, add 1 to count_plus; if the distance <- threshold, subtract 1 from count_minus; Step A4: Repeat steps A2 and A3 until all points in the point cloud are calculated; if count_plus×count_plus<0, it is considered that there are point clouds on both sides of the plane, otherwise, it is considered that there are point clouds on only one side of the plane.

6. The method for calibrating multiple radars in an automatic loading scenario according to claim 2, characterized in that: The step S23 comprises the following steps: Step S231: Calculate a vector V perpendicular to the original ground normal N and the normal Z to be aligned by cross product operation, and use it as the rotation axis; wherein V=N^Z, wherein N^ is the ground normal of the antisymmetric operation; Step S232: Calculate the rotation angle α=arccos(N·Z) of the original ground normal N along the rotation axis V to the normal Z by the law of cosines; Step S233: Using the rotation axis V and the rotation angle α, the Rodriguez formula is used to obtain the rotation matrix Rg that makes the ground of the point cloud horizontal: R g =cos(α)I+(1-cos(α))VV T +sin(α)V^.

7. The multi-radar calibration method in an automatic loading scenario according to claim 1 is characterized in that: The step S3 comprises the following steps: Step S31: Calculate the initial value of the radar posture transformation 2 T 10 ; Wherein: α0 is the initial value of the 180° rotation angle around the Z axis preset in terms of rotation; t x The preset translation distance along the X axis in translation; Step S32: Based on the initial value 2 T 10 Calculate the rough value of the pose transformation between radars; Step S321: Use the common viewpoint clipping method to clip the point clouds P1 and P2 of radar 1 and radar 2 respectively to obtain point clouds P 1c and P 2c ; Step S322: Use the point-to-point ICP algorithm to perform rough matching and obtain the matching result rigid body transformation T tmp ; Step S323: Initial value 2 T 10 Update to T tmp ; Step S324: Repeat steps S321 to S323 until the set number of iterations is reached, and the final 2 T 10 As a rough value of the pose transformation between radars; Step S33: Calculate the precise value of the inter-radar posture transformation based on the rough value of step S32 2 T1: Step S331: Use the common viewpoint clipping method to clip the point clouds P1 and P2 of radar 1 and radar 2 respectively, and set the clipping angle threshold to Fov×1.1 to obtain the clipped common viewpoint point cloud P' of radar 1 and radar 2. 1c and P' 2c ; Step S332: Calculate the point cloud P' using the normal estimation algorithm 1c and P' 2c Normal estimation, obtain normal point cloud N 1c and N 2c ; Step S333: Use the point-to-surface ICP point cloud matching algorithm to calculate the exact match of the two point clouds and obtain the rigid body transformation of the matching result 2 T1.

8. The method for calibrating multiple radars in an automatic loading scenario according to claim 7, characterized in that: In step S321, the common viewing angle clipping method includes the following steps: Step C1: Get initial value 2 T 10 And the rotation matrix of radar 1 and radar 2 aligned with the ground respectively 1 R g and 2 R g ; Step C2: Rotation matrices of radar 1 and radar 2 1 R g and 2 R g Constructing rigid body transformations 1' T g and 2' T g : Step C3: Calculate the pose transformation of radar 1 and radar 2 1 T g and 2 T g , 1 T g = 2 T 10 × 1' T g , 2 T g = 2 T 10 × 2 'T g ; Step C4: Calculate the coordinates p1 and p2 of radar 1 and radar 2 on the ground respectively: p1= 1 T g [0 0 0 1] T ,p2= 2 T g [0 0 0 1] T ; Step C5: Calculate the x-axis vector V of radar 1 and radar 2 respectively 1X and V 2X : V 1x = g T1[1 0 0 0] T ,V 2x = g T2[1 0 0 0] T ; Step C6: Calculate the vector V1j from each point in the point cloud of radar 1 to radar 2 and the vector Vj from each point in the point cloud of radar 2 to radar 2 respectively. 2k : IN 1j =P 1j -p1,V 2k =P 2k -p2; Where: j is the jth point in the point cloud of radar 1; k is the kth point in the point cloud of radar 2; Step C7: Calculate the x-axis angle α1j between each point in the point cloud of radar 1 and radar 2, and the x-axis angle α1j between each point in the point cloud of radar 2 and radar 1 2k : Step C8: Save all the points in the point cloud of Radar 1 that satisfy |α1j| < Fov×1.5 into the point cloud P1c; save all the points in the point cloud of Radar 2 that satisfy |α 2k | < Fov×1.5 into the point cloud P 2c , as the common view point cloud; where Fov is the field of view angle of the laser.

9. The method for calibrating multiple radars in an automatic loading scenario according to claim 1, characterized in that: The step S4 comprises the following steps: Step S41: transform the point cloud of radar 1 processed in step S2 into P'1= 2 T1×P1, calculate the merged point cloud P merge =P'1+P2; Step S42: Use voxel filtering to filter the point cloud P merge Downsample and get P downsample , remove the point clouds that result in duplication after merging; Step S43: According to the preset height of the loader, the point cloud P of the range where the loader rail is located is cut out. cut ; Step S44: Using the ICP algorithm to transform the preset loader rail point cloud P rail With point cloud P cut Perform point cloud matching to obtain the rigid body transformation matrix T of the left and right rails respectively rail-left and T rail-right ; Step S45: Calculate the x-axis offset and the y-axis offset: Bias x =0.5(T rail-left [0,3]+T rail-right [0,3]),Bias y =0.5(T rail-left [1,3]+T rail-right [1,3]); Step S46: Calculate the yaw axis offset Angle z : Angle z =0.5(Angle rail-left +Angle rail-right ); Where: Angle rail-left is the rigid body transformation matrix T rail-left The Z-axis component of the calculated Euler angle; Angle rail-right is the rigid body transformation matrix T rail-right The Z-axis component of the calculated Euler angle; Step S47: Calculate the rotation matrix R, translation vector M and rigid body transformation matrix of the alignment workspace coordinate system workspace T merge : M=[Bias x Bias y Bias z ] T ; Step S48: Calculate the transformation matrix from radar 1 to the loader workspace: workspace T1= workspace T merge 2 T1 1 T g ; Calculate the transformation matrix from radar 2 to the loader workspace: workspace T2= workspace T merge 2 T g 。 10. A multi-radar calibration system in an automatic loading scenario, based on the multi-radar calibration method in an automatic loading scenario according to any one of claims 1 to 9, characterized in that: The invention comprises a radar point cloud data preprocessing module, a radar and ground alignment module, a point cloud matching and alignment module between radars, and a radar and workspace coordinate system alignment module; the radar and ground alignment module is used to constrain the solution space range of the point cloud alignment through ground alignment processing, and the point cloud matching and alignment module between radars is used to clip the radar point cloud through the method of clipping the common view area range of the field of view angle and realize the matching and alignment of the point clouds between radars.

Images