Congruent triangle-based laser radar and millimeter wave radar joint calibration method

Abstract

The invention relates to a congruent triangle-based laser radar and millimeter wave radar joint calibration method. The method comprises the following steps of: 1, placing three objects in a public sensing range of a laser radar and a millimeter wave radar; 2, forming a group of calibration data by the space coordinates of the center point of each object in the laser radar coordinate system and the space coordinates of the center point of each object in the millimeter wave radar coordinate system; 3, repeatedly executing the step 1 and the step 2 for multiple times; obtaining multiple groups of calibration data of the three objects at the same position; step 4, finding a corresponding relation between the laser radar coordinate system and the millimeter wave radar coordinate system according to the calibration data, and realizing joint calibration of the laser radar and the millimeter wave radar based on a congruent triangle; the method can better adapt to various different environments and different vehicle models, does not depend on special markers, can perform rapid joint calibration on the laser radar and the millimeter wave radar, and has high efficiency and relatively high robustness.

Description

Technical Field

[0001] The present invention relates to the field of autonomous driving technology, and in particular to a joint calibration method of a laser radar and a millimeter-wave radar based on congruent triangles. Background Art

[0002] At present, most autonomous vehicles use multi-sensor fusion to achieve detection or positioning, among which LiDAR and millimeter-wave radar are essential sensors in autonomous vehicles. In the multi-sensor fusion module, there must be a unified coordinate system. Therefore, before using millimeter-wave radar and LiDAR for sensor fusion, the two sensors must be jointly calibrated in space to ensure that the data obtained by the two sensors have a unified reference standard and can be converted to each other; however, in the existing calibration algorithms, there is a lack of mature calibration algorithms to achieve efficient and high-precision calibration of the two sensors. Due to the large difference between the output characteristics of millimeter-wave radar and LiDAR, it is very difficult to directly transform through the output targets of the two, and the feasibility is not high in the development algorithm.

[0003] In response to this, Chinese patent number CN111796248A discloses a joint calibration method for laser radar and millimeter wave radar, in which the millimeter wave radar and laser radar are set in a common sensing area, and a corner reflector that can be moved in the vertical direction is set; the laser radar outputs the point cloud information of the corresponding beam, and obtains the spatial coordinates of the corner vertex of the corner reflector in the laser radar coordinate system according to the front and rear spatial position relationship of the corner reflector, and obtains the spatial coordinates of the vertex of the corner reflector in the millimeter wave radar coordinate system according to the original point cloud information output by the millimeter wave radar; the corner reflector is moved to obtain multiple sets of calibration data when the corner reflector is in different positions; the correspondence between the laser radar coordinate system and the millimeter wave radar coordinate system is found according to the calibration data; however, this method relies on markers (corner reflectors), and needs to move the markers multiple times, which increases the manual workload during calibration and is inefficient. Moreover, when the markers are on the same straight line, only the calibration data of the same field of view angle is constrained, and the data of each field of view angle of the sensor cannot be guaranteed to be coincident. Summary of the invention

[0004] The purpose of the present invention is to address the deficiencies in the prior art and provide a joint calibration method for laser radar and millimeter wave radar based on congruent triangles.

[0005] The present invention is achieved by adopting the following technical solutions:

[0006] A laser radar and millimeter wave radar joint calibration method based on congruent triangles, wherein the laser radar and the millimeter wave radar are arranged at preset positions on the vehicle body;

[0007] The method comprises the following steps:

[0008] Step 1: placing three objects within the common sensing range of the laser radar and the millimeter-wave radar to ensure that the photosensitive units of the laser radar and the millimeter-wave radar can scan the entire outline of each object;

[0009] Step 2: The laser radar outputs the obstacle BoundingBox information, thereby obtaining the spatial coordinates of the center point of each object in the laser radar coordinate system; the millimeter wave radar outputs the original point cloud information of the obstacle, thereby obtaining the spatial coordinates of the center point of each object in the millimeter wave radar coordinate system; the spatial coordinates of the center point of each object in the laser radar coordinate system and the spatial coordinates in the millimeter wave radar coordinate system constitute a set of calibration data;

