GM-PHD multi-target tracking method for measuring auxiliary weight correction of 4D millimeter wave radar

Abstract

The invention discloses a GM-PHD multi-target tracking method for weight correction assisted by 4D millimeter wave radar measurement. Firstly, target coordinates and speed data are collected through a millimeter wave radar, parameters and Gaussian items are initialized, and a target initial state and a covariance model are constructed. And secondly, performing state prediction by using a CKF method, generating a prediction state of a survival target through propagation of cubic points, updating the prediction state, introducing a new target, and dynamically adjusting the number and the state of the target in combination with a PHD model. And then, a measurement-assisted weight correction strategy is adopted, an optimal Gaussian component is screened based on maximum likelihood estimation, a PHD function is updated in combination with Kalman gain, and the tracking precision is improved. And finally, redundant Gaussian components are removed through pruning and merging, similar Gaussian components are merged, and the calculation complexity is reduced. According to the method, efficient, real-time and accurate multi-target tracking is realized by utilizing the advantages of high-precision measurement of the millimeter wave radar and multi-target tracking of the MCGMPHD algorithm.

Description

Technical Field

[0001] The invention belongs to the technical field of multi-target tracking, and in particular relates to a GM-PHD multi-target tracking method with 4D millimeter-wave radar measurement-assisted weight correction. Background Art

[0002] With the development of autonomous driving, intelligent transportation, security monitoring and other fields, the demand for multi-target tracking technology is gradually increasing, especially in autonomous driving scenarios, where vehicles and pedestrians need to be identified and tracked in real time to improve safety. Millimeter-wave radar has gradually become a key sensor for multi-target tracking due to its high resolution and penetration, especially in low visibility and occlusion conditions. Compared with visual sensors, millimeter-wave radar has greater advantages in environmental adaptability. However, traditional multi-target tracking algorithms, such as Kalman filtering and nearest neighbor association algorithms, are suitable for single targets or simple environments, but are prone to problems such as tracking loss and misassociation in complex environments, and have low computational efficiency, making it difficult to meet real-time requirements.

[0003] In order to overcome these limitations, probability hypothesis density (PHD) filtering has been introduced into the field of multi-target tracking. PHD filtering is based on the theory of random finite sets, which can effectively estimate the number and state of targets and realize multi-target tracking without explicit association. However, the traditional PHD method still has shortcomings in the introduction of dynamic targets, correction of association weights and computational complexity in high-density scenes. Therefore, based on the data of 4D millimeter-wave radar, the present invention proposes a GM-PHD multi-target tracking method with measurement-assisted weight correction to improve tracking accuracy and computational efficiency.

[0004] The method first uses a 4D millimeter-wave radar to obtain the coordinates and velocity data of the target and initialize the state model of the target. Through the cubic Kalman filter (CKF), the system uses cubic point propagation in state prediction to generate and update the target prediction state. Subsequently, the system introduces the new target state and adds the PHD model to dynamically adjust the number of targets. Using the measurement-assisted weight correction strategy, the system performs maximum likelihood estimation on the matching degree between the measured data and the predicted state, selects the Gaussian components that match the measured data, and corrects them through the Kalman gain, thereby improving the accuracy of state estimation.

[0005] In multiple time steps, the system repeats prediction, measurement and state update operations, gradually improves the state estimation of the target, and optimizes the PHD function through pruning and merging operations to remove low-weight redundant components to reduce the computational load. This pruning and merging strategy improves the real-time performance and accuracy of the system in complex scenarios.

[0006] The present invention takes advantage of the GM-PHD algorithm of high-precision measurement and measurement-assisted weight correction of millimeter-wave radar to effectively deal with the sensitivity to measurement errors and target state uncertainty in multi-target tracking, as well as clutter interference, false detection, and nonlinear target motion. It is suitable for high-precision real-time tracking of multiple targets in 4D Hainan convenience stores in complex environments such as autonomous driving, intelligent transportation, and security monitoring. Summary of the invention

