Joint track tracking method based on millimeter wave radar and monocular camera

Abstract

The invention relates to the technical field of multi-target tracking, in particular to a track joint tracking method based on a millimeter-wave radar and a monocular camera, which comprises the following steps: firstly, in a data acquisition stage, the monocular camera directly acquires 3D positions and uncertainty of pedestrians from 2D images through a monocular 3D detection deep learning network, the millimeter-wave radar performs clustering tracking on point clouds, and the 3D positions and the uncertainty of the pedestrians are acquired; the position and the speed of the pedestrian on the aerial view are obtained, then external calibration operation is carried out on detection frames of the two sensors, and coordinate systems of different sensors are unified to the same coordinate system. And then, in a Kalman filtering stage, Kalman filtering prediction and updating are respectively carried out on the millimeter wave radar and the monocular camera to obtain a single-sensor tracking trajectory. And finally, in a trajectory combination stage, calculating a cross covariance matrix between the trajectory of the millimeter wave radar and the trajectory of the camera, then performing cross covariance matrix fusion to output a final tracking trajectory, and starting a next round of iteration.

Description

Technical Field

[0001] The present invention relates to the technical field of multi - target tracking, and particularly to a method for jointly tracking trajectories based on a millimeter - wave radar and a monocular camera. Background Art

[0002] Current multi - target tracking methods generally use a single sensor. However, in a complex and changing perception environment, affected by factors such as weather, light intensity, and complex environment, it may lead to insufficient perception ability or even the risk of sensor failure. Therefore, in order to obtain accurate and reliable environmental perception results, a multi - sensor fusion method can be adopted to fuse data from heterogeneous sensors, giving full play to the complementary advantages of the sensors, so as to obtain more robust and accurate environmental perception. Since different types of sensors have their own advantages and disadvantages in detection accuracy and range, and environmental adaptability. Among them, a monocular camera can achieve object detection and semantic understanding that a millimeter - wave radar cannot, such as traffic light and traffic sign recognition, pedestrian gesture recognition, etc. The millimeter - wave radar can provide information about the target's position and speed, which is exactly what the monocular camera lacks. Therefore, the fusion of a millimeter - wave radar and a monocular camera can achieve better performance than other sensor fusion methods.

[0003] There are currently two problems with existing target tracking methods based on the fusion of millimeter - wave radar and monocular camera. On the one hand, existing methods usually back - project the detection results of the millimeter - wave radar onto the image plane of the monocular camera to generate regions of interest (ROIs), and then use probability - based filters for tracking. Although such methods have made progress in fusing multi - source heterogeneous data, they are overly dependent on a single sensor. When the detection error of a single sensor increases or even fails temporarily, it will affect the accuracy and stability of the overall system. On the other hand, existing methods do not consider the correlation between sensors and still assume that the measurements of each sensor are independent. In fact, although the measurement noises of the two sensors are uncorrelated, due to the same process noise in the target motion process, the two state estimates are not uncorrelated, and there is a correlation between sensors. Existing methods ignore the mutual relationship between sensors, which will lead to redundant errors in the system and affect the tracking accuracy of the overall system. Summary of the Invention

[0004] The purpose of the present invention is to provide a method for jointly tracking trajectories based on a millimeter - wave radar and a monocular camera, aiming to solve the technical problems of insufficient accuracy and stability of the system caused by over - reliance on single - sensor detection and the inability to coordinate multi - sensor joint tracking in existing tracking methods.

[0005] To achieve the above - mentioned purpose, the present invention provides a method for jointly tracking trajectories based on a millimeter - wave radar and a monocular camera, including the following steps:

[0006] Step 1: Obtain and preprocess monocular camera data;

[0007] Step 2: Obtain and process millimeter-wave radar data;

[0008] Step 3: Externally calibrate the monocular camera and the millimeter-wave radar, and unify the millimeter-wave radar measurement values and the monocular camera measurement values into the same coordinate system;

[0009] Step 4: Use Kalman filtering to separately track the targets detected by the millimeter-wave radar and the monocular camera to obtain tracking trajectories, calculate the cross-covariance matrix for trajectory fusion, and obtain the final tracking trajectory.

