Target modeling and tracking method based on elliptic trajectory constraint

Abstract

The invention belongs to the technical field of target constraint tracking, and discloses a target modeling and tracking method based on elliptic trajectory constraint. The method comprises the following steps: generating elliptical orbit target motion data, and generating measurement data through a sensor; analyzing constraint conditions met by the motion of the elliptical orbit target; establishing a tracking model; introducing the elliptic trajectory constraint information as pseudo measurement into unscented Kalman filtering, and performing unscented Kalman filtering processing; and performing state estimation, and estimating an error covariance to obtain an observation result. According to the method, modeling is carried out on the motion of the elliptical orbit to describe the motion of the elliptical orbit, constraints required by a target moving along the elliptical orbit are defined, constraint conditions are incorporated into a tracking process, the constraints are integrated into unscented Kalman filtering as additional pseudo-measurements, and simulation experiments prove that the tracking precision can be greatly improved; and on the other hand, a powerful framework is established for improving the effectiveness of an 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 direction in the field of target tracking, and has broad application prospects in military and civilian fields such as radar-based ground target tracking, monitoring and tracking of river / river / port ship targets, etc. The constraints imposed on target motion by roads, routes or waterways contain prior information about the target state. Describing and using constraint information in the tracking system can significantly improve tracking accuracy.

[0003] Object tracking in real-world scenarios is constrained by the object's own motion characteristics and the external physical environment of the motion. For example, vehicles traveling on a highway are constrained by the geometry of the road, and typically follow a specific trajectory within these road boundaries. Similarly, satellites are subject to gravity and their motion is constrained by orbit-specific factors, among other factors. The trajectory of the object will be determined by the constraints. Therefore, the association between constraints and the state of the object can be expressed in a variety of ways. Introducing prior information into filter tracking methods has shown a significant impact.

[0004] Pseudo-measurement methods have been widely used in the field of constrained target tracking algorithms. Augmented measurement equations are used to integrate constraint information, while measurement vectors supplement pseudo-measurements to integrate constraints. Existing academic research has explored the concept of constrained target tracking. However, the main focus of these studies is on linear trajectories, especially straight trajectories. There are fewer studies on nonlinear trajectories, especially how to use elliptical trajectory constraints to improve target tracking accuracy. No effective and comprehensive solution has been proposed so far. 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: 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: Introduce the elliptical trajectory constraint information into the tracking system through the augmented measurement equation to establish the tracking model; 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.

[0007] Furthermore, 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 for observation, the sensor can observe the distance and azimuth of the target, and the sensor measurement data is obtained as follows: , and Represents the distance error and angle error of the sensor. The standard equation for the target to move in an elliptical trajectory is: .

[0008] Furthermore, in step 2, the constraint conditions satisfied by the elliptical trajectory target motion are analyzed to obtain the elliptical trajectory constraint information, including the following steps: S2: Simplify the formula in step S1 to get the formula ; S3: Derivative the formula obtained in step S2 with respect to time to obtain the velocity constraint of the elliptical trajectory. The formula is: , and Represent the speed of the x-axis and y-axis of the ellipse respectively; S4: The parametric equation of the ellipse is obtained by using 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 parametric equation of the ellipse obtained in step S4 with respect to time 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 ellipse trajectory constraint information: .

[0009] Furthermore, in step 3, the elliptical trajectory constraint information is introduced into the tracking system through the augmented measurement equation, and a tracking model is established, including augmenting the elliptical trajectory constraint information into the measurement vector as a pseudo measurement using a pseudo measurement method, introducing the elliptical trajectory constraint information into the tracking system through the augmented measurement equation, and using an extended unscented Kalman filter method in the state estimation process; S9: Based on the formula obtained in step S8, the complete ellipse trajectory constraint information also includes the semi-major axis a, the semi-minor axis b, and the center position of the circle. The state vector after augmentation ,for ; S10: The elliptical trajectory constraint information included in step S8 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 the pseudo measurement of the elliptical trajectory constraint information is introduced into the measurement equation, the original constraint state problem is transformed into a general nonlinear filtering problem for processing.

[0010] 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, including 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 ; K is time, T is the sampling time interval; S16: Initial covariance matrix The element calculation formula is .

[0011] Furthermore, in step 5, state estimation is performed and error covariance is estimated to obtain observation results, including the following steps: S17: After obtaining the target initial state and covariance, the target initial state and the measurement are augmented 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 the weight is ,in , and These are all empirical parameters of the sigma point in the unscented Kalman filter.

[0012] Beneficial effects: The present invention provides a target modeling and tracking method based on elliptical trajectory constraints. On the one hand, the elliptical trajectory motion is modeled to describe the elliptical trajectory motion and define 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