[0007] The purpose of the present invention is to provide a GM-PHD multi-target tracking method with 4D millimeter-wave radar measurement-assisted weight correction. It uses millimeter-wave radar to obtain the coordinates and velocity data of the target, initializes Gaussian terms and related parameters, and establishes an initial state model. A cubic Kalman filter (CKF) is used for state prediction, and the predicted state of the surviving target is generated and updated by calculating and propagating cubic points. Subsequently, the system generates a state estimate of the new target, and introduces a probability hypothesis density (PHD) model to achieve dynamic adjustment of the number and state of the target. Through the measurement-assisted weight correction strategy, the system performs maximum likelihood estimation on the matching degree between the measured data and the predicted state, screens out the Gaussian components that meet the measured data, corrects the Kalman gain, and further updates the PHD function. In multiple time steps, the system continuously repeats this process to continuously optimize the estimate of the target state. Finally, through pruning and merging operations, redundant Gaussian components with smaller weights are removed and similar components are merged, reducing the computational complexity, thereby achieving real-time multi-target high-precision tracking of 4D millimeter-wave radar.

[0008] The GM-PHD multi-target tracking method of the present invention with 4D millimeter wave radar measurement assisted weight correction comprises the following steps:

[0009] Step 1: Place a 4D millimeter wave radar in the target area. The target moves in front of the radar, and the radar collects target movement data in frames. The collected data is processed by three-dimensional Fourier transform, constant false alarm detection, clustering, and centroid calculation to obtain the target's coordinates and speed data.

[0010] Step 2: According to the target coordinates and speed data obtained in step 1, initialize the parameters and Gaussian terms, construct the initial state and covariance of the target, and establish the target state model. Specifically, the following steps are included:

[0011] Step 2 (a): Based on the result of step 1, obtain the coordinates and speed data of the target. Use this data as the mean of the Gaussian distribution It is used to represent the initial state of the target. Then, according to the noise model in the target measurement process, the target covariance matrix used to describe the uncertainty in the measurement is initialized in and Denote the measurement errors in position and velocity respectively, where j represents the jth target. After that, the same weight is assigned to N targets, and the weight of each target is

[0012] Step 2 (ii): Based on the result of step 2 (i), the initialized target state vector, covariance and weight are introduced into the initialization formula of the PHD function, which is: in, is the initial weight of the jth target, is a Gaussian distribution, indicating the initial state of the jth target, with a mean of The covariance is J0 is the number of targets initially detected. Therefore, the initialized PHD function can be obtained as:

[0013]

[0014] Step 3: According to the target initial state obtained in step 2, the target state is predicted using the cubic Kalman filter (CKF), and the nonlinear state transfer is processed by the cubic point propagation method, that is, the next target state is predicted by the current state transfer model to obtain the predicted state mean m k|k-1 and the predicted covariance P k|k-1 The specific steps include:

[0015] Step 3 (a) Based on the target state vector dimension n, the system generates 2n cubic points ξ i The formula for generating a cubic point is: where e i is a unit vector.

[0016] Step 3 (2): Based on the results of step 3 (1) and step 2, use the state mean m of the previous time step k-1 and the covariance matrix P k-1 , maps the cubic point to the actual state space and uses it as the basis for the prediction of the current time step. The mapping formula is: Among them, m k-1 is the state mean of the previous time step, P k-1 is the covariance matrix.

[0017] Step 3 (iii): Based on the result obtained in step 3 (ii), each mapping cubic point x is transformed into i For nonlinear propagation, use the formula: Describes the state evolution from the previous time step k-1 to the current step k.

[0018] Step 3 (4): Based on the results of step 3 (3), all propagated cubic points After weighted averaging, using the formula: Get the predicted state mean m of the current time step k|k-1 . Also use the formula Calculate the prediction covariance P k|k-1 . Where Q is the process noise covariance matrix, which is used to reflect the uncertainty introduced by external interference or system dynamics. Finally, the system returns the predicted state mean m k|k-1 and the predicted covariance P k|k-1 .

[0019] Step 4: According to the predicted target state obtained in step 3, the new target detected by the millimeter-wave radar is compared with the predicted target. For the measurement data that cannot be matched, it is regarded as a new target and processed as a candidate target. The new target is initialized and weighted according to steps 2 and 3, and finally the state mean of the new target is obtained. With covariance

[0020] Step 5: Based on the target mean and predicted covariance obtained in step 4, the PHD function is used to represent the probability distribution of the target. The formula is: Where D k (x) is the PHD function, which represents the target density at state x, is the weight of the jth Gaussian component, is the state mean of the j-th target, is the state covariance matrix of the jth target. Specifically, it includes the following steps:

[0021] Step 5: Based on the results of step 3 and step 4, the following steps are specifically included:

[0022] Step 5 (a) For each survival target, use the formula Solve its state propagation, where is the predicted state mean of the target at time step k, F is the state transfer matrix used to describe the target state, Bu k is the control input, which represents the external control of the target. The covariance propagation formula is in is the covariance matrix of the target at time step k, and Q is the process noise covariance matrix.

[0023] Step 5 (ii): Calculate the weight of the survival target based on the result of step 3. in is the weight of the target in the previous time step, P surv is the probability of the target surviving at the current time. The PHD function of the surviving target is:

[0024]

[0025] Step 5 (iii): Based on the result of step 4, calculate the PHD function of the new target. The formula is: in, is the weight of the new target, is the initial state mean of the new target, is the initial covariance matrix of the new target.

[0026] Step 5 (iv), based on the results of step 5 (ii) and step 5 (iii), obtain the PHD prediction function of the current time step, in is the PHD function of the survival target, D birth (x) is the PHD function of the new target.

[0027] Step 6: Based on the updated PHD function obtained in step 5, the cubic points are propagated again using the cubic Kalman filter (CKF) method. The formula is:

[0028] Step 7: The latest cube point value obtained in step 6 Update the formula using measurements: Get the predicted measurement value after cubic point mapping. Then calculate the measurement likelihood value, the formula is: where d is the measurement dimension, is the target predicted measurement, is the innovation covariance matrix. Then the weight correction formula is Get the weight of each target, where is the weight of the jth target at time step k, p D,k is the target detection probability, which indicates the probability that the target is detected by the sensor at the current time step. is the target weight of the previous time step, is the measurement likelihood function, K k (z k ) is the normalization coefficient, Indicates that, where λ k is the clutter intensity, is the likelihood function of the background noise.

[0029] Step 8: Based on each target weight and target measurement likelihood value obtained in step 7, use the maximum likelihood selection method to obtain the state component with the largest combination value. where j * is the index of the most likely target state at the current time step. The target weight obtained in step 7 is compared with the threshold value W set in advance. h After comparison, it will be greater than the threshold W h The corresponding state component and covariance Input it into the Kalman filter again to update the Kalman filter. where K is the Kalman gain and R is the measurement noise covariance.

[0030] Step 9: After the prediction is completed, the target state is pruned and Gaussian components are merged. If Then delete the corresponding Gaussian component, otherwise keep it. Then, if the two Gaussian components satisfy ‖w k (i) -w k (j) ‖<threshold, they are considered similar, using the formula, Merge similar Gaussian variables.

[0031] Step 10: Based on the results of pruning and merging in step 9, obtain the final estimated number of targets and the corresponding target states.

[0032] Beneficial Effects

[0033] The present invention uses millimeter wave radar to obtain target coordinates and velocity data. First, the Gaussian terms and parameters are initialized according to the acquired target state to establish an initial state model. Secondly, the state prediction is performed through the cubic Kalman filter (CKF), the cubic points are calculated and propagated, and the predicted state of the surviving target is generated and updated. Then, the system generates a new target state estimate and introduces the PHD (probability hypothesis density) model to achieve dynamic adjustment of the number and state of the target. After that, the system uses a measurement-assisted weight correction strategy to perform maximum likelihood estimation on the matching degree between the measured data and the predicted state, screens the Gaussian components that meet the measured data, corrects the Kalman gain, and further updates the PHD function. In multiple time steps, the system repeats this process continuously to gradually improve the target state estimate. Finally, the system performs pruning and merging to remove redundant Gaussian components with smaller weights and merge similar components to reduce computational complexity and achieve high-precision tracking of multiple targets by 4D millimeter wave radar. BRIEF DESCRIPTION OF THE DRAWINGS

[0034] Figure 1 It is a flow chart of the present invention;

[0035] Figure 2 Obtain target coordinates and speed flow chart for 4D millimeter wave radar; Specific implementation plan

[0036] The present invention will be further described below in conjunction with the accompanying drawings:

[0037] like Figure 1 A GM-PHD multi-target tracking method with 4D millimeter-wave radar measurement-assisted weight correction is shown, which specifically includes the following steps:

[0038] Step 1: Place a 4D millimeter wave radar in the target area. The target moves in front of the radar, and the radar collects target movement data in frames. The collected data is processed by three-dimensional Fourier transform, constant false alarm detection, clustering, and centroid calculation to obtain the target's coordinates and speed data.

[0039] Step 2: According to the target coordinates and speed data obtained in step 1, initialize the parameters and Gaussian terms, construct the initial state and covariance of the target, and establish the target state model. Specifically, the following steps are included:

[0040] Step 2 (a): Based on the result of step 1, obtain the coordinates and speed data of the target. Use this data as the mean of the Gaussian distribution It is used to represent the initial state of the target. Then, according to the noise model in the target measurement process, the target covariance matrix used to describe the uncertainty in the measurement is initialized in and Denote the measurement errors in position and velocity respectively, where j represents the jth target. After that, the same weight is assigned to N targets, and the weight of each target is

[0041] Step 2 (ii): Based on the result of step 2 (i), the initialized target state vector, covariance and weight are introduced into the initialization formula of the PHD function, which is: in, is the initial weight of the jth target, is a Gaussian distribution, indicating the initial state of the jth target, with a mean of The covariance is J0 is the number of targets initially detected. Therefore, the initialized PHD function can be obtained as:

[0042]

[0043] Step 3: According to the target initial state obtained in step 2, the target state is predicted using the cubic Kalman filter (CKF), and the nonlinear state transfer is processed by the cubic point propagation method, that is, the next target state is predicted by the current state transfer model to obtain the predicted state mean m k|k-1 and the predicted covariance P k|k-1 The specific steps include:

[0044] Step 3 (a) Based on the target state vector dimension n, the system generates 2n cubic points ξ i The formula for generating a cubic point is: where e i is a unit vector.

[0045] Step 3 (2): Based on the results of step 3 (1) and step 2, use the state mean m of the previous time step k-1 and the covariance matrix P k-1 , maps the cubic point to the actual state space and uses it as the basis for the prediction of the current time step. The mapping formula is: Among them, m k-1 is the state mean of the previous time step, P k-1 is the covariance matrix.

[0046] Step 3 (iii): Based on the result obtained in step 3 (ii), each mapping cubic point x is transformed into i For nonlinear propagation, use the formula: Describes the state evolution from the previous time step k-1 to the current step k.

[0047] Step 3 (4): Based on the results of step 3 (3), all propagated cubic points After weighted averaging, using the formula: Get the predicted state mean m of the current time step k|k-1 . Also use the formula Calculate the prediction covariance P k|k-1 . Where Q is the process noise covariance matrix, which is used to reflect the uncertainty introduced by external interference or system dynamics. Finally, the system returns the predicted state mean m k|k-1 and the predicted covariance P k|k-1 .

[0048] Step 4: According to the predicted target state obtained in step 3, the new target detected by the millimeter-wave radar is compared with the predicted target. For the measurement data that cannot be matched, it is regarded as a new target and processed as a candidate target. The new target is initialized and weighted according to steps 2 and 3, and finally the state mean of the new target is obtained. With covariance

[0049] Step 5: Based on the target mean and predicted covariance obtained in step 4, the PHD function is used to represent the probability distribution of the target. The formula is: Where D k (x) is the PHD function, which represents the target density at state x, is the weight of the jth Gaussian component, is the state mean of the j-th target, is the state covariance matrix of the jth target. Specifically, it includes the following steps:

[0050] Step 5: Based on the results of step 3 and step 4, the following steps are specifically included:

[0051] Step 5 (a) For each survival target, use the formula Solve its state propagation, where is the predicted state mean of the target at time step k, F is the state transfer matrix used to describe the target state, Bu k is the control input, which represents the external control of the target. The covariance propagation formula is in is the covariance matrix of the target at time step k, and Q is the process noise covariance matrix.

[0052] Step 5 (ii): Calculate the weight of the survival target based on the result of step 3. in is the weight of the target in the previous time step, P surv is the probability of the target surviving at the current time. The PHD function of the surviving target is:

[0053]

[0054] Step 5 (iii): Based on the result of step 4, calculate the PHD function of the new target. The formula is: in, is the weight of the new target, is the initial state mean of the new target, is the initial covariance matrix of the new target.

