A three-dimensional multi-extent target tracking method based on inter-target reference bias state

Abstract

The application discloses a three-dimensional multi-extended target tracking method based on a target-to-target reference deviation state, which determines a reference target, uses the deviation between the reference target and an extended target to track the extended target, and further obtains the track of the extended target, so that the posture and contour of the multi-extended target in a three-dimensional space can be accurately estimated; meanwhile, the multi-extended target is tracked based on the reference target, the posture and contour state of the extended target can be accurately estimated during the occlusion of the extended target, and the problem of tracking loss after the occlusion of the extended target is effectively solved. In addition, the state vector of the reference target is used to establish the state vector of all the extended targets, and the speeds of all the extended targets in the application are different and change with time, so that the application has a wider application scenario compared with the traditional method of tracking all contour points through the extended target particles.

Description

TECHNICAL FIELD

[0001] The application belongs to the technical field of multi-target tracking, and particularly relates to a three-dimensional multi-extended target tracking method based on inter-target reference deviation state. BACKGROUND

[0002] With the rapid development of radar technology and the reduction of its cost, more and more three-dimensional laser radar sensors are applied to various scenes. In some privacy-sensitive occasions, the use of laser radar sensors can quickly and accurately position and track without the shortcomings of visual sensors easily infringing privacy. However, due to the high resolution of the sensor, multiple point cloud data will be generated for one target, and distinguishing the shape of the target is beneficial for further judgment, so it is considered to be processed 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 between the sensor and the target. When the extended target is blocked by other targets, the tracking performance of the traditional algorithm will inevitably further decrease. Therefore, the application provides a three-dimensional multi-extended target tracking method based on inter-target reference deviation state. SUMMARY

[0003] The application aims at the deficiencies of the prior art, and provides a three-dimensional multi-extended target tracking method, which improves the tracking performance when targets are mutually blocked and has excellent contour estimation capability.

[0004] The three-dimensional multi-extended target tracking method based on inter-target reference deviation state provided by the application comprises the following steps:

[0005] Step one, selecting any one of the multiple extended targets as a reference target; collecting the state vector of the reference target through the point cloud data of the sensor; constructing the state vector of all the extended targets based on the state vector of the reference target wherein, is the contour state; is the attitude reference deviation state, and are the mass center coordinates of the reference target in the three-dimensional local coordinate system at the current time k and the velocity and are the displacement deviation and the velocity deviation of the first extended target relative to the reference target at the current time k; is the attitude error deviation vector of the first extended target in the three-dimensional local coordinate system at the current time k; is the angular velocity vector of the i-th extended target at time k in the local coordinate system; Γ is the number of extended targets.

[0006] Step two, predicting the state vector of multiple extended targets at time k according to the state vector of multiple extended targets at time k-1

[0007] Step three, obtaining the predicted density of the extended targets combining the predicted density of the extended targets and the point cloud data to construct the joint density of the point cloud data z k and the associated latent variable r k,j wherein z k is the set of point cloud data is the time-varying unknown mixing weight,

[0008] Step four, obtaining the estimated reference bias state of the extended targets by using variational inference obtaining the centroid coordinates, velocity and attitude quaternion of the extended targets according to the estimated reference bias state

[0009] Step five, repeating steps two to four to obtain the centroid coordinates, velocity and attitude quaternion of the extended targets at different times, completing the tracking of all extended targets and obtaining the tracks of all extended targets.

[0010] As a preferred embodiment, in step four, the method for obtaining the centroid coordinates and velocity of the extended targets is as follows: the centroid coordinates and velocity of the reference target are and the centroid coordinates and velocity of the extended targets other than the reference target are and

[0011] As a preferred embodiment, in step four, the method for obtaining the attitude quaternion of the extended targets is as follows:

[0012]

[0013] wherein e is the error quaternion relative to the reference quaternion q , and its expression is as follows:

[0014]

[0015] ​​​​​As preferred, in step five, after each round of iteration, the pose error bias vector is set to 0.

[0016] As preferred, in step three, the point cloud data is obtained by:

[0017]

