A dynamic target state estimation and trajectory prediction method

Abstract

This invention relates to the field of moving target perception technology, and more particularly to a dynamic target state estimation and trajectory prediction method, comprising: constructing a nonlinear motion equation for a flying target to obtain a dynamic model; using an extended Kalman filter to perform online joint estimation of the flying target's state and model parameters to obtain a filtered state; introducing Mahalanobis distance to perform statistical consistency testing on observation information and adaptively adjusting the observation noise covariance; constructing a dynamic residual compensation mechanism and adaptively adjusting the compensation intensity based on the filtered covariance information; designing a time-domain attenuation strategy based on residual autocorrelation to extend the dynamic residual compensation mechanism to a multi-step prediction process, generating a compensated predicted trajectory sequence; integrating the filtered state and the compensated predicted trajectory sequence to output a complete target trajectory including position, velocity, and covariance envelope. This invention significantly improves the accuracy of state estimation and the stability of trajectory prediction compared to traditional methods.

Description

Technical Field

[0001] This invention relates to the field of moving target perception technology, and in particular to a dynamic target state estimation and trajectory prediction method based on Mahalanobis distance extended Kalman filter (MD-EKF) and adaptive dynamic residual compensation. Background Technology

[0002] In applications such as robot interaction, autonomous driving, and hazardous operations, motion target perception technology based on sensor fusion is the foundation for the system to achieve autonomous decision-making. The core components of this technology include target detection, target tracking, and real-time trajectory prediction. Among them, trajectory prediction provides a preliminary decision-making basis for the physical interaction between the robot and the target by estimating the target's motion state in advance.

[0003] In existing technologies, the Extended Kalman Filter (EKF) is widely used for state estimation of nonlinear systems due to its computational efficiency advantage. However, the traditional EKF has the following technical drawbacks:

[0004] First, the observation noise covariance is usually preset to a fixed value, lacking an active suppression mechanism for anomalous observations. When the sensor experiences significant observational bias due to occlusion, ghosting, or background interference, the filter over-confides in the anomalous observations, leading to a shift in state estimation.

[0005] Second, the dynamic model suffers from systematic biases. Although the nonlinear model considering air resistance and Magnus force can describe the target's motion trend, wind disturbances, parameter uncertainties, and unmodeled dynamic effects still exist in the actual environment, resulting in a persistent deviation between the model prediction and the actual motion, and lacking online compensation methods.

[0006] Third, the error in multi-step trajectory prediction accumulates with the step size. Existing methods typically use the state estimation results directly for open-loop multi-step recursion without performing temporal correlation analysis and compensation for model errors. This leads to a decline in long-term prediction accuracy and makes it difficult to meet the prediction stability requirements of highly dynamic interactive tasks. Summary of the Invention

[0007] To address the technical problems of insufficient robustness in dynamic target state estimation, lack of online compensation for dynamic model bias, and accumulation of multi-step prediction errors in existing technologies, this invention proposes a dynamic target state estimation and trajectory prediction method based on Mahalanobis distance extended Kalman filter (MD-EKF) and adaptive dynamic residual compensation. The specific technical solution is as follows:

[0008] A dynamic target state estimation and trajectory prediction method includes the following steps:

[0009] Step 1: Construct a dynamic model of the flying target;

[0010] Step 2: Perform extended Kalman filter state estimation on the flight target's state and model parameters;

[0011] Step 3: Introduce the filtered state estimated by Mahalanobis distance test, and adaptively adjust the observation noise covariance according to the test results to suppress abnormal observations and dynamically adjust the filter gain;

[0012] Step 4: Construct a dynamic residual compensation mechanism, and adaptively adjust the compensation intensity by combining the filter covariance information to estimate and correct the model prediction bias online;

[0013] Step 5: Design a time-domain decay strategy based on residual autocorrelation, extend the dynamic residual compensation mechanism to the multi-step prediction process, and generate a compensated multi-step prediction trajectory sequence;

[0014] Step 6: Integrate the filtered state estimate with the compensated multi-step predicted trajectory sequence to output the target trajectory.

[0015] Further, step 1 specifically involves: simplifying the flying target into a six-degree-of-freedom rigid body, performing dynamic analysis on gravity, air resistance, and Magnus force, and establishing the following nonlinear equations of motion:

[0016]

[0017] in, For gravity, For air resistance, For Magnus force, For target mass; gravity air resistance And Magnus The details are as follows:

[0018]

[0019]

[0020]

