A target modeling and tracking method based on elliptical trajectory constraints

Abstract

The present invention belongs to the technical field of target constraint tracking, and discloses a method for modeling and tracking a target based on an elliptical trajectory constraint. The method includes generating elliptical trajectory target motion data and generating measurement data through a sensor; analyzing the constraint conditions satisfied by the elliptical trajectory target motion; establishing a tracking model; introducing the elliptical trajectory constraint information as a pseudo-measurement into an unscented Kalman filter, and performing unscented Kalman filter processing; performing state estimation, and simultaneously estimating the error covariance to obtain an observation result. The present invention models the elliptical trajectory motion to describe the elliptical trajectory motion, and defines the constraints required for the target moving along the elliptical trajectory, and incorporates these constraints into the tracking process. These constraints are integrated into the unscented Kalman filter as additional pseudo-measurements, and simulation experiments have shown that the tracking accuracy can be greatly improved. On the other hand, the present invention establishes a powerful framework for improving the effectiveness of the elliptical target tracking mechanism.

Description

Technical Field

[0001] The present invention mainly relates to the technical field of target constraint tracking, and in particular to a target modeling and tracking method based on elliptical trajectory constraints. Background Art

[0002] Constraint estimation is an important research area in target tracking, with broad application prospects in military and civilian fields, including radar-based ground target tracking and the surveillance and tracking of ships in rivers, streams, and ports. Constraints imposed on target motion by roads, routes, or waterways contain prior information about the target's state. Describing and utilizing these constraints in tracking systems can significantly improve tracking accuracy.

[0003] Object tracking in real-world scenarios is constrained by the object's inherent motion characteristics and the physical environment surrounding it. For example, vehicles on highways are constrained by the geometry of the road and typically follow specific trajectories within these road boundaries. Similarly, satellites are subject to gravity, and their motion is constrained by factors such as orbital constraints. The trajectory of an object is determined by these constraints. Therefore, the relationship between constraints and the object's state can be expressed in a variety of ways. Incorporating prior information into filter tracking methods has shown significant impact.

[0004] Pseudo-measurement methods have been widely used in constrained target tracking algorithms. Augmented measurement equations are used to incorporate constraint information, while measurement vectors supplement pseudo-measurements to incorporate the constraints. Existing academic research has explored the concept of constrained target tracking. However, this research has primarily focused on linear trajectories, particularly straight trajectories. Research on nonlinear trajectories is less extensive, and in particular, how to utilize elliptical trajectory constraints to improve target tracking accuracy has yet to be proposed. Summary of the Invention

[0005] The purpose of the present invention is to provide a target modeling and tracking method based on elliptical trajectory constraints. In view of the current lack of elliptical trajectory constraint tracking, the present invention improves the tracking accuracy by introducing elliptical constraint information as pseudo-measurement into the filtering process.

[0006] To achieve the above object, the present invention provides a target modeling and tracking method based on elliptical trajectory constraints, comprising the following steps:

[0007] Step 1: Generate elliptical trajectory target motion data and generate measurement data through sensors;

[0008] Step 2: Analyze the constraints satisfied by the elliptical trajectory target motion and obtain the elliptical trajectory constraint information;

[0009] Step 3: The elliptical trajectory constraint information is augmented into the measurement vector as a pseudo-measurement using the pseudo-measurement method. The elliptical trajectory constraint information is introduced into the tracking system through the augmented measurement equation, and the extended unscented Kalman filter method is used in the state estimation process.

[0010] S9: The complete elliptical trajectory constraint information also includes the semi-major axis a, semi-minor axis b, and the center position The state vector after augmentation ,for ;

[0011] S10: The elliptical trajectory constraint information is augmented into the measurement equation by simple term transfer, and the augmented measurement equation is obtained as follows: ;

[0012] S11: For targets subject to elliptical trajectory constraints, the noise corresponding to the corresponding constraints is zero, and the corresponding measurement noise covariance is augmented. The augmented measurement covariance is , O is a zero matrix, n is the number of constraints;

[0013] S12: After introducing the pseudo measurement of the elliptical trajectory constraint information into the measurement equation, the original constraint state problem is transformed into a general nonlinear filtering problem for processing;