[0010] Step 3: Repeat step 1 and step 2 multiple times, and based on the correlation of the acquisition time, obtain multiple frames of sensor data of the laser radar and the millimeter wave radar at the same time, so as to obtain multiple sets of calibration data of the three objects at the same position;

[0011] Step 4: Find the correspondence between the laser radar coordinate system and the millimeter wave radar coordinate system according to the calibration data, and realize the joint calibration of the laser radar and the millimeter wave radar based on congruent triangles.

[0012] As a further explanation of the invention, in step 2, the laser radar outputs obstacle BoundingBox information, thereby obtaining the spatial coordinates of the center point of each object in the laser radar coordinate system, including the following:

[0013] First, the point cloud data of the lidar is preprocessed, and then the obstacles are clustered. Then, the clustered obstacles are fitted with a BoundingBox, and then the obstacle properties are calculated, including the length, width, height, and center point, so as to obtain the spatial coordinates of the center point of each object in the lidar coordinate system.

[0014] As a further explanation of the invention, in step 2, the millimeter wave radar outputs the original point cloud information of the obstacle, thereby obtaining the spatial coordinates of the center point of each object in the millimeter wave radar coordinate system, including the following:

[0015] The millimeter-wave radar outputs the obstacle rectangle detection results, and then calculates the obstacle properties, including vertices and center points, and assigns the actual installation height of the millimeter-wave radar to the height value at the center point, thereby obtaining the spatial coordinates of the center point of each object in the millimeter-wave radar coordinate system.

[0016] As a further explanation of the invention, in the step 2, a triangle is constructed using the spatial coordinates of the center points of the three objects in the laser radar coordinate system, and the slope of each side of the triangle in the spatial coordinates of the laser radar coordinate system is calculated; and a triangle is constructed using the spatial coordinates of the center points of the three objects in the millimeter wave radar coordinate system, and the slope of each side of the triangle in the spatial coordinates of the millimeter wave radar coordinate system is calculated.

[0017] As a further explanation of the invention, in step three, the spatial coordinate values ​​of the laser radar in each set of calibration data are traversed and averaged; and the spatial coordinate values ​​of the millimeter wave radar in each set of calibration data are traversed and averaged.

[0018] As a further illustration of the invention, the step 4 includes the following steps:

[0019] First, the pairwise mean coordinate point difference values ​​in the laser radar coordinate system are obtained, and the slope of each side of the triangle in the laser radar coordinate system is calculated from this, and then the corresponding inclination angle is obtained; and the pairwise mean coordinate point difference values ​​in the millimeter wave radar coordinate system are obtained, and the slope of each side of the triangle in the millimeter wave radar coordinate system is calculated, and then the corresponding inclination angle is obtained. Based on congruent triangles, the joint calibration of the laser radar and millimeter wave radar is realized.

[0020] Compared with the prior art, the present invention has the following beneficial technical effects:

[0021] The present invention designs a joint calibration method for laser radar and millimeter-wave radar based on congruent triangles. Compared with the existing calibration algorithm, the method can better adapt to a variety of different environments and different vehicle models, and does not rely on special markers. Based on congruent triangles, the laser radar and millimeter-wave radar can be quickly calibrated together, which is highly efficient and robust. BRIEF DESCRIPTION OF THE DRAWINGS

[0022] Figure 1 It is a schematic diagram of the relative positions of three objects of the present invention;

[0023] Figure 2 It is a schematic diagram of obstacles recognized by the laser radar of the present invention;

[0024] Figure 3 This is a schematic diagram of the millimeter wave radar identifying obstacles of the present invention;

[0025] Figure 4 is a schematic diagram of the laser radar of the present invention identifying the relative positions of three objects;

[0026] Figure 5 It is a schematic diagram of the millimeter wave radar of the present invention identifying the relative positions of three objects. DETAILED DESCRIPTION