[0010] Optionally, the execution process of Step 1 is specifically to use a 2D pose detector to extract the 2D joint coordinates of pedestrians in the input image, and then input the detected joint data into a lightweight feed-forward depth estimation neural network, and the feed-forward depth estimation neural network captures the uncertainty caused by observation noise; finally, the monocular 3D detection of the camera simultaneously obtains the 3D position estimation of the pedestrian and the uncertainty of the position estimation.

[0011] Optionally, the execution process of Step 2 includes the following steps:

[0012] Step 2.1: Obtain the distance information of the target by performing Range-FFT on the collected intermediate-frequency signal, obtain the speed information of the target by performing Doppler-FFT, and finally obtain a matrix with the row vector being the distance and the column vector being the speed;

[0013] Step 2.2: Use a constant false alarm rate to distinguish targets from noise and achieve reliable detection of targets;

[0014] Step 2.3: Generate target point cloud data, and then perform clustering analysis to group adjacent points into the same category to represent the same target;

[0015] Step 2.4: According to the target results obtained by clustering, use a multi-target tracking algorithm to obtain target information.

[0016] Optionally, in Step 3, each frame of data of the camera and the millimeter-wave radar is time-aligned according to the closest timestamp, and the expression for unifying the millimeter-wave radar measurement values and the monocular camera measurement values into the same coordinate system is as follows:

[0017]

[0018] where is the millimeter-wave radar coordinate system, is the camera coordinate system, and R and T are the rotation matrix and the translation matrix respectively, and their values are obtained through pre-calibration.

[0019] Optionally, in step 4, a motion model is established in the two-dimensional Cartesian coordinate system of the bird's-eye view, and the state of the pedestrian is represented as:

[0020] x = (p, v) T = (p x , p y , v x , v y ) T

[0021] where p is the position of the pedestrian and v is the speed of the pedestrian;

[0022] The corresponding motion model and sensor measurement model are:

[0023] x k = Fx k-1 + w k

[0024]

[0025] where x k is the state vector of the pedestrian at time k, F is the state transition matrix for specifying kinematic characteristics, w k is the process noise, satisfying w k ~ N(0, Q k ), Q k represents the covariance matrix of the process noise; and are the measurement vectors of the camera and millimeter-wave radar at time k, H Camera and H Radar are their corresponding measurement matrices; and are the measurement noises of the camera and millimeter-wave radar respectively, satisfying and and represent the covariance matrices of the measurement noises of the camera and millimeter-wave radar respectively; ΔT is the time step.

[0026] Optionally, during the execution of step 3, Kalman filtering is used to track the targets detected by the millimeter-wave radar and the camera respectively. The tracking consists of two steps: Kalman filter prediction and Kalman filter update. In the prediction stage, based on the state estimate value at the previous moment, the motion model is used to predict the state estimate of the fusion trajectory at the current moment and the error covariance matrix

[0027]

[0028] In the Kalman filter update stage, the Kalman gain after update is obtained for the monocular camera and millimeter-wave radar respectively through Kalman filter update and state estimation and as well as the error covariance matrix and

[0029]

[0030] Optionally, in the trajectory association stage of step 4, by introducing the cross-covariance matrix to describe the correlation between the individual tracking trajectories of the millimeter-wave radar and the monocular camera, after obtaining the Kalman gain, the cross-covariance matrix between the state estimations of the two sensors can be calculated and The calculation is as follows:

[0031]

[0032] When calculating the fused trajectory and the error covariance matrix the cross-covariance matrix is used to eliminate the correlation between the sensor trajectories. This correlation is due to the information redundancy brought by the common motion noise contained in the motion processes of the same target in different sensors. By eliminating this redundancy and common error, the final fused tracking result is more accurate

[0033]

[0034] Specifically, here is obtained by independent tracking of each individual sensor and fused. Its fusion weight value is equal for each individual sensor. Therefore, the final state estimation value will not overly rely on a certain individual sensor. Even when the detection error of a certain individual sensor increases or even fails temporarily, the fused trajectory can still maintain stable tracking through normal sensors and Kalman filter, without trajectory mutation and strong trajectory fluctuation, and finally making the fused result more robust

