Expanded target tracking method based on adaptive angle variance

Abstract

The invention belongs to the technical field of maneuvering target tracking, and particularly relates to an extended target tracking method based on adaptive angle variance. Comprising the following steps: step 1, modeling a sideslip angle between an orientation angle and a velocity angle as a sideslip angle model of Gaussian noise with a zero mean value and an adaptive variance; and step 2, based on integration of the sideslip angle model and pseudo measurement, estimating a motion state xk, a size state Xk and an orientation angle theta k of the extended target so as to realize extended target tracking.

Description

Technical Field

[0001] The present invention belongs to the technical field of maneuvering target tracking, and in particular relates to an extended target tracking method based on adaptive angular variance. Background Art

[0002] Traditional target tracking assumes that the resolution of the sensor is small relative to the target size, so the target is usually modeled as a measurement of a point source. However, with the improvement of the resolution of modern sensors (for example, automotive millimeter-wave radar), these sensors may obtain multiple measurements from a target within each time step. In this case, the target can be regarded as an extended object (EO). In addition to estimating the kinematic state of the target, extended object tracking (EOT) also requires estimating the morphological information (size and orientation angle) of the extended target. Many scholars have developed different EOT models, such as the random matrix (RM) model and the random hyper-surface model (RHM).

[0003] In many scenarios, the heading angle of an extended target varies over time while the target size remains constant. Therefore, it is reasonable to model the heading angle and shape separately. Several methods have been proposed to estimate the heading angle of extended targets, often assuming an elliptical or rectangular target shape. In these methods, the target's shape is parameterized by its semi-axis length and heading angle, resulting in better tracking performance. In practical applications, there is an inherent coupling between the heading angle and velocity angle of the target's center of mass. They are not always exactly equal, but are highly correlated. In particular, when the target turns, there may be an angle between the heading angle and velocity angle, often referred to as the sideslip angle. The most popular methods currently assume that the heading angle and velocity angle are independent. Some researchers assume that the heading angle and velocity angle are equal, but this assumption is not applicable to tracking maneuvering targets. Some researchers have proposed using the sideslip angle to establish a relationship between the velocity angle and the heading angle, but directly modeling the sideslip angle is extremely difficult. Other studies have derived an equation that relates the angle between the heading angle and velocity angle. However, this equation is overdetermined, and analytical solutions for the variables are not available in practical applications. In short, although some existing methods utilize the coupling information between heading angle and velocity angle, they fail to solve this problem well. Directly applying these methods may lead to poor performance. Summary of the Invention

[0004] The present invention proposes an extended target tracking method based on adaptive angular variance, which can adapt to most dynamic scenarios by utilizing the relevant prior information of the target's heading angle and velocity angle to assist EOT. This method can adaptively handle the target's linear motion and turning maneuvers without the need for complex mathematical models.

[0005] To achieve the above objectives, the technical solutions of the present invention are as follows:

[0006] The embodiment of the present application provides an extended target tracking method based on adaptive angular variance, comprising the following steps:

[0007] Step 1: Model the sideslip angle between the heading angle and the velocity angle as a sideslip angle model with Gaussian noise of zero mean and adaptive variance;

[0008] Step 2: Based on the integration of the sideslip angle model and pseudo-measurement, the target motion state x is extended. k , size status X k and the orientation angle θ k Estimation, thus achieving extended target tracking.

[0009] Optionally, the sideslip angle model of the present invention is:

[0010]

[0011] in, represents the velocity angle, θ k represents the orientation angle, S k represents the adaptive variance;

[0012] Adaptive variance S k The morphological parameters are and the scale parameter is The inverse gamma distribution is used to model:

[0013]

[0014] Optionally, the present invention will expand the target motion state x k , size status X k , orientation angle θ k ;

[0015]

[0016] in, represents the center of mass position; is the Cartesian velocity vector; Indicates the square of the length of the target semi-axis 1; Indicates the square of the length of the target semi-axis 2.

[0017] Optionally, the step 1 of the present invention further constructs a data structure including N k The measurement model of the extended target:

[0018]

[0019] in, is the Cartesian position of the target; D k The estimated matrix of the target extended shape; H k represents the measurement matrix; is white noise; λ is the Gaussian approximation when the measurement source is uniformly distributed within the ellipse; R k is the covariance of the true measurement noise; is the rotation matrix, is the size state at time step k predicted at time step k-1.

[0020] Optionally, the target motion state x of the present invention k The model is:

[0021]

[0022] f(x k )=arctan(G2x k ,G1x k )

[0023]