[0014] Step 4: Introduce the elliptical trajectory constraint information as pseudo-measurement into the unscented Kalman filter and perform unscented Kalman filter processing;

[0015] Step 5: Perform state estimation and estimate the error covariance to obtain the observation results.

[0016] Furthermore, generating the elliptical trajectory target motion data in step 1 and generating measurement data through the radar sensor includes the following steps:

[0017] S1: Suppose a target moves in an elliptical trajectory in a two-dimensional plane, and the coordinates of the center of the ellipse are , the semi-major axis and semi-minor axis of the ellipse are a and b respectively, and the coordinates of any point of the target in the two-dimensional plane are , at time k, the coordinates of the target in the two-dimensional plane are ;

[0018] Using sensors to observe, the sensor can observe the distance and azimuth of the target, and the sensor measurement data is obtained as , and Representing the distance error and angle error of the sensor, the standard equation for the target's elliptical trajectory motion is .

[0019] Furthermore, in step 2, the constraints satisfied by the elliptical trajectory target motion are analyzed to obtain elliptical trajectory constraint information, including the following steps:

[0020] S2: Simplify the formula in step S1 to get the formula ;

[0021] S3: Derivative the formula obtained in step S2 with time to obtain the velocity constraint of the elliptical trajectory. The formula is: , and Represent the speed of the ellipse x-axis and y-axis respectively;

[0022] S4: The parametric equation of the ellipse is obtained by the standard equation of the ellipse in step S1: , From the center of the ellipse Start and point to any point on the ellipse The angle between the ray and the major axis of the ellipse;

[0023] S5: angular velocity is the angle The rate of change over time t, and The relationship is , Indicates the angle between the target position at the initial moment and the main axis of the ellipse;

[0024] S6: Derivative the time of the parametric equation of the ellipse obtained in step S4 to obtain the x-axis and y-axis velocities of the ellipse with respect to the angular velocity The formula is ;

[0025] S7: Simplify the formula obtained in step S6 to obtain the formula ;

[0026] S8: Combine the formulas in step S2, step S3, and step S7 to obtain the complete elliptical trajectory constraint information: .

[0027] Furthermore, in step 4, the elliptical trajectory constraint is introduced as a pseudo measurement into the unscented Kalman filter, and the unscented Kalman filter processing is performed, which includes the following steps:

[0028] S13: Obtain the distance and azimuth of the target in polar coordinates, and convert the distance of polar coordinates into and azimuth Transformed into the Cartesian coordinate system, the transformation formula is ;

[0029] S14: is the bias compensation factor, which can be obtained by the covariance of the azimuth measurement The calculation formula is ;

[0030] S15: Extract the initial state of the target from two measurements with K = 1 and 2 and the initial covariance , initial state The calculation formula is ;

[0031] Initial covariance The calculation formula is ;

[0032] K is time, T is the sampling time interval;

[0033] S16: Initial covariance matrix The element calculation formula is .

[0034] Furthermore, in step 5, state estimation is performed and error covariance is estimated to obtain observation results, which includes the following steps:

[0035] S17: After obtaining the target initial state and covariance, augment the target initial state and measurement respectively, K is greater than or equal to 3, and the initial covariance, process noise covariance, and observation noise covariance are augmented accordingly according to step S11;

[0036] S18: Filtering is performed using an unscented Kalman filter. The filtering process of the unscented Kalman filter is as follows:

[0037] The prediction process is:

[0038] ;

[0039] ;

[0040] ;

[0041] ;

[0042] The update process is:

[0043] ;

[0044] ;

[0045] ;

[0046] ;

[0047] ;

[0048] ;

[0049] ;

[0050] ;

[0051] Where n represents the state vector Dimension, j is calculated as ;

[0052] Representation matrix The j-th row or j-th column of , and the calculation formula related to weight is ,in , and These are all empirical parameters of the sigma point in the unscented Kalman filter.

[0053] The present invention provides a target modeling and tracking method based on elliptical trajectory constraints. On the one hand, by modeling the elliptical trajectory motion to describe the elliptical trajectory motion and defining the constraints required for the target moving along the elliptical trajectory, these constraints are incorporated into the tracking process. These constraints are integrated into the unscented Kalman filter as additional pseudo-measurements. Simulation experiments have shown that the tracking accuracy can be greatly improved. On the other hand, the present invention establishes a powerful framework for improving the effectiveness of the elliptical target tracking mechanism. BRIEF DESCRIPTION OF THE DRAWINGS

