A simple and accurate method for joint calibration of lidar and millimeter-wave radar

Abstract

The present invention provides a simple and accurate method for joint calibration of laser radar and millimeter wave radar, comprising placing calibration objects in sequence according to a preset calibration object placement rule with the millimeter wave radar as the center; establishing a model coordinate system based on the calibration object, and generating a model point cloud corresponding to the calibration object; performing automatic detection of the dynamic difference set of the calibration object position on N groups of three-dimensional point clouds acquired by the laser radar to obtain the actual point cloud of the laser radar calibration object; calculating the conversion relationship between the model point cloud and each group of actual point clouds of the calibration object, and converting the centroid S of the model point cloud into the actual point cloud of the calibration object; M Convert to the laser radar coordinate system to get the point set S L ; Perform dynamic difference automatic detection of the corner reflector position on the N groups of point cloud data collected by the millimeter wave radar to obtain the actual point set S of the corner reflector in the millimeter wave radar coordinate system R ; According to the point set S in the laser radar coordinate system L And the point set S in the millimeter wave radar coordinate system R , and calculate the transformation matrix between the two coordinate systems as the joint calibration result.

Description

Technical Field

[0001] The present application relates to the technical field of multi-sensor joint calibration, and in particular to a simple and accurate method for joint calibration of a laser radar and a millimeter-wave radar. Background Art

[0002] LiDAR and millimeter-wave radar are commonly used sensors in many fields such as unmanned driving and robot navigation. They play a key role in environmental perception and obstacle detection. However, due to their respective characteristics and different measurement principles, there is a mismatch between the two sensors. LiDAR can provide high-precision distance and position information, but it performs poorly in bad weather conditions. In contrast, millimeter-wave radar has good performance in all weather conditions, but its spatial resolution is relatively low. Therefore, the first step to accurately match the data of LiDAR and millimeter-wave radar is to jointly calibrate the two, that is, to obtain the precise coordinate transformation relationship between LiDAR and millimeter-wave radar, that is, to calculate the external parameter matrix of the two.

[0003] The patent "A joint calibration method for laser radar and millimeter wave radar" (publication number CN111796248A) uses a corner reflector and installs a photosensitive unit at the vertex of the corner reflector for calibration, limiting the photosensitive unit to be located exactly on a certain beam of the laser radar. Due to the certain angular resolution of the laser radar, the photosensitive unit may not be detected by the laser radar at a long distance, and manual intervention is required to place the photosensitive unit to a suitable position; due to the discreteness of the laser point cloud, this placement process is very difficult. The patent "A data calibration method for laser radar and millimeter wave radar in highway tunnels" (publication number CN114755645A) converts the three-dimensional data of the laser radar into a two-dimensional plane and calibrates the millimeter wave radar in a two-dimensional plane to improve the detection of targets in highway tunnels. It is used to solve the detection of targets in tunnels, mainly the detection of vehicle targets, so the calibration accuracy requires the resolution of the vehicle size level. The calibration accuracy of this method is not high, resulting in a large limitation in the scope of application. In addition, this method requires that the installation heights of the laser radar and millimeter-wave radar be equal and high enough, resulting in a small application range. For example, the installation heights of the laser radar and millimeter-wave radar installed on the vehicle are usually not equal. The patent "Construction and calibration method of a multi-sensor online external parameter calibration device" (publication number CN117554982A) uses a number of corner reflectors with different properties of coatings on the inner surface to arrange and combine to construct a calibration device, and uses the laser radar and millimeter-wave radar to obtain the vertex point set inside the corner reflector for matching and calibration. Its laser radar is greatly affected by the reflectivity of different coatings on the corner reflector, the size of the corner reflector, and the ambient light intensity in the acquisition of the laser point cloud of the corner reflector, and it is not easy to obtain stable data; because multiple corner reflectors are required, when these corner reflectors are placed on the same device, either the device will be very large, or the resolution of the millimeter-wave radar needs to be high enough, resulting in inconvenience and limitation in application. In addition, when the reflectivity of the corner reflector is too high, the mirror-like reflection of its triangular cone often makes it impossible for the LiDAR to collect point cloud data from the corner reflector; when the reflectivity of the corner reflector is not high, the millimeter-wave radar cannot distinguish it from the environmental point cloud. Furthermore, for the calculation of the corner reflector vertex of the LiDAR point cloud data, it uses the laser point cloud surface reconstruction method instead of the corner reflector model point cloud matching method, which has poor robustness.

[0004] The patent "One-time joint calibration method for multi-type sensor calibration for intelligent driving" (publication number CN116416319A) is affected by the resolution of millimeter-wave radar. Although multiple corner reflectors are placed on the three-dimensional tower checkerboard, they cannot be distinguished in the millimeter-wave radar point cloud, so the one-to-one correspondence between the laser radar and the millimeter-wave radar of the corner reflector cannot be established, which easily leads to calibration failure. At the same time, the patent does not use the difference set method. It is very difficult and not easy to implement to select the data of the three-dimensional tower checkerboard and the corner reflector from all the one-time data.

[0005] In summary, in response to the joint calibration needs of LiDAR and millimeter-wave radar, it is necessary to develop a special calibration object to stably provide a high-precision feature point set. At the same time, it is necessary to formulate rules for the placement of calibration objects to ensure that the calibration object point cloud data acquired by the millimeter-wave radar and LiDAR can be accurately extracted. In addition, it is also necessary to study and design high-precision and efficient point set matching algorithms to improve the efficiency, robustness and accuracy of the calibration process. Therefore, the present invention develops a lightweight calibration object that only uses one corner reflector. On the basis of ensuring lightness, it proposes to place the calibration objects in sequence according to double straight lines, and provides three methods to solve the initial value of ICP to ensure calibration accuracy. It proposes automatic detection of the dynamic difference set of the calibration object position in the LiDAR point cloud and the dynamic difference set of the corner reflector position in the millimeter-wave radar point cloud to achieve automatic extraction of the target object. Summary of the invention

[0006] The present application provides a simple and accurate method for the joint calibration of LiDAR and millimeter-wave radar, aiming to solve the problems of the existing joint calibration technology, such as the lack of calibration objects that can continuously and stably provide high-precision feature points, difficulty in identifying multiple corner reflectors, inability to accurately extract the calibration object point cloud data of LiDAR and millimeter-wave radar, and the time-consuming and low-accuracy point set matching algorithm.

