Three-dimensional multi-extension target tracking method based on inter-target reference deviation state
Through a three-dimensional multi-scaling target tracking method based on the reference deviation state between targets, using variational inference and Gaussian process model, the performance degradation problem of traditional methods in the case of target occlusion is solved, and the accuracy of multi-scaling target pose and contour state is achieved. It is estimated that the application scenario has been expanded.
Patent Information
- Application Number
- CN202411051383.5
- Authority / Receiving Office
- CN · China
- Patent Type
- Applications(China)
- Current Assignee / Owner
- Filing Date
- 2024-08-01
- Publication Date
- 2024-11-15
- Estimated Expiration
- 2044-08-01
AI Technical Summary
The traditional three-dimensional multi-scaling target tracking algorithm has degraded performance under target occlusion, making it difficult to accurately estimate the target's pose and contour state.
Using a three-dimensional multi-extended target tracking method based on the reference deviation state between targets, the state vector of the extended target is constructed by selecting the reference target, and using variational inference and Gaussian process model to obtain the center of mass coordinates, velocity and pose quaternions of the extended target. Achieve tracking and track estimation of multiple scaling targets.
The tracking performance under target occlusion is improved, and the posture and contour state of multiple expansion targets can be accurately estimated in three-dimensional space, expanding the application scenario, and avoiding the tracking loss problem in traditional methods.
Smart Images