[0027] like Figure 1-5 As shown in the figure, the laser radar and millimeter-wave radar joint calibration method based on congruent triangles, the laser radar and millimeter-wave radar are set at the preset position of the vehicle body, and the relative position between the two is fixed. It should be noted that as long as there is a common area for identification, there is no specific requirement for the installation position of the laser radar and the millimeter-wave radar; because the laser radar and the millimeter-wave radar are scanning in the direction of the vehicle, the coordinate axis directions corresponding to the laser radar coordinate system and the millimeter-wave radar coordinate system can be selected to be consistent; the forward direction of the vehicle is the positive direction of the X-axis, the left is the positive direction of the Y-axis, and the right-hand coordinate system is the positive direction of the Z-axis upward;

[0028] The method comprises the following steps:

[0029] Step 1: Place three objects within the common sensing range of the LiDAR and millimeter-wave radar to ensure that the photosensitive units of the LiDAR and millimeter-wave radar can scan the entire outline of each object; the height of each object is not less than 1.5 meters; this facilitates filtering and elimination of ground points when the LiDAR identifies each object;

[0030] Step 2: The laser radar outputs the obstacle BoundingBox (i.e., bounding box or bounding frame, used to represent the three-dimensional spatial range of the obstacle) information, thereby obtaining the spatial coordinates of the center point of each object in the laser radar coordinate system; the millimeter wave radar outputs the original point cloud information of the obstacle, thereby obtaining the spatial coordinates of the center point of each object in the millimeter wave radar coordinate system; the spatial coordinates of the center point of each object in the laser radar coordinate system and the spatial coordinates in the millimeter wave radar coordinate system constitute a set of calibration data;

[0031] Step 3: Repeat step 1 and step 2 multiple times, and based on the correlation of the acquisition time, obtain multi-frame sensor data of the laser radar and the millimeter-wave radar at the same time, so as to obtain multiple sets of calibration data of the three objects at the same position, that is, the laser radar can collect the corresponding multi-frame sensor data, and the millimeter-wave radar can collect the corresponding multi-frame sensor data. Based on the correlation of the acquisition time, the multi-frame sensor data of the laser radar and the millimeter-wave radar at the same time are obtained, and multiple sets of calibration data of the three objects at the same position can be obtained; there is no need to move the objects, which ensures the coincidence, validity and convenience of the data; it can also avoid the need to move the markers multiple times in the existing calibration technology, which increases the manual workload during calibration;

[0032] Step 4: Find the correspondence between the laser radar coordinate system and the millimeter-wave radar coordinate system based on the calibration data, and realize the joint calibration of the laser radar and the millimeter-wave radar based on congruent triangles.

[0033] Furthermore, in step 2, the laser radar outputs the obstacle BoundingBox information, thereby obtaining the spatial coordinates of the center point of each object in the laser radar coordinate system, including the following:

[0034] First, the point cloud data of the lidar is preprocessed, and then the obstacles are clustered. Then, the clustered obstacles are fitted with the BoundingBox, and then the obstacle properties, including the center point, are calculated to obtain the spatial coordinates of the center point of each object in the lidar coordinate system. Specifically, in the ubuntu (a free operating system using the Linux kernel) + ROS1 (a flexible framework for writing robot software) system, the lidar coordinate system is mainly used. The ground points are first removed to separate the ground points from the non-ground points to avoid the connection and noise interference between obstacles, and complete the preprocessing of the lidar point cloud data. After removing the ground points, there are no ground lines connecting the obstacles on the road surface, so the independent obstacles can be clustered, and the obstacles can be separated separately to complete the point cloud segmentation and clustering of the obstacles; then fit through the external BoundingBox, such as Figure 2 As shown, A SL , A XL , B SL , B XL , C SL , C XL , D SL , D XL , represents the bounding box vertex to find A iLA ; Use the same method to find B iLB , C iLC , which is the coordinate of the center point of each object detected in the laser radar coordinate system.

[0035] Furthermore, in step 2, the millimeter wave radar outputs the original point cloud information of the obstacle, thereby obtaining the spatial coordinates of the center point of each object in the millimeter wave radar coordinate system, including the following:

[0036] The millimeter-wave radar outputs the obstacle rectangle detection result, and then calculates the obstacle attributes, including the vertices and center point, and assigns the actual installation height of the millimeter-wave radar to the height value at the center point, thereby obtaining the spatial coordinates of the center point of each object in the millimeter-wave radar coordinate system. Specifically, in the ubuntu+ROS1 system, the millimeter-wave radar will directly output the obstacle detection result. Since the result is a rectangle, the height value is 0, and the actual installation height of the millimeter-wave radar is assigned to the height value in the detection result. In the subsequent millimeter-wave radar center point coordinates, the millimeter-wave radar height is the installation height value of the millimeter-wave radar, and the output results are all output in the millimeter-wave radar coordinate system. For example Figure 3 As shown, AOR , B OR , C OR , D OR , represents the vertices of the rectangle, find A iRA ; Use the same method to find B iRB , C iRC ; That is, the center point coordinates of each object detected in the millimeter wave radar coordinate system.

[0037] Furthermore, in step 2, the spatial coordinates of the center points of the three objects in the laser radar coordinate system are used: A iLA (x iLA ,y iLA ,z iLA ),B iLB (x iLB ,y iLB ,z iLB ),C iLC (x iLC ,y iLC ,z iLC ), construct a triangle, and find the slope of each side of the triangle in the spatial coordinates of the lidar coordinate system: and

[0038] And use the spatial coordinates of the center points of the three objects in the millimeter wave radar coordinate system: A iRA (x iRA ,y iRA ,z iRA ),B iRB (x iRB ,y iRB ,z iRB ),C iRC (x iRC ,y iRC ,z iRC ), construct a triangle, and find the slope of each side of the triangle in the space coordinates of the millimeter wave radar coordinate system: and

[0039] Furthermore, in step 3, the laser radar coordinate values ​​in each set of calibration data are traversed and averaged: A' iLA (x' iLA ,y' iLA ,z' iLA ),B' iLB (x' iLB ,y' iLB ,z' iLB ),C' iLC (x' iLC ,y' iLC ,z'iLC );

[0040] And the millimeter wave radar coordinate values ​​in each set of calibration data are traversed and averaged:

[0041] A′ iRA (x′ iRA ,y′ iRA ,z′ iRA ),B′ iRB (x′ iRB ,y′ iRB ,z′ iRB ),C′ iRC (x′ iRC ,y′ iRC ,z′ iRC ).

[0042] Generally speaking, the more data collected, the smaller the calibration result error and the higher the accuracy.

[0043] Furthermore, step 4 includes the following steps:

[0044] First, calculate A' in the laser radar coordinate system iLA , B' iLB and C' iLC The difference between the two mean coordinate points can be used to calculate the mean slope of the straight line A'1B'1, straight line B'1C'1 and straight line A'1C'1 in the laser radar coordinate system: and The corresponding tilt angle: and

[0045] Then calculate A' in the millimeter wave radar coordinate system iRA , B' iRB and C' iRC The difference between the two coordinate points can be used to calculate the slopes of the straight lines A'2B'2, B'2C'2 and A'2C'2 in the millimeter-wave radar coordinate system: and The corresponding tilt angle: and

[0046] Based on congruent triangles, two different types of data are matched, namely A'1B'1=A'2B'2, B'1C'1=B'2C'2, A'1C'1=A'2C'2, The corresponding angles are equal and the corresponding points are equal, so the relative position and posture between the LiDAR and the millimeter-wave radar can be accurately calculated, thereby realizing the joint and precise calibration of the LiDAR and the millimeter-wave radar; the original point cloud information output by the millimeter-wave radar after calibration and fusion can be matched with the obstacle BoundingBox information output by the LiDAR in real time. This means that no matter how the object moves or changes, both radars can accurately capture its position information, which can provide reliable data support for subsequent applications such as autonomous driving and robot navigation.

[0047] The present invention is not limited to the specific technical solutions described in the above embodiments. In addition to the above embodiments, the present invention may also have other implementation modes. For those skilled in the art, any technical solutions formed by any modification, equivalent replacement, improvement, etc. made within the spirit and principle of the present invention should be included in the protection scope of the present invention.

