Millimeter wave radar-camera space calibration system and method based on self-pose estimation

Abstract

The invention discloses a millimeter wave radar-camera space calibration system and method based on self-pose estimation. The system comprises a data acquisition and preprocessing module, a dynamic interference suppression module, a radar pose estimation module, a camera pose estimation module and a cross-modal alignment module. According to the invention, the cross-modal space calibration between the radar and the camera can be carried out by respectively estimating the poses of the millimeter-wave radar and the camera relative to the coordinate system set in the multi-modal sensing system, so that the cross-modal feature matching process is avoided, the problem of dependence on auxiliary equipment such as a special target or a laser radar in the prior art is solved, and the accuracy of calibration is improved. The problem of high time cost caused by pair-by-pair matching among multiple sensors is solved; through a time sequence random sampling consistency algorithm, the tolerance of a space calibration algorithm to dynamic interference points is improved, and the influence of the dynamic interference points on the algorithm performance is reduced.

Description

Technical Field

[0001] The present invention belongs to the technical field of multimodal perception and spatial geometry calibration, and specifically relates to a millimeter-wave radar-camera spatial calibration system and method based on self-pose estimation. Background Art

[0002] In recent years, devices such as autonomous vehicles, mobile robots, and drones have increasingly integrated sensors of different modalities to form multimodal perception systems to enhance their environmental awareness. Cameras and millimeter-wave radar are two widely used sensor types. Cameras, with their high resolution and rich texture information, are suitable for tasks such as target recognition and scene understanding. Millimeter-wave radar can directly measure the distance and relative velocity of objects, making it suitable for dynamic target detection and tracking. Furthermore, millimeter-wave radar has strong penetration, enabling stable operation in adverse weather and low-light conditions such as rain, fog, and dust, where camera performance is limited. The complementary advantages of these two sensors contribute to the construction of more comprehensive and robust perception systems.

[0003] Multi-sensor spatial calibration is a crucial foundation for multimodal perception technology. Accurate spatial calibration facilitates the alignment and fusion of data from different modalities. However, in practical applications, the spatial relationship between sensors is often difficult to maintain due to sensor installation errors, platform vibration, and drift during long-term operation. If calibration errors are large, the information collected by different sensors will be difficult to effectively associate within a unified coordinate system, thus affecting the accuracy and stability of tasks such as target detection, tracking, and mapping. Therefore, achieving high-precision spatial calibration is crucial for ensuring the performance of multimodal perception systems.

[0004] The essence of multi-sensor spatial calibration is to calculate the spatial transformation matrix between the corresponding coordinate systems of different sensors. Currently, the main millimeter-wave radar-camera spatial calibration methods include the following:

[0005] (1) Spatial calibration methods based on calibration targets: These methods typically deploy multiple calibration targets, such as corner reflectors with a checkerboard pattern, in the shared field of view of the millimeter-wave radar and camera to facilitate simultaneous detection in both the millimeter-wave and visual modalities. However, these methods rely on specially designed calibration targets, which are often difficult to obtain in practical applications. In addition, the targets may be affected by occlusion or field of view limitations, which reduces calibration accuracy.

[0006] (2) Motion trajectory-based spatial calibration methods: These methods do not rely on specific targets, but instead use the motion trajectory of the observed target as a calibration cue to achieve spatial alignment between multiple sensors. However, these methods usually implicitly assume that the sensors have overlapping fields of view. In practical scenarios such as autonomous driving, sensors are often installed in different directions to maximize perception coverage. This limitation requires calibration to be performed pair by pair between sensors, which significantly increases the time cost.

[0007] (3) LiDAR-based spatial calibration method: This type of method introduces LiDAR as an intermediate modality, and successively completes the spatial calibration of millimeter-wave radar-LiDAR and camera-LiDAR, thereby indirectly realizing the calibration between millimeter-wave radar and camera; however, due to cost constraints, many multimodal perception systems are not equipped with LiDAR, which limits the scope of application of this type of method.

[0008] Based on this, the present invention designs a millimeter-wave radar-camera spatial calibration system and method based on self-pose estimation, which enables each sensor to estimate its own pose relative to the same reference object, thereby achieving high-precision multi-sensor spatial calibration. Summary of the Invention

[0009] To address the shortcomings of the aforementioned prior art, the present invention aims to provide a millimeter-wave radar-camera spatial calibration system and method based on self-pose estimation, thereby addressing the existing millimeter-wave radar-camera spatial calibration technology's reliance on auxiliary equipment such as specialized targets or lidar, as well as the high time cost associated with pairwise matching between multiple sensors. This system eliminates the need for cross-modal feature matching, instead enabling efficient and universal spatial calibration by having each sensor estimate its own pose relative to the same reference object and then directly calculating the spatial transformation relationship between the two.

[0010] In order to achieve the above object, the technical solution adopted by the present invention is as follows:

[0011] The present invention provides a millimeter-wave radar-camera spatial calibration system based on self-pose estimation, comprising: a data acquisition and preprocessing module, a dynamic interference suppression module, a radar pose estimation module, a camera pose estimation module, and a cross-modal alignment module;