[0054] Figure 1 is a flow chart of a target modeling and tracking method based on elliptical trajectory constraints according to an embodiment of the present invention;

[0055] Figure 2 It is an elliptical trajectory target motion trajectory and tracking trajectory diagram involved in an embodiment of the present invention;

[0056] Figure 3 is a graph showing a root mean square error (RMS) curve of target tracking position involved in an embodiment of the present invention;

[0057] Figure 4 3 is a target tracking speed root mean square error curve diagram involved in an embodiment of the present invention. DETAILED DESCRIPTION

[0058] The preferred structure and implementation method of the present invention will be further described below in conjunction with the accompanying drawings and specific embodiments. Example 1

[0059] like Figures 1 to 4As shown, an embodiment of the present invention discloses a target modeling and tracking method based on elliptical trajectory constraints. Figure 1 This is a flow chart of a target modeling and tracking method based on elliptical trajectory constraints involved in an embodiment of the present invention.

[0060] A target modeling and tracking method based on elliptical trajectory constraints includes the following steps:

[0061] Step 1: Generate target elliptical trajectory motion data and generate measurement data through the radar sensor;

[0062] S1: For a target moving along an elliptical trajectory in a two-dimensional plane, its standard elliptical trajectory can be described by an elliptical equation. Assume that the coordinates of the ellipse center are , the semi-major axis and semi-minor axis of the ellipse are a and b respectively, and the coordinates of any point of the ellipse in the two-dimensional plane are , then the standard equation of the ellipse is as follows:

[0063] (Formula 1);

[0064] Step 2: Analyze the constraints satisfied by the elliptical trajectory target motion and obtain the elliptical trajectory constraint information, including the following steps;

[0065] S2: Simplifying (Formula 1) we can get:

[0066] (Formula 2);

[0067] S3: Taking the time derivative of (Equation 2), we can obtain the velocity constraint of the elliptical trajectory:

[0068] (Formula 3);

[0069] in and Indicates the speed of the ellipse along the x-axis and y-axis.

[0070] S4: The parametric equation of the ellipse can be obtained from the standard equation of the ellipse as follows:

[0071] (Formula 4);

[0072] in It can be understood as the center of the ellipse Start and point to a point on the ellipse The angle between the ray and the major axis of the ellipse.

[0073] S5: angular velocity is the angle The rate of change over time t, that is, , and The relationship is as follows:

[0074] (Formula 5);

[0075] in Indicates the angle between the target position at the initial moment and the main axis of the ellipse.

[0076] S6: By differentiating the parametric equation of the ellipse with respect to time t, we can obtain the x-axis and y-axis velocities with respect to the angular velocity. The expression is as follows:

[0077] (Formula 6);

[0078] S7: Arrange (Equation 6) to obtain

[0079] (Formula 7);

[0080] S8: Combining (Equation 2), (Equation 3), and (Equation 7), we can get the complete elliptical trajectory constraint information, which is

[0081] (Equation 8);

[0082] Step 3: Introduce the elliptical trajectory constraint information into the tracking system through the augmented measurement equation to establish a tracking model;

[0083] Traditional two-dimensional target tracking uses the target's position, azimuth, and sensor update time to estimate the target's state (including its current position, velocity, and angular velocity). This paper uses pseudo-measurement methods to augment the trajectory constraint information into the measurement vector as pseudo-measurements. This constraint information is introduced into the tracking system via the augmented measurement equations, and an extended unscented Kalman filter is used in the state estimation process.

[0084] S9: From (Equation 8), we can see that the complete trajectory constraint information also includes the semi-major axis a, the semi-minor axis b, and the center position The state vector after augmentation as follows:

[0085] (Formula 9);

[0086] S10: The trajectory constraint information contained in (Equation 8) is augmented into the measurement equation by simple transposition, and the augmented measurement equation is obtained as follows:

[0087] (Equation 10);

[0088] S11: For targets subject to elliptical trajectory constraints, the constraint relationship always holds. Therefore, the noise corresponding to the corresponding constraint is zero, that is, = 0, i is the pseudo measurement corresponding to the i-th constraint. The corresponding measurement noise covariance is also augmented, and the augmented measurement covariance is as follows:

[0089] (Equation 11);

[0090] Where O is the zero matrix and n is the number of constraints.

[0091] S12: After introducing the pseudo-measurement of the elliptical trajectory constraint information into the measurement equation, the original constraint state problem can be transformed into a general nonlinear filtering problem.

[0092] Step 4: Introduce the elliptical trajectory constraint as a pseudo-measurement into the unscented Kalman filter and perform unscented Kalman filtering processing; the present invention adopts the unscented Kalman filter (UKF) for processing. The UKF filtering process with the elliptical trajectory constraint is as follows:

[0093] Initialize the unscented Kalman filter (k=1, 2), where k can be understood as the time step, for example, k=1 can be understood as the first second.

[0094] S13: Generally speaking, the sensor obtains the distance and azimuth of the target in polar coordinates, and converts the distance of polar coordinates into the azimuth by unbiased measurement conversion method. and azimuth Transformed into the Cartesian coordinate system, the transformation formula is as follows:

[0095] (Equation 12);

[0096] S14: Among them is the bias compensation factor, which can be obtained by the covariance of the azimuth measurement The calculation formula is as follows:

[0097] (Equation 13);

[0098] S15: Then extract the initial state of the target from the first two measurements (the first two measurements refer to the measurement with K=1 and the measurement with K=2) and the initial covariance , the calculation formulas are as follows:

[0099] (Equation 14);

[0100] (Equation 15);

[0101] Where T is the sampling time interval, the initial covariance matrix The element calculation formula is as follows:

[0102] (Equation 16);

[0103] Step 5: Perform state estimation and estimate the error covariance to obtain the observation results;

[0104] Filtering process (k 3)

[0105] S17: After obtaining the target initial state and covariance, the target initial state and the measurement are augmented respectively, and the initial covariance, process noise covariance and observation noise covariance are augmented accordingly according to step S11.

[0106] S18: UKF is then used for filtering. The UKF filtering process is as follows:

[0107] predict:

[0108] (Equation 17);

[0109] (Equation 18);

[0110] (Equation 19);

[0111] (Equation 20);

[0112] renew:

[0113] (Equation 21);

[0114] (Equation 22);

[0115] (Equation 23);

[0116] (Equation 24);

[0117] (Equation 25);

[0118] (Equation 26);

[0119] (Equation 27);

[0120] (Equation 28);

[0121] Where n represents the state vector The dimension, j, is calculated as follows:

[0122] (Equation 29);

[0123] Representation matrix The j-th row or j-th column of , and the weight-related calculations are calculated as follows:

[0124] (Equation 30);

[0125] in , and These are all empirical parameters of the sigma point in UKF. Example 2

[0126] The target modeling and tracking method based on elliptical trajectory constraints in this embodiment is basically the same as that in embodiment 1, except that:

[0127] like Figure 2 As shown, the target described in the present invention has the characteristics of elliptical trajectory motion, and a corresponding tracking model is established based on the elliptical motion constraints.

[0128] Assume that a target follows an elliptical trajectory constraint and the sensor can observe the target's distance and azimuth. First, use the one-dimensional modeling method to generate the target's one-dimensional motion state, and then transfer the one-dimensional motion state to the elliptical trajectory in the Cartesian coordinate system. The one-dimensional motion state includes the distance and speed of the target along the trajectory. Assume that the target's one-dimensional state vector is ,in is the distance traveled at time k, is the velocity at time k. The target's motion state equation in one-dimensional space is as follows:

[0129] (Equation 31);

[0130] in is zero-mean Gaussian white noise, the state transfer matrix and noise distribution matrix They are:

[0131] (Equation 32);

[0132] (Equation 33);

[0133] From the perspective of the parametric equation of the ellipse, the most important thing to know when mapping a straight line segment to the ellipse is the movement distance. Corresponding angle First, use Ramanujan's formula to approximate the circumference of the ellipse C, the formula is as follows:

[0134] (Equation 34);

[0135] according to Find the proportion p of the movement distance in the circumference of the ellipse, assuming that the circumference of the ellipse is the same as the corresponding angle Is a linear relationship, then with the movement distance Corresponding ellipse angle The corresponding relationship is as follows:

[0136] (Equation 35)

[0137] Assume that the initial one-dimensional state of the target is , the standard deviation of the one-dimensional process noise is , the semi-major axis of the elliptical trajectory is , the semi-minor axis is The sensor is located at the origin of the coordinate system, and the standard deviation of the distance observed by the sensor is , the azimuth standard deviation is , the sensor scanning interval is T = 1 second. The scenario simulation assumes that the target satisfies the elliptical trajectory constraint motion, and the starting point of the target in the elliptical trajectory motion is ,The simulation duration is 200 seconds, and a total of 100 Monte Carlo simulations are performed.

[0138] like Figure 2 , Figure 3 As shown in the figure, in order to verify the tracking effect after introducing constraints, this paper compares the effect of traditional unconstrained unscented Kalman filter. In order to evaluate the tracking effect, this paper introduces the root mean square error (RMSE) to measure the error between the true value and the predicted value. The RMSE calculation formula is as follows:

[0139] (Equation 36);

[0140] Where n is the observation time, is the true value, is the predicted value.

[0141] To more comprehensively evaluate estimation performance, the posterior Cramer-Raolower bound (PCRLB) is introduced to provide a theoretical limit on estimation accuracy. Unlike the classic CRLB, PCRLB considers the impact of prior knowledge on estimation performance and provides a lower bound on the variance of any unbiased estimator given a known prior distribution.

[0142] The covariance matrix P of PCRLB is the state vector The inverse of the Fisher information matrix, that is:

[0143] (Equation 37);

[0144] Based on the target state equation, sensor measurement equation and pseudo-measurement, the Fisher information matrix can be expressed as:

[0145] (Equation 38);

[0146] in

[0147] (Equation 39);

[0148] in is the operator symbol for the second-order partial derivative.

[0149] For linear dynamic systems, the recursive expression of the Fisher information matrix is ​​as follows:

[0150] (Equation 40);

[0151] 0 of which is the state transition matrix, is the process noise matrix, To measure the contribution to the Cramer-Rao lower bound, the following calculation is performed:

[0152] (Formula 41);

[0153] Where, is the first-order partial derivative operator symbol, is the state vector, is the pseudo-measurement constraint function, is the measurement noise covariance matrix. In the system of the present invention,

[0154] (Formula 42);

[0155] The form is as follows:

[0156] in

[0157] (Formula 43);

[0158] according to Figure 2 The results shown in the figure show that the elliptical trajectory moving target in the scene is tracked using conventional unscented Kalman filter and constrained unscented Kalman filter respectively. Figure 3 , Figure 4 The tracking results shown show that the constrained unscented Kalman filter can effectively improve the tracking accuracy.

[0159] The present invention provides a target modeling and tracking method based on elliptical trajectory constraints. On the one hand, by modeling the elliptical trajectory motion to describe the elliptical trajectory motion and defining the constraints required for the target moving along the elliptical trajectory, these constraints are incorporated into the tracking process. These constraints are integrated into the unscented Kalman filter as additional pseudo-measurements. Simulation experiments have shown that the tracking accuracy can be greatly improved. On the other hand, the present invention establishes a powerful framework for improving the effectiveness of the elliptical target tracking mechanism.

[0160] Finally, it should be noted that the above are only preferred embodiments of the present invention and are not intended to limit the present invention. Although the present invention has been described in detail with reference to the embodiments, those skilled in the art can still modify the technical solutions described in the aforementioned embodiments or make equivalent replacements for some of the technical features therein. However, 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. A target modeling and tracking method based on elliptical trajectory constraints, characterized in that: The following steps are involved: Step 1: Generate elliptical trajectory target motion data and generate measurement data through sensors; Step 2: Analyze the constraints satisfied by the elliptical trajectory target motion and obtain the elliptical trajectory constraint information; Step 3: The elliptical trajectory constraint information is augmented into the measurement vector as a pseudo-measurement using the pseudo-measurement method. The elliptical trajectory constraint information is introduced into the tracking system through the augmented measurement equation, and the extended unscented Kalman filter method is used in the state estimation process. S9: The complete elliptical trajectory constraint information also includes the semi-major axis a, semi-minor axis b, and the center position The state vector after augmentation ,for ; S10: The elliptical trajectory constraint information is augmented into the measurement equation by simple term transfer, and the augmented measurement equation is obtained as follows: ； S11: For targets subject to elliptical trajectory constraints, the noise corresponding to the corresponding constraints is zero, and the corresponding measurement noise covariance is augmented. The augmented measurement covariance is , O is a zero matrix, n is the number of constraints; S12: After introducing the pseudo measurement of the elliptical trajectory constraint information into the measurement equation, the original constraint state problem is transformed into a general nonlinear filtering problem for processing; Step 4: Introduce the elliptical trajectory constraint information as pseudo-measurement into the unscented Kalman filter and perform unscented Kalman filter processing; Step 5: Perform state estimation and estimate the error covariance to obtain the observation results.