[0055] Step 5 (iv), based on the results of step 5 (ii) and step 5 (iii), obtain the PHD prediction function of the current time step, in is the PHD function of the survival target, D birth (x) is the PHD function of the new target.

[0056] Step 6: Based on the PHD function obtained in step 5, the cubic points are propagated again using the cubic Kalman filter (CKF) method. The formula is:

[0057] Step 7: The latest cube point value obtained in step 6 Update the formula using measurements: Get the predicted measurement value after cubic point mapping. Then calculate the measurement likelihood value, the formula is: where d is the measurement dimension, is the target prediction measurement, S k (j) is the innovation covariance matrix. Then the weight correction formula is Get the weight of each target, where is the weight of the jth target at time step k, p D,k is the target detection probability, which indicates the probability that the target is detected by the sensor at the current time step. is the target weight of the previous time step, is the measurement likelihood function, K k (z k ) is the normalization coefficient, Indicates that, where λ k is the clutter intensity, is the likelihood function of the background noise.

[0058] Step 8: Based on each target weight and target measurement likelihood value obtained in step 7, use the maximum likelihood selection method to obtain the state component with the largest combination value. where j * is the index of the most likely target state at the current time step. The target weight obtained in step 7 is compared with the threshold value W set in advance. h After comparison, it will be greater than the threshold W h The corresponding state component and covariance Input it into the Kalman filter again to update the Kalman filter. where K is the Kalman gain and R is the measurement noise covariance.

[0059] Step 9: After the prediction is completed, the target state is pruned and Gaussian components are merged. If Then delete the corresponding Gaussian component, otherwise keep it. Then, if the two Gaussian components satisfy ‖w k (i) -w k (j) ‖<threshold, they are considered similar, using the formula, Merge similar Gaussian variables.

[0060] Step 10: Based on the results of pruning and merging in step 9, obtain the final estimated number of targets and the corresponding target states.

Claims

1. A GM-PHD multi-target tracking method with 4D millimeter-wave radar measurement assisted weight correction, characterized in that The steps include: Step 1: Place a 4D millimeter wave radar in the target area. The target moves in front of the radar, and the radar collects target movement data in frames. The collected data is processed by three-dimensional Fourier transform, constant false alarm detection, clustering, and centroid calculation to obtain the target's coordinates and speed data. Step 2: Based on the target coordinates and velocity data obtained in step 1, initialize the parameters and Gaussian terms, construct the initial state and covariance of the target, and establish the target state model. Step 3: According to the target initial state obtained in step 2, the target state is predicted using the cubic Kalman filter (CKF), and the nonlinear state transfer is processed by the cubic point propagation method, that is, the next target state is predicted by the current state transfer model to obtain the predicted state mean m k|k-1 and the predicted covariance P k|k-1 . Step 4: According to the predicted target state obtained in step 3, the new target detected by the millimeter-wave radar is compared with the predicted target. For the measurement data that cannot be matched, it is regarded as a new target and processed as a candidate target. The new target is initialized and weighted according to steps 2 and 3, and finally the state mean of the new target is obtained. With covariance Step 5: Based on the target mean and predicted covariance obtained in step 4, the PHD function is used to represent the probability distribution of the target. The formula is: Where D k (x) is the PHD function, which represents the target density at state x, is the weight of the jth Gaussian component, is the state mean of the j-th target, is the state covariance matrix of the j-th target. Step 6: Based on the PHD function obtained in step 5, the cubic points are propagated again using the cubic Kalman filter (CKF) method. The formula is: Step 7: The latest cube point value obtained in step 6 Update the formula using measurements: Get the predicted measurement value after cubic point mapping. Then calculate the measurement likelihood value, the formula is: where d is the measurement dimension, is the target predicted measurement, is the innovation covariance matrix. Then the weight correction formula is Get the weight of each target, where is the weight of the jth target at time step k, p D,k is the target detection probability, which indicates the probability that the target is detected by the sensor at the current time step. is the target weight of the previous time step, is the measurement likelihood function, K k (z k ) is the normalization coefficient, Indicates that, where λ k is the clutter intensity, is the likelihood function of the background noise. Step 8: Based on each target weight and target measurement likelihood value obtained in step 7, use the maximum likelihood selection method to obtain the state component with the largest combination value. where j * is the index of the most likely target state at the current time step. The target weight obtained in step 7 is compared with the threshold value W set in advance. h After comparison, it will be greater than the threshold W h The corresponding state component and covariance Input it into the Kalman filter again to update the Kalman filter. where K is the Kalman gain and R is the measurement noise covariance. Step 9: After the prediction is completed, the target state is pruned and Gaussian components are merged. If Then delete the corresponding Gaussian component, otherwise keep it. Then, if the two Gaussian components satisfy ‖w k (i) -w k (j) ‖<threshold, they are considered similar, using the formula, Merge similar Gaussian variables. Step 10: Based on the results of pruning and merging in step 9, obtain the final estimated number of targets and the corresponding target states.