[0035] Finally, trajectory management is performed in the output. For each new trajectory, it will be published after successfully matching enough frames, and for each trajectory that has not matched any detections for a long time, it will be deleted

[0036] The present invention provides a method for jointly tracking the trajectories of a millimeter-wave radar and a monocular camera. First, in the data acquisition stage, the monocular camera directly obtains the 3D position and uncertainty of pedestrians from 2D images through a monocular 3D detection deep learning network. The millimeter-wave radar clusters and tracks the point cloud to obtain the position and velocity of pedestrians on the bird's-eye view. Subsequently, the detection frames of the two sensors are externally calibrated to unify the coordinate systems of different sensors into the same coordinate system. Then, in the Kalman filtering stage, Kalman filtering prediction and update are performed on the millimeter-wave radar and the monocular camera respectively to obtain the single-sensor tracking trajectories. Finally, in the trajectory joint stage, the cross-covariance matrix between the millimeter-wave radar and the camera trajectories is calculated to quantify the correlation between the two sensors, and then the cross-covariance matrix is fused to output the final tracking trajectory, and the next round of iteration is started. The present invention overcomes the problem of relying on a single sensor and can achieve robust tracking under harsh weather conditions such as low light and thick fog. In addition, the correlation between sensors is quantified through the cross-covariance matrix, reducing the redundant error in sensor fusion, and the tracking trajectory result is more accurate than the traditional method. BRIEF DESCRIPTION OF THE DRAWINGS

[0037] In order to more clearly illustrate the technical solutions in the embodiments of the present invention or the prior art, the following will briefly introduce the drawings required for the description of the embodiments or the prior art. Obviously, the following drawings are only some embodiments of the present invention. For those of ordinary skill in the art, without creative efforts, other drawings can be obtained based on these drawings.

[0038] Figure 1 is a schematic block diagram of a method for jointly tracking the trajectories of a millimeter-wave radar and a monocular camera according to the present invention.

[0039] Figure 2 is a schematic diagram of the monocular 3D detection process of a method for jointly tracking the trajectories of a millimeter-wave radar and a monocular camera according to the present invention.

[0040] Figure 3 is a schematic diagram of the millimeter-wave radar signal processing process according to the present invention.

[0041] Figure 4 is a schematic diagram of the trajectory joint process according to the present invention.

[0042] Figure 5 is a schematic diagram of the comparison result of the tracking accuracy in a normal scenario in a specific embodiment of the present invention.

[0043] Figure 6 is a schematic diagram of the comparison result of the tracking robustness in low light and thick fog scenarios in a specific embodiment of the present invention. DETAILED DESCRIPTION OF THE EMBODIMENTS

[0044] Embodiments of the present invention will be described in detail below. Examples of the embodiments are shown in the accompanying drawings, where like or similar reference numerals denote like or similar elements or elements having like or similar functions throughout. The embodiments described below by referring to the accompanying drawings are exemplary and are intended to explain the present invention and should not be construed as limiting the present invention.

[0045] The present invention provides a method for jointly tracking the trajectories of a millimeter-wave radar and a monocular camera, comprising the following steps:

[0046] Step 1: Obtain and preprocess the data of the monocular camera;

[0047] Step 2: Obtain and process the data of the millimeter-wave radar;

[0048] Step 3: Externally calibrate the monocular camera and the millimeter-wave radar, and unify the millimeter-wave radar measurement values and the monocular camera measurement values into the same coordinate system;

[0049] Step 4: Use Kalman filtering to separately track the targets detected by the millimeter-wave radar and the monocular camera to obtain tracking trajectories, calculate the cross-covariance matrix for trajectory fusion, and obtain the final tracking trajectory.