2. The target modeling and tracking method based on elliptical trajectory constraints according to claim 1, characterized in that: In step 1, the elliptical trajectory target motion data is generated, and the measurement data is generated by the radar sensor, including the following steps: S1: Suppose a target moves in an elliptical trajectory in a two-dimensional plane, and the coordinates of the center of the ellipse are , the semi-major axis and semi-minor axis of the ellipse are a and b respectively, and the coordinates of any point of the target in the two-dimensional plane are , at time k, the coordinates of the target in the two-dimensional plane are ; Using sensors to observe, the sensor can observe the distance and azimuth of the target, and the sensor measurement data is obtained as , and Representing the distance error and angle error of the sensor, the standard equation for the target's elliptical trajectory motion is .

3. The target modeling and tracking method based on elliptical trajectory constraints according to claim 2, characterized in that: In step 2, the constraints satisfied by the elliptical trajectory target motion are analyzed to obtain the elliptical trajectory constraint information, which includes the following steps: S2: Simplify the formula in step S1 to get the formula ; S3: Derivative the formula obtained in step S2 with time to obtain the velocity constraint of the elliptical trajectory. The formula is: , and Represent the speed of the ellipse x-axis and y-axis respectively; S4: The parametric equation of the ellipse is obtained by the standard equation of the ellipse in step S1: , From the center of the ellipse Start and point to any point on the ellipse The angle between the ray and the major axis of the ellipse; S5: angular velocity is the angle The rate of change over time t, and The relationship is , Indicates the angle between the target position at the initial moment and the main axis of the ellipse; S6: Derivative the time of the parametric equation of the ellipse obtained in step S4 to obtain the x-axis and y-axis velocities of the ellipse with respect to the angular velocity The formula is ; S7: Simplify the formula obtained in step S6 to obtain the formula ; S8: Combine the formulas in step S2, step S3, and step S7 to obtain the complete elliptical trajectory constraint information: 。 4. The target modeling and tracking method based on elliptical trajectory constraints according to claim 1, characterized in that: In step 4, the elliptical trajectory constraint is introduced as a pseudo-measurement into the unscented Kalman filter, and the unscented Kalman filter processing is performed, which includes the following steps: S13: Obtain the distance and azimuth of the target in polar coordinates, and convert the distance of polar coordinates into and azimuth Transformed into the Cartesian coordinate system, the transformation formula is ; S14: is the bias compensation factor, which can be obtained by the covariance of the azimuth measurement The calculation formula is ; S15: Extract the initial state of the target from two measurements with K = 1 and 2 and the initial covariance , initial state The calculation formula is ; Initial covariance The calculation formula is ; T is the sampling time interval; S16: Initial covariance matrix The element calculation formula is .

5. The target modeling and tracking method based on elliptical trajectory constraints according to claim 4, characterized in that: In step 5, the state is estimated and the error covariance is estimated to obtain the observation results, which includes the following steps: S17: After obtaining the target initial state and covariance, augment the target initial state and measurement respectively, K is greater than or equal to 3, and the initial covariance, process noise covariance, and observation noise covariance are augmented accordingly according to step S11; S18: Filtering is performed using an unscented Kalman filter. The filtering process of the unscented Kalman filter is as follows: The prediction process is: ； ； ； ； The update process is: ； ； ； ； ； ； ； ； Where n represents the state vector Dimension, j is calculated as ; Representation matrix The j-th row or j-th column of , and the calculation formula related to weight is ,in , and These are all empirical parameters of the sigma point in the unscented Kalman filter.

Images