[0021] in, It is the acceleration due to gravity. Represents the target spin angular velocity. For target speed; The drag coefficient, The Magnus force coefficient, For cross-sectional area, For radius, This refers to air density.

[0022] Further, step 2 specifically involves: using an extended Kalman filter to extend the state of the flying target into an augmented state vector containing position, velocity, and model parameters; describing the system dynamic model using a nonlinear state transition function; and performing a prediction step and an update step at each time step, as shown in the following expression:

[0023]

[0024]

[0025] in, Prior state estimation refers to estimation based on... Time information Prediction of the state at any given moment; for Posterior state estimation at time step; To estimate the covariance a priori; To estimate the covariance in the posterior timescale; The state transition Jacobian matrix; Let be the process noise covariance.

[0026] Furthermore, in step 3, the statistical distance between the current observation and the predicted observation is measured by calculating the Mahalanobis distance. A preset threshold is introduced, and when the Mahalanobis distance exceeds the preset threshold, a scaling factor is constructed. and adaptive information covariance The expression is as follows:

[0027]

[0028]

[0029] in The Mahalanobis distance, For the preset threshold, To observe the noise covariance, The observation matrix;

[0030] Based on the adaptive information covariance Calculate Kalman gain :

[0031] ,

[0032] And perform state update and covariance update:

[0033]

[0034]

[0035] in, Kalman gain; for Posterior state estimation at time t; It is an identity matrix.

[0036] Furthermore, step 4 specifically includes:

[0037] The acceleration residual is defined as follows:

[0038]

[0039] in To observe the acceleration, numerical differentiation was used to obtain it; The acceleration predicted for the model is calculated from the nonlinear equations of motion representing the dynamic model;

[0040] Update the velocity of the flying target based on the feedback from the acceleration residual. The calculation equation is as follows:

[0041]

[0042] in Indicates the time step of sampling; The adaptive acceleration feedback gain is dynamically adjusted based on the covariance information estimated by the extended Kalman filter, and its expression is:

[0043]

[0044] in, Based on the feedback gain, nominal compensation strength; The attenuation coefficient; The velocity subcovariance is the value of the extended Kalman filter estimate; tr(⋅) is the matrix trace, representing the uncertainty of the velocity estimate.

[0045] Furthermore, in step 5, a residual history window is established for the most recent L acceleration residuals:

[0046]

[0047] Introducing regularization terms Calculate the first-order autocorrelation coefficient of the residuals to reflect the persistence of the disturbance:

[0048] And based on the autocorrelation coefficient With residual variance Adaptive adjustment of attenuation factor:

[0049] in:

[0050]

[0051]

[0052] and These are the preset maximum and minimum attenuation factors, respectively. The standard variance;

[0053] When the residuals exhibit strong correlation and stable amplitude, the attenuation factor is automatically increased to enhance long-term compensation; otherwise, it is rapidly attenuated to avoid erroneous extrapolation.

[0054] Substitute the attenuation factor into the multi-step recursive equation:

[0055]

[0056] Generate a compensated multi-step predicted trajectory sequence, where B is the control input matrix.

[0057] Furthermore, in step 6, the multi-step predicted trajectory sequence and the filtered state estimate are time-aligned and coordinate-unified, and the covariance envelope at the corresponding time is added as an uncertainty measure to form a standardized target trajectory output.

[0058] Beneficial effects: This invention suppresses abnormal observations by using Mahalanobis distance and suppresses the accumulation of multi-step prediction errors by using adaptive residual compensation. Compared with traditional methods, it has significant improvements in state estimation accuracy and trajectory prediction stability, and is very suitable for target tracking and trajectory prediction in highly dynamic interactive tasks. Attached Figure Description

[0059] Figure 1 This is a flowchart of a dynamic target state estimation and trajectory prediction method based on MD-EKF and adaptive dynamic residual compensation according to an embodiment of the present invention;

[0060] Figure 2 This is a schematic diagram of the force analysis of a flying target according to an embodiment of the present invention;

[0061] Figure 3 This is a schematic diagram of the EKF state estimation results according to an embodiment of the present invention;

[0062] Figure 4 This is a comparison chart of the robustness of Mahalanobis distance in embodiments of the present invention;

[0063] Figure 5 This is a schematic diagram of the residual compensation prediction trajectory according to an embodiment of the present invention;

[0064] Figure 6 This is the output diagram of the predicted trajectory in an embodiment of the present invention. Detailed Implementation