[0012] The data acquisition and preprocessing module is used to collect millimeter wave intermediate frequency signal data and camera video data during the movement of the multimodal perception system, and process the millimeter wave intermediate frequency signal data into millimeter wave point cloud, and process the camera video data into RGB video and depth video respectively;

[0013] The dynamic interference suppression module is used to suppress interference from dynamic reflectors in the millimeter wave point cloud to obtain a point cloud of static reflectors;

[0014] The radar pose estimation module is used to estimate the pose of the millimeter wave radar relative to the coordinate system set in the multimodal perception system from the position information and velocity information of the point cloud of the static reflector;

[0015] The camera pose estimation module is used to estimate the pose of the camera relative to the world coordinate system for each frame of the video from the RGB video data; then, through the information of the odometer sensor (axle encoder, inertial measurement unit or global positioning system, etc.) of the multimodal perception system, obtain the motion trajectory of the multimodal perception system relative to the world coordinate system, and calculate the pose of the camera relative to the coordinate system set in the multimodal perception system;

[0016] The cross-modal alignment module is used to continuously adjust the millimeter-wave radar pose obtained by the radar pose estimation module based on the millimeter-wave radar pose, camera pose, point cloud of static reflectors, and depth video to obtain the final millimeter-wave radar pose.

[0017] Furthermore, the dynamic interference suppression module uses a temporal random sample consensus algorithm (Temporal RANSAC) to suppress interference from dynamic reflectors in the millimeter wave point cloud, as follows:

[0018] (11) For the first frame of millimeter wave point cloud, the Random Sample Consensus (RANSAC) algorithm is used to randomly sample points in the millimeter wave point cloud, and the pose of the millimeter wave radar is estimated by the radar pose estimation module, and the average pose estimation error is calculated; this is repeated for N times. iter In each round of iteration, the average pose estimation error of the current round is compared with the historical optimal average pose estimation error, and the smaller of the two is taken as the new historical optimal average pose estimation error. The radar pose corresponding to the historical optimal average pose estimation error is recorded as the radar pose optimal estimation. Based on the radar pose optimal estimation, the pose estimation error of all points in the millimeter wave point cloud of the current frame is calculated, where the points whose pose estimation error is less than the specified threshold ∈ are set as internal points, and the remaining points are set as external points.

[0019] (12) For all subsequent frames of millimeter wave point cloud, the random sampling consistency confidence C of the previous frame of millimeter wave data is calculated based on the calculation results of the random sampling consistency algorithm of the previous frame. (t-1) ,as follows:

[0020]

[0021] Where Δe i is the decrease value of the historical optimal average pose estimation error in the i-th iteration, n dropis the number of times the historical optimal average pose estimation error decreases during the entire iteration process, e max and e min are the maximum and minimum values ​​of the historical optimal average pose estimation error during the entire iteration process; N inlier is the number of inliers in the previous frame, N total is the total number of points in the previous frame of millimeter-wave point cloud; α is the set weight, which is used to balance the influence of the two terms before and after the formula;

[0022] (13) For the current frame point cloud P (t) Based on the random sampling consistency confidence C of the previous frame of millimeter wave data, the point cloud that satisfies the following formula is selected

[0023]

[0024] in, is the three-dimensional coordinate of the point, is the velocity of the corresponding point measured by millimeter-wave radar; is the forward speed of the multimodal perception system; and Point The horizontal and vertical angle values ​​of the coordinates of are converted from the Cartesian coordinate system to the polar coordinate system, ψ (t-1) and φ (t-1) are the azimuth and pitch angles of the optimal estimated millimeter-wave radar pose of the previous frame respectively; ∈ is the set threshold;

[0025] (14) Based on the filtered point cloud The random sampling consistency algorithm randomly samples the points, and the pose of the millimeter-wave radar estimated by the radar pose estimation module is used to calculate the weighted average pose estimation error. The weight of each point is The definition is as follows:

[0026]

[0027] Among them, δ and λ are the set control values; Indicates the nearest neighbor point of the point in the previous frame point cloud; express is an interior point or exterior point, and e is the natural logarithm.

[0028] Furthermore, the radar pose estimation module estimates the pose of the millimeter-wave radar relative to the coordinate system set in the multimodal perception system using a radar pose estimation algorithm based on a velocity projection physical model, as follows:

[0029] (21) The collected multi-frame millimeter wave point cloud is divided into a straight-line phase and a turning phase according to the odometer sensor information of the multimodal perception system. Specifically, the absolute value of the angular velocity in the odometer sensor information is taken. When it is less than the specified threshold, it is considered to be a straight-line phase, and when it is greater than the specified threshold, it is considered to be a turning phase;

[0030] (22) For the millimeter wave point cloud in the straight-line phase, the velocity vector v of the millimeter wave radar is solved by the least squares method. c ,as follows:

[0031]

[0032] st||v c ||=v f

[0033] Among them, v f is the forward speed of the multimodal perception system; v i is the measured velocity vector of the i-th point, Calculation of radar velocity vector v based on Rodriguez rotation formula c and the measured velocity vector v i The rotation matrix R between r ;