[0050] For details, please refer to Figure 1 , Figure 1 is a principle block diagram of a method for jointly tracking the trajectories of a millimeter-wave radar and a monocular camera according to the present invention. First, in the data acquisition stage, the monocular camera directly obtains the 3D position and uncertainty of a pedestrian from a 2D image through a monocular 3D detection deep learning network. The millimeter-wave radar clusters and tracks the point cloud to obtain the position and speed of the pedestrian on the bird's-eye view. Subsequently, an external calibration operation is performed on the detection frames of the two sensors to unify the coordinate systems of different sensors into the same coordinate system. Then, in the Kalman filtering stage, Kalman filtering prediction and update are respectively performed on the millimeter-wave radar and the monocular camera to obtain single-sensor tracking trajectories. Finally, in the trajectory joint stage, the cross-covariance matrix between the millimeter-wave radar and the camera trajectories is calculated to quantify the correlation between the two sensors, and then the cross-covariance matrix fusion output is performed, and the next round of iteration is started.

[0051] The following is a further description in combination with specific implementation steps:

[0052] Step 1 is the process of obtaining and preprocessing the data of the monocular camera. As Figure 2 shown is a schematic diagram of the monocular 3D detection process.

[0053] First, a 2D pose detector is used to extract the 2D joint coordinates of pedestrians in the input image. Then, the detected joint data is input into a lightweight feed-forward depth estimation neural network. This network captures the uncertainty caused by observation noise. Finally, the monocular 3D detection of the camera simultaneously obtains the 3D position estimate of the pedestrian and the uncertainty of the position estimate.

[0054] Step 2 is the process of obtaining and processing millimeter-wave radar data. Figure 3 It is the signal processing flow of the millimeter-wave radar.

[0055] First, the collected intermediate-frequency signal is processed by Range-FFT to obtain the distance information of the target, and Doppler-FFT is used to obtain the velocity information of the target. Finally, a matrix with the row vector being the distance and the column vector being the velocity is obtained. Secondly, the Constant False-Alarm Rate (CFAR) is used to distinguish the target from the noise to achieve reliable detection of the target. DoA Estimate is used to determine the angle of the target. Then, based on the results estimated by Range-FFT, Doppler-FFT, and DoA Estimate, the target point cloud data is generated. Cluster analysis is performed on the point cloud data, and adjacent points are grouped into the same category to represent the same target. Finally, based on the target results obtained by clustering, a multi-target tracking algorithm is used to obtain information such as the position, velocity, and azimuth angle of the target to achieve the detection of the target motion state by the millimeter-wave radar.

[0056] In the calibration process of Step 3, each frame of data of the camera and the millimeter-wave radar is time-aligned according to the closest timestamp. Since the measurement values of the millimeter-wave radar and the camera are not in the same coordinate system, in order to make the subsequent tracking results more accurate, external calibration of the two sensors is required before fusion to unify the millimeter-wave radar measurement values and the camera measurement values into the same coordinate system.

[0057]

[0058] Among them is the millimeter-wave radar coordinate system. is the camera coordinate system. R and T are the rotation matrix and the translation matrix respectively, and their values are obtained through pre-calibration.

[0059] The process of trajectory association in Step 4 is as Figure 4 shown.

[0060] First, the present invention conducts mathematical modeling on multi-target tracking. To facilitate the derivation of target motion, a motion model is established in the two-dimensional Cartesian coordinate system of the bird's-eye view. The state of a pedestrian can be represented by the position p and velocity v in the Cartesian coordinate system. Therefore, the state of a pedestrian can be expressed as:

[0061] x = (p, v) T = (p x , p y , v x , v y ) T

[0062] Since the motion state of the pedestrian remains relatively stable during movement, the motion state model of the pedestrian can be established as a uniform motion model. The uniform motion model assumes that the speed of the target remains constant within the measurement interval, and the motion model and the measurement models corresponding to the two sensors can be obtained:

[0063] x k = Fx k-1 + w k

[0064]

[0065] where x k is the state vector of the pedestrian at time k, F is the state transition matrix used to specify the kinematic characteristics, and w k is the process noise, satisfying w k ~ N(0, Q k ), and Q k represents the covariance matrix of the process noise. and are the measurement vectors of the camera and the millimeter-wave radar at time k, respectively, and H Camera and H Radar are their corresponding measurement matrices. and are the measurement noises of the camera and the millimeter-wave radar, respectively, satisfying and and represent the covariance matrices of the measurement noises of the camera and the millimeter-wave radar, respectively. ΔT is the time step.