[0007] The present application provides a simple and accurate method for joint calibration of a laser radar and a millimeter wave radar, the method comprising the following steps:

[0008] S1: With the millimeter-wave radar as the center, the calibration objects are placed in sequence according to the preset calibration object placement rules and the specified collection order;

[0009] S2: Establish a model coordinate system based on the calibration object and generate a model point cloud M corresponding to the calibration object;

[0010] S3: Perform automatic detection of the dynamic difference of the calibration object position on the N groups of three-dimensional point clouds obtained by the laser radar to obtain the actual point clouds of the calibration object LidarROI1, LidarROI2, ...LidarROI in the laser radar coordinate system. N ;

[0011] S4: according to the model point cloud M and N groups of actual point clouds of the calibration object LidarROI1, LidarROI2, ...LidarROI N , determine the conversion relationship between the model point cloud and each set of actual lidar point clouds According to each The centroid S of the model point cloud M Transform to the laser radar coordinate system to obtain the point set S L ;

[0012] S5: Perform dynamic difference automatic detection of the corner reflector position on the N groups of point cloud data collected by the millimeter wave radar to obtain the actual point set S of the corner reflector in the millimeter wave radar coordinate system. R ;

[0013] S6: According to the point set S in the laser radar coordinate system L And the actual point set S of the corner reflector in the millimeter wave radar coordinate system R , determine the rough registration matrix F between the laser radar coordinate system and the millimeter wave radar coordinate system init ;

[0014] S7: According to the rough registration matrix F init , solve the precise registration matrix F of the laser radar coordinate system and the millimeter wave radar coordinate system as the joint calibration result.

[0015] Furthermore, with the millimeter wave radar as the center, the calibration objects are placed in sequence according to the preset calibration object placement rules and the specified collection order, including:

[0016] The millimeter-wave radar is used as the center, and the millimeter-wave radar's own coordinate system is used as the calibration object placement coordinate system. The positive direction of the millimeter-wave radar coordinate system's X-axis is defined as the radar's forward direction; the Y-axis is defined as the horizontal direction, perpendicular to the X-axis; the Z-axis is the vertical axis, perpendicular to the X-axis and the Y-axis, following the right-hand coordinate system rule. The calibration objects are placed in sequence along the straight lines on both sides parallel to the positive direction of the millimeter-wave radar coordinate system's X-axis, including:

[0017] S11: First, determine the distance of the left straight line relative to the center of the millimeter-wave radar in the Y-axis direction;

[0018] Then, place the calibration object along the left straight line in the positive direction of the X-axis of the millimeter-wave radar coordinate system, record the coordinates (x, y) of each placement, ensure that the interval between each placement is not less than 0.5 meters, and perform at least 3 placements;

[0019] S12: First, determine the distance of the right straight line relative to the center of the millimeter-wave radar in the Y-axis direction;

[0020] Place the calibration object along the right straight line in the positive direction of the X-axis of the millimeter-wave radar coordinate system; record the coordinates (x, y) of each placement, ensure that the interval between each placement is not less than 0.5 meters, and place at least 3 times; the calibration object includes a single or multiple cubes and a corner reflector; the material of the cube is a material with wave-absorbing properties;

[0021] If the calibration object includes multiple cubes, the corner reflector is placed at the centroid of the absorbing material, and only one corner reflector is placed on the calibration object. Absorbing material refers to a type of material that can absorb or greatly reduce the electromagnetic wave energy received on the surface. The cube is made of materials with absorbing properties, which can effectively absorb more than 80% of the electromagnetic wave energy emitted by the millimeter-wave radar, and can also reflect the laser beam emitted by the laser radar. The corner reflector can guide and reflect the signal, thereby realizing the precise positioning of the millimeter-wave radar on the calibration object.

[0022] Furthermore, a model coordinate system based on the calibration object is established, and a model point cloud M corresponding to the calibration object is generated, including:

[0023] Taking the lower left point of the calibration object as the origin, establishing an x-axis, a y-axis and a z-axis respectively along the distribution direction of the calibration object to establish the model coordinate system;

[0024] The model point cloud M having a point spacing of less than 1 cm of the calibration object in the model coordinate system is generated according to the structural information of the calibration object.

[0025] Furthermore, the N groups of three-dimensional point clouds acquired by the laser radar are automatically detected by dynamic difference sets of the calibration object positions to obtain the actual point clouds of the calibration object laser radar in the laser radar coordinate system LidarROI1, LidarROI2, …LidarROI N ,include:

[0026] S31: before the calibration object is placed, a background data point cloud set not including the calibration object in a laser radar coordinate system is collected as the laser radar global background point cloud data LidarData0;

[0027] S32: According to the preset calibration object placement rules, the calibration objects are placed in sequence according to the specified collection order and the foreground cloud data LidarData containing the calibration objects in the multi-frame LiDAR coordinate system are collected in sequence i ,i∈[1,N];

[0028] S33: For each set of data LidarData i (i∈[1,N]), according to the preset calibration object placement rules and the measured offset position between the laser radar and the millimeter wave radar, the ROI of the calibration object is determined, and the area is separated from the background LidarData0 and the foreground LidarDatai They are extracted from the laser radar data respectively, and a difference operation is performed to obtain the initial three-dimensional point set;

[0029] Based on the shape and contour features of the calibration object, the initial three-dimensional point set is filtered to remove the stray point cloud, and finally the calibration object point clouds LidarROI1, LidarROI2, ... LidarROI in N Lidar coordinate systems are obtained. N .

[0030] Further, according to the model point cloud M and N groups of actual point clouds of the calibration object LidarROI1, LidarROI2, ...LidarROI N , determine the conversion relationship between the model point cloud and each set of actual lidar point clouds include:

[0031] The model point cloud M generated in the model coordinate system and the actual point cloud of the calibration object in the lidar coordinate system LidarROI1, LidarROI2, ... LidarROI N , using the iterative closest point algorithm, perform 3D point cloud registration to determine the conversion relationship between the model point cloud and each set of actual lidar point clouds Use each The centroid S of the model point cloud M Transform to the laser radar coordinate system to obtain the point set S L ,include:

[0032] S41: For each set of calibration objects, the actual point cloud LidarROI i (i∈[1,N]), using the iterative closest point algorithm, perform 3D point cloud registration with the model point cloud M generated in the model coordinate system to determine the transformation relationship between the model point cloud and the actual point cloud of the lidar under this set of data

[0033] S42: According to Determine the position of the centroid of the model point cloud of the current group of data in the laser radar coordinate system: This is the centroid of the current group of data. The centroid of the data is recorded in S L middle.

[0034] In the formula, is the transformation relationship between the laser radar coordinate system and the model coordinate system under this set of data, S M is the centroid of the point cloud in the model coordinate system, S L Represents the point set of the centroid position of multiple calibration objects placed in the laser radar coordinate system. L In the left straight line, place the S L Defined as S placed along the right straight line LDefined as

[0035] Since the corner reflector is placed at the centroid of the calibration object, the centroid of the default model point cloud is the position of the internal vertex of the corner reflector; therefore, the point set of the centroid of the calibration object is the point set of the positions where the corner reflector is placed multiple times.

[0036] Furthermore, the N groups of point cloud data collected by the millimeter-wave radar are automatically detected by dynamic difference sets of the corner reflector positions to obtain the actual point set S of the corner reflector in the millimeter-wave radar coordinate system. R ,include:

[0037] S51: Before placing the calibration object, a set of background data point cloud sets not including the calibration object in the millimeter-wave radar coordinate system within a fixed time period is collected using the millimeter-wave radar as the millimeter-wave radar global background data RadarData0;

[0038] S52: According to the preset calibration object placement rules, the calibration objects are placed in sequence according to the specified collection order and multiple groups of foreground data point cloud sets RadarData1, RadarData2, ... RadarData containing the calibration objects in the millimeter wave radar coordinate system within a fixed time period are collected in sequence N ;

[0039] S53: For each set of point cloud data RadarData i (i∈[1,N]), according to the preset calibration object placement rules, determine the ROI of the calibration object, and divide the area from background RadarData0 and foreground RadarData i The three-dimensional points are extracted from the millimeter-wave radar data and the difference operation is performed as the initial three-dimensional point set;

[0040] The point cloud with the highest reflection intensity of the initial three-dimensional point set is used as the corresponding internal vertex of the corner reflector, and finally the actual point set S of the corner reflector in the N millimeter-wave radar coordinate system is obtained. R In S R In the left straight line, place the S R Defined as S placed along the right straight line R Defined as

[0041] Further, according to the model point cloud centroid point set S in the laser radar coordinate system L And the actual point set S of the corner reflector in the millimeter wave radar coordinate system R , determine the rough registration matrix F between the laser radar coordinate system and the millimeter wave radar coordinate system init ;include:

[0042] According to the centroid point set S of the model point cloud in the laser radar coordinate system L And the actual point set S of the corner reflector in the millimeter wave radar coordinate system R , use any of the three methods, point-to-point solution, minimum error straight line solution, and double straight line average method, to determine the rough registration matrix F between the laser radar coordinate system and the millimeter wave radar coordinate system init .

[0043] Furthermore, if a point-to-point solution is adopted, the method includes:

[0044] The centroid point set S of the model point cloud in the laser radar coordinate system L and the actual point set S of the corner reflector in the millimeter wave radar coordinate system R The transformation matrix F of the two coordinate systems is obtained by decentralizing and solving the covariance equation. init , as the initial matrix of the iterative closest point algorithm, the three-dimensional point cloud in the lidar coordinate system and the three-dimensional point cloud in the millimeter-wave radar coordinate system are ICP-aligned to obtain the transformation relationship F between the lidar coordinate system and the millimeter-wave radar coordinate system.

[0045] Furthermore, if the minimum error straight line is used for solving, the method includes:

[0046] S71: Yes and Perform straight line fitting respectively to obtain the direction vectors L1 and L2 of the left and right straight lines;

[0047] S72: Yes and Perform straight line fitting respectively to obtain the direction vectors R1 and R2 of the left and right straight lines;

[0048] S73: Obtain the rotation matrix Rotation1 between L1 and R1 through the Rodriguez rotation formula, and calculate the centroid point set of the model point cloud in the laser radar coordinate system on the L1 line The mean coordinate point L1 c , the actual point set of the corner reflector in the millimeter-wave radar coordinate system on the R1 line The mean coordinate point R1 c , using L1 c 、R1 c And the rotation matrix Rotation1, calculate the translation vector T1, the determination method is as follows:

[0049] Rotation1=I+sin(θ)A+(1-cos(θ))A 2

[0050] T1=R1 c-Rotation1*L1 c

[0051] in, A is The antisymmetric matrix of ;

[0052] S74: Use Rotation1 and T1 to determine the centroid point set of the model point cloud in the laser radar coordinate system contained in L1 The actual point set of the corner reflector in the millimeter-wave radar coordinate system contained in R1 The fitting error error1;

[0053]

[0054] S75: Obtain the rotation matrix Rotation2 between L2 and R2 through the Rodriguez rotation formula, and calculate the centroid point set of the model point cloud in the laser radar coordinate system on the L2 line The mean coordinate point L2 c , the actual point set of the corner reflector in the millimeter-wave radar coordinate system on the R2 line The mean coordinate point R2 c , using L2 c , R2 c And the rotation matrix Rotation2, determine the translation vector T2, the method is as follows:

[0055] Rotation2=I+sin(θ)A+(1-cos(θ))A 2

[0056] T2=R2 c -Rotation2*L2 c

[0057] in, A is The antisymmetric matrix of ;

[0058] S76: Use Rotation2 and T2 to calculate the centroid point set of the model point cloud in the laser radar coordinate system contained in L2 The actual point set of the corner reflector in the millimeter-wave radar coordinate system contained in R2 The fitting error error2;

[0059] S77: Compare the size of error1 and error2, select the rotation matrix Rotation and translation vector T with smaller fitting error to construct the transformation matrix F init , as the initial matrix of the iterative closest point algorithm, the method is as follows:

[0060] Finit =Rotation|T=error_1≤error_2? Rotation1|T1:Rotation2|T2.

[0061] According to the initial matrix F init , perform ICP registration on the three-dimensional point cloud in the laser radar coordinate system and the three-dimensional point cloud in the millimeter wave radar coordinate system, so as to obtain the conversion relationship F between the laser radar coordinate system and the millimeter wave radar coordinate system. Further, such as the double straight line averaging method, the method includes:

[0062] S71: Yes and Perform straight line fitting respectively to obtain the direction vectors L1 and L2 of the left and right straight lines;

[0063] S72: Yes and Perform straight line fitting respectively to obtain the direction vectors R1 and R2 of the left and right straight lines;

[0064] S73: Obtain the rotation matrix Rotation1 between L1 and R1 through the Rodriguez rotation formula, and calculate the centroid point set of the model point cloud in the laser radar coordinate system on the L1 line The mean coordinate point L1 c , the actual point set of the corner reflector in the millimeter-wave radar coordinate system on the R1 line The mean coordinate point R1 c , using L1 c 、R1 c And the rotation matrix Rotation1, determine the translation vector T1, the method is as follows:

[0065] Rotation1=I+sin(θ)A+(1-cos(θ))A 2

[0066] T1=R1 c -Rotation1*L1 c

[0067] in, A is The antisymmetric matrix of ;

[0068] S74: Obtain the rotation matrix Rotation2 between L2 and R2 through the Rodriguez rotation formula, and calculate the centroid point set of the model point cloud in the laser radar coordinate system on the L2 line The mean coordinate point L2 c , the actual point set of the corner reflector in the millimeter-wave radar coordinate system on the R2 line The mean coordinate point R2 c, using L2 c , R2 c And the rotation matrix Rotation2, determine the translation vector T2, the method is as follows:

[0069] Rotation2=I+sin(θ)A+(1-cos(θ))A 2

[0070] T2=R2 c -Rotation2*L2 c

[0071] in, A is The antisymmetric matrix of ;

[0072] S75: Use the rotation matrix Rotation1, the translation vector T1, the rotation matrix Rotation2, and the translation vector T2 to solve the average rotation matrix Rotation and the average translation vector T to construct the transformation matrix F init , as the initial matrix of the iterative closest point algorithm, where the average value of the rotation matrix is ​​obtained by converting the average rotation angle under a fixed rotation order, as follows:

[0073] Rotation=Eurla2Rotation((Rotation2Eurla(Rotation1)+Rotation2Eurla(Rotation2)) / 2)

[0074] T=(T1+T2) / 2

[0075] F init =Rotation|T.

[0076] According to the initial matrix F init , the three-dimensional point cloud in the laser radar coordinate system and the three-dimensional point cloud in the millimeter wave radar coordinate system are ICP registered, so as to obtain the conversion relationship F between the laser radar coordinate system and the millimeter wave radar coordinate system. Compared with the prior art, the present invention has the following significant advantages:

[0077] 1. The present invention uses a combination of an absorbing material and a corner reflector as a calibration object. The absorbing material can effectively absorb the electromagnetic wave signal emitted by the millimeter wave radar and reflect the laser beam emitted by the laser radar. The calibration object can continuously and stably provide high-precision feature points.

[0078] 2. The present invention places the calibration object according to preset placement rules. When extracting the point cloud data of the laser radar and millimeter-wave radar, the ROI of the calibration object is extracted according to the placement rules, which can avoid the interference of point clouds at other positions. It also adopts automatic detection of dynamic difference sets of the calibration object position in the laser radar data and automatic detection of dynamic difference sets of the corner reflector position in the millimeter-wave radar data to accurately extract the point cloud data of the calibration object in the laser radar and millimeter-wave radar coordinate systems.

[0079] 3. The present invention provides three methods for calculating the rough registration matrix of laser radar and millimeter wave radar as the initial value of subsequent ICP, thereby improving the accuracy and speed of ICP. BRIEF DESCRIPTION OF THE DRAWINGS

[0080] Figure 1 This is a flowchart of a simple and accurate method for joint calibration of LiDAR and millimeter wave radar;

[0081] Figure 2 Define the coordinate system for the calibration object.

[0082] Figure 3 This is the model diagram of the calibration object. DETAILED DESCRIPTION

[0083] In order to make the objectives, technical solutions and advantages of the present application clearer, the implementation methods of the present application will be further described in detail below with reference to the accompanying drawings.

[0084] Example 1

[0085] The laser radar is a DJI Horizon, the millimeter wave radar is an ARS548 (4D high-resolution imaging millimeter wave radar), and the calibration object model is as follows: Figure 3 .

[0086] A simple and accurate method for joint calibration of laser radar and millimeter wave radar includes the following steps:

[0087] S1: Establish a millimeter wave coordinate system with the ARS548 millimeter wave radar as the center, and place the calibration objects in the specified acquisition order according to the preset calibration object placement rules;

[0088] S2: Establish a model coordinate system based on the calibration object and generate a model point cloud M corresponding to the calibration object;

[0089] S3: Perform automatic detection of the dynamic difference of the calibration object position on the N groups of three-dimensional point clouds obtained by the Lidar to obtain the actual point clouds of the calibration object LidarROI1, LidarROI2, ... LidarROI in the Lidar coordinate system. N ;

[0090] S4: according to the model point cloud M and N groups of actual point clouds of the calibration object LidarROI1, LidarROI2, ...LidarROI N , determine the conversion relationship between the model point cloud and each set of actual lidar point clouds

[0091] S5: According to each The centroid S of the model point cloud M Transform to the laser radar coordinate system to obtain the point set S L ;

[0092] S6: Perform dynamic difference automatic detection of the corner reflector position on the N groups of point cloud data collected by the millimeter wave radar to obtain the actual point set S of the corner reflector in the millimeter wave radar coordinate system. R ;

[0093] S7: Based on the centroid S of the model point cloud in the laser radar coordinate system L And the point set S in the millimeter wave radar coordinate system R , determine the rough registration matrix F between the laser radar coordinate system and the millimeter wave radar coordinate system init ;

[0094] S8: According to the rough registration matrix F init , solve the precise registration matrix F of the laser radar coordinate system and the millimeter wave radar coordinate system as the joint calibration result.

[0095] Specifically, in step S1, a millimeter wave coordinate system is established with the center of the ARS548 millimeter wave radar shell as the origin. Combined with the installation position and field of view of the ARS548 millimeter wave radar and the DJI Horizon LiDAR, the calibration object placement rules are pre-set, and the calibration is placed in sequence according to the specified acquisition order. Suppose the DJI Horizon LiDAR is installed at a height of 1.8m, with a known horizontal field of view of 81.7° and a vertical field of view of 25.1°; the ARS548 millimeter wave radar is installed at a height of 0.8m, with a known horizontal field of view of ±60° and a vertical field of view of ±15° within 100m.