[0065] To make the objectives, technical solutions, and technical effects of this invention clearer, the invention will be further described in detail below with reference to the accompanying drawings and embodiments.

[0066] like Figure 1 As shown, this embodiment of the invention provides a dynamic target state estimation and trajectory prediction method based on MD-EKF and adaptive dynamic residual compensation, specifically including the following steps:

[0067] Step 1: Construct a dynamic model of the flight target: Based on the dynamic characteristics of the flight target, establish nonlinear motion equations to obtain a dynamic model.

[0068] In this embodiment, spinning spheres such as tennis balls and ping-pong balls are used as typical flight targets. The target is simplified as a six-degree-of-freedom rigid body, and its dynamics are analyzed as follows: Figure 2 As shown. When a target is freely flying in the air, it is mainly affected by gravity. air resistance And Magnus .in, Vertically downwards, In the opposite direction of flight speed, Perpendicular to the plane formed by the direction of the ball's rotation and velocity. The nonlinear equations of motion are established through dynamic analysis:

[0069]

[0070] in, For target mass. Gravity. air resistance And Magnus The details are as follows:

[0071]

[0072]

[0073]

[0074] in, It is the acceleration due to gravity. Represents the target spin angular velocity. The target speed. The drag coefficient, The Magnus force coefficient, For cross-sectional area, For radius, This refers to air density.

[0075] In another embodiment, if the target is a non-rotating or low-speed flying object, the Magnus force can be ignored. We only consider gravity and air resistance to simplify the calculations.

[0076] Step 2, Joint estimation of EKF state and covariance parameters: The state and model parameters of the flight target are jointly estimated online using the extended Kalman filter (EKF) to obtain the filtered state estimate.

[0077] Specifically, the extended Kalman filter is used to expand the state into an augmented state vector that includes position, velocity, and model parameters. Through nonlinear state transition function Describe the system's dynamic model, which performs prediction and update steps at each time step, as shown in the following expression:

[0078]

[0079]

[0080] in, Prior state estimation refers to estimation based on... Time information Prediction of the state at any given moment; for Posterior state estimation at time step; To estimate the covariance a priori; To estimate the covariance in the posterior timescale; The state transition Jacobian matrix is: ; Let be the process noise covariance.

[0081] In this embodiment, the sampling frequency is set to 100Hz~1000Hz, preferably 200Hz. The state estimation results are as follows: Figure 3 As shown.

[0082] Step 3, Mahalanobis distance robust processing: Mahalanobis distance is introduced to perform statistical consistency tests on the observation information. Based on the test results, the observation noise covariance matrix is ​​adaptively adjusted to achieve suppression of abnormal observations and dynamic adjustment of the filtering gain.

[0083] Specifically, through calculation The difference between the observation vector and the prediction vector at any given time is the observation information. :

[0084] ,

[0085] in, For the observation vector, This is the observation matrix.

[0086] Calculate the Mahalanobis distance to measure Statistical distance between observed and predicted values ​​at time points Introduce a preset threshold. The observation noise covariance is dynamically adjusted based on the Mahalanobis distance. When, let the scaling factor and adaptive information covariance for:

[0087]

[0088]

[0089] in, The value ranges from 2.5 to 4.0; in this embodiment, it is taken as... . To observe the noise covariance.

[0090] Based on the adaptive information covariance Calculate Kalman gain :

[0091] ,

[0092] And perform state update and covariance update:

[0093]

[0094]

[0095] in, Kalman gain; for Posterior state estimation at time t; It is an identity matrix.

[0096] This step reduces the Kalman gain, enhances prediction confidence, and is used to update the state. Robustness comparison results are as follows: Figure 4 As shown, in scenarios with anomalous observations, the state estimation of traditional EKF exhibits significant shifts, while the method described in this embodiment can effectively suppress the impact of anomalous observations.

[0097] Step 4: Adaptive dynamic residual compensation: Construct a dynamic residual compensation mechanism. This mechanism combines the filter covariance information to adaptively adjust the compensation intensity, thereby compensating for the model prediction bias online and correcting the flight state prediction value.

[0098] Specifically, the acceleration residual is defined, and its calculation expression is as follows:

[0099]

[0100] Among them, the observed acceleration Acceleration predicted by the model is obtained from numerical differentiation. It is calculated from the aforementioned nonlinear equation of motion.

[0101] Based on the acceleration residual feedback, the flight state estimation results are corrected and the velocity is updated. The calculation equation is as follows:

[0102]