2. The GM-PHD multi-target tracking method of claim 1 with 4D millimeter-wave radar measurement assisted weight correction is characterized in that The step 2 comprises the following steps: Step 2: Initialize the parameters and Gaussian terms according to the results of step 1, construct the initial state and covariance of the target, and establish the target state model. Specifically, the following steps are included: Step 2 (a): Based on the result of step 1, obtain the coordinates and speed data of the target. Use this data as the mean of the Gaussian distribution It is used to represent the initial state of the target. Then, according to the noise model in the target measurement process, the target covariance matrix used to describe the uncertainty in the measurement is initialized in and Denote the measurement errors in position and velocity respectively, where j represents the jth target. After that, the same weight is assigned to N targets, and the weight of each target is Step 2 (ii): Based on the result of step 2 (i), the initialized target state vector, covariance and weight are introduced into the initialization formula of the PHD function, which is: in, is the initial weight of the jth target, is a Gaussian distribution, indicating the initial state of the jth target, with a mean of The covariance is J0 is the number of targets initially detected. Therefore, the initialized PHD function can be obtained as:

3. The GM-PHD multi-target tracking method of claim 1 with 4D millimeter-wave radar measurement assisted weight correction is characterized in that The step three comprises the following steps: Step 3: Based on the results of step 2, the following steps are specifically included: Step 3 (a) Based on the target state vector dimension n, the system generates 2n cubic points ξ i The formula for generating a cubic point is: where e i is a unit vector. Step 3 (2): Based on the results of step 3 (1) and step 2, use the state mean m of the previous time step k-1 and the covariance matrix P k-1 , maps the cubic point to the actual state space and uses it as the basis for the prediction of the current time step. The mapping formula is: Among them, m k-1 is the state mean of the previous time step, P k-1 is the covariance matrix. Step 3 (iii): Based on the result obtained in step 3 (ii), each mapping cubic point x is transformed into i For nonlinear propagation, use the formula: Describes the state evolution from the previous time step k-1 to the current step k. Step 3 (4): Based on the results of step 3 (3), all propagated cubic points After weighted averaging, using the formula: Get the predicted state mean m of the current time step k|k-1 . Also use the formula Calculate the prediction covariance P k|k-1 . Where Q is the process noise covariance matrix, which is used to reflect the uncertainty introduced by external interference or system dynamics. Finally, the system returns the predicted state mean m k|k-1 and the predicted covariance P k|k-1 .

4. The GM-PHD multi-target tracking method of claim 1 with 4D millimeter-wave radar measurement assisted weight correction is characterized in that The step five comprises the following steps: Step 5: Based on the results of step 3 and step 4, the following steps are specifically included: Step 5 (a) For each survival target, use the formula Solve its state propagation, where is the predicted state mean of the target at time step k, F is the state transfer matrix used to describe the target state, Bu k is the control input, which represents the external control of the target. The covariance propagation formula is in is the covariance matrix of the target at time step k, and Q is the process noise covariance matrix. Step 5 (ii): Calculate the weight of the survival target based on the result of step 3. in is the weight of the target in the previous time step, P surv is the probability of the target surviving at the current time. The PHD function of the surviving target is: Step 5 (iii): Based on the result of step 4, calculate the PHD function of the new target. The formula is: in, is the weight of the new target, is the initial state mean of the new target, is the initial covariance matrix of the new target. Step 5 (iv), based on the results of step 5 (ii) and step 5 (iii), obtain the PHD prediction function of the current time step, in is the PHD function of the survival target, D birth (x) is the PHD function of the new target.

Images