[0034] (23) For the millimeter wave point cloud in the turning phase, based on the kinematic bicycle model, each moment of the turning of the multimodal perception system is regarded as a circular motion, and the center of the circular motion is calculated as follows:

[0035] x ICR =-R·sinΦ,y ICR =R·cosΦ

[0036] Where, Φ is the yaw angle in the odometer sensor information; R is the distance between the center of the circle and the coordinate system set in the multimodal perception system, is the rate of change of the yaw angle, that is, the angular velocity of the multimodal perception system; the translation vector t of the millimeter-wave radar relative to the coordinate system set in the multimodal perception system is solved by the least squares method. r The X-axis translation and Y-axis translation are as follows:

[0037]

[0038] in, and are the X-axis coordinate value and Y-axis coordinate value of the center of the circular motion at the i-th moment, r is the radius of the circular motion, θ t is the angle between the tangential direction of the circular motion and the front of the multimodal perception system, L is the distance between the front and rear wheels of the multimodal perception system, θ r is the angle between the radar direction and the front of the multimodal perception system; translation vector t r The z-axis translation of is taken as the initial estimate by artificial measurement;

[0039] (24) The position T of the millimeter wave radar relative to the coordinate system set in the multimodal perception system r It is obtained from the following formula:

[0040]

[0041] Furthermore, the camera pose estimation module calculates the pose of the camera relative to the coordinate system set in the multimodal perception system, as follows:

[0042] (31) Set the camera coordinate system corresponding to the first frame of the RGB video as the world coordinate system, and use ORB-SLAM3 to estimate the transformation matrix T of each frame in the RGB video data relative to the world coordinate system t ;

[0043] (32) According to the odometer sensor information of the multimodal perception system, the rotation matrix R of the multimodal perception system at each moment is s It is obtained from the following formula:

[0044]

[0045] Among them, Δθ is the yaw angle change of the multimodal perception system relative to the initial state recorded by the odometer sensor; the translation vector t of the multimodal perception system at each moment is s It is obtained from the following formula:

[0046]

[0047] Among them, x s ,y s and z s The displacement of the multimodal perception system on different coordinate axes relative to the initial state recorded by the odometer sensor; finally, the pose transformation matrix T of the multimodal perception system is calculated. s :

[0048]

[0049] (33) Calculate the camera's position relative to the coordinate system set in the multimodal perception system, that is, the transformation matrix T from the camera coordinate system to the coordinate system set in the multimodal perception system c , T c =T s T t .

[0050] Furthermore, the cross-modal alignment module obtains the final millimeter-wave radar pose through a cross-modal depth alignment algorithm, as follows:

[0051] (41) Pass Convert the point cloud P from the millimeter-wave radar coordinate system to the camera coordinate system to obtain the point cloud in the camera coordinate system q i is the three-dimensional coordinate of the point in the camera coordinate system, x′ i , y′ i and z′ i They are the coordinate values ​​of the X-axis, Y-axis and Z-axis respectively;

[0052] (42) Project the point cloud of the camera coordinate system to the pixel coordinate system through the pinhole imaging model π(·) to obtain (u i ,v i )=π(q i ),u i and v i are the X-axis and Y-axis coordinate values ​​of the point in the pixel coordinate system respectively;

[0053] (43) The millimeter-wave radar pose obtained by the radar pose estimation module is adjusted by the Newton method based on the following formula to obtain the final millimeter-wave radar pose, as follows:

[0054]

[0055] Among them, D(u i ,v i ) is the coordinate of the depth image of the current frame of the depth video (u i ,v i )'s depth value.

[0056] The present invention also provides a millimeter wave radar-camera spatial calibration method based on self-pose estimation. Based on the above system, the steps are as follows:

[0057] 1) Collect millimeter-wave intermediate frequency signal data and camera video data during the motion of the multimodal perception system, process the millimeter-wave intermediate frequency signal data into millimeter-wave point clouds, and process the camera video data into RGB video and depth video respectively;

[0058] 2) Estimate the transformation matrix T of each frame in the RGB video data relative to the world coordinate system t , and then calculate the multimodal perception system's pose transformation matrix T at each moment based on the odometer sensor information of the multimodal perception system s , and finally calculate the camera's pose T relative to the coordinate system set in the multimodal perception system c =T s T t ;

[0059] 3) Using a random sampling consistency algorithm, randomly sample points in the first frame of the millimeter wave point cloud; using a temporal random sampling consistency algorithm, first filter out the non-first frame millimeter wave point cloud, and then randomly sample points in the non-first frame millimeter wave point cloud;

[0060] 4) A radar pose estimation algorithm based on a velocity projection physics model estimates the pose of the millimeter-wave radar relative to the coordinate system set in the multimodal perception system;

[0061] 5) Repeat steps 3) and 4). In each iteration, compare the current round average pose estimation error with the historical optimal average pose estimation error. The smaller of the two is taken as the new historical optimal average pose estimation error, and the millimeter-wave radar pose corresponding to the historical optimal average pose estimation error is recorded as the radar pose optimal estimate. If it is the first frame of the millimeter-wave point cloud, the random sampling consistency algorithm is used to calculate the average pose estimation error. If it is not the first frame of the millimeter-wave point cloud, the time series random sampling consistency algorithm is used to calculate the weighted average pose estimation error. This continues until the pose estimation error is less than the specified threshold or the number of iterations reaches the specified maximum number of iterations.