[0018] where, is the unit vector of the extended target centroid to the point cloud; is the covariance matrix of the th target; is the radial function noise; is the sensor measurement noise.

[0019] As preferred, in step two, the predicted state vector is expressed as:

[0020]

[0021] where, k is the state transition matrix; is the estimated state vector at time k-1.

[0022] As preferred, in step three, the joint density is obtained by:

[0023] The predicted density of the extended target is expressed as:

[0024]

[0025] where, is the predicted contour state at time k; is the predicted covariance, and are the predicted pose covariance and the predicted contour covariance, respectively; is the Dirichlet distribution.

[0026] For each point cloud data z k,j , the associated latent variable is established, where, In the associated latent variable , one element is 1 and the rest are equal to 0. For the associated latent variable , its distribution

[0027] The likelihood function of the point cloud data is constructed as For:

[0028]

[0029] where r k is the associated latent variable; n k is the number of point cloud data on the extended target; is a nonlinear measurement function; is a measurement noise covariance matrix.

[0030] Point cloud data z k and the associated latent variable r k The joint distribution of the expression is:

[0031]

[0032] As preferred, the prediction covariance P k∣k-1 The acquisition method is as follows:

[0033]

[0034] where F k is the state transition matrix; Q k-1 is the covariance matrix of process noise; P k-1 is the estimated covariance at time k-1.

[0035] As preferred, in step four, the method of variational inference is as follows:

[0036] (1) Construct the multi-target state pose reference deviation state Contour state Mixed weight and the joint probability density function of the associated latent variable r k is:

[0037]

[0038] (2) According to the joint probability density function respectively get the variational distribution about the pose reference deviation state Contour state Mixed weight and the associated latent variable r k and are:

[0039]

[0040] where, is the spherical angle pair of the point cloud relative to the first extended target centroid​​ and are the predicted pose reference bias state and the predicted contour state, respectively; c \π is an arbitrary constant term independent of the mixing weight . is the number of contour points of the th extended target.

[0041] (3) Obtain the associated latent variable r at time k by fixed-point iteration variational distribution and k estimate the pose reference bias state estimate the contour state the mixing weight and the estimated covariance P k .

[0042] As a preferred, the method for obtaining the estimated contour state is:

[0043] Obtain the point cloud data in the local coordinate system is:

[0044]

[0045] wherein, is the rotation matrix from global to local; is the observation matrix.

[0046] According to the point cloud data obtain the spherical angle pair heading angle and pitch angle in the local coordinate system, and the expression is:

[0047]

[0048] Learn the radial function of the three-dimensional target shape using the GP model The expression of the radial function is:

[0049]

[0050] wherein, is the covariance matrix of the th target; is the noise of the radial function, is the covariance of the radial function noise, and the 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. It 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 can accurately estimate the posture and contour state of the extended target during occlusion, effectively solving the problem of tracking loss after occlusion of the extended target.

[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 This is a principle flow chart of an embodiment of the present invention.

[0061] Figure 2 Schematic diagram for establishing a reference deviation state for the present invention. DETAILED DESCRIPTION

[0062] The present invention will be further described below with reference to 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 k-th time extended target 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 center of mass coordinate 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 posture 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 transition 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 Obey 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] As Figure 2 shown, assuming the motion state of the extended target is approximately uniform motion, the expression of the attitude reference deviation state is:

[0072]

[0073] 1-2. Establishing the motion model of the attitude reference deviation state

[0074] According to the parameterization of the attitude, the direction of the first extended target is:

[0075]

[0076] where, is the attitude quaternion of the first extended target at time k-1, is the error quaternion relative to the reference quaternion ; and is the quaternion multiplication symbol.

[0077] The expression of the error quaternion is:

[0078]

[0079] The quaternion components are subject to the unit length constraint

[0080] 1-3. Establishing the extended target contour model

[0081] As a kind of random model, Gaussian process (GP) has the probability distribution of learning arbitrary unknown function online. The radial function of three-dimensional target shape is learned using GP model where, is the spherical angle pair of the contour point of the i th in the first extended target relative to the center of mass in the local coordinates, and are the heading angle and the pitch angle of the contour point respectively, is the contour point number of the first extended target. The contour state of the extended target is:

[0082]

[0083] where, is the contour point number of the first​ the contour state of each extended target,

[0084] the contour state of each extended target can be described by a finite set of contour points . When the prior knowledge of the contour points of targets is lacking, the heading angle and the pitch angle can be sampled uniformly on the unit sphere and stacked into a matrix Since the radial function is unknown, it can be described and learned online by a Gaussian process model. Without loss of generality, it is assumed that the mean function of the Gaussian process obeys a zero-mean Gaussian distribution where σ r is the hyperparameter of the GP. Then the radial function can be expressed as:

[0085]

[0086] where is the covariance function of the th target; and

[0087] The covariance function is a modified squared exponential function (SE), whose expression is:

[0088]

[0089] where σ f , is the hyperparameter of the GP; is the distance function, whose expression is:

[0090]

[0091] Step two, establish a multi-extended target measurement model based on the reference deviation state

[0092] Assume that at time k, the sensor receives n k position point cloud data from the contour of the extended target For the point cloud data of the th target in the global coordinate system its measurement equation is:

[0093]

[0094] where is the observation matrix; is the unit vector from the centroid of the extended target to the point cloud; the measurement noise for the sensor, the measurement noise covariance for the sensor, σ 2 the variance; the nonlinear measurement function; the spherical angular pair of the point cloud in local coordinates with respect to the first target centroid; diag is the diagonal matrix notation.

[0095] The extended target in this embodiment is a vehicle, and the sensor is a lidar.

[0096] Online learning of the radial function using the GP model The expression of the radial function from equation (6) is:

[0097]

[0098] where, is the covariance matrix of the first target; is the noise of the radial function, is the covariance of the radial function noise.

[0099] The expression of the covariance matrix is:

[0100]

[0101] where K is the matrix of the covariance function.

[0102] The expression of the covariance of the radial function noise is:

[0103]

[0104] Therefore, the measurement equation of equation (9) can be redefined as:

[0105]

[0106] where, is the new measurement noise, is the measurement noise covariance matrix, and its expression is:

[0107]

[0108] The expression of the unit vector is:

[0109]

[0110] where ||·||2 is the Euclidean norm.

[0111] Point cloud data is expressed in the local coordinate system as:

[0112]

[0113] where, is the global-to-local rotation matrix, whose expression is:

[0114]

[0115] According to the point cloud data the spherical angle pair under the local coordinate system is obtained the expression of the heading angle and the pitch angle is:

[0116]

[0117] where ξ, η, ζ are the x, y, z axes of the local coordinate system corresponding to the extended target, respectively.

[0118] Step three, obtaining the predicted state of the extended target.

[0119] The estimated state vector x k-1 and the estimated covariance P k-1 of the extended target at the previous time k-1 are obtained; the state vector at the initial time is obtained by preprocessing the point cloud data collected by the sensor through the DBSCAN clustering algorithm. According to formula (1), the reference bias state vector of the multiple extended targets is predicted as:

[0120]

[0121] where, is the predicted state vector at time k; is the estimated state vector at time k-1.

[0122] The expression of the predicted covariance P k∣k-1 at time k is:

[0123]

[0124] where P k-1 is the estimated covariance at time k-1.

[0125] Step four, constructing the mixed probability density function of the point cloud data likelihood

[0126] The predicted density of the extended target The expression of the likelihood function is:

[0127]

[0128] where, is a time-varying mixing weight, is the predicted contour state at time k; is the predicted contour covariance at time k; is a Dirichlet distribution, which is in the form of:

[0129]

[0130] The likelihood function of the point cloud data is obtained according to formula (9) is:

[0131]

[0132] Since it is analytically intractable to update the predicted density of the complete target by using the likelihood function of the point cloud data, it is necessary to define the associated latent variable r k,j to call the variational Bayesian technique to approximately infer the intractable posterior probability density.