[0096] More specifically, the calibration object includes a single or multiple cubes and a corner reflector, and the cube is made of a material with wave-absorbing properties, such as EVA foam. Figure 3 As shown in the figure, two cases are illustrated, namely, the calibration object includes a single cube and multiple cubes; when the calibration object includes a single cube, the corner reflector is placed on the cube; when the calibration object includes multiple cubes, the corner reflector is placed at the centroid of the foam block. The foam can reflect the laser beam emitted by the laser radar while effectively absorbing the electromagnetic wave signal emitted by the millimeter-wave radar, and the corner reflector can guide and reflect the signal, thereby realizing the precise positioning of the millimeter-wave radar on the calibration object.

[0097] This embodiment uses 10 cubes to construct the three-dimensional shape of the calibration object. The calibration object only needs to use one corner reflector, which is advantageous in terms of cost and portability compared to other patents that use multiple corner reflectors.

[0098] More specifically, the millimeter-wave radar is used as the center, and the millimeter-wave radar's own coordinate system is used as the coordinate system for the calibration object placement. The positive direction of the X-axis of the ARS548 millimeter-wave radar coordinate system is defined as the forward direction of the radar; the Y-axis is defined as the horizontal direction, perpendicular to the X-axis, extending from the radar to the left as the positive direction; the Z-axis is the vertical axis, and the positive direction is defined as perpendicular to the X-axis and the Y-axis upward, following the right-hand coordinate system rule, such as Figure 2 As shown. Place the calibration objects in sequence along the straight lines on both sides parallel to the positive direction of the X-axis of the millimeter wave radar coordinate system, including:

[0099] S11: First, determine the distance of the left straight line relative to the center of the millimeter-wave radar in the Y-axis direction;

[0100] Then, place the calibration object along the left straight line in the positive direction of the X-axis of the millimeter-wave radar coordinate system, record the coordinates (x, y) of each placement, ensure that the interval between each placement is not less than 0.5 meters, and perform at least 3 placements;

[0101] S12: First, determine the distance of the right straight line relative to the center of the millimeter-wave radar in the Y-axis direction;

[0102] Place the calibration object along the right straight line in the positive direction of the X-axis of the millimeter-wave radar coordinate system; record the coordinates (x, y) of each placement, ensure that the interval between each placement is not less than 0.5 meters, and perform at least 3 placements;

[0103] In this embodiment, the position and spacing of the calibration objects are precisely adjusted considering the optimal observation distance range of the laser radar and the millimeter wave radar. The starting position (x: 3m, y: ±1m), the spacing of 1m, and 10 groups on each side can be considered to ensure accurate and stable data during the millimeter wave radar detection process. The specific coordinate data is shown in Table 1:

[0104] Table 1: Coordinate data record table

[0105]

[0106] Specifically, Figure 3As shown, in step S2, the model coordinate system is established with the vertex of the lower left corner of the calibration object as the origin, the right direction as the positive direction of the X axis, the forward direction as the positive direction of the Y axis, and the upward direction as the positive direction of the Z axis. A model point cloud M with a point spacing of 1 cm is generated in the model coordinate system according to the structural information of the calibration object model. Considering the optimal observation distance range of the laser radar and millimeter wave radar and the influence of the density of the point cloud on the accuracy of the subsequent point cloud registration results, a square with a side length of 0.15m and a corner reflector are selected to build the calibration object model.

[0107] Specifically, the step S3 includes the following steps:

[0108] S31: before the calibration object is placed, a background data point cloud set not including the calibration object in a laser radar coordinate system is collected as global background point cloud data LidarData0;

[0109] S32: According to the preset calibration object placement rules, the calibration objects are placed in sequence according to the specified collection order and the foreground cloud data LidarData containing the calibration objects in the multi-frame LiDAR coordinate system are collected in sequence i ,i∈[1,N];

[0110] S33: For each set of data LidarData i (i∈[1,N]), according to the preset calibration object placement rules and the measured offset position between the laser radar and the millimeter wave radar, the ROI of the calibration object is determined, and the area is separated from the background LidarData0 and the foreground LidarData i The three-dimensional point sets are extracted from the laser radar data, and the difference operation is performed to obtain the initial three-dimensional point set; the stray point cloud is filtered out of the initial three-dimensional point set based on the shape and contour features of the calibration object, and finally the calibration object point clouds LidarROI1, LidarROI2, ... LidarROI in N laser radar coordinate systems are obtained. N .

[0111] Specifically, in step S4, according to the calibration object model point cloud M generated in step 2 and the calibration object actual point cloud LidarROI1, LidarROI2, ... LidarROI generated in step 3 N , determine the conversion relationship between the model point cloud and each set of actual lidar point clouds Use each The centroid S of the model point cloud M Transform to the laser radar coordinate system to obtain the point set S L ,include:

[0112] S41: For each set of calibration objects, the actual point cloud LidarROI i(i∈[1,N]), using the iterative closest point algorithm, perform 3D point cloud registration with the model point cloud M generated in the model coordinate system to determine the transformation relationship between the model point cloud and the actual point cloud of the lidar under this set of data

[0113] S42: According to Determine the position of the centroid of the model point cloud of this set of data in the laser radar coordinate system: This is the centroid of the data set, which is recorded in S L middle.

[0114] In the formula, is the transformation relationship between the laser radar coordinate system and the model coordinate system under this set of data, S M is the centroid of the point cloud in the model coordinate system, S L Represents the point set of the centroid position of multiple calibration objects placed in the laser radar coordinate system. L In the left straight line, place the S L Defined as S placed along the right straight line L Defined as

[0115] Since the corner reflector is placed at the centroid of the calibration object, the centroid position of the default model point cloud (x:3m, y:0m, z:0m) is the position of the vertex inside the corner reflector. Therefore, the point set of the centroid of the calibration object is the point set of the multiple placements of the corner reflector.

[0116] Specifically, the step S6 includes the following steps:

[0117] S61: Before placing the calibration object, a set of background data point cloud sets not including the calibration object in the millimeter-wave radar coordinate system within a fixed time period is collected using the millimeter-wave radar as global background point cloud data RadarData0;

[0118] S62: According to the preset calibration object placement rules, the calibration objects are placed in sequence according to the specified collection order and multiple groups of foreground cloud data RadarData1, RadarData2, ... RadarData containing the calibration objects in the millimeter wave radar coordinate system within a fixed time are collected in sequence N ;

[0119] S63: For each set of point cloud data RadarData i (i∈[1,N]), according to the preset calibration object placement rules, determine the ROI of the calibration object, and divide the area from the background RadarData0 and the foreground RadarData i The three-dimensional points are extracted from the millimeter-wave radar data and the difference operation is performed as the initial three-dimensional point set;

[0120] The point cloud with the highest reflection intensity of the initial three-dimensional point set is used as the corresponding internal vertex of the corner reflector, and finally the actual point set S of the corner reflector in the N millimeter-wave radar coordinate system is obtained. R In S R In the left straight line, place the S R Defined as S placed along the right straight line R Defined as

[0121] Specifically, in step S7, according to the centroid S of the model point cloud in the laser radar coordinate system L And the actual point set S of the corner reflector in the millimeter wave radar coordinate system R , determine the rough registration matrix F between the laser radar coordinate system and the millimeter wave radar coordinate system init . The following steps are included:

[0122] The present application provides three methods for solving the rough registration matrix of the laser radar coordinate system and the millimeter wave radar coordinate system, namely: point-to-point solution, minimum error straight line solution, and double straight line average solution. This embodiment uses the minimum error straight line solution.

[0123] The minimum error straight line solution initial matrix includes the following steps:

[0124] S71: Yes and Perform straight line fitting respectively to obtain the direction vectors L1 and L2 of the left and right straight lines;

[0125] S72: Yes and Perform straight line fitting respectively to obtain the direction vectors R1 and R2 of the left and right straight lines;

[0126] S73: Obtain the rotation matrix Rotation1 between L1 and R1 through the Rodriguez rotation formula, and calculate the centroid point set of the model point cloud in the laser radar coordinate system on the L1 line The mean coordinate point L1 c , the actual point set of the corner reflector in the millimeter-wave radar coordinate system on the R1 line The mean coordinate point R1 c , using L1 c 、R1 c And the rotation matrix Rotation1, calculate the translation vector T1, the determination method is as follows:

[0127] Rotation1=I+sin(θ)A+(1-cos(θ))A 2

[0128] T1=R1 c -Rotation1*L1 c

[0129] in, A is The antisymmetric matrix of ;

[0130] S74: Use Rotation1 and T1 to determine the centroid point set of the model point cloud in the laser radar coordinate system contained in L1 The actual point set of the corner reflector in the millimeter-wave radar coordinate system contained in R1 The fitting error error1;

[0131]

[0132] S75: Obtain the rotation matrix Rotation2 between L2 and R2 through the Rodriguez rotation formula, and calculate the centroid point set of the model point cloud in the laser radar coordinate system on the L2 line The mean coordinate point L2 c , the actual point set of the corner reflector in the millimeter-wave radar coordinate system on the R2 line The mean coordinate point R2 c , using L2 c , R2 c And the rotation matrix Rotation2, determine the translation vector T2, the method is as follows:

[0133] Rotation2=I+sin(θ)A+(1-cos(θ))A 2

[0134] T2=R2 c -Rotation2*L2 c

[0135] in, A is The antisymmetric matrix of ;

[0136] S76: Use Rotation2 and T2 to calculate the centroid point set of the model point cloud in the laser radar coordinate system contained in L2 The actual point set of the corner reflector in the millimeter-wave radar coordinate system contained in R2 The fitting error error2;

[0137] S77: Compare the size of error1 and error2, select the rotation matrix Rotation and translation vector T with smaller fitting error to construct the transformation matrix F init , as the initial matrix of the iterative closest point algorithm, the method is as follows:

[0138] F init =Rotation|T=error_1≤error_2? Rotation1|T1:Rotation2|T2.

[0139] According to the initial matrix F init , the three-dimensional point cloud in the laser radar coordinate system and the three-dimensional point cloud in the millimeter wave radar coordinate system are ICP registered to obtain the transformation relationship F between the laser radar coordinate system and the millimeter wave radar coordinate system.

[0140] The above-described embodiments of the present application do not constitute a limitation on the protection scope of the present application.

Claims