[0014] Figure 1 is a flow chart of a target modeling and tracking method based on elliptical trajectory constraints involved in an embodiment of the present invention; Figure 2 It is an elliptical trajectory target motion trajectory and tracking trajectory diagram involved in an embodiment of the present invention; Figure 3 is a target tracking position root mean square error curve diagram involved in an embodiment of the present invention; Figure 4 It is a target tracking speed root mean square error curve diagram involved in an embodiment of the present invention. DETAILED DESCRIPTION

[0015] The preferred mechanism and implementation method of the present invention are further described below in conjunction with the accompanying drawings and specific implementation methods. Example 1

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

[0017] A target modeling and tracking method based on elliptical trajectory constraints includes the following steps: Step 1: Generate target elliptical trajectory motion data and generate measurement data through radar sensor; S1: For a target moving along an elliptical trajectory in a two-dimensional plane, its standard elliptical trajectory can be described by the ellipse equation. Assume that 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 ellipse in the two-dimensional plane are , then the standard equation of the ellipse is as follows: (Formula 1); Step 2: Analyze the constraints satisfied by the elliptical trajectory target motion and obtain the elliptical trajectory constraint information, including the following steps; S2: Simplifying (Formula 1) yields: (Formula 2); S3: By taking the time derivative of (Equation 2), we can obtain the velocity constraint of the elliptical trajectory: (Formula 3); in and Represents the speed of the ellipse's x-axis and y-axis.

[0018] S4: The parametric equation of the ellipse can be obtained through the standard equation of the ellipse as follows: (Formula 4); in It can be understood as the center of the ellipse Start, pointing to a point on the ellipse The angle between the ray and the major axis of the ellipse.

[0019] S5: Angular velocity is the angle The rate of change over time t, that is, , and The relationship is as follows: (Formula 5); in Indicates the angle between the target position at the initial moment and the main axis of the ellipse.

[0020] 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: (Formula 6); S7: Arrange (Equation 6) to obtain (Formula 7); S8: Combining (Equation 2), (Equation 3), and (Equation 7), we can get the complete elliptical trajectory constraint information, which is (Formula 8); Step 3: Introduce the elliptical trajectory constraint information into the tracking system through the augmented measurement equation to establish the tracking model; Traditional two-dimensional target tracking can estimate the state of the target (including the current position, velocity, and angular velocity of the target) by using the target position, azimuth, and sensor update time. The present invention uses a pseudo-measurement method to augment the trajectory constraint information into the measurement vector as a pseudo-measurement. The constraint information is introduced into the tracking system through the augmented measurement equation, and an extended unscented Kalman filter method is used in the state estimation process.

[0021] 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: (Formula 9); S10: The trajectory constraint information contained in (Equation 8) is augmented into the measurement equation by simple term transfer, and the augmented measurement equation is obtained as follows: (Formula 10); 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 condition. The corresponding measurement noise covariance is also augmented, and the augmented measurement covariance is as follows: (Formula 11); Where O is the zero matrix and n is the number of constraints.

[0022] 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.

[0023] Step 4: Introduce the elliptical trajectory constraint as a pseudo measurement into the unscented Kalman filter and perform unscented Kalman filter processing; the present invention uses unscented Kalman filter (UKF) for processing, and the UKF filtering process with elliptical trajectory constraint is as follows: 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.

[0024] S13: Generally speaking, the sensor obtains the distance and azimuth of the target in polar coordinates, and converts the distance of polar coordinates into and azimuth Transformed into the Cartesian coordinate system, the transformation formula is as follows: (Formula 12); S14: is the bias compensation factor, which can be obtained by the covariance of the azimuth measurement The calculation formula is as follows: (Formula 13); S15: Then extract the initial state of the target from the first two measurements (the first two measurements refer to the measurement of K=1 and the measurement of K=2) and the initial covariance , the calculation formulas are as follows: (Formula 14); (Formula 15); Where T is the sampling time interval, the initial covariance matrix The element calculation formula is as follows: (Formula 16); Step 5: Perform state estimation and estimate the error covariance to obtain the observation results; Filtering process (k 3) 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.

[0025] S18: UKF is then used for filtering. The UKF filtering process is as follows: predict: (Formula 17); (Formula 18); (Formula 19); (Formula 20); renew: (Formula 21); (Formula 22); (Formula 23); (Formula 24); (Formula 25); (Formula 26); (Formula 27); (Formula 28); Where n represents the state vector The dimension, j, is calculated as follows: (Formula 29); Representation Matrix The j-th row or j-th column of , and the weight-related calculations are calculated as follows: (Formula 30); in , and These are all empirical parameters of the sigma point in UKF. Example 2

[0026] 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: 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.