[0066] Then, the Kalman filter is used to track the targets detected by the millimeter-wave radar and the camera respectively. The tracking consists of two steps: Kalman filter prediction and Kalman filter update. In the prediction stage, based on the state estimate value at the previous moment, the motion model is used to predict the state estimate of the fusion trajectory at the current moment and the error covariance matrix

[0067]

[0068] In the Kalman filter update stage, the updated Kalman gains and state estimates are obtained for the monocular camera and the millimeter-wave radar respectively through Kalman filter updates and and the error covariance matrix and

[0069]

[0070] In the trajectory association stage, the independent tracking trajectories of the millimeter-wave radar and the monocular camera are obtained. By introducing the cross-covariance matrix to describe the correlation between the trajectories of the two sensors, this correlation reflects the mutual influence and association degree between the trajectories. After obtaining the Kalman gain, the cross-covariance matrix between the state estimates of the two sensors can be calculated and The calculation is as follows:

[0071]

[0072] When calculating the fused trajectory and the error covariance matrix the correlation between the sensor trajectories is eliminated through the cross-covariance matrix. This correlation is the information redundancy brought by the common motion noise contained in the motion processes of the same target in different sensors. By eliminating this redundancy and common error, the final fused tracking result is more accurate

[0073]

[0074] Specifically, here is obtained by the independent tracking of each individual sensor and fused. Its fusion weight value is equal for each individual sensor. Therefore, the final state estimate value will not overly rely on a certain individual sensor. Even when the detection error of a certain individual sensor increases or even fails temporarily, the fused trajectory can still maintain stable tracking through the normal sensors and Kalman filtering, without trajectory mutation and strong trajectory fluctuations, and finally making the fused result more robust

[0075] Finally, trajectory management is performed in the output. For each new trajectory, it will be published after successfully matching enough frames, and for each trajectory that has not been matched with any detections for a long time, it will be deleted

[0076] Furthermore, the present invention is also assisted by specific embodiments and comparisons with different methods:

[0077] Figure 5Comparison of tracking accuracy in normal scenarios. 5(a) uses only the camera to track the trajectory. 5(b) uses only the millimeter-wave radar to track the trajectory. 5(c) Comparison of the fusion method proposed in this invention and the traditional fusion method. 5(d) Comparison of OSPA distances of different tracking methods (the smaller the OSPA distance, the closer it is to the true trajectory, that is, the higher the tracking accuracy and the better the tracking performance).

[0078] Figure 6 The robustness comparison of tracking in dark light and dense fog scenes. 6(a) The picture taken by the camera in the dark light scene. 6(b) The picture taken by the camera in the dense fog scene. 6(c) The track is tracked using only the monocular camera in the dark light scene. 6(d) The track is tracked using only the millimeter wave radar in the dark light scene. 6(e) The track comparison between the proposed fusion method and the traditional fusion method in the dark light scene. 6(f) The track is tracked using only the monocular camera in the dense fog scene. 6(g) The track is tracked using only the millimeter wave radar in the dense fog scene. 6(h) The track comparison between the proposed fusion method and the traditional fusion method in the dense fog scene.

[0079] In summary, the present invention proposes a joint tracking method based on millimeter-wave radar and monocular camera, which overcomes the problem of dependence on a single sensor and can achieve robust tracking in adverse weather conditions such as dark light and dense fog. In addition, the correlation between sensors is quantified by the cross-covariance matrix, which reduces the redundant error in sensor fusion, and the tracking trajectory result is more accurate than the traditional method.

[0080] What is disclosed above is only one or more preferred embodiments of the present invention, which certainly cannot be used to limit the scope of rights of the present invention. Ordinary technicians in this field can understand that all or part of the processes of implementing the above embodiments and making equivalent changes according to the claims of the present invention still fall within the scope of the invention.

Claims

1. A joint tracking method based on millimeter wave radar and monocular camera trajectory, characterized in that: The following steps are involved: Step 1: Obtain and preprocess the monocular camera data; Step 2: Millimeter wave radar data acquisition and processing; Step 3: Perform external calibration on the monocular camera and millimeter-wave radar to unify the millimeter-wave radar measurement values ​​and the monocular camera measurement values ​​into the same coordinate system; Step 4: Use Kalman filtering to track the targets detected by the millimeter-wave radar and the monocular camera respectively to obtain tracking trajectories, calculate the cross-covariance matrix for trajectory fusion, and obtain the final tracking trajectory.