1. A simple and accurate method for joint calibration of laser radar and millimeter wave radar, characterized in that: The method comprises the following steps: S1: With the millimeter-wave radar as the center, the calibration objects are placed in sequence according to the preset calibration object placement rules and the specified collection order; S2: Establish a model coordinate system based on the calibration object and generate a model point cloud M corresponding to the calibration object; S3: Perform automatic detection of the dynamic difference of the calibration object position on the N groups of three-dimensional point clouds obtained by the laser radar, and obtain the actual point clouds of the calibration object laser radar in the N groups of laser radar coordinate systems LidarROI1, LidarROI2, ...LidarROI N ; S4: according to the model point cloud M and N groups of actual point clouds of the calibration object LidarROI1, LidarROI2, ...LidarROI N , determine the conversion relationship between the model point cloud and each set of actual lidar point clouds According to each The centroid S of the model point cloud M Transform to the laser radar coordinate system to obtain the point set S L ; S5: Perform dynamic difference automatic detection of the corner reflector position on the N groups of point cloud data collected by the millimeter wave radar to obtain the actual point set S of the corner reflector in the millimeter wave radar coordinate system. R ; S6: According to the point set S in the laser radar coordinate system L And the actual point set S of the corner reflector in the millimeter wave radar coordinate system R , determine the rough registration matrix F between the laser radar coordinate system and the millimeter wave radar coordinate system init ; S7: According to the rough registration matrix F init , solving the precise registration matrix F of the laser radar coordinate system and the millimeter wave radar coordinate system as a joint calibration result; Centered on the millimeter-wave radar, the calibration objects are placed in the specified acquisition order according to the preset calibration object placement rules, including: With the millimeter-wave radar as the center, the millimeter-wave radar's own coordinate system is used as the calibration object placement coordinate system; the positive direction of the millimeter-wave radar coordinate system's X-axis is defined as the radar's forward direction; the Y-axis is defined as the horizontal direction, perpendicular to the X-axis; the Z-axis is the vertical axis, perpendicular to the X-axis and the Y-axis, and follows the right-hand coordinate system rule; Place the calibration objects in order along the straight lines on both sides parallel to the positive direction of the X-axis of the millimeter-wave radar coordinate system, including: S11: First, determine the distance of the left straight line relative to the center of the millimeter-wave radar in the Y-axis direction; Then, place the calibration object along the left straight line in the positive direction of the X-axis of the millimeter-wave radar coordinate system, record the coordinates (x, y) of each placement, ensure that the interval between each placement is not less than 0.5 meters, and perform at least 3 placements; S12: First, determine the distance of the right straight line relative to the center of the millimeter-wave radar in the Y-axis direction; Place the calibration object along the right straight line in the positive direction of the X-axis of the millimeter-wave radar coordinate system; record the coordinates (x, y) of each placement, ensure that the interval between each placement is not less than 0.5 meters, and perform at least 3 placements; The calibration object includes a single or multiple cubes and a corner reflector; the material of the cube is a material with wave absorbing properties; If the calibration object includes multiple cubes, the corner reflector is placed at the centroid of the absorbing material, and only one corner reflector is placed on the calibration object; For the N groups of 3D point clouds acquired by the Lidar, the calibration object position dynamic difference automatic detection is performed to obtain the actual point clouds of the calibration object LidarROI1, LidarROI2, …LidarROI in the N Lidar coordinate systems. N ,include: S31: before the calibration object is placed, a background data point cloud set excluding the calibration object in a laser radar coordinate system is collected as the laser radar global background point cloud data LidarData0; S32: According to the preset calibration object placement rules, the calibration objects are placed in sequence according to the specified collection order and the foreground cloud data LidarData containing the calibration objects in the multi-frame LiDAR coordinate system are collected in sequence i ,i∈[1,N]; S33: For each set of point cloud data LidarData i , i∈[1,N], according to the preset calibration object placement rules and the measured offset position between the laser radar and the millimeter wave radar, the ROI of the calibration object is determined, and the area is divided from the background LidarData0 and the foreground LidarData i They are extracted from the laser radar data respectively, and a difference operation is performed to obtain the initial three-dimensional point set; Based on the shape and contour features of the calibration object, the initial three-dimensional point set is filtered to remove the stray point cloud, and finally the calibration object point clouds LidarROI1, LidarROI2, ... LidarROI in N Lidar coordinate systems are obtained. N .

2. The simple and accurate method for joint calibration of laser radar and millimeter wave radar according to claim 1 is characterized in that: Establish a model coordinate system based on the calibration object and generate the model point cloud M corresponding to the calibration object, including: Taking the lower left point of the calibration object as the origin, establishing an x-axis, a y-axis and a z-axis respectively along the distribution direction of the calibration object to establish the model coordinate system; The model point cloud M having a point spacing of less than 1 cm of the calibration object in the model coordinate system is generated according to the structural information of the calibration object.

3. The simple and accurate method for joint calibration of laser radar and millimeter wave radar according to claim 1 is characterized in that: According to the model point cloud M and N groups of actual point clouds of the calibration object LidarROI1, LidarROI2, ...LidarROI N , determine the conversion relationship between the model point cloud and each set of actual lidar point clouds According to each The centroid S of the model point cloud M Transform to the laser radar coordinate system to obtain the point set S L ,include: S41: For each set of calibration objects, the actual point cloud LidarROI i , i∈[1,N], using the iterative closest point algorithm, perform 3D point cloud registration with the model point cloud M generated in the model coordinate system to determine the conversion relationship between the model point cloud and the actual point cloud of the lidar under this set of data S42: According to Determine the position of the centroid of the model point cloud of the current group of data in the laser radar coordinate system: This is the centroid of the current group of data, and the centroid of the data is recorded in S L middle; In the formula, is the transformation relationship between the laser radar coordinate system and the model coordinate system under this set of data, S M is the centroid of the point cloud in the model coordinate system, S L Represents the point set of the centroid position of multiple calibration objects placed in the laser radar coordinate system; L In the left straight line, place the S L Defined as S placed along the right straight line L Defined as Since the corner reflector is placed at the centroid of the calibration object, the centroid of the default model point cloud is the position of the internal vertex of the corner reflector; therefore, the point set of the centroid of the calibration object is the point set of the positions where the corner reflector is placed multiple times.

4. The simple and accurate method for joint calibration of laser radar and millimeter wave radar according to claim 3 is characterized in that: The N groups of point cloud data collected by the millimeter-wave radar are automatically detected by dynamic difference set detection of the corner reflector position to obtain the actual point set S of the corner reflector in the millimeter-wave radar coordinate system. R ,include: S51: Before the calibration object is placed, a set of background data point clouds not including the calibration object in the millimeter-wave radar coordinate system within a fixed time period is collected using the millimeter-wave radar as the millimeter-wave radar global background point cloud data RadarData0; S52: According to the preset calibration object placement rules, the calibration objects are placed in sequence according to the specified collection order and multiple groups of foreground cloud data RadarData1, RadarData2, ... RadarData of the calibration objects are collected in sequence in the millimeter wave radar coordinate system within a fixed time. N ; S53: For each set of point cloud data RadarData i , i∈[1,N], according to the preset calibration object placement rules, determine the ROI of the calibration object, and divide the area from background RadarData0 and foreground RadarData i The three-dimensional points are extracted from the millimeter-wave radar data and the difference operation is performed as the initial three-dimensional point set; The point cloud with the highest reflection intensity in the initial three-dimensional point set is taken as the corresponding internal vertex of the corner reflector, and finally the actual point set S of the corner reflector in the N millimeter-wave radar coordinate system is obtained. R ; in S R In the left straight line, place the S R Defined as S placed along the right straight line R Defined as 5. The simple and accurate method for joint calibration of laser radar and millimeter wave radar according to claim 4 is characterized in that: According to the centroid point set S of the model point cloud in the laser radar coordinate system L And the actual point set S of the corner reflector in the millimeter wave radar coordinate system R , determine the rough registration matrix F between the laser radar coordinate system and the millimeter wave radar coordinate system init ;include: According to the centroid point set S of the model point cloud in the laser radar coordinate system L And the actual point set S of the corner reflector in the millimeter wave radar coordinate system R , use any of the three methods, point-to-point solution, minimum error straight line solution, and double straight line average method, to determine the rough registration matrix F between the laser radar coordinate system and the millimeter wave radar coordinate system init .