[0062] 6) Based on the optimal estimate of the radar pose, the millimeter-wave point cloud is converted from the millimeter-wave radar coordinate system to the camera coordinate system, and then projected to the pixel coordinate system. The millimeter-wave radar pose estimate is adjusted using a cross-modal depth alignment algorithm. If the residual after solving the optimization problem using the Newton method is less than a specified threshold, the final millimeter-wave radar pose is obtained. Otherwise, based on the adjusted millimeter-wave radar pose estimate, the point cloud is converted from the camera coordinate system to the millimeter-wave radar coordinate system, and steps 3) to 6) are repeated with the point cloud converted back to the millimeter-wave radar coordinate system.

[0063] 7) Multiply the camera pose in step 2) by the final millimeter-wave radar pose in step 6) to obtain the spatial transformation matrix between the millimeter-wave radar coordinate system and the camera coordinate system.

[0064] Beneficial effects of the present invention:

[0065] 1. The present invention can perform cross-modal spatial calibration between the millimeter-wave radar and camera by separately estimating their positions relative to the coordinate system set in the multimodal perception system, avoiding the process of cross-modal feature matching. This solves the existing technology's reliance on auxiliary equipment such as special targets or lidar, as well as the high time cost caused by pair-by-pair matching between multiple sensors.

[0066] 2. The present invention improves the tolerance of the spatial calibration algorithm to dynamic interference points through the time-series random sampling consistency algorithm, and reduces the impact of dynamic interference points on the algorithm performance.

[0067] 3. The present invention can realize millimeter-wave radar pose estimation based on millimeter-wave point cloud and odometer sensor data; it can also fine-tune the initial radar pose based on depth video, further improving the accuracy of radar pose estimation.

[0068] 4. The present invention achieves high-precision millimeter-wave radar-camera spatial calibration based on self-pose estimation, providing basic technical support for multimodal perception systems such as self-driving cars and mobile robots. The present invention also has low environmental requirements, is not easily affected by interference, has high robustness, and can operate normally in complex environments. BRIEF DESCRIPTION OF THE DRAWINGS

[0069] Figure 1 This is a principle block diagram of the system of the present invention.

[0070] Figure 2 Flowchart of the method of the present invention. DETAILED DESCRIPTION

[0071] In order to facilitate understanding by those skilled in the art, the present invention will be further described below with reference to embodiments and drawings. The contents mentioned in the embodiments are not intended to limit the present invention.

[0072] Reference Figure 1 As shown, a millimeter-wave radar-camera spatial calibration system based on self-pose estimation of the present invention includes: a data acquisition and preprocessing module, a dynamic interference suppression module, a radar pose estimation module, a camera pose estimation module and a cross-modal alignment module;

[0073] The data acquisition and preprocessing module is used to collect millimeter wave intermediate frequency signal data and camera video data during the movement of the multimodal perception system, and process the millimeter wave intermediate frequency signal data into millimeter wave point cloud, and process the camera video data into RGB video and depth video respectively;

[0074] The dynamic interference suppression module is used to suppress interference from dynamic reflectors in the millimeter wave point cloud to obtain a point cloud of static reflectors;

[0075] The dynamic interference suppression module uses the Temporal Random Sample Consensus (Temporal RANSAC) algorithm to suppress interference from dynamic reflectors in the millimeter wave point cloud, as follows:

[0076] (11) For the first frame of millimeter wave point cloud, the Random Sample Consensus (RANSAC) algorithm is used to randomly sample points in the millimeter wave point cloud, and the pose of the millimeter wave radar is estimated by the radar pose estimation module, and the average pose estimation error is calculated; this is repeated for N times. iterIn each round of iteration, the average pose estimation error of the current round is compared with the historical optimal average pose estimation error, and the smaller of the two is taken as the new historical optimal average pose estimation error. The radar pose corresponding to the historical optimal average pose estimation error is recorded as the radar pose optimal estimation. Based on the radar pose optimal estimation, the pose estimation error of all points in the millimeter wave point cloud of the current frame is calculated, where the points whose pose estimation error is less than the specified threshold ∈ are set as internal points, and the remaining points are set as external points.

[0077] (12) For all subsequent frames of millimeter wave point cloud, the random sampling consistency confidence C of the previous frame of millimeter wave data is calculated based on the calculation results of the random sampling consistency algorithm of the previous frame. (t-1) ,as follows:

[0078]

[0079] Where Δe i is the decrease value of the historical optimal average pose estimation error in the i-th iteration, n drop is the number of times the historical optimal average pose estimation error decreases during the entire iteration process, e max and e min are the maximum and minimum values ​​of the historical optimal average pose estimation error during the entire iteration process; N inlier is the number of inliers in the previous frame, N total is the total number of points in the previous frame of millimeter-wave point cloud; α is the set weight, which is used to balance the influence of the two terms before and after the formula;

[0080] (13) For the current frame point cloud P (t) Based on the random sampling consistency confidence C of the previous frame of millimeter wave data, the point cloud that satisfies the following formula is selected