[0024] in, represents the estimate of the motion state at time step k predicted at time step k-1, The predicted covariance matrix representing the motion state; The estimated matrix representing the inverse of the target extended form; express About variable S k expectations; k represents pseudo measurement; G1 and G2 represent given matrices;

[0025] q x (x k ) is the probability distribution expression, select q x (x k ) is used as the estimated value of the target motion state.

[0026] Optionally, the target orientation angle θ of the present invention is k The model is:

[0027]

[0028] in, represents the derivative matrix of the target rotation matrix; tr(·) represents the trace operation; q θ (θ k ) is the probability distribution expression, select q θ (θ k ) is used as the estimated value of the heading angle.

[0029] Optionally, the size state X of the present invention k The model is:

[0030]

[0031] in,

[0032]

[0033] n=1,2

[0034] Among them, q X (X k ) is the probability distribution expression, select q X (X k ) as an estimate of the size state.

[0035] Optionally, the morphological parameters of the present invention and scale parameters for:

[0036]

[0037] Beneficial effects:

[0038] First, the present invention models the sideslip angle between the heading angle and the velocity angle as a sideslip angle model with Gaussian noise of zero mean and adaptive variance, which can handle scenarios containing both straight-line motion and turning maneuvers.

[0039] Second, the present invention obtains an analytical solution for the state to be estimated based on the sideslip angle model. The resulting EOT method is concise and highly interpretable, making it easy to apply in practice.

[0040] Third, the present invention conducted simulation experiments, and the simulation results showed that the proposed tracking method has high adaptability and ease of use, and has achieved significant improvements in the accuracy of heading angle estimation and overall performance. The proposed method is superior to existing methods in performance. BRIEF DESCRIPTION OF THE DRAWINGS

[0041] In order to more clearly illustrate the technical solutions of the embodiments of the present invention, the following briefly introduces the drawings required for use in the embodiments. Obviously, the drawings described below are only some embodiments of the present invention. For ordinary technicians in this field, other drawings can be obtained based on these drawings without paying any creative work.

[0042] Figure 1 Comparison diagram of the non-drifting driving state and the lateral drifting driving state in the method of the present invention; (a) non-drifting driving state, (b) lateral drifting driving state;

[0043] Figure 2 Estimating the trajectory and expansion graph at the selected time step of the method of the present invention;

[0044] Figure 3 This is a graph showing the algorithm comparison and estimation results of the method of the present invention in S1;

[0045] Figure 4 This is a diagram showing the algorithm comparison and estimation results of the method of the present invention in S2. DETAILED DESCRIPTION

[0046] The embodiments of the present invention are described in detail below with reference to the accompanying drawings.

[0047] It should be noted that, in the absence of conflict, the following embodiments and features in the embodiments may be combined with each other; and, based on the embodiments in this disclosure, all other embodiments obtained by persons of ordinary skill in the art without creative work are within the scope of protection of this disclosure.

[0048] It should be noted that various aspects of the embodiments within the scope of the appended claims are described below. It should be apparent that the aspects described herein can be embodied in a wide variety of forms, and any specific structure and / or function described herein is merely illustrative. Based on this disclosure, it should be understood by those skilled in the art that an aspect described herein can be implemented independently of any other aspect, and two or more of these aspects can be combined in various ways. For example, any number of aspects described herein can be used to implement an apparatus and / or practice a method. In addition, other structures and / or functionalities other than one or more of the aspects described herein can be used to implement this apparatus and / or practice this method.

[0049] The present invention provides an extended target tracking method based on adaptive angular variance, which includes the following steps:

[0050] Step 1: Model the sideslip angle between the heading angle and the velocity angle as a sideslip angle model with Gaussian noise of zero mean and adaptive variance;

[0051] Step 2: Based on the integration of the sideslip angle model and pseudo-measurement, estimate the extended target motion state x k , size status X k and the orientation angle θ k .

[0052] The process of the present invention is described in detail below:

[0053] Step 1: Problem modeling: For extended target tracking EOT, consider estimating the motion state x of the elliptical extended target k 、Form X k and the orientation angle θ k , which is defined as follows:

[0054]

[0055] in, represents the center of mass position; is the Cartesian velocity vector; shape X k It is parameterized by the square of the semi-axis length and is defined as a diagonally positive definite matrix; towards the angle θ k It represents the counterclockwise rotation angle starting from the positive direction of the x-axis. k The modeling will be discussed later.

[0056] 1. Sideslip angle model: In order to model the sideslip angle u k ,Here two main factors are considered: the driving state and the sport mode of EO.