6. The simple and accurate method for joint calibration of laser radar and millimeter wave radar according to claim 5, characterized in that: The method of solving the rough registration matrix point by point includes: The centroid point set S of the model point cloud in the laser radar coordinate system L and the actual point set S of the corner reflector in the millimeter wave radar coordinate system R The transformation matrix F of the two coordinate systems is obtained by decentralizing and solving the covariance equation. init , as the initial matrix of the iterative closest point algorithm, the three-dimensional point cloud in the lidar coordinate system and the three-dimensional point cloud in the millimeter-wave radar coordinate system are ICP-aligned to obtain the transformation relationship F between the lidar coordinate system and the millimeter-wave radar coordinate system.

7. The simple and accurate method for joint calibration of laser radar and millimeter wave radar according to claim 5, characterized in that: The minimum error line is used to solve the rough registration matrix. The method includes: S71: Yes and Perform straight line fitting respectively to obtain the direction vectors L1 and L2 of the left and right straight lines; S72: Yes and Perform straight line fitting respectively to obtain the direction vectors R1 and R2 of the left and right straight lines; S73: Obtain the rotation matrix Rotation1 between L1 and R1 through the Rodriguez rotation formula, and calculate the centroid point set of the model point cloud in the laser radar coordinate system on the L1 line The mean coordinate point L1 c , the calibration point set in the millimeter wave radar coordinate system on the R1 line The mean coordinate point R1 c , using L1 c 、R1 c And the rotation matrix Rotation1, determine the translation vector T1, the method is as follows: Rotation1=I+sin(θ)A+(1-cos(θ))A 2 T1=R1 c -Rotation1*L1 c in, A is The antisymmetric matrix of ; S74: Use Ritation1 and T1 to determine the centroid point set of the model point cloud in the laser radar coordinate system contained in L1 The actual point set of the corner reflector in the millimeter-wave radar coordinate system contained in R1 The fitting error error1; S75: Obtain the rotation matrix Rotation2 between L2 and R2 through the Rodriguez rotation formula, and calculate the centroid point set of the model point cloud in the laser radar coordinate system on the L2 line The mean coordinate point L2 c , the actual point set of the corner reflector in the millimeter-wave radar coordinate system on the R2 line The mean coordinate point R2 c , using L2 c , R2 c And the rotation matrix Rotation2, determine the translation vector T2, the method is as follows: Rotation2=I+sin(θ)A+(1-cos(θ))A 2 <h2 style=";text-align:left;direction:ltr">T2 = R2<h2 style=";text-align:left;direction:ltr"> c <h2 style=";text-align:left;direction:ltr"> -Rotation2*L2<h2 style=";text-align:left;direction:ltr"> c in, A is The antisymmetric matrix of ; S76: Use Rotation2 and T2 to calculate the centroid point set of the model point cloud in the laser radar coordinate system contained in L2 The actual point set of the corner reflector in the millimeter-wave radar coordinate system contained in R2 The fitting error error2; S77: Compare the size of error1 and error2, select the rotation matrix Rotation and translation vector T with smaller fitting error to construct the transformation matrix F init , as the initial matrix of the iterative closest point algorithm, the method is as follows: F init =Rotation|T=error_1≤error_2?Rotation1|T1:Rotation2|T2; According to the initial matrix F init , the three-dimensional point cloud in the laser radar coordinate system and the three-dimensional point cloud in the millimeter wave radar coordinate system are ICP registered to obtain the transformation relationship F between the laser radar coordinate system and the millimeter wave radar coordinate system.

8. The simple and accurate method for joint calibration of laser radar and millimeter wave radar according to claim 5, characterized in that: The double straight line average method is used to solve the rough registration matrix. The methods include: S71: Yes and Perform straight line fitting respectively to obtain the direction vectors L1 and L2 of the left and right straight lines; S72: Yes and Perform straight line fitting respectively to obtain the direction vectors R1 and R2 of the left and right straight lines; S73: Obtain the rotation matrix Rotation1 between L1 and R1 through the Rodriguez rotation formula, and calculate the centroid point set of the model point cloud in the laser radar coordinate system on the L1 line The mean coordinate point L1 c , the actual point set of the corner reflector in the millimeter-wave radar coordinate system on the R1 line The mean coordinate point R1 c , using L1 c 、R1 c And the rotation matrix Rotation1, determine the translation vector T1, the method is as follows: Rotation1=I+sin(θ)A+(1-cos(θ))A 2 T1=R1 c -Rotation1*L1 c in, A is The antisymmetric matrix of ; S74: Obtain the rotation matrix Rotation2 between L2 and R2 through the Rodriguez rotation formula, and calculate the centroid point set of the model point cloud in the laser radar coordinate system on the L2 line The mean coordinate point L2 c , the actual point set of the corner reflector in the millimeter-wave radar coordinate system on the R2 line The mean coordinate point R2 c , using L2 c , R2 c And the rotation matrix Rotation2, determine the translation vector T2, the method is as follows: Rotation2=I+sin(θ)A+(1-cos(θ))A 2 <h2 style=";text-align:left;direction:ltr">T2 = R2<h2 style=";text-align:left;direction:ltr"> c <h2 style=";text-align:left;direction:ltr"> -Rotation2*L2<h2 style=";text-align:left;direction:ltr"> c in, A is The antisymmetric matrix of ; S75: Use the rotation matrix Rotation1, the translation vector T1, the rotation matrix Rotation2, and the translation vector T2 to solve the average rotation matrix Rotation and the average translation vector T to construct the transformation matrix F init , as the initial matrix of the iterative closest point algorithm, where the average value of the rotation matrix is ​​obtained by converting the average rotation angle under a fixed rotation order as follows: T=(T1+T2) / 2 F init =Rotation|T According to the initial matrix F init , the three-dimensional point cloud in the laser radar coordinate system and the three-dimensional point cloud in the millimeter wave radar coordinate system are ICP registered to obtain the transformation relationship F between the laser radar coordinate system and the millimeter wave radar coordinate system.

Images