[0081]

[0082] in, is the three-dimensional coordinate of the point, is the velocity of the corresponding point measured by millimeter-wave radar; is the forward speed of the multimodal perception system; and Point The horizontal and vertical angle values ​​of the coordinates of are converted from the Cartesian coordinate system to the polar coordinate system, ψ (t-1) and φ (t-1) are the azimuth and pitch angles of the optimal estimated millimeter-wave radar pose of the previous frame respectively; ∈ is the set threshold;

[0083] (14) Based on the filtered point cloud The random sampling consistency algorithm randomly samples the points, and the pose of the millimeter-wave radar estimated by the radar pose estimation module is used to calculate the weighted average pose estimation error. The weight of each point is The definition is as follows:

[0084]

[0085] Among them, δ and λ are the set control values; Indicates the nearest neighbor point of the point in the previous frame point cloud; express is an interior point or exterior point, and e is the natural logarithm.

[0086] The radar pose estimation module is used to estimate the pose of the millimeter wave radar relative to the coordinate system set in the multimodal perception system from the position information and velocity information of the point cloud of the static reflector;

[0087] The radar pose estimation module estimates the pose of the millimeter-wave radar relative to the coordinate system set in the multimodal perception system using a radar pose estimation algorithm based on a velocity projection physical model, as follows:

[0088] (21) The collected multi-frame millimeter wave point cloud is divided into a straight-line phase and a turning phase according to the odometer sensor information of the multimodal perception system. Specifically, the absolute value of the angular velocity in the odometer sensor information is taken. When it is less than the specified threshold, it is considered to be a straight-line phase, and when it is greater than the specified threshold, it is considered to be a turning phase;

[0089] (22) For the millimeter wave point cloud in the straight-line phase, the velocity vector v of the millimeter wave radar is solved by the least squares method. c ,as follows:

[0090]

[0091] st||v c ||=v f

[0092] Among them, v f is the forward speed of the multimodal perception system; v i is the measured velocity vector of the i-th point, Calculation of radar velocity vector v based on Rodriguez rotation formula c and the measured velocity vector v i The rotation matrix R between r ;

[0093] (23) For the millimeter wave point cloud in the turning phase, based on the kinematic bicycle model, each moment of the turning of the multimodal perception system is regarded as a circular motion, and the center of the circular motion is calculated as follows:

[0094] x ICR =-R·sinΦ,y ICR =R·cosΦ

[0095] Where, Φ is the yaw angle in the odometer sensor information; R is the distance between the center of the circle and the coordinate system set in the multimodal perception system, is the rate of change of the yaw angle, that is, the angular velocity of the multimodal perception system; the translation vector t of the millimeter-wave radar relative to the coordinate system set in the multimodal perception system is solved by the least squares method. r The X-axis translation and Y-axis translation are as follows:

[0096]

[0097] in, and are the X-axis coordinate value and Y-axis coordinate value of the center of the circular motion at the i-th moment, r is the radius of the circular motion, θ t is the angle between the tangential direction of the circular motion and the front of the multimodal perception system, L is the distance between the front and rear wheels of the multimodal perception system, θ r is the angle between the radar direction and the front of the multimodal perception system; translation vector t r The z-axis translation of is taken as the initial estimate by artificial measurement;

[0098] (24) The position T of the millimeter wave radar relative to the coordinate system set in the multimodal perception system r It is obtained from the following formula:

[0099]

[0100] The camera pose estimation module is used to estimate the pose of the camera relative to the world coordinate system for each frame of the video from the RGB video data; then, through the information of the odometer sensor (axle encoder, inertial measurement unit or global positioning system, etc.) of the multimodal perception system, obtain the motion trajectory of the multimodal perception system relative to the world coordinate system, and calculate the pose of the camera relative to the coordinate system set in the multimodal perception system;

[0101] The camera pose estimation module calculates the pose of the camera relative to the coordinate system set in the multimodal perception system, as follows:

[0102] (31) Set the camera coordinate system corresponding to the first frame of the RGB video as the world coordinate system, and use ORB-SLAM3 to estimate the transformation matrix T of each frame in the RGB video data relative to the world coordinate system t ;

[0103] (32) According to the odometer sensor information of the multimodal perception system, the rotation matrix R of the multimodal perception system at each moment is s It is obtained from the following formula:

[0104]

[0105] Among them, Δθ is the yaw angle change of the multimodal perception system relative to the initial state recorded by the odometer sensor; the translation vector t of the multimodal perception system at each moment is s It is obtained from the following formula:

[0106]

[0107] Among them, x s ,y s and z s The displacement of the multimodal perception system on different coordinate axes relative to the initial state recorded by the odometer sensor; finally, the pose transformation matrix T of the multimodal perception system is calculated. s :

[0108]

[0109] (33) Calculate the camera's position relative to the coordinate system set in the multimodal perception system, that is, the transformation matrix T from the camera coordinate system to the coordinate system set in the multimodal perception system c , T c =T s T t .

[0110] The cross-modal alignment module is used to continuously adjust the millimeter-wave radar pose obtained by the radar pose estimation module based on the millimeter-wave radar pose, camera pose, point cloud of static reflectors, and depth video to obtain the final millimeter-wave radar pose;