[0057] Assuming the Ackermann steering geometry and no-drift driving state, the comparison between the no-drift driving state and the lateral drift driving state is shown in the figure below. Figure 1 As shown, the blue solid line represents the heading angle, the red dotted line represents the speed angle, and the angle between these line segments represents the sideslip angle u k , among which, for Figure 1 (a) shows the drift-free driving state. During the straight-line motion, u k is almost equal to zero and can be regarded as a system error caused by the turning maneuver. Figure 1 (b) The lateral drift driving state, u k Random variations, almost independent of the motion model, and therefore difficult to model accurately. Therefore, in the above two cases, u k have different uncertainties, which makes it impossible to use a unified accuracy model to describe u k . Because of its description of u k The uncertainty (including deviation) has a strong ability, and it has the simplicity of calculation, so in the present invention, u k The variance of is used for modeling and serves as the state variable to be estimated.

[0058] In this case, u k Treat it as a random variable and describe it using Gaussian noise with random variance as follows:

[0059]

[0060] in, In addition, by using the morphological parameters and the scale parameter is The inverse gamma distribution is used to model the variance S k , as shown below:

[0061]

[0062] Then, S k It can be estimated adaptively like other state variables. To simplify the notation, let Θ k ={x k ,θ k ,X k ,S k}.

[0063] This embodiment: (a) In the absence of further information, the u in model (2) is constructed by zero-mean Gaussian distribution and adaptive variance. k This modeling method can not only describe the interdependence between velocity angle and heading angle, but also express u k (b)u k The adaptive variance of makes it possible to model complex scenarios including straight-line motion and turning maneuvers. As the motion changes, the uncertainty caused by the sensor and the sideslip angle also change, resulting in u k The variance changes to adapt to different scene changes.

[0064] 2. Measurement model: At time step k, the sensor generates a k A set of extended target EO measurement values, denoted as By using the matched linearization method, the following measurement model is proposed:

[0065]

[0066] Where r = 1, 2, ..., N k ; is the Cartesian position of the target; is a known matrix; It is white noise; E ε[x] represents the expectation of x with respect to the variable ε; λ = 1 / 4 is the Gaussian approximation when the measurement source is uniformly distributed within the ellipse; R k is the covariance of the true measurement noise; is the rotation matrix.

[0067] The form of formula (4) can solve the problem of no conjugate prior, that is, it is independent of R k By combining Equations (2) and (4) with the dynamic model of all states, we will discuss in step 2 how to estimate the posterior density p(Θ k |Z k ).

[0068] Step 2: Use the variational Bayesian VB method to implement EOT.

[0069] 1. Pseudo-measurement method and linearization: The pseudo-measurement method is used to process the sideslip angle model defined in Equation (2), which helps to derive the estimation method. Since Equation (2) is nonlinear for the estimator, a statistical linearization method is used here. Given G1 = [0, 0, 1, 0] and G2 = [0, 0, 0, 1], the nonlinear function The linearized form in formula (2) is:

[0070]

[0071] in,

[0072] f(x k )=arctan(G2x k ,G1x k ) (6)

[0073] in, and The quantities that need to be calculated are all about x k The expected values of x in the previous iteration depend on k The sufficient statistics (mean and covariance) of these variables can be estimated by using approximation techniques of different moments, such as unscented transformation (UT) and Gaussian Hermite quadrature (GHQ).

[0074] Then, by substituting Equation (5) into Equation (2), we can get the final pseudo-measurement form:

[0075] y k =θ k -A k x k +u k (7)

[0076] in, and

[0077] 2. Likelihood function: By assuming that the measurement conditions at time step k are independent, the true measurement value Z k The likelihood function can be expressed as:

[0078]

[0079] Similarly, there are:

[0080]

[0081] Combine equations (8) and (9) and assume that Likelihood function It can be broken down into:

[0082]

[0083] 3. Variational measurement update: In the present invention, since it is difficult to find the posterior density p(Θ k |Z k ), so the VB method will be used to find an approximate analytical solution.

[0084] At time step k, the following common assumptions are made to predict the density:

[0085]

[0086] in, and are the prediction parameters calculated in the time update step, corresponding to x k-1 ,θ k-1 , and S k-1 .

[0087] The factorized approximation of the posterior density is:

[0088] q Θ (Θ k )≈q x (x k )q θ (θ k )q X (X k )q S (S k )(12)

[0089] To obtain the above factorized density, minimize the KL divergence in the VB method to calculate the difference between the factorized density and the true posterior density. The solution obtained satisfies the following equation:

[0090]

[0091] in, Represents the difference between the set The expectation of the external variable, Express about The constant term of , the joint density can be expressed as:

[0092]