Figure CN118962662A_ABST
Abstract
Description
Technical Field
[0001] The invention belongs to the technical field of multi-target tracking, and in particular relates to a three-dimensional multi-extended target tracking method based on reference deviation states between targets. Background Art
[0002] With the rapid development of radar technology and the reduction of its cost, more and more three-dimensional lidar sensors are being used in various scenarios. In some privacy-sensitive occasions, the use of sensors such as lidar can quickly and accurately locate and track without the disadvantage of visual sensors that easily violate privacy. And because of its high sensor resolution, one target will generate multiple point cloud data, and because identifying the shape of the target is conducive to further judgment, it is considered to be treated as an extended target. For three-dimensional multi-extended target tracking, the tracking performance of the sensor observing the extended target is not only affected by traditional factors such as detection probability and false alarm rate, but also closely related to the geometric relationship of "sensor-target". When the extended target is blocked by other targets, the tracking performance of the traditional algorithm is bound to further decline. To this end, the present invention proposes a three-dimensional multi-extended target tracking method based on the reference deviation state between targets. Summary of the invention
[0003] The purpose of the present invention is to address the deficiencies of the prior art and propose a three-dimensional multi-extended target tracking method to improve the tracking performance when targets occlude each other and have excellent contour estimation capability.
[0004] The present invention provides a three-dimensional multi-extended target tracking method based on the reference deviation state between targets, comprising the following steps:
[0005] Step 1: Select any one of the multiple extended targets as a reference target; obtain the state vector of the reference target through the point cloud data collected by the sensor; construct the state vectors of all extended targets based on the state vector of the reference target in, It is the outline state; is the attitude reference deviation state, and are the centroid coordinates of the k reference target at the current moment in the three-dimensional local coordinate system and speed and are the kth The displacement deviation and velocity deviation of an extended target relative to a reference target; is the kth The attitude error deviation vector of the extended target; is the kth The angular velocity vector of the extended target; Γ is the number of expansion targets.
[0006] Step 2: Predict the state vector of k extended targets at the current moment based on k-1 at the previous moment
[0007] Step 3: Get the predicted density of the extended target Prediction density with extended targets and point cloud data Construct point cloud data k and the associated latent variable r k,j The joint density Among them, Z k Point cloud data gather; is the time-varying unknown mixing weight,
[0008] Step 4: Use variational inference to obtain the estimated reference deviation state of the extended target According to the estimated reference deviation state Get the center of mass coordinates, velocity, and attitude quaternion of the extended target.
[0009] Step 5: loop steps 2 to 4, obtain the center of mass coordinates, velocity and attitude quaternion of the extended target at different times, complete the tracking of all extended targets, and obtain the tracks of all extended targets.
[0010] Preferably, in step 4, the method for obtaining the center of mass coordinates and speed of the extended target is: the center of mass coordinates and speed of the reference target are and The center of mass coordinates and velocity of the extended targets except the reference target are and
[0011] As a preferred embodiment, in the step 4, the attitude quaternion of the extended target The method to obtain is as follows:
[0012]
[0013] in, is relative to the reference quaternion The error quaternion is expressed as:
[0014]
[0015] As a preference, in step 5, after each cycle is completed, the posture error deviation vector Set to 0.
[0016] Preferably, in step 3, the point cloud data The method to obtain is:
[0017]
[0018] in, is the unit vector from the center of mass of the extended target to the point cloud; For the The covariance matrix of the targets; is a radial function Noise; Measure the noise for the sensor.
[0019] As a preference, in the step 2, the predicted state vector The expression is:
[0020]
[0021] Among them, F k is the state transfer matrix; Estimate the state vector at time k-1.
[0022] Preferably, in step 3, the combined density The acquisition process is as follows:
[0023] Prediction density of expansion targets The expression is:
[0024]
[0025] in, is the predicted profile state at time k; is the prediction covariance, and They are the predicted attitude covariance and the predicted contour covariance respectively; is the Dirichlet distribution.
[0026] For each point cloud data z k,j Establishing associated latent variables in, In the associated latent variable In the example, one element is 1 and the rest are 0. Its distribution
[0027] Constructing the likelihood function of point cloud data for:
[0028]
[0029] Among them, r k is the associated latent variable; n k To expand the amount of point cloud data on the target; is a nonlinear measurement function; is the measurement noise covariance matrix.
[0030] Point cloud data k and the associated latent variable r k The joint distribution of The expression is:
[0031]
[0032] As a preference, the predicted covariance P k∣k-1 The method to obtain is as follows:
[0033]
[0034] Among them, F k is the state transfer matrix; Q k-1 is the covariance matrix of process noise; P k-1 Estimated covariance for the k-1 time step.
[0035] Preferably, in step 4, the variational inference method is as follows:
[0036] (1) Constructing the attitude reference deviation state in the multi-target state Profile Status Mixing Weight and the associated latent variable r k The joint probability density function of for:
[0037]
[0038] (2) According to the joint probability density function Get the attitude reference deviation status respectively Profile Status Mixing Weight and the associated latent variable r k The variational distribution of and for:
[0039]
[0040] in, is the point cloud relative to the spherical angle pairs of the extended target's centroid; and are respectively the predicted attitude reference deviation state and the predicted contour state; c \π is the blend weight Irrelevant arbitrary constant terms; For the The number of contour points of the extended target.
[0041] (3) Variational distribution through fixed point iteration and Get the associated latent variable r at time k k , estimated attitude reference deviation state Estimated contour status Mixing Weight and the estimated covariance P k .
[0042] Preferably, the estimated profile state The method to obtain is:
[0043] Get point cloud data in the local coordinate system for:
[0044]
[0045] in, is the global to local rotation matrix; is the observation matrix.
[0046] Based on point cloud data Get the spherical angle pair in the local coordinate system Heading Angle and pitch angle The expression is:
[0047]
[0048] Learning radial functions of 3D object shapes using GP models Radial function The expression is:
[0049]
[0050] in, For the The covariance matrix of the targets; is the noise of radial function, is the covariance of the radial function noise, and its expression is:
[0051]
[0052] Where K is the matrix of the covariance function; For the The covariance function of the targets; and
[0053] Covariance function The expression is:
[0054]
[0055] Among them, σ f , is the hyperparameter of GP; is the distance function.
[0056] Preferably, the extended target is a vehicle; and the sensor for collecting the reference target state vector is a laser radar.
[0057] The present invention has the following beneficial effects:
[0058] 1. The present invention determines a reference target and uses the deviation between the reference target and the extended target to track the extended target, thereby obtaining the track of the extended target, and can accurately estimate the posture and contour of multiple extended targets in three-dimensional space; at the same time, by tracking multiple extended targets based on the reference target, it is possible to accurately estimate the posture and contour state of the extended target during occlusion, effectively solving the problem of tracking loss after the extended target is occluded.
[0059] 2. The present invention establishes the state vectors of all extended targets based on the state vector of the reference target. Compared with the traditional method of tracking all contour points by extending the target particles, the speeds of all extended targets in the present invention are different and change with time, which has a wider range of application scenarios. BRIEF DESCRIPTION OF THE DRAWINGS
[0060] Figure 1 It is a principle flow chart of an implementation mode of the present invention.
[0061] Figure 2 A schematic diagram of establishing a reference deviation state for the present invention. DETAILED DESCRIPTION
[0062] The present invention will be further described below in conjunction with the accompanying drawings.
[0063] like Figure 1 As shown, a three-dimensional multi-extended target tracking method based on reference deviation state includes the following steps:
[0064] Step 1: Establish reference deviation state and corresponding target contour state
[0065] In this embodiment, the current moment is recorded as moment k, and the previous moment is recorded as moment k-1.
[0066] 1-1. Establishing an evolution model of the extended target state of the extended target
[0067] Taking the first extended target as the reference target, the evolution model of the extended target state of the extended target at time k is established as follows:
[0068] x k =F k x k-1 +v k-1 (1)
[0069] Among them, x k is the state vector at time k, It is the outline state; is the attitude reference deviation state, and As the centroid coordinates of the reference target at time k in the three-dimensional local coordinate system and speed and are the displacement deviation and velocity deviation of the extended target relative to the reference target at time k respectively; is the kth moment in the three-dimensional local coordinate system The attitude error deviation vector of the extended target; is the kth moment in the three-dimensional local coordinate system The angular velocity vector of the extended target; F k is the state transfer matrix, and are the attitude reference deviation state matrix and the contour state transfer matrix respectively; v k is the process noise vector at time k, is the attitude process noise; is the contour process noise; blkdiag(·) is the block diagonal matrix constructor; Γ is the number of expansion targets.
[0070] Assume that the process noise vector v at time k-1 is k-1 It obeys zero-mean Gaussian white noise, that is, v k-1 ~N(0,Q k-1 ), where the covariance matrix of the process noise is and are the process noise covariance matrices corresponding to the attitude reference deviation state and the contour state respectively.
[0071] like Figure 2 As shown in the figure, assuming that the motion state of the extended target is approximately uniform motion, the attitude reference deviation state The expression is:
[0072]
[0073] 1-2. Establishing the attitude reference deviation state motion model
[0074] According to the posture parameterization, Extension target direction for:
[0075]
[0076] in, is the k-1th moment The attitude quaternion of the extended target, is relative to the reference quaternion is the error quaternion; ⊙ is the quaternion multiplication symbol.
[0077] Error Quaternion The expression is:
[0078]
[0079] Quaternion components obey unit length constraints
[0080] 1-3. Establishing the extended target contour model
[0081] As a random model, Gaussian process (GP) has the ability to learn the probability distribution of any unknown function online. The GP model is used to learn the radial function of the three-dimensional target shape. in, For the The spherical angle pair of the i-th contour point in the extended object relative to the center of mass in local coordinates, and are the heading angle and pitch angle of the contour point respectively, For the The number of contour points of the extended target. Establish the contour state of the extended target for:
[0082]
[0083] in, For the The outline status of the extended target,
[0084] In the Gaussian process framework, the contour state of each extended target is A finite set of contour points can be used When there is no prior knowledge of the target contour points, the heading angle and pitch angle The samples can be evenly spaced on the unit sphere and stacked into a matrix Since the radial function is unknown, it can be described and learned online using the Gaussian process model. Without loss of generality, it is assumed that the mean function of the Gaussian process follows a zero-mean Gaussian distribution. Among them, σ r is the hyperparameter of GP. Then the radial function It can be expressed as:
[0085]
[0086] in, For the The covariance function of the targets; and
[0087] Covariance function is the modified square exponential function (SE), and its expression is:
[0088]
[0089] Among them, σ f , is the hyperparameter of GP; is the distance function, and its expression is:
[0090]
[0091] Step 2: Establish a multi-extended target measurement model based on reference deviation state
[0092] Assume that at time k the sensor receives n k Point cloud data of locations For the global coordinate system Point cloud data of the target The measurement equation is:
[0093]
[0094] in, is the observation matrix; is the unit vector from the center of mass of the extended target to the point cloud; is the sensor measurement noise, is the sensor measurement noise covariance, σ 2 is the variance; is a nonlinear measurement function; is the point cloud in local coordinates relative to the The spherical angle pairs of the extended target mass center, diag is the symbol for a diagonal matrix.
[0095] The extended target in this embodiment is a vehicle, and the sensor is a laser radar.
[0096] Online learning of radial functions using GP model The radial function is obtained from formula (6): The expression is:
[0097]
[0098] in, For the The covariance matrix of the targets; is the noise of radial function, is the covariance of the radial function noise.
[0099] Covariance matrix The expression is:
[0100]
[0101] Where K is the matrix of the covariance function.
[0102] Covariance of radial noise function The expression is:
[0103]
[0104] Therefore, the measurement equation of formula (9) can be redefined as:
[0105]
[0106] in, is the new measurement noise, is the measurement noise covariance matrix, which is expressed as:
[0107]
[0108] Unit Vector The expression is:
[0109]
[0110] where ‖·‖2 is the Euclidean norm.
[0111] Point cloud data In the local coordinate system it is expressed as:
[0112]
[0113] in, is the global to local rotation matrix, which is expressed as:
[0114]
[0115] Based on point cloud data Get the spherical angle pair in the local coordinate system Heading Angle and pitch angle The expression is:
[0116]
[0117] Among them, ξ, η, ζ are the x, y, and z axes of the local coordinate system corresponding to the extended target, respectively.
[0118] Step 3: Get the predicted status of the extended target.
[0119] Get the estimated state vector x of the extended target at the previous moment k-1 k-1 and the estimated covariance P k-1 ; The state vector at the initial moment is obtained by preprocessing the point cloud data collected by the sensor through the DBSCAN clustering algorithm. According to formula (1), the reference deviation state vector of the multi-extended target is predicted as:
[0120]
[0121] in, is the predicted state vector at time k; Estimate the state vector at time k-1.
[0122] The predicted covariance P at time k k∣k-1 The expression is:
[0123]
[0124] Among them, P k-1 is the estimated covariance at time k-1.
[0125] Step 4: Construct a mixed probability density function of the likelihood of point cloud data
[0126] Prediction density of expansion targets The expression is:
[0127]
[0128] in, is the time-varying unknown mixing weight, is the predicted profile state at time k; is the predicted profile covariance at time k; is a Dirichlet distribution, which has the following form:
[0129]
[0130] According to formula (9), the likelihood function of point cloud data is obtained for:
[0131]
[0132] Since updating the predicted density of the complete target using the likelihood function of the point cloud data is analytically unsolvable, it is necessary to define the associated latent variable r k,j , to call variational Bayesian techniques to approximately infer the difficult posterior probability density.
[0133] For each point cloud data z k,j Establishing associated latent variables in, In the associated latent variable In the example, one element is 1 and the rest are 0. Its distribution can be defined as:
[0134]
[0135] Assuming that all point cloud data in a frame are conditionally independent, the above formula can be rewritten as
[0136]
[0137] By correlating the latent variable r k,j , the likelihood function Can be redefined as:
[0138]
[0139] Point cloud data k and the associated latent variable r k,j The joint density The expression is:
[0140]
[0141] Step 5: Variational Inference
[0142] 5-1. Constructing the joint probability density function of multiple target states
[0143] Attitude reference deviation status Profile Status Mixing Weight and the associated latent variable r k,j The joint probability density function of is defined as follows:
[0144]
[0145] 5-2. Inferring associated latent variables
[0146] Correlation latent variable r k The approximate distribution of is inferred as follows:
[0147]
[0148] in, For about r k The variational distribution of E \r For about r k expectations; \r is the latent variable r associated with k An arbitrary constant term that is irrelevant.
[0149] The expectation is related to the latent variable r k The relevant items are broken down into:
[0150]
[0151] Finally, we get the associated latent variable r k The density form is as follows:
[0152]
[0153] 5-3. Inferring the attitude reference deviation state
[0154] Attitude reference deviation status The approximate distribution of is inferred as follows:
[0155]
[0156] in, About The variational distribution of For about expectations; The deviation state from the attitude reference An arbitrary constant term that is irrelevant.
[0157] The expected deviation from the attitude reference state The relevant items are broken down into:
[0158]
[0159] in, is the predicted attitude covariance at time k.
[0160] Finally, the attitude reference deviation state is obtained The density form is as follows:
[0161]
[0162] 5-4. Inferring the contour status
[0163] Profile Status The approximate distribution of is inferred as follows:
[0164]
[0165] in, For about The variational distribution of For about expectations; For the contour state An arbitrary constant term that is irrelevant.
[0166] The expected and outlined states The relevant items are broken down into:
[0167]
[0168] Finally, the contour state is obtained The density form is as follows:
[0169]
[0170] 5-5. Inferring Mixed Weights
[0171] Mixing Weight The approximate distribution of is inferred as follows:
[0172]
[0173] in, For about The variational distribution of E \π For about expectations; \π is the blend weight An arbitrary constant term that is irrelevant.
[0174] The expected value and the mixed weight The relevant items are broken down into:
[0175]
[0176] Among them, c \π With blend weights An arbitrary constant term that is irrelevant.
[0177] Finally, the mixed weight The density form is as follows:
[0178]
[0179] Variational distribution via fixed point iteration and The density form of the associated latent variable r at time k is obtained k , estimated attitude reference deviation state Estimated contour status Mixing Weight and the estimated covariance Among them, get the estimated contour state The iterative process is the process of step 2.
[0180] Step 6: Get the extended target status
[0181] According to the attitude reference deviation state Get the center of mass coordinates, velocity and attitude quaternion of different extended targets at time k. Among them, the center of mass coordinates and velocity of the reference target are and The center of mass coordinates and velocity of the extended targets except the reference target are The attitude quaternion of the extended target is:
[0182]
[0183] After obtaining the extended target state, the deviation vector Set to 0.
[0184] Step 7: Repeat steps 3 to 6 to obtain the center of mass coordinates, velocity and attitude quaternion of the extended target at different times, track the extended target, and obtain the track of the extended target.
Claims
1. A three-dimensional multi-extended target tracking method based on reference deviation states between targets, characterized in that: The following steps are involved: Step 1: Select any one of the multiple extended targets as a reference target; Construct the state vectors of all extended targets based on the state vector of the reference target in, It is the outline state; is the attitude reference deviation state, and are the centroid coordinates of the k reference target at the current moment in the three-dimensional local coordinate system and speed and are the displacement deviation and velocity deviation of the kth extended target relative to the reference target at the current moment, respectively; is the attitude error deviation vector of the kth extended target at the current moment in the three-dimensional local coordinate system; is the angular velocity vector of the l-th extended target at the current time k in the three-dimensional local coordinate system; l = 1,…,Γ; Γ is the number of extended targets; Step 2: Predict the state vector of k extended targets at the current moment based on k-1 at the previous moment Step 3: Get the predicted density of the extended target Prediction density with extended targets and point cloud data Construct point cloud data k and the associated latent variable r k,j The joint density Among them, Z k Point cloud data gather; is the time-varying unknown mixing weight, Step 4: Use variational inference to obtain the estimated reference deviation state of the extended target According to the estimated reference deviation state Get the center of mass coordinates, velocity and attitude quaternion of the extended target; Step 5: loop steps 2 to 4, obtain the center of mass coordinates, velocity and attitude quaternion of the extended target at different times, and obtain the track of all extended targets.
2. The three-dimensional multi-extended target tracking method based on the reference deviation state between targets according to claim 1 is characterized in that: In the step 4, the method for obtaining the center of mass coordinates and speed of the extended target is: the center of mass coordinates and speed of the reference target are and The center of mass coordinates and velocity of the extended targets except the reference target are and 3. The three-dimensional multi-extended target tracking method based on inter-target reference deviation state according to claim 1, characterized in that: In step 4, the attitude quaternion of the extended target The method to obtain is as follows: in, is relative to the reference quaternion The error quaternion is expressed as:
4. The three-dimensional multi-extended target tracking method based on inter-target reference deviation state according to claim 1, characterized in that: The extended target is a vehicle, and the sensor for collecting the reference target state vector is a laser radar.
5. The three-dimensional multi-extended target tracking method based on inter-target reference deviation state according to claim 1, characterized in that: In step 3, point cloud data The method to obtain is: in, is the unit vector from the center of mass of the extended target to the point cloud; is the covariance matrix of the lth target; is a radial function Noise; Measure the noise for the sensor.
6. The three-dimensional multi-extended target tracking method based on inter-target reference deviation states according to claim 1, characterized in that: In the step 2, the predicted state vector The expression is: Among them, F k is the state transfer matrix; Estimate the state vector at time k-1.
7. The three-dimensional multi-extended target tracking method based on inter-target reference deviation states according to claim 1, characterized in that: In step 3, the joint density The acquisition process is as follows: Prediction density of expansion targets The expression is: in, is the predicted profile state at time k; is the prediction covariance, and They are the predicted attitude covariance and the predicted contour covariance respectively; is Dirichlet distribution; For each point cloud data z k,j Establishing associated latent variables in, In the associated latent variable In the example, one element is 1 and the rest are 0; for the associated latent variable Its distribution Constructing the likelihood function of point cloud data for: Among them, r k is the associated latent variable; n k To expand the amount of point cloud data on the target; is a nonlinear measurement function; is the measurement noise covariance matrix; Point cloud data k and the associated latent variable r k The joint distribution of The expression is:
8. The three-dimensional multi-extended target tracking method based on inter-target reference deviation states according to claim 7, characterized in that: The predicted covariance P k∣k-1 The method to obtain is as follows: Among them, F k is the state transfer matrix; Q k-1 is the covariance matrix of process noise; P k-1 Estimated covariance for the k-1 time step.
9. The three-dimensional multi-extended target tracking method based on inter-target reference deviation states according to claim 7, characterized in that: In the step 4, the variational inference method is as follows: (1) Constructing the attitude reference deviation state in the multi-target state Profile Status Mixing Weight and the associated latent variable r k The joint probability density function of for: (2) According to the joint probability density function Get the attitude reference deviation status respectively Profile Status Mixing Weight and the associated latent variable r k The variational distribution of and for: in, is the spherical angle pair of the point cloud relative to the centroid of the l-th extended target; and are respectively the predicted attitude reference deviation state and the predicted contour state; c \π is the blend weight Irrelevant arbitrary constant term; i=1,2,...,N l ; N l is the number of contour points of the lth extended target; (3) Variational distribution through fixed point iteration and Get the associated latent variable r at time k k , estimated attitude reference deviation state Estimated contour status Mixing Weight and the estimated covariance P k .
10. The three-dimensional multi-extended target tracking method based on inter-target reference deviation states according to claim 9, characterized in that: The estimated profile state The method to obtain is: Get point cloud data in the local coordinate system for: in, is the global to local rotation matrix; H l is the observation matrix; Based on point cloud data Get the spherical angle pair in the local coordinate system Heading Angle and pitch angle The expression is: Learning radial functions of 3D object shapes using GP models Radial function The expression is: in, is the covariance matrix of the lth target; is the noise of radial function, is the covariance of the radial function noise, and its expression is: Where K is the matrix of the covariance function; is the covariance function of the lth target; l'∈[1,Γ] and l'≠l; Covariance function The expression is: Among them, σ f ,σ l is the hyperparameter of GP; is the distance function.
Citation Information
Patent Citations
Probabilistic data correlation filtering extended target tracking method based on Gaussian process
CN108734725A
Extended target tracking method based on GP-VSMM-JPDA
CN109633590A
Target tracking and deviation compensation method in Doppler environment
CN115616912A
Target tracking method and device, equipment and medium
CN115755022A
Multi-sensor fusion method and system for intelligent driving vehicle
WO2024001629A1