[0111] The cross-modal alignment module obtains the final millimeter-wave radar pose through a cross-modal depth alignment algorithm, as follows:

[0112] (41) Pass Convert the point cloud P from the millimeter-wave radar coordinate system to the camera coordinate system to obtain the point cloud in the camera coordinate system q i is the three-dimensional coordinate of the point in the camera coordinate system, x′ i , y′ i and z′ iThey are the coordinate values ​​of the X-axis, Y-axis and Z-axis respectively;

[0113] (42) Project the point cloud of the camera coordinate system to the pixel coordinate system through the pinhole imaging model π(·) to obtain (u i ,v i )=π(q i ),u i and v i are the X-axis and Y-axis coordinate values ​​of the point in the pixel coordinate system respectively;

[0114] (43) The millimeter-wave radar pose obtained by the radar pose estimation module is adjusted by the Newton method based on the following formula to obtain the final millimeter-wave radar pose, as follows:

[0115]

[0116] Among them, D(u i ,v i ) is the coordinate of the depth image of the current frame of the depth video (u i ,v i )'s depth value.

[0117] Reference Figure 2 As shown, the present invention also provides a millimeter wave radar-camera spatial calibration method based on self-pose estimation. Based on the above system, the steps are as follows:

[0118] 1) Collect millimeter-wave intermediate frequency signal data and camera video data during the motion of the multimodal perception system, process the millimeter-wave intermediate frequency signal data into millimeter-wave point clouds, and process the camera video data into RGB video and depth video respectively;

[0119] 2) Estimate the transformation matrix T of each frame in the RGB video data relative to the world coordinate system t , and then calculate the multimodal perception system's pose transformation matrix T at each moment based on the odometer sensor information of the multimodal perception system s , and finally calculate the camera's pose T relative to the coordinate system set in the multimodal perception system c =T s T t ;

[0120] 3) Using a random sampling consistency algorithm, randomly sample points in the first frame of the millimeter wave point cloud; using a temporal random sampling consistency algorithm, first filter out the non-first frame millimeter wave point cloud, and then randomly sample points in the non-first frame millimeter wave point cloud;

[0121] 4) A radar pose estimation algorithm based on a velocity projection physics model estimates the pose of the millimeter-wave radar relative to the coordinate system set in the multimodal perception system;

[0122] 5) Repeat steps 3) and 4). In each iteration, compare the current round average pose estimation error with the historical optimal average pose estimation error. The smaller of the two is taken as the new historical optimal average pose estimation error, and the millimeter-wave radar pose corresponding to the historical optimal average pose estimation error is recorded as the radar pose optimal estimate. If it is the first frame of the millimeter-wave point cloud, the random sampling consistency algorithm is used to calculate the average pose estimation error. If it is not the first frame of the millimeter-wave point cloud, the time series random sampling consistency algorithm is used to calculate the weighted average pose estimation error. This continues until the pose estimation error is less than the specified threshold or the number of iterations reaches the specified maximum number of iterations.

[0123] 6) Based on the optimal estimate of the radar pose, the millimeter-wave point cloud is converted from the millimeter-wave radar coordinate system to the camera coordinate system, and then projected to the pixel coordinate system. The millimeter-wave radar pose estimate is adjusted using a cross-modal depth alignment algorithm. If the residual after solving the optimization problem using the Newton method is less than a specified threshold, the final millimeter-wave radar pose is obtained. Otherwise, based on the adjusted millimeter-wave radar pose estimate, the point cloud is converted from the camera coordinate system to the millimeter-wave radar coordinate system, and steps 3) to 6) are repeated with the point cloud converted back to the millimeter-wave radar coordinate system.

[0124] 7) Multiply the camera pose in step 2) by the final millimeter-wave radar pose in step 6) to obtain the spatial transformation matrix between the millimeter-wave radar coordinate system and the camera coordinate system.

[0125] The present invention has many specific application paths. The above is only a preferred embodiment of the present invention. It should be pointed out that for ordinary technicians in this technical field, several improvements can be made without departing from the principles of the present invention. These improvements should also be considered as the scope of protection of the present invention.

Claims

1. A millimeter wave radar-camera spatial calibration system based on self-pose estimation, characterized in that: include: Data acquisition and preprocessing module, dynamic interference suppression module, radar pose estimation module, camera pose estimation module and cross-modal alignment module; The data acquisition and preprocessing module is used to collect millimeter wave intermediate frequency signal data and camera video data during the movement of the multimodal perception system, and process the millimeter wave intermediate frequency signal data into millimeter wave point cloud, and process the camera video data into RGB video and depth video respectively; The dynamic interference suppression module is used to suppress interference from dynamic reflectors in the millimeter wave point cloud to obtain a point cloud of static reflectors; The radar pose estimation module is used to estimate the pose of the millimeter wave radar relative to the coordinate system set in the multimodal perception system from the position information and velocity information of the point cloud of the static reflector; The camera pose estimation module is used to estimate the pose of the camera relative to the world coordinate system for each frame of the video from the RGB video data; then, through the odometer sensor information of the multimodal perception system, obtain the motion trajectory of the multimodal perception system relative to the world coordinate system, and calculate the pose of the camera relative to the coordinate system set in the multimodal perception system; The cross-modal alignment module is used to continuously adjust the millimeter-wave radar pose obtained by the radar pose estimation module based on the millimeter-wave radar pose, camera pose, point cloud of static reflectors, and depth video to obtain the final millimeter-wave radar pose.