[0103] in Indicates the time step; The adaptive acceleration feedback gain is dynamically adjusted based on the covariance information of the EKF, and its expression is:

[0104]

[0105] in, The basic feedback gain and nominal compensation strength range from 0.3 to 0.8, and in this embodiment, it is taken as 0.5. The attenuation coefficient ranges from 0.01 to 0.1; in this embodiment, it is set to 0.05. The velocity sub-covariance is defined by the extended Kalman filter (EKF) estimate; tr(⋅) is the matrix trace, representing the uncertainty in the velocity estimation. When the EKF lacks confidence in the velocity estimation, the residual feedback weight is reduced to avoid erroneous compensation.

[0106] Step 5, Multi-step Recursive Prediction: Based on the current state estimate, a time-domain decay strategy based on residual autocorrelation is designed. The dynamic residual compensation mechanism is extended to the multi-step prediction process. The decay factor is adaptively adjusted to balance short-term prediction accuracy and long-term prediction stability, generating a compensated multi-step prediction trajectory sequence, such as... Figure 5 As shown.

[0107] After obtaining the acceleration residual at the current moment, the above strategy not only uses it for immediate correction but also extrapolates its impact to subsequent prediction steps through an exponential decay model, generating a trajectory sequence for future moments. Compared to uncompensated open-loop prediction, the method in this embodiment can effectively suppress error accumulation during multi-step prediction.

[0108] Specifically, this embodiment maintains the most recent One acceleration residual, The value range is 5 to 15. A residual history window is established:

[0109]

[0110] Introducing regularization terms Calculate the first-order autocorrelation coefficient of the residuals to reflect the persistence of the disturbance:

[0111]

[0112] And based on the autocorrelation coefficient With residual variance Adaptive adjustment of attenuation factor:

[0113]

[0114] in:

[0115]

[0116]

[0117] and These are the preset maximum and minimum attenuation factors, respectively. The value ranges from 0.7 to 0.95. The value ranges from 0.1 to 0.3. The standard variance is denoted as .

[0118] When the residuals exhibit strong correlation and stable amplitude, the attenuation factor is automatically increased to enhance long-term compensation; otherwise, it decays rapidly to avoid erroneous extrapolation.

[0119] Substitute the attenuation factor into the multi-step recursive equation:

[0120]

[0121] Generate a compensated multi-step predicted trajectory sequence, where B is the control input matrix.

[0122] Step 6: Predicted trajectory output: Integrate the filtered state estimate with the compensated multi-step predicted trajectory sequence to output a complete target trajectory including position, velocity, and uncertainty envelope.

[0123] By integrating the filtered state and compensated prediction sequence obtained from the preceding steps, the trajectory sequence obtained from the multi-step prediction is time-aligned and coordinate-unified with the filtered state estimate. The covariance envelope at the corresponding time step is added as an uncertainty measure to form a standardized trajectory output.

[0124]

[0125] The final predicted trajectory is as follows Figure 6 As shown. The trajectory output can directly serve downstream control decisions, forming a complete technical closed loop from perception to prediction.

[0126] In summary, this invention achieves the suppression of anomalous observations and dynamic adjustment of filter gain by introducing Mahalanobis distance to perform statistical consistency testing on observational information and adaptively adjusting the observation noise covariance. By constructing an acceleration residual feedback mechanism and combining it with filter covariance information to adaptively adjust the compensation intensity, the invention enables online estimation and correction of dynamic model errors. Furthermore, by designing a time-domain attenuation strategy based on residual autocorrelation, the compensation mechanism is extended to the multi-step prediction process, and the attenuation factor is adaptively adjusted to balance short-term prediction accuracy and long-term prediction stability, thereby suppressing error accumulation in the multi-step prediction process and forming a complete technical closed loop from perception to prediction.

[0127] Experimental results show that the proposed method achieves good results in both dynamic target state estimation and trajectory prediction, and has significantly improved the accuracy of state estimation and the stability of trajectory prediction compared with traditional methods.

[0128] The above description is merely a preferred embodiment of the present invention and is not intended to limit the invention in any way. Although the implementation process of the present invention has been described in detail above, those skilled in the art can still modify the technical solutions described in the foregoing examples or make equivalent substitutions for some of the technical features. All modifications and equivalent substitutions made within the spirit and principles of the present invention should be included within the protection scope of the present invention.

Claims