[0027] Assume that a target obeys the elliptical trajectory constraint, and the sensor can observe the distance and azimuth of the target. First, use the one-dimensional modeling method to generate the one-dimensional motion state of the target, 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 one-dimensional state vector of the target is ,in is the distance traveled at time k, is the velocity at time k. The motion state equation of the target in one-dimensional space is as follows: (Formula 31); in is zero-mean Gaussian white noise, the state transfer matrix and the noise distribution matrix They are: (Formula 32); (Formula 33); From the parametric equation of the ellipse, the most important thing to map a straight line segment to an ellipse is to know the movement distance. Corresponding angle First, use Ramanujan's formula to approximate the circumference of the ellipse C, the formula is as follows: (Formula 34); according to Find the proportion p of the moving distance in the circumference of the ellipse, assuming that the circumference of the ellipse is the corresponding angle is a linear relationship, then it is related to the movement distance The corresponding ellipse angle The corresponding relationship is as follows: (Formula 35) 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 ellipse 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 moves in an elliptical trajectory constraint, and the starting point of the target in the elliptical trajectory is ,The simulation duration is 200 seconds, and a total of 100 Monte Carlo simulations are performed.

[0028] like Figure 2 , Figure 3 As shown, in order to verify the tracking effect after introducing constraints, the present invention 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: (Formula 36); Where n is the observation time, is the true value, is the predicted value.

[0029] In order to more comprehensively evaluate the estimation performance, the posterior Cramer-Raolower bound (PCRLB) is introduced to give the theoretical limit of estimation accuracy. Different from the classic CRLB, PCRLB takes into account the impact of prior knowledge on estimation performance and provides a lower limit on the variance of any unbiased estimator when the prior distribution is known.

[0030] The covariance matrix P of PCRLB is the state vector The inverse of the Fisher information matrix, that is: (Formula 37); Based on the target state equation, sensor measurement equation and pseudo measurement, the Fisher information matrix can be expressed as: (Formula 38); in (Formula 39); in is the operator symbol for the second-order partial derivative.

[0031] For linear dynamic systems, the recursive expression of the Fisher information matrix is ​​as follows: (Formula 40); 0 of which is the state transfer matrix, is the process noise matrix, To measure the contribution to the Cramer-Rao lower bound, the following calculation is performed: (Formula 41); In the formula, 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, (Formula 42); The form is as follows: in (Formula 43); 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 filtering and constrained unscented Kalman filtering respectively. Figure 3 , Figure 4 The tracking results shown show that the constrained unscented Kalman filter can effectively improve the tracking accuracy.

[0032] The present invention provides a target modeling and tracking method based on elliptical trajectory constraints. On the one hand, the elliptical trajectory motion is modeled to describe the elliptical trajectory motion and define 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.

[0033] 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 substitutions for some of the technical features therein. However, any modifications, equivalent substitutions, improvements, etc. made within the spirit and principles of the present invention should be included in the protection scope 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: Introduce the elliptical trajectory constraint information into the tracking system through the augmented measurement equation to establish the tracking model; 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 is 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 for observation, the sensor can observe the distance and azimuth of the target, and the sensor measurement data is obtained as follows: , and Represents the distance error and angle error of the sensor. The standard equation for the target to move in an elliptical trajectory is: .

3. The target modeling and tracking method based on elliptical trajectory constraints according to claim 2 is characterized in that: In step 2, the constraint conditions 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 respect to time to obtain the velocity constraint of the elliptical trajectory. The formula is: , and Represent the speed of the x-axis and y-axis of the ellipse respectively; S4: The parametric equation of the ellipse is obtained by using 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 parametric equation of the ellipse obtained in step S4 with respect to time 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 ellipse trajectory constraint information: .

4. The target modeling and tracking method based on elliptical trajectory constraints according to claim 1 or 3, characterized in that: In step 3, the elliptical trajectory constraint information is introduced into the tracking system through the augmented measurement equation, and a tracking model is established, including using a pseudo-measurement method to augment the elliptical trajectory constraint information into the measurement vector as a pseudo-measurement, and the elliptical trajectory constraint information is introduced into the tracking system through the augmented measurement equation, and an extended unscented Kalman filter method is used in the state estimation process; S9: Based on the formula obtained in step S8, the complete ellipse trajectory constraint information also includes the semi-major axis a, the semi-minor axis b, and the center position of the circle. The state vector after augmentation ,for ; S10: The elliptical trajectory constraint information included in step S8 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 the pseudo measurement of the elliptical trajectory constraint information is introduced into the measurement equation, the original constraint state problem is transformed into a general nonlinear filtering problem for processing.

5. 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 into the unscented Kalman filter as a pseudo measurement, 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 .

6. The target modeling and tracking method based on elliptical trajectory constraints according to claim 5 is characterized in that: In step 5, the state is estimated and the error covariance is estimated to obtain the observation result, including the following steps: S17: After obtaining the target initial state and covariance, the target initial state and the measurement are augmented 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 The dimension, j is calculated as ; Representation Matrix The j-th row or j-th column of , and the calculation formula related to the weight is ,in , and These are all empirical parameters of the sigma point in the unscented Kalman filter.

Images