2. The millimeter wave radar-camera spatial calibration system based on self-pose estimation according to claim 1, characterized in that: The dynamic interference suppression module uses a temporal random sampling consistency algorithm to suppress interference from dynamic reflectors in the millimeter wave point cloud, specifically as follows: (11) For the first frame of millimeter wave point cloud, the random sampling consistency algorithm is used to randomly sample points in the millimeter wave point cloud, and the pose of the millimeter wave radar is estimated by the radar pose estimation module, and the average pose estimation error is calculated; it is repeated N times. iter In each round of iteration, the average pose estimation error of the current round is compared with the historical optimal average pose estimation error, and the smaller of the two is taken as the new historical optimal average pose estimation error. The radar pose corresponding to the historical optimal average pose estimation error is recorded as the radar pose optimal estimation. Based on the radar pose optimal estimation, the pose estimation error of all points in the millimeter wave point cloud of the current frame is calculated, where the points whose pose estimation error is less than the specified threshold ∈ are set as internal points, and the remaining points are set as external points. (12) For all subsequent frames of millimeter wave point cloud, the random sampling consistency confidence C of the previous frame of millimeter wave data is calculated based on the calculation results of the random sampling consistency algorithm of the previous frame. (t-1) ,as follows: Where Δe i is the decrease value of the historical optimal average pose estimation error in the i-th iteration, n drop is the number of times the historical optimal average pose estimation error decreases during the entire iteration process, e max and e min are the maximum and minimum values ​​of the historical optimal average pose estimation error during the entire iteration process; N inlier is the number of inliers in the previous frame, N total is the total number of points in the previous frame of millimeter-wave point cloud; α is the set weight, which is used to balance the influence of the two terms before and after the formula; (13) For the current frame point cloud P (t) Based on the random sampling consistency confidence C of the previous frame of millimeter wave data, the point cloud that satisfies the following formula is selected in, is the three-dimensional coordinate of the point, is the velocity of the corresponding point measured by millimeter-wave radar; is the forward speed of the multimodal perception system; and Point The horizontal and vertical angle values ​​of the coordinates of are converted from the Cartesian coordinate system to the polar coordinate system, ψ (t-1) and φ (t-1) are the azimuth and pitch angles of the optimal estimated millimeter-wave radar pose of the previous frame respectively; ∈ is the set threshold; (14) Based on the filtered point cloud The random sampling consistency algorithm randomly samples the points, and the pose of the millimeter-wave radar estimated by the radar pose estimation module is used to calculate the weighted average pose estimation error. The weight of each point is The definition is as follows: Among them, δ and λ are the set control values; Indicates the nearest neighbor point of the point in the previous frame point cloud; express is an interior point or exterior point, and e is the natural logarithm.

3. The millimeter wave radar-camera spatial calibration system based on self-pose estimation according to claim 2, characterized in that: The radar pose estimation module estimates the pose of the millimeter-wave radar relative to the coordinate system set in the multimodal perception system using a radar pose estimation algorithm based on a velocity projection physical model, as follows: (21) The collected multi-frame millimeter wave point cloud is divided into a straight-line phase and a turning phase according to the odometer sensor information of the multimodal perception system. Specifically, the absolute value of the angular velocity in the odometer sensor information is taken. When it is less than the specified threshold, it is considered to be a straight-line phase, and when it is greater than the specified threshold, it is considered to be a turning phase; (22) For the millimeter wave point cloud in the straight-line phase, the velocity vector v of the millimeter wave radar is solved by the least squares method. c ,as follows: s.t.||v c ||=v f Among them, v f is the forward speed of the multimodal perception system; v i is the measured velocity vector of the i-th point, Calculation of radar velocity vector v based on Rodriguez rotation formula c and the measured velocity vector v i The rotation matrix R between r ; (23) For the millimeter wave point cloud in the turning phase, based on the kinematic bicycle model, each moment of the turning of the multimodal perception system is regarded as a circular motion, and the center of the circular motion is calculated as follows: x ICR =-R·sinΦ,y ICR =R·cosΦ Where, Φ is the yaw angle in the odometer sensor information; R is the distance between the center of the circle and the coordinate system set in the multimodal perception system, is the rate of change of the yaw angle, that is, the angular velocity of the multimodal perception system; the translation vector t of the millimeter-wave radar relative to the coordinate system set in the multimodal perception system is solved by the least squares method. r The X-axis translation and Y-axis translation are as follows: in, and are the X-axis coordinate value and Y-axis coordinate value of the center of the circular motion at the i-th moment, r is the radius of the circular motion, θ t is the angle between the tangential direction of the circular motion and the front of the multimodal perception system, L is the distance between the front and rear wheels of the multimodal perception system, θ r is the angle between the radar direction and the front of the multimodal perception system; translation vector t r The z-axis translation of is taken as the initial estimate by artificial measurement; (24) The position T of the millimeter wave radar relative to the coordinate system set in the multimodal perception system r It is obtained from the following formula:

4. The millimeter wave radar-camera spatial calibration system based on self-pose estimation according to claim 3, characterized in that: The camera pose estimation module calculates the pose of the camera relative to the coordinate system set in the multimodal perception system, as follows: (31) Set the camera coordinate system corresponding to the first frame of the RGB video as the world coordinate system, and use ORB-SLAM3 to estimate the transformation matrix T of each frame in the RGB video data relative to the world coordinate system t ; (32) According to the odometer sensor information of the multimodal perception system, the rotation matrix R of the multimodal perception system at each moment is s It is obtained from the following formula: Among them, Δθ is the yaw angle change of the multimodal perception system relative to the initial state recorded by the odometer sensor; the translation vector t of the multimodal perception system at each moment is s It is obtained from the following formula: Among them, x s ,y s and z s The displacement of the multimodal perception system on different coordinate axes relative to the initial state recorded by the odometer sensor; finally, the pose transformation matrix T of the multimodal perception system is calculated. s : (33) Calculate the camera's position relative to the coordinate system set in the multimodal perception system, that is, the transformation matrix T from the camera coordinate system to the coordinate system set in the multimodal perception system c , T c =T s T t .

5. The millimeter wave radar-camera spatial calibration system based on self-pose estimation according to claim 4, characterized in that: The cross-modal alignment module obtains the final millimeter-wave radar pose through a cross-modal depth alignment algorithm, as follows: (41) Pass Convert the point cloud P from the millimeter-wave radar coordinate system to the camera coordinate system to obtain the point cloud in the camera coordinate system q i is the three-dimensional coordinate of the point in the camera coordinate system, x′ i , y′ i and z′ i They are the coordinate values ​​of the X-axis, Y-axis and Z-axis respectively; (42) Project the point cloud of the camera coordinate system to the pixel coordinate system through the pinhole imaging model π(·) to obtain (u i ,v i )=π(q i ),u i and v i are the X-axis and Y-axis coordinate values ​​of the point in the pixel coordinate system respectively; (43) The millimeter-wave radar pose obtained by the radar pose estimation module is adjusted by the Newton method based on the following formula to obtain the final millimeter-wave radar pose, as follows: Among them, D(u i ,v i ) is the coordinate of the depth image of the current frame of the depth video (u i ,v i )'s depth value.

6. A millimeter-wave radar-camera spatial calibration method based on self-pose estimation, based on the system according to any one of claims 1 to 5, characterized in that: The steps are as follows: 1) Collect millimeter-wave intermediate frequency signal data and camera video data during the motion of the multimodal perception system, process the millimeter-wave intermediate frequency signal data into millimeter-wave point clouds, and process the camera video data into RGB video and depth video respectively; 2) Estimate the transformation matrix T of each frame in the RGB video data relative to the world coordinate system t , and then calculate the multimodal perception system's pose transformation matrix T at each moment based on the odometer sensor information of the multimodal perception system s , and finally calculate the camera's pose T relative to the coordinate system set in the multimodal perception system c =T s T t ; 3) Using a random sampling consistency algorithm, randomly sample points in the first frame of the millimeter wave point cloud; using a temporal random sampling consistency algorithm, first filter out the non-first frame millimeter wave point cloud, and then randomly sample points in the non-first frame millimeter wave point cloud; 4) A radar pose estimation algorithm based on a velocity projection physics model estimates the pose of the millimeter-wave radar relative to the coordinate system set in the multimodal perception system; 5) Repeat steps 3) and 4). In each iteration, compare the current round average pose estimation error with the historical optimal average pose estimation error. The smaller of the two is taken as the new historical optimal average pose estimation error. The millimeter-wave radar pose corresponding to the historical optimal average pose estimation error is recorded as the optimal radar pose estimate. If it is the first frame of millimeter wave point cloud, the random sampling consistency algorithm is used to calculate the average pose estimation error; if it is not the first frame of millimeter wave point cloud, the time series random sampling consistency algorithm is used to calculate the weighted average pose estimation error; until the pose estimation error is less than the specified threshold or the number of iterations reaches the specified maximum number of iterations; 6) Based on the optimal estimate of the radar pose, the millimeter-wave point cloud is converted from the millimeter-wave radar coordinate system to the camera coordinate system, then projected to the pixel coordinate system. The millimeter-wave radar pose estimate is adjusted using a cross-modal depth alignment algorithm. If the residual after solving the optimization problem using the Newton method is less than a specified threshold, the final millimeter-wave radar pose is obtained. Otherwise, based on the adjusted millimeter-wave radar pose estimation, the point cloud is converted from the camera coordinate system to the millimeter-wave radar coordinate system, and steps 3) to 6) are repeated to convert the point cloud back to the millimeter-wave radar coordinate system; 7) Multiply the camera pose in step 2) by the final millimeter-wave radar pose in step 6) to obtain the spatial transformation matrix between the millimeter-wave radar coordinate system and the camera coordinate system.

Images