1. A method for dynamic target state estimation and trajectory prediction, characterized in that, It includes the following steps: Step 1: Construct a dynamic model of the flying target; Step 2: Perform extended Kalman filter state estimation on the flight target's state and model parameters; Step 3: Introduce the filtered state estimated by Mahalanobis distance test, and adaptively adjust the observation noise covariance according to the test results to suppress abnormal observations and dynamically adjust the filter gain; Step 4: Construct a dynamic residual compensation mechanism. This mechanism combines the filtering covariance information to adaptively adjust the compensation intensity, thereby estimating and compensating for the model prediction bias online to correct the state prediction value. Step 5: Design a time-domain decay strategy based on residual autocorrelation, extend the dynamic residual compensation mechanism to the multi-step prediction process, and generate a compensated multi-step prediction trajectory sequence; Step 6: Integrate the filtered state estimate with the compensated multi-step predicted trajectory sequence to output the target trajectory.

2. The dynamic target state estimation and trajectory prediction method as described in claim 1, characterized in that, Step 1 specifically involves: simplifying the flight target into a six-degree-of-freedom rigid body, performing dynamic analysis on gravity, air resistance, and Magnus force, and establishing the following nonlinear equations of motion: ， in, For gravity, For air resistance, For Magnus force, For target mass; gravity air resistance And Magnus The details are as follows: ， ， ， in, It is the acceleration due to gravity. Represents the target spin angular velocity. For target speed; The drag coefficient, The Magnus force coefficient, For cross-sectional area, For radius, This refers to air density.

3. The dynamic target state estimation and trajectory prediction method as described in claim 1, characterized in that, Step 2 specifically involves: using an extended Kalman filter to extend the state of the flight target into an augmented state vector containing position, velocity, and model parameters; describing the system dynamic model using a nonlinear state transition function; and performing prediction and update steps at each time step, as shown in the following expression: ， ， in, Prior state estimation refers to estimation based on... Time information Prediction of the state at any given moment; for Posterior state estimation at time step; To estimate the covariance a priori; To estimate the covariance in the posterior timescale; The state transition Jacobian matrix; Let be the process noise covariance.

4. The dynamic target state estimation and trajectory prediction method as described in claim 3, characterized in that, In step 3, the Mahalanobis distance is calculated to measure the statistical distance between the current observed value and the predicted observed value. A preset threshold is introduced, and when the Mahalanobis distance exceeds the preset threshold, a scaling factor is constructed. and adaptive information covariance The expression is as follows: ， ， in The Mahalanobis distance, For the preset threshold, To observe the noise covariance, The observation matrix; Based on the adaptive information covariance Calculate Kalman gain : ， And perform state update and covariance update: ， ， in, Kalman gain; for Posterior state estimation at time t; It is an identity matrix.

5. The dynamic target state estimation and trajectory prediction method as described in claim 1, characterized in that, Step 4 specifically involves: The acceleration residual is defined as follows: ， in To observe the acceleration, numerical differentiation was used to obtain it; The acceleration predicted for the model is calculated from the nonlinear equations of motion representing the dynamic model; Update the velocity of the flying target based on the feedback from the acceleration residual. The calculation equation is as follows: ， in Indicates the sampling time step; The adaptive acceleration feedback gain is dynamically adjusted based on the covariance information estimated by the extended Kalman filter, and its expression is: ， in, Based on the feedback gain, nominal compensation strength; The attenuation coefficient; The velocity subcovariance is the value of the extended Kalman filter estimate; tr(⋅) is the matrix trace, representing the uncertainty of the velocity estimate.

6. The dynamic target state estimation and trajectory prediction method as described in claim 5, characterized in that, In step 5, a residual history window is established for the most recent L acceleration residuals: ， Introducing regularization terms Calculate the first-order autocorrelation coefficient of the residuals to reflect the persistence of the disturbance: ， And based on the autocorrelation coefficient With residual variance Adaptive adjustment of attenuation factor: ， in: ， ， and These are the preset maximum and minimum attenuation factors, respectively. The standard variance; When the residuals exhibit strong correlation and stable amplitude, the attenuation factor is automatically increased to enhance long-term compensation; otherwise, it is rapidly attenuated to avoid erroneous extrapolation. Substitute the attenuation factor into the multi-step recursive equation: ， Generate a compensated multi-step predicted trajectory sequence, where B is the control input matrix.

7. The dynamic target state estimation and trajectory prediction method as described in claim 1, characterized in that, In step 6, the multi-step predicted trajectory sequence and the filtered state estimate are time-aligned and coordinate-unified, and the covariance envelope at the corresponding time is added as an uncertainty measure to form a standardized target trajectory output.

Images