Claims

1. A joint calibration method of laser radar and millimeter wave radar based on congruent triangles, characterized in that: The laser radar and the millimeter wave radar are arranged at preset positions on the vehicle body; The method comprises the following steps: Step 1: placing three objects within the common sensing range of the laser radar and the millimeter-wave radar to ensure that the photosensitive units of the laser radar and the millimeter-wave radar can scan the entire outline of each object; Step 2: The laser radar outputs the obstacle BoundingBox information, thereby obtaining the spatial coordinates of the center point of each object in the laser radar coordinate system; the millimeter wave radar outputs the original point cloud information of the obstacle, thereby obtaining the spatial coordinates of the center point of each object in the millimeter wave radar coordinate system; the spatial coordinates of the center point of each object in the laser radar coordinate system and the spatial coordinates in the millimeter wave radar coordinate system constitute a set of calibration data; Step 3: Repeat step 1 and step 2 multiple times, and based on the correlation of the acquisition time, obtain multiple frames of sensor data of the laser radar and the millimeter wave radar at the same time, so as to obtain multiple sets of calibration data of the three objects at the same position; Step 4: Find the correspondence between the laser radar coordinate system and the millimeter wave radar coordinate system according to the calibration data, and realize the joint calibration of the laser radar and the millimeter wave radar based on congruent triangles.

2. The laser radar and millimeter wave radar joint calibration method based on congruent triangles according to claim 1, characterized in that: In step 2, the laser radar outputs the obstacle BoundingBox information, thereby obtaining the spatial coordinates of the center point of each object in the laser radar coordinate system, including the following: First, the point cloud data of the lidar is preprocessed, and then the obstacles are clustered. Then, the clustered obstacles are fitted with a BoundingBox, and then the obstacle properties are calculated, including the length, width, height, and center point, so as to obtain the spatial coordinates of the center point of each object in the lidar coordinate system.

3. The laser radar and millimeter wave radar joint calibration method based on congruent triangles as claimed in claim 2, characterized in that: In the step 2, the millimeter wave radar outputs the original point cloud information of the obstacle, thereby obtaining the spatial coordinates of the center point of each object in the millimeter wave radar coordinate system, including the following: The millimeter-wave radar outputs the obstacle rectangle detection results, and then calculates the obstacle properties, including vertices and center points, and assigns the actual installation height of the millimeter-wave radar to the height value at the center point, thereby obtaining the spatial coordinates of the center point of each object in the millimeter-wave radar coordinate system.

4. The laser radar and millimeter wave radar joint calibration method based on congruent triangles as claimed in claim 3 is characterized in that: In the step 2, a triangle is constructed using the spatial coordinates of the center points of the three objects in the laser radar coordinate system, and the slope of each side of the triangle in the laser radar coordinate system spatial coordinates is calculated; and a triangle is constructed using the spatial coordinates of the center points of the three objects in the millimeter wave radar coordinate system, and the slope of each side of the triangle in the millimeter wave radar coordinate system spatial coordinates is calculated.

5. The laser radar and millimeter wave radar joint calibration method based on congruent triangles as claimed in claim 4, characterized in that: In the step three, the spatial coordinate values ​​of the laser radar in each set of calibration data are traversed and averaged; and the spatial coordinate values ​​of the millimeter wave radar in each set of calibration data are traversed and averaged.

6. The laser radar and millimeter wave radar joint calibration method based on congruent triangles as claimed in claim 5, characterized in that: The step 4 comprises the following steps: First, the pairwise mean coordinate point difference values ​​in the laser radar coordinate system are obtained, and the slope of each side of the triangle in the laser radar coordinate system is calculated from this, and then the corresponding inclination angle is obtained; and the pairwise mean coordinate point difference values ​​in the millimeter wave radar coordinate system are obtained, and the slope of each side of the triangle in the millimeter wave radar coordinate system is calculated, and then the corresponding inclination angle is obtained. Based on congruent triangles, the joint calibration of the laser radar and millimeter wave radar is realized.

Images