Target real-time trajectory prediction method based on extended state observer and related device
By extending the uncertainty of the target trajectory model into additional states through an extended state observer, and designing an extended state observer for real-time estimation, the error problem in target trajectory prediction is solved, and higher accuracy trajectory prediction is achieved.
Patent Information
- Application Number
- CN202510988806.4
- Authority / Receiving Office
- CN · China
- Patent Type
- Applications(China)
- Current Assignee / Owner
- Filing Date
- 2025-07-17
- Publication Date
- 2025-10-28
AI Technical Summary
In existing technologies for real-time target trajectory prediction, the difference between the assumed target trajectory model and the target motion model leads to errors between the predicted trajectory and the actual trajectory, making accurate prediction difficult.
An extended state observer-based approach is adopted to extend the uncertainty of the target trajectory model into additional states. The extended state observer is used for real-time estimation and compensation. The extended state observer is designed and discretized to achieve real-time trajectory prediction of the target.
It improves the accuracy of target trajectory prediction, overcomes the effects of model uncertainty and measurement noise, simplifies the implementation process, and facilitates engineering applications.
Smart Images
Figure CN120850583A_ABST
Abstract
Description
Technical Field
[0001] This invention relates to the field of real-time trajectory prediction technology, specifically to a method and related apparatus for real-time trajectory prediction of targets based on an extended state observer. Background Technology
[0002] Real-time target trajectory prediction utilizes the target's historical states to predict its future trajectory. In engineering, physics-based prediction methods are commonly employed. For real-time trajectory prediction of slowly varying or fixed target models, constant velocity models, constant acceleration models, turning rate models, or hybrid models of these can achieve accurate trajectory prediction. However, for dynamic target trajectories, these methods suffer from model uncertainty, thus failing to achieve accurate trajectory prediction. The Kalman filter algorithm can mitigate the impact of model uncertainty and measurement noise on target trajectory prediction by considering the covariance of process noise and measurement noise. However, this method requires knowledge of the covariance of process noise and measurement noise, which must be obtained through theoretical derivation, experimental calibration, or online adaptive testing, thus limiting its application in engineering.
[0003] Traditional prediction methods based on physical models typically assume that the target's trajectory follows a fixed pattern, such as constant velocity, constant acceleration, constant turning rate, or a hybrid model. These assumptions are effective for predicting the trajectory of slow-moving targets or targets with fixed models. However, the motion parameters of targets in dynamic models are not uniform or constant, leading to discrepancies between the assumed model and the actual target motion model, resulting in errors between the predicted and actual trajectories. The Kalman filter is an efficient recursive filtering algorithm with trajectory prediction capabilities. This method requires assuming a motion model for the target, knowing the uncertainty of the motion model (i.e., the covariance of process noise and measurement noise), which needs to be obtained through theoretical derivation, experimental calibration, or online adaptive methods. Furthermore, the noise must satisfy a Gaussian distribution. Clearly, these conditions are difficult to meet in practical engineering. Therefore, various extended Kalman filters have been developed. However, these algorithms are theoretically complex, involving a large amount of specialized knowledge. From theoretical derivation to code implementation, personnel with relevant professional backgrounds are required, thus limiting the widespread application of this method in engineering. Therefore, in summary, the current real-time trajectory prediction of targets is subject to errors between the predicted trajectory and the actual trajectory due to the difference between the assumed target trajectory model and the target motion model, making it difficult to achieve accurate prediction of the target trajectory. Summary of the Invention
[0004] This invention provides a method and related apparatus for real-time target trajectory prediction based on an extended state observer. The purpose is to solve the problem that in current real-time target trajectory prediction, the difference between the assumed target trajectory model and the target motion model may cause errors between the predicted trajectory and the actual trajectory, making it difficult to achieve accurate prediction of the target trajectory.
[0005] To achieve the above objectives, the present invention adopts the following technical solution: This invention provides a method for real-time target trajectory prediction based on an extended state observer, comprising the following steps: S1. Based on the number of states of the trajectory that the target needs to predict, determine the polynomial model of the assumed target trajectory; S2. Using the target's historical trajectory data and the least squares numerical calculation method, the best fitting curve is obtained, and the polynomial model parameters are solved. S3. Convert the polynomial model parameters obtained from the solution into state-space equations; S4. Extend the model uncertainty in the state-space equation to additional states and construct the extended state-space equation; S5. Based on the constructed extended state-space equations, design an extended state observer; S6. Discretize the designed extended state observer to obtain a discretized extended state observer, thereby realizing real-time trajectory prediction of the target.
[0006] In some implementations, in S1, the polynomial model includes the following formula: (1); in It is a real number; It is a time variable; The assumed target trajectory is a polynomial model.
[0007] In some implementations, the following formula is used to solve for the polynomial model parameters in S2: (2); in, A matrix composed of historical time series. , For model parameters, , For historical trajectory data, .
[0008] In some implementations, the state-space equations in S3 include the following formulas: (3); in, The first derivative of the target's trajectory state. The state of the target's trajectory. The system matrix is... matrix; The input matrix is... matrix; The output matrix is... matrix; To control the input.
[0009] In some implementations, in S4, the extended state-space equations include the following formulas: (4) in, The extended system matrix is: matrix, The extended input matrix is: matrix.
[0010] In some implementations, in S5, the extended state observer includes the following formula: (5); in, The first derivative of the target trajectory state estimate. This is the state estimate of the target trajectory. is the observer gain matrix.
[0011] In some implementations, in S6, the discretized extended state observer includes the following formula: (6); in, Here is the state transition matrix. , Sampling time; To control the input matrix, ; The observer gain after discretization .
[0012] This invention also provides a real-time target trajectory prediction system based on an extended state observer. This system implements the aforementioned real-time target trajectory prediction method based on an extended state observer. The system includes a polynomial model module, a parameter solving module, a parameter transformation module, a model extension module, an observer design module, and an observer discretization module; wherein: Polynomial Model Module: Used to determine a polynomial model of the assumed target trajectory based on the number of states of the trajectory that the target needs to predict; The parameter solving module is used to obtain the best-fit curve and solve the polynomial model parameters by using the target's historical trajectory data and the least squares numerical calculation method. Parameter transformation module: used to convert the polynomial model parameters obtained from the solution into state-space equations; Model expansion module: used to expand the model uncertainties in the state-space equations into additional states, and construct expanded state-space equations; Observer design module: Used to design extended state observers based on the constructed extended state space equations; Observer Discretization Module: This module is used to discretize the designed extended state observer to obtain a discretized extended state observer, thereby enabling real-time trajectory prediction of the target.
[0013] The present invention also provides a computer device, including a memory, a processor, and a computer program stored in the memory and executable on the processor, wherein the processor executes the computer program to implement the steps of the method described above.
[0014] The present invention also provides a computer-readable storage medium storing a computer program that, when executed by a processor, implements the steps of the method described above.
[0015] Compared with the prior art, the target real-time trajectory prediction method and related device based on an extended state observer of the present invention have the following beneficial effects: This invention proposes a real-time target trajectory prediction method based on an extended state observer. Addressing the real-time target trajectory prediction problem, it introduces the concept of an extended state observer. Compared to traditional physical model-based prediction methods, which do not consider the impact of physical model uncertainties on target trajectory prediction accuracy, this invention uses an extended state observer to expand the physical model's uncertainties into an additional state for real-time estimation and compensation, thereby improving the accuracy of target trajectory prediction. Compared to the Kalman filter method, which requires prior knowledge of process noise and measurement noise, this invention does not require such prior knowledge. By using the extended state observer method, it overcomes the impact of model uncertainties and measurement noise on target trajectory prediction through expanding process noise into an additional state and setting the observer gain. This method can compensate for the uncertainties of the assumed target trajectory model, improving the accuracy of real-time target trajectory prediction. It does not require assuming the covariance of target trajectory model uncertainty and measurement noise; model uncertainties can be compensated by adjusting the observer gain through trial and error. It can estimate the target trajectory online in real time, and its design is relatively simple and convenient, making it easy to implement in engineering. Furthermore, this invention also provides a target real-time trajectory prediction system based on an extended state observer, which can realize a target real-time trajectory prediction method based on an extended state observer.
[0016] On the other hand, the present invention provides a computer device that, through a processor executing a specific computer program, can efficiently implement the steps of the method of the present invention. When performing data processing tasks, the computer device can accurately perform numerical calculations and logical judgments. At the same time, since the computer program has high stability and reliability, the accuracy and consistency of the data processing results can be ensured.
[0017] Thirdly, the present invention provides a computer-readable storage medium. By programming the steps of the method of the present invention into a computer program and storing it on the computer-readable storage medium, users can easily load these programs onto any compatible computer device and execute them without rewriting or converting the code, which greatly improves the convenience and flexibility of program execution. Attached Figure Description
[0018] The accompanying drawings are provided to further understand the invention and constitute a part of this invention. The illustrative embodiments of the invention and their descriptions are used to explain the invention and do not constitute an improper limitation of the invention.
[0019] Figure 1 This is a schematic diagram of the target trajectory prediction curve in an embodiment of the target real-time trajectory prediction method based on an extended state observer according to the present invention. Figure 2 This is a schematic diagram of the target trajectory prediction error (position) curve in an embodiment of a target real-time trajectory prediction method based on an extended state observer according to the present invention. Figure 3 This is a schematic diagram of the target trajectory prediction error (velocity) curve in an embodiment of a target real-time trajectory prediction method based on an extended state observer according to the present invention. Detailed Implementation
[0020] To make the objectives, technical solutions, and advantages of the embodiments of the present invention clearer, the technical solutions of the embodiments of the present invention will be clearly and completely described below with reference to the accompanying drawings. Obviously, the described embodiments are only some embodiments of the present invention, and not all embodiments. The components of the embodiments of the present invention described and shown in the accompanying drawings can generally be arranged and designed in various different configurations.
[0021] Therefore, the following detailed description of the embodiments of the invention provided in the accompanying drawings is not intended to limit the scope of the claimed invention, but merely to illustrate selected embodiments of the invention. All other embodiments obtained by those skilled in the art based on the embodiments of the invention without inventive effort are within the scope of protection of the invention.
[0022] Similar labels and letters in the following figures indicate similar items; therefore, once an item is defined in one figure, it does not need to be further defined and explained in subsequent figures.
[0023] It should be noted that the terms "first", "second", etc. in the description and claims of the present invention and the above-mentioned drawings are used to distinguish similar objects and are not necessarily used to describe a specific order or sequence. It should be understood that the numbers used in this way can be interchanged where appropriate, so that the embodiments of the present invention described herein can be implemented in an order other than those illustrated or described herein. In addition, the terms "including" and "having" and any variations thereof are intended to cover non-exclusive inclusions. For example, a process, method, system, product or device that includes a series of steps or units is not necessarily limited to those steps or units clearly listed, but may include other steps or units that are not clearly listed or inherent to these processes, methods, products or devices.
[0024] This invention discloses a real-time target trajectory prediction method based on an extended state observer, comprising the following steps: S1. Based on the number of states of the trajectory that the target needs to predict, determine the polynomial model of the assumed target trajectory; S2. Using the target's historical trajectory data and the least squares numerical calculation method, the best fitting curve is obtained, and the polynomial model parameters are solved. S3. Convert the polynomial model parameters obtained from the solution into state-space equations; S4. Extend the model uncertainty in the state-space equation to additional states and construct the extended state-space equation; S5. Based on the constructed extended state-space equations, design an extended state observer; S6. Discretize the designed extended state observer to obtain a discretized extended state observer, thereby realizing real-time trajectory prediction of the target.
[0025] This invention presents a real-time trajectory prediction method for targets based on an extended state observer. It establishes a polynomial model to adapt to different motion states, dynamically optimizes model parameters by fitting historical data using the least squares method, converts the model parameters into state-space equations, expands model uncertainty into additional states, and designs an extended state observer to achieve real-time prediction through discretization. By estimating and compensating for model uncertainty in real time through extended states, this method, compared to Kalman filtering which requires pre-setting the noise covariance but does not require knowing the noise statistical characteristics, simplifies the complexity through discretization design, thus meeting real-time requirements.
[0026] The purpose of this invention is to introduce the concept of an extended state observer into the field of real-time target trajectory prediction. The specific method for real-time target trajectory prediction based on the extended state observer is as follows: Step 1: Determine the number of states of the target trajectory as needed. The polynomial model for the assumed target trajectory is determined as follows: (1) in It is a real number; It is a time variable; The assumed target trajectory is a polynomial model.
[0027] Step 2: Utilize the target's historical trajectory data The best-fit curve is found using the least squares numerical calculation method, which involves solving for the linear model parameters of the target trajectory in step 1. The calculation method is as follows: (2) in, A matrix composed of historical time series. , For model parameters, , For historical trajectory data, .
[0028] Step 3: Convert the target trajectory polynomial model obtained in Step 2 into state-space form, let... Then the polynomial model of the target trajectory can be transformed into: (3) in, The state of the target's trajectory. ; This is called the system matrix. The matrix is matrix; This is called the input matrix. The matrix is matrix; This is called the output matrix. The matrix is matrix; This is called the control input.
[0029] Step 4: To overcome the uncertainty of the assumed target trajectory polynomial model, the uncertainty is extended to additional states of the target trajectory. And assume The states in the state-space equations of step 3 Expand to Then formula (3) can be rewritten as: (4); The system matrix The matrix is Matrix; Input matrix The matrix is matrix.
[0030] Step 5: For the extended state-space equations in Step 4, design the extended state observer as follows: (5) in, This is the state estimate of the target trajectory. The observer gain matrix is determined through proper design. Make It's by Hurwitz.
[0031] Step 6: Discretize the extended state observer from Step 5. The discretized result is: (6); in, Here is the state transition matrix. , Sampling time; To control the input matrix, ; The observer gain after discretization ,definition eigenvalues Calculate separately By making eigenvalues Find Alternatively, parameters can be determined through trial and error, based on the accuracy, speed, and noise levels of the target trajectory prediction. .
[0032] The above six steps can be used to predict the real-time trajectory of the target.
[0033] In some embodiments, the real-time trajectory prediction method of the present invention employs a zero-order preserved discretization method for discretizing the observer. In practical applications, the discretization method of the present invention can be replaced with the Euler discretization method or the bilinear transformation method to obtain the same discretized model of the observer.
[0034] The target real-time trajectory prediction method of the present invention expands the model uncertainty into an additional state. In actual working conditions, the model uncertainty of the present invention can be expanded into multiple additional states connected by integrals, so as to realize higher-order compensation of the model uncertainty of the target trajectory, thereby improving the prediction performance of the target trajectory.
[0035] This invention relates to a real-time target trajectory prediction method based on an extended state observer. It employs a linear observer gain. In some operating conditions, the linear observer gain of this invention can be replaced with a nonlinear observer gain or an adaptive gain, which can achieve rapid compensation for the uncertainty of the target trajectory model, thereby improving the prediction performance of the target trajectory.
[0036] This invention relates to a target real-time trajectory prediction method based on an extended state observer. It employs an integer-order linear observer. In some operating conditions, the integer-order observer of this invention can be replaced with a fractional-order observer, which can achieve finite-time prediction of the target trajectory.
[0037] This invention relates to a real-time target trajectory prediction method based on an extended state observer. It uses a traditional linear observer, but in some operating conditions, the traditional linear observer can be replaced with a sliding mode observer to improve the robustness of target trajectory prediction.
[0038] The target real-time trajectory prediction method based on extended state observer proposed in this invention predicts the trajectory of a single target. In some working conditions, the single target trajectory prediction of this invention can be changed to multi-target trajectory prediction, thereby realizing the real-time prediction of multi-target trajectories.
[0039] This invention also provides a real-time target trajectory prediction system based on an extended state observer. The system implements the aforementioned real-time target trajectory prediction method based on an extended state observer. The system includes a polynomial model module, a parameter solving module, a parameter transformation module, a model extension module, an observer design module, and an observer discretization module; wherein: Polynomial Model Module: Used to determine a polynomial model of the assumed target trajectory based on the number of states of the trajectory that the target needs to predict; The parameter solving module is used to obtain the best-fit curve and solve the polynomial model parameters by using the target's historical trajectory data and the least squares numerical calculation method. Parameter transformation module: used to convert the polynomial model parameters obtained from the solution into state-space equations; Model expansion module: used to expand the model uncertainties in the state-space equations into additional states, and construct expanded state-space equations; Observer design module: Used to design extended state observers based on the constructed extended state space equations; Observer Discretization Module: This module is used to discretize the designed extended state observer to obtain a discretized extended state observer, thereby enabling real-time trajectory prediction of the target.
[0040] The present invention also provides a computer device, including a memory, a processor, and a computer program stored in the memory and executable on the processor, wherein the processor executes the computer program to implement the steps of the method described above.
[0041] The present invention also provides a computer-readable storage medium storing a computer program that, when executed by a processor, implements the steps of the method described above.
[0042] The following detailed description of the target real-time trajectory prediction method and related apparatus based on the extended state observer of the present invention will be provided through specific embodiments.
[0043] This embodiment uses one-dimensional time series prediction of the target trajectory's position and velocity to illustrate the implementation process of the present invention, including the following steps: 1) Predict the position and velocity of the target trajectory as needed, assuming the target trajectory model is a second-order polynomial in time as shown below: (1) in It is a real number; It is a time variable; The assumed target trajectory is a polynomial model.
[0044] 2) Utilize the three most recent data points of the target's historical trajectory And the least squares numerical calculation method to calculate the linear model parameters of the target trajectory The calculation method is as follows: (2); in, , , .
[0045] 3) Convert the second-order polynomial model of the target trajectory obtained in 2) into a state-space form, let Then the polynomial model of the target trajectory can be transformed into: (3); in, The state of the target's trajectory. ; This is called the system matrix. ; This is called the input matrix. ; This is called the output matrix. ; This is called the control input.
[0046] 4) To overcome the uncertainty of the assumed polynomial model of the target trajectory, the aforementioned uncertainty is extended to additional states of the target trajectory. And assume The states in the state-space equations of 3) Expand to Then formula (3) can be rewritten as: (4); Among them, the system matrix Input matrix .
[0047] 5) For the extended state-space equations in 4), the extended state observer is designed as follows: (5); in, This is the state estimate of the target trajectory. To increase the observer gain, through proper design Make It's by Hurwitz.
[0048] 6) Discretize the extended state observer from step 5). The discretized result is: (6); in, Here is the state transition matrix. , Sampling time; To control the input matrix, ; The observer gain after discretization The parameters are determined through trial and error, based on the accuracy, speed, and noise levels of the target trajectory prediction. .
[0049] like Figures 1 to 3 As shown, the present invention uses a sampling rate of 100Hz for the trajectory. To perform a one-step prediction, assuming the target trajectory is a second-order polynomial, the observer gain is set to... .Depend on Figure 1 , Figure 2 and Figure 3 As can be seen, the target real-time trajectory prediction method based on the extended state observer of the present invention can realize the real-time prediction of the target trajectory.
[0050] Those skilled in the art will understand that embodiments of the present invention can be provided as methods, systems, or computer program products. Therefore, the present invention can take the form of a completely hardware embodiment, a completely software embodiment, or an embodiment combining software and hardware aspects. Moreover, the present invention can take the form of a computer program product implemented on one or more computer-usable storage media (including but not limited to disk storage, CD-ROM, optical storage, etc.) containing computer-usable program code.
[0051] This invention is described with reference to flowchart illustrations and / or block diagrams of methods, apparatus (systems), and computer program products according to embodiments of the invention. It should be understood that each block of the flowchart illustrations and / or block diagrams, and combinations of blocks in the flowchart illustrations and / or block diagrams, can be implemented by computer program instructions. These computer program instructions can be provided to a processor of a general-purpose computer, special-purpose computer, embedded processor, or other programmable data processing apparatus to produce a machine, such that the instructions, which execute via the processor of the computer or other programmable data processing apparatus, generate instructions for implementing the flowchart illustrations and / or block diagrams. Figure 1 a process or multiple processes and / or boxes Figure 1 A device that provides the functions specified in one or more boxes.
[0052] These computer program instructions may also be stored in a computer readable memory that can direct a computer or other programmable data processing device to work in a specific manner, so that the instructions stored in the computer readable memory produce an article of manufacture comprising an instruction device, which implements the process Figure 1 a process or multiple processes and / or boxes Figure 1 The function specified in one or more boxes.
[0053] These computer program instructions can also be loaded onto a computer or other programmable data processing device so that a series of operational steps are executed on the computer or other programmable device to produce a computer-implemented process, thereby providing the instructions executed on the computer or other programmable device for implementing the process. Figure 1 a process or multiple processes and / or boxes Figure 1 A step that specifies a function in one or more boxes. Finally, it should be noted that the above description is merely a preferred embodiment of the present invention and is not intended to limit the present invention in any way. Anyone skilled in the art can readily implement the present invention according to the description and above. Any modifications, alterations, or equivalent variations made using the technical content disclosed above are equivalent embodiments of the present invention. Furthermore, any modifications, alterations, or variations made to the above embodiments based on the essential technology of the present invention are still within the protection scope of the present invention.
Claims
1. A method for real-time target trajectory prediction based on an extended state observer, characterized in that, Includes the following steps: S1. Based on the number of states of the trajectory that the target needs to predict, determine the polynomial model of the assumed target trajectory; S2. Using the target's historical trajectory data and the least squares numerical calculation method, the best fitting curve is obtained, and the polynomial model parameters are solved. S3. Convert the polynomial model parameters obtained from the solution into state-space equations; S4. Extend the model uncertainty in the state-space equation to additional states and construct the extended state-space equation; S5. Based on the constructed extended state-space equations, design an extended state observer; S6. Discretize the designed extended state observer to obtain a discretized extended state observer, thereby realizing real-time trajectory prediction of the target.
2. The target real-time trajectory prediction method based on an extended state observer according to claim 1, characterized in that, In S1, the polynomial model includes the following formula: （1）； in It is a real number; It is a time variable; The assumed target trajectory is a polynomial model.
3. The target real-time trajectory prediction method based on an extended state observer according to claim 1, characterized in that, In S2, the polynomial model parameters are solved using the following formula: （2）； in, A matrix composed of historical time series. , For model parameters, , For historical trajectory data, .
4. The target real-time trajectory prediction method based on an extended state observer according to claim 1, characterized in that, In S3, the state-space equations include the following formulas: （3）； in, The first derivative of the target's trajectory state. The state of the target's trajectory. The system matrix is... matrix; The input matrix is... matrix; The output matrix is... matrix; To control the input.
5. The target real-time trajectory prediction method based on an extended state observer according to claim 1, characterized in that, In S4, the extended state-space equations include the following formulas: （4） in, The extended system matrix is: matrix, The extended input matrix is: matrix.
6. The target real-time trajectory prediction method based on an extended state observer according to claim 1, characterized in that, In S5, the extended state observer includes the following formula: （5）； in, The first derivative of the target trajectory state estimate. This is the state estimate of the target trajectory. is the observer gain matrix.
7. The target real-time trajectory prediction method based on an extended state observer according to claim 1, characterized in that, In S6, the discretized extended state observer includes the following formula: （6） in, Here is the state transition matrix. , Sampling time; To control the input matrix, ; The observer gain after discretization .
8. A real-time trajectory prediction system for a target based on an extended state observer, characterized in that, The system is used to implement the target real-time trajectory prediction method based on an extended state observer according to any one of claims 1-7. The system includes a polynomial model module, a parameter solving module, a parameter transformation module, a model extension module, an observer design module, and an observer discretization module; wherein: Polynomial Model Module: Used to determine a polynomial model of the assumed target trajectory based on the number of states of the trajectory that the target needs to predict; The parameter solving module is used to obtain the best-fit curve and solve the polynomial model parameters by using the target's historical trajectory data and the least squares numerical calculation method. Parameter transformation module: used to convert the polynomial model parameters obtained from the solution into state-space equations; Model expansion module: used to expand the model uncertainties in the state-space equations into additional states, and construct expanded state-space equations; Observer design module: Used to design extended state observers based on the constructed extended state space equations; Observer Discretization Module: This module is used to discretize the designed extended state observer to obtain a discretized extended state observer, thereby enabling real-time trajectory prediction of the target.
9. A computer device comprising a memory, a processor, and a computer program stored in the memory and executable on the processor, characterized in that, When the processor executes the computer program, it implements the steps of the method as described in any one of claims 1-7.
10. A computer-readable storage medium storing a computer program, characterized in that, When the computer program is executed by a processor, it implements the steps of the method as described in any one of claims 1-7.