[0093] in, and p(Θ k |Z k-1 ) are shown in Equations (10) and (11) respectively. By iteratively using Equation (13), q can be obtained after initialization. x (x k ),q θ (θ k ),q X (X k ) and q S (S k ).

[0094] For x k and θ k :

[0095] q x (x k ) can be calculated by substituting formula (14) into formula (13) and respectively Set to x k and θ k To update. The update formula is as follows:

[0096]

[0097] in,

[0098]

[0099] And there is,

[0100]

[0101] where tr(A) represents the trace of the matrix A, and

[0102] Based on the parameters obtained above, we can get q x (x k ) and q θ (θ k), this embodiment selects the expectation of the probability distribution as the estimated value of the state, and we can get: when x k Obey q x (x k ) this probability density, x k The estimated value is When θ k Obey q θ (θ k ) this probability density, θ k The estimated value is

[0103] For X k and S k Calculation:

[0104] If we substitute equation (14) into equation (13) and Set to X k and S k , then X k and S k The update of can be calculated as follows:

[0105]

[0106] in,

[0107]

[0108] Based on the parameters obtained above, we can get q X (X k ) and q S (S k ), this embodiment selects the expectation of the probability distribution as the estimated value of the state, and we can get: k Obey q X (X k ) is the probability density, its estimated value is Where diag(·) is a diagonal matrix operation; when S k Obey q S (S k ) this probability density, S k The estimated value is

[0109] This embodiment models the sideslip angle as zero-mean Gaussian noise and infers its variance. Furthermore, the variance is modeled as an inverse gamma distribution. Based on this model, a variational Bayesian (VB) method is derived to recursively estimate the state. A pseudo-measurement is constructed using the sideslip angle model, which can be integrated into the VB framework to obtain an analytical solution for the state to be estimated.

[0110] In order to verify the effectiveness of the above method, the algorithm based on pseudo measurement and variance adaptation is called "EOT-AD" in the steps. 0 To deal with The method is called “EOT-FIX”. After derivation, it is found that the only modification in the final result is to change Replace with (R 0 ) -1 , to adapt to EOT-FIX.

[0111] The performance of the proposed algorithm was evaluated through simulation experiments. Comparisons were made with the EOT-AD, EOT-FIX, and the related "EOT-VB" algorithm. Algorithm performance was assessed by comparing the root mean square error (RMSE) of the center of mass position and orientation angle, as well as the Hausdorff distance, from 100 Monte Carlo (MC) experiments. Target motion was modeled using an approximate constant velocity (CV) model.

[0112] Implementation Cases

[0113] The effects of the present invention can be illustrated by the following experiments.

[0114] We consider an EO with a major semi-axis of 2 m and a minor semi-axis of 1 m in a two-dimensional plane. The target moves along a straight line at a constant speed of 8.3 m / s starting from the origin. The number of measurements at each time follows a Poisson distribution with a mean of 10, and the sampling interval T is set to 0.1 s. The noise covariance in all measurement models is set to R k =diag(0.1 2 m 2 ,0.1 2 m 2 ). The number of iterations for all VB methods is set to 10, and for the uniformly distributed extended target, λ = 1 / 4. The forgetting factor of the morphological dynamics is set to The forgetting factor of the adaptive variance is set to During initialization, the dynamic state is initialized using the measured values from the first two steps, and the semi-axis length is initialized based on the true value.

[0115] Scenario 1 (S1): Consider an EO whose trajectory is a straight line and whose heading angle is -π / 4rad. Assume that in S1, the heading angle is consistent with the velocity angle. The process noise variance of the dynamic state and the heading angle is set as Initialized to 0.013 rad 2, R 0 =0.1rad 2 (EOT-FIX). For the heading angle, set and

[0116] Figure 2 (a) shows the estimated trajectory and expansion at selected time steps in S1. Figure 3 The Hausdorff distance and heading angle RMSE of each algorithm are shown. As can be seen, EOT-AD significantly outperforms EOT-VB and EOT-FIX. EOT-FIX performs slightly better than EOT-VB because EOT-FIX considers the fixed variance of Equation (7), while the latter does not. Because the dynamic model matches the actual situation and adopts adaptive variance estimation, EOT-AD is able to provide accurate state estimation in just a few iterations, demonstrating the effectiveness of EOT-AD in handling linear motion scenarios.

[0117] Scenario 2 (S2): Assume the target has high maneuverability. The extended target (EO) has an initial heading angle of -π / 4 rad. Throughout the trajectory, there are three turns with yaw rates of Δω1 = π / 6 rad / s (time steps 12-26), Δω2 = π / 5 rad / s (time steps 38-62), and Δω3 = π / 6 rad / s (time steps 74-103). When the EO moves in a straight line, the velocity angle is the same as the heading angle.