2. The method for joint tracking of millimeter wave radar and monocular camera trajectory according to claim 1, characterized in that: The execution process of step 1 is specifically to use a 2D posture detector to extract the 2D joint coordinates of the pedestrian in the input image, and then input the detected joint data into a lightweight feed-forward depth estimation neural network, which captures the uncertainty caused by observation noise; Finally, the camera monocular 3D detection simultaneously obtains the pedestrian's 3D position estimation and the uncertainty of the position estimation.

3. The joint tracking method based on millimeter wave radar and monocular camera trajectory as claimed in claim 2 is characterized in that: The execution process of step 2 includes the following steps: Step 2.1: Use Range-FFT to obtain the distance information of the target, and use Doppler-FFT to obtain the speed information of the target, and finally obtain a matrix with row vectors as distance and column vectors as speed; Step 2.2: Use constant false alarm rate to distinguish between target and noise to achieve reliable detection of the target; Step 2.3: Generate target point cloud data, and then perform cluster analysis to classify adjacent points into the same category, representing the same target; Step 2.4: Based on the target results obtained by clustering, a group tracking algorithm is used to obtain target information.

4. The method for joint tracking based on millimeter wave radar and monocular camera trajectory according to claim 3, characterized in that: In step 3, each frame of camera and millimeter-wave radar data is time-aligned according to the closest timestamp, and the millimeter-wave radar measurement value and the monocular camera measurement value are unified into the same coordinate system. The expression is as follows: in is the millimeter wave radar coordinate system, is the camera coordinate system, R and T are the rotation matrix and translation matrix respectively, and their values ​​are obtained by pre-calibration.

5. The method for joint tracking based on millimeter wave radar and monocular camera trajectory according to claim 4, characterized in that: In step 4, we choose to establish a motion model in the two-dimensional Cartesian coordinate system of the bird's-eye view, and the state of the pedestrian is expressed as: x=(p,v) T =(p x ,p y ,v x ,v y ) T Among them, p is the position of the pedestrian, v is the speed of the pedestrian; The corresponding motion model and sensor measurement model are: x k =Fx k-1 +w k where x k is the state vector of the pedestrian at time k, F is the state transfer matrix, which is used to specify the kinematic characteristics, and w k is the process noise, satisfying w k ~N(0,Q k ), Q k represents the covariance matrix of the process noise; and are the measurement vectors of the camera and millimeter-wave radar at time k, respectively, Camera and H Radar is the corresponding measurement matrix; and are the measurement noise of the camera and millimeter-wave radar, respectively, satisfying and and They represent the covariance matrices of the camera and millimeter-wave radar measurement noise respectively; ΔT is the time step.

6. The method for joint tracking based on millimeter wave radar and monocular camera trajectory according to claim 5, characterized in that: During the execution of step 3, Kalman filtering is used to track the targets detected by the millimeter-wave radar and the camera respectively. Tracking consists of two steps: Kalman filter prediction and Kalman filter update. In the prediction stage, the motion model is used to predict the state estimate of the fusion trajectory at the current moment based on the state estimate value at the previous moment. and the error covariance matrix In the Kalman filter update stage, the monocular camera and millimeter wave radar are updated through Kalman filtering to obtain the updated Kalman gain and State Estimation and And the error covariance matrix and Where I is the identity matrix:

7. The method for joint tracking based on millimeter wave radar and monocular camera trajectory according to claim 6, characterized in that: In the trajectory joint stage in step 4, the cross-covariance matrix is ​​introduced to describe the correlation between the individual tracking trajectories of the millimeter-wave radar and the monocular camera. After obtaining the Kalman gain, the cross-covariance matrix between the state estimates of the two sensors can be calculated. and The calculation is as follows: In calculating the fusion trajectory and the error covariance matrix When , the correlation between sensor trajectories is eliminated through the cross-covariance matrix; in It is obtained by independent tracking of each individual sensor and The fusion weights are equal for each individual sensor.

Images