[0133] For each point cloud data z k,j , the associated latent variable r is established, where, In the associated latent variable r , one element is 1 and the rest of the elements are equal to 0. For the associated latent variable r , 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] Through the associated latent variable r k,j , the likelihood function can be redefined as:

[0138]

[0139] The expression of the joint density k of the point cloud data z k,j and the associated latent variable r is:

[0140]

[0141] Step five, variational inference

[0142] 5-1. Constructing the joint probability density function of the multi-object state

[0143] pose reference bias state profile state mixing weight and the associated latent variable r k,j The joint probability density function of the associated latent variable r

[0144]

[0145] 5-2. Inferring the associated latent variable

[0146] The approximate distribution of the associated latent variable r k is inferred as follows:

[0147]

[0148] where, is the variational distribution over r k ; E \r is the expectation over r k ; c \r is an arbitrary constant term independent of the associated latent variable r k .

[0149] The term in the expectation that is related to the associated latent variable r k is factored out as:

[0150]

[0151] The density form of the associated latent variable r k is finally obtained as:

[0152]

[0153] 5-3. Inferring the pose reference bias state

[0154] The approximate distribution of the pose reference bias state is inferred as follows:

[0155]

[0156] where, is the variational distribution over ; is the expectation over ; is an arbitrary constant term independent of the pose reference bias state .

[0157] The term in the expectation that is related to the pose reference bias state The term related to the contour state

[0158]

[0159] where, is the predicted pose covariance at time k.

[0160] The pose reference bias state is approximated as follows:

[0161]

[0162] 5-4, inferring the contour state

[0163] The contour state is approximated as follows:

[0164]

[0165] where, is the variational distribution over ; is the expectation over ; is an arbitrary constant term independent of the contour state .

[0166] The term related to the contour state in the expectation is decomposed as:

[0167]

[0168] The density form of the contour state is finally obtained as:

[0169]

[0170] 5-5, inferring the mixing weight

[0171] The mixing weight is approximated as follows:

[0172]

[0173] where, is the variational distribution over ; E \π is the expectation over ; c \π is an arbitrary constant term independent of the mixing weight .

[0174] The term related to the mixing weight in the expectation is decomposed as:

[0175]

[0176] where c \π arbitrary constant term independent of the mixing weight

[0177] Finally, the density form of the mixing weight is as follows:

[0178]

[0179] By the density form of the fixed point iterative variational distribution and , the associated latent variable r k at time k is obtained, the attitude reference deviation state is estimated, the contour state is estimated, the mixing weight is estimated, and the covariance is estimated. The iterative process for obtaining the estimated contour state is the process of step two.

[0180] Step six, obtaining the extended target state

[0181] The centroid coordinates, velocity, and attitude quaternion of different extended targets at time k are obtained according to the attitude reference deviation state . Among them, the centroid coordinates and velocity of the reference target are and The centroid coordinates and velocity of the extended target other than the reference target are The attitude quaternion of the extended target is:

[0182]

[0183] After obtaining the extended target state, the deviation vector is set to 0.

[0184] Step seven, repeat steps three to six to obtain the centroid coordinates, velocity, and attitude quaternion of the extended target at different times, realize the tracking of the extended target, and obtain the track of the extended target.

Claims

1. A three-dimensional multi-extended target tracking method based on inter-target reference deviation states, characterized by: The following steps are involved: Step 1: Select any one of the multiple expansion 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 l-th extended target relative to the reference target at the current moment, l≠1; is the posture 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 moment 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 the previous moment k-1 Step 3: Get the predicted density of the extended target Prediction density combined with expansion target 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, r k is the associated latent variable; 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 to 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 inter-target reference deviation states according to claim 1, characterized in that: In the fourth step, 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 states 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 states 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 states 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 the 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 pose covariance and the predicted contour covariance respectively; is the 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 likelihood function for 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 density 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 k-1 time steps.

9. The three-dimensional multi-extended target tracking method based on inter-target reference deviation states according to claim 7, characterized in that: In 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 Variational distribution of and for: in, is the spherical angle pair of the point cloud relative to the center of mass 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 state 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 contour 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.

Images