[0118] In S2, the process noise variances of the kinematic state and the heading angle were manually adjusted to obtain the best performance of EOT-VB and were set to and Set to 0.008 rad 2 , R 0 =0.3rad 2 (EOT-FIX). Other parameter settings are the same as in S1.

[0119] Figure 2 (b) shows the estimated trajectory and extended information at the selected time step in S2. Hausdorff distance, orientation angle RMSE, adaptive variance S k The comparison results of the sideslip angle are shown in Figure 4 (a)-(d). We can observe that EOT-AD can effectively estimate the sideslip angle and heading angle and shows the best overall performance in terms of Hausdorff distance. Figure 4 (c) It can be seen that the S of EOT-AD kIt can be adaptively adjusted as the motion changes. In addition, as the turning rate increases, the difference between the heading angle and the speed angle estimated by each algorithm (i.e., the estimated sideslip angle) also increases accordingly, such as Figure 4 (d) However, since EOT-AD uses Equation (7) with variance adaptation, its estimated sideslip angle is closer to the true value than other algorithms. This will lead to improved heading angle estimation and enhance the overall performance of EOT-AD.

[0120] The present invention proposes a new model with variance adaptation for estimating the sideslip angle between the heading angle and the velocity angle. The model can handle both straight-line motion and turning maneuvers.

[0121] Based on this model, a VB method is derived for approximate reasoning. The resulting EOT method has a simple and analytical form, making it easy to apply. Simulation results show that the proposed method outperforms existing methods in terms of performance.

[0122] In summary, the above are only preferred embodiments of the present invention and are not intended to limit the scope of protection of the present invention. Any modifications, equivalent replacements, improvements, etc. made within the spirit and principles of the present invention should be included in the scope of protection of the present invention.

Claims

1. An extended target tracking method based on adaptive angular variance, characterized in that: The following steps are involved: Step 1: Model the sideslip angle between the heading angle and the velocity angle as a sideslip angle model with Gaussian noise of zero mean and adaptive variance; Step 2: Based on the integration of the sideslip angle model and pseudo-measurement, the target motion state x is extended. k , size status X k and the orientation angle θ k Estimation, thus achieving extended target tracking.

2. The extended target tracking method based on adaptive angular variance according to claim 1, characterized in that: The sideslip angle model is: in, represents the velocity angle, θ k represents the orientation angle, S k represents the adaptive variance; Adaptive variance S k The morphological parameters are and the scale parameter is The inverse gamma distribution is used to model:

3. The extended target tracking method based on adaptive angular variance according to claim 2, characterized in that: Expand the target motion state x k , size status X k , orientation angle θ k ; in, represents the center of mass position; is the Cartesian velocity vector; and Represents the square of the length of the two semi-axes of the target.

4. The extended target tracking method based on adaptive angular variance according to claim 2, characterized in that: The step 1 also constructs a data set containing N based on the data collected by the sensor. k The measurement model of the extended target: in, is the Cartesian position of the target; D k The estimated matrix of the target extended shape; H k represents the measurement matrix; is white noise; λ is the Gaussian approximation when the measurement source is uniformly distributed within the ellipse; R k is the covariance of the true measurement noise; is the rotation matrix, is the size state at time step k predicted at time step k-1.

5. The extended target tracking method based on adaptive angular variance according to claim 4, characterized in that: The target motion state x k The model is: f(x k )=arctan(G2x k ,G1x k ) in, represents the estimate of the motion state at time step k predicted at time step k-1, The predicted covariance matrix representing the motion state; The estimated matrix representing the inverse of the target extended form; express About variable S k expectations; k represents pseudo measurement; G1 and G2 represent given matrices; q x (x k ) is the probability distribution expression, select q x (x k ) is used as the estimated value of the target motion state.

6. The extended target tracking method based on adaptive angular variance according to claim 5, characterized in that: The target heading angle θ k The model is: in, represents the derivative matrix of the target rotation matrix; tr(·) represents the trace operation; represents the predicted covariance matrix of the heading angle; q θ (θ k ) is the probability distribution expression, select q θ (θ k ) is used as the estimated value of the heading angle.

7. The extended target tracking method based on adaptive angular variance according to claim 5, characterized in that: The size status X k The model is: in, Among them, q X (X k ) is the probability distribution expression, select q X (X k ) as an estimate of the size state.

8. The extended target tracking method based on adaptive angular variance according to claim 2, characterized in that: The morphological parameters and scale parameters for:

Images