Variable structure interacting multiple model estimation method based on rolling horizon estimation

Abstract

This invention relates to a variable-structure interactive multi-model estimation method based on rolling time-domain estimation, belonging to the field of vehicle control technology. The purpose of this invention is to design a novel classification method using residual information, thereby reducing the dependence on model probability accuracy. The steps of this invention are: determining the system model set; in each iteration cycle, for all models in the model set, given a fixed time domain, if there is only one model with a certain time domain length, it is directly output as the interaction value; the interactive multi-model estimation from model set to model set based on rolling time-domain estimation is defined as the model set composed of all models adjacent to the main model. This invention reduces the dependence on model probability accuracy, better ensures the accuracy of model classification, has higher estimation precision, and solves the problem of non-interaction caused by different start times of the corresponding time domains of each model.

Description

Technical Field

[0001] This invention belongs to the field of vehicle control technology. Background Technology

[0002] Target tracking, which obtains the actual trajectory of a target through tracking filtering, is highly practical. Maneuvering targets refer to targets in motion whose movement patterns constantly change according to the actual situation, and the corresponding maneuvers also change accordingly. Estimators based on a single model, such as Kalman filtering, cannot meet these requirements. Interactive multi-model estimation algorithms have been widely used in target tracking because they can match various maneuvers of the tracked target. However, their model set is fixed, requiring estimation of all models in each iteration, resulting in a high computational burden. Furthermore, including models that do not match the current maneuver in the estimation can actually reduce the final accuracy.

[0003] Therefore, variable structure multi-model estimation algorithms have been widely applied. Among them, the Model Group Switching (MGS) algorithm, which employs model activation and termination, has good performance, but its tracking performance is affected by the partitioning of the model set and the topology; its performance deteriorates when the target's maneuvering speed is too high. The Increased Expectation Model (EMA) algorithm has significantly better tracking performance than the IMM algorithm, but it is only suitable for model sets with changing parameters and cannot be directly applied to model sets with changing structures. The Adaptive Mesh Interactive Multi-Model Algorithm (AGIMM) method achieves model set adaptation by adaptively adjusting the mesh, essentially a model self-learning algorithm. However, for highly maneuverable targets, the central mesh position of the adaptive mesh algorithm is unstable in the early stages of target pattern changes, resulting in large tracking errors. Unlike the above three variable structure multi-model estimation algorithms, the Probable Model Set (LMS) algorithm is applicable to model sets composed of different types of motion models. It uses model probabilities to divide the models in the model set into primary models, important models, and unlikely models, activates models adjacent to the primary models, retains the primary and important models, and discards unlikely models. It is a widely used variable structure multi-model estimation method. This method can match the corresponding model set according to the target's motion state and has a good tracking effect, but it still has the following shortcomings: First, the method is highly dependent on the accuracy of the model probability, but its Markov transition matrix is ​​prior and fixed, which makes the estimation of the model probability not necessarily accurate; Second, the method only uses the information of the target in the previous moment, while its motion is continuous, which is not conducive to the estimation accuracy. Summary of the Invention

[0004] The purpose of this invention is to design a new classification method using residual information, which is a variable structure interactive multi-model estimation method based on rolling time-domain estimation that reduces the dependence on model probability accuracy.

[0005] The steps of this invention are:

[0006] S1. Determine the system model set, which consists of multiple different models. Given the discretized state equations of these models:

[0007]

[0008] In the formula, It is the state variable of the system at time k, and the superscript i indicates the model corresponding to the system, F i This is the state transition matrix, where the subscript i indicates the corresponding model. It is the external input of the system. It is process noise;

[0009] The observation equation of the model is:

[0010]

[0011] In the formula, H i It is a measurement matrix, y k These are system measurements. It measures noise;

[0012] S2. In each iteration cycle, for all models in the model set, a fixed time domain is given, the length of which is denoted as N. e_max Then find the last moment in the direction from the beginning to the end of the fixed time domain where no information exists, and use the historical information from its next moment to the current moment as the new time domain for the model.

[0013] S3. If there is only one model with a certain time domain length, the output is directly used as the interaction value. The formula for interaction between models with the same time domain length is as follows:

[0014]

[0015]

[0016] In the formula, and These are the state estimates and covariance matrices of each model at the initial time in the time domain. and These are the mixture estimates and mixture covariance matrix of the models after interaction. Here, N in the subscript... e It corresponds to the time domain of model i. M represents the number of models involved in the estimation. It is model m (j) Transition to model m (i) The conditional model probability;

[0017] The formula for the model probability is:

[0018]

[0019] In the formula, It is the model probability from the previous time step. It is the normalization factor, π ji ∈[0,1] is model m (j) To model m (i) The transition probability, if π ji If > 0, it means that model m (j) and model m (i) Adjacent to each other;

[0020] S4, from the model set To model set Interactive multi-model estimation based on rolling time-domain estimation, denoted as

[0021] S4.1 Filtering: The estimated values ​​for each model are computed in parallel using a rolling time-domain estimation algorithm. The estimation problem is as follows:

[0022]

[0023]

[0024]

[0025]

[0026] In the formula, It is the initial state. It is the prior estimate of the initial state. Q is the covariance, and Q and R are parameter matrices. The former represents confidence in the accuracy of the model, and the latter represents confidence in the accuracy of the measurement.

[0027] Formula (6) can be used to calculate The estimated state at the current time can then be obtained using the following formula.

[0028]

[0029]

[0030] covariance matrix and It is obtained through multiple iterations of formula (9);

[0031] in addition, and Calculated using the following formula:

[0032]

[0033]

[0034] S4.2 Model Probability Update: Utilizing Residuals Covariance Matrix Calculate the likelihood function of each model at the current time. and model probability The formula is as follows:

[0035]

[0036]

[0037] S4.3 Estimation Fusion: Combining the estimates obtained from each model Covariance Matrix Based on model probability Fusion to obtain the estimated value The covariance matrix P k The formula is as follows:

[0038]

[0039]

[0040] S5, Probability of Model and residual Calculate the variables The formula is as follows:

[0041]

[0042] in accordance with The relationship between the thresholds t1 and t2, and the model set. The models in the model are divided into: main model Important Models and unlikely models

[0043] S6, Definition The model set consists of all models adjacent to the main model. If there is no main model, then the model set is... If the set is empty, proceed to the next step; otherwise, obtain the model set. here It is a model set The complement of . Run the algorithm. Obtain the state estimates, covariance, and model probabilities of the newly activated model: Indicates the model set To model set The IMM-MHE estimation corresponds to formulas (3)-(15);

[0044] S7, Fusion Model Set and model set Obtain the global fusion state estimate, covariance matrix at the current time step, and the initial state estimates, covariance matrices, and model probabilities for each model at the next time step. Update model set

[0045] S8. Define a discardable model set. This model set consists of unlikely models that are not adjacent to any primary model. If the set of discardable models is empty, proceed to the next step; otherwise, let... here This is for Those model sets with lower probabilities ensure At least K models remain;

[0046] S9, Output And return to step two.

[0047] The beneficial effects of this invention are:

[0048] 1. Compared with the possible model set algorithm, the VSIMM-MHE algorithm proposed in this invention uses a novel model classification method. By introducing residual information, it reduces the dependence on the accuracy of model probability and better ensures the accuracy of model classification. 2. Compared with the possible model set algorithm, the VSIMM-MHE algorithm proposed in this invention uses rolling time-domain estimation for each model in the model set at the current time, satisfying the continuity of target motion, fully utilizing the historical information of the tracked target, and achieving higher estimation accuracy.

[0049] 3. This invention proposes a temporal adaptive method to solve the problem that some models cannot fill in the fixed temporal information due to model activation and discarding. Simultaneously, it also proposes a new interaction rule to solve the problem of non-interaction caused by the different start times of the temporal domains corresponding to each model. Attached Figure Description

[0050] Figure 1 This is a flowchart of the VSIMM-MHE estimation method of the present invention;

[0051] Figure 2 This is a diagram showing the number of models running VSIMM-MHE under the double line-changing condition;

[0052] Figure 3This is a diagram of the maximum probability models of VSIMM-MHE and LMS under the double-track operation condition. Detailed Implementation

[0053] This invention relates to a variable structure multi-model estimation method, specifically a variable structure interactive multi-model estimation method based on rolling time-domain estimation, belonging to the field of target tracking technology.

[0054] This invention proposes a variable structure interactive multiple model estimation method (VSIMM-MHE) based on rolling time-domain estimation. A novel classification method is designed using residual information, reducing the dependence on model probability accuracy. Introducing the time domain into VSIMM-MHE fully utilizes the target's historical time information, resulting in higher accuracy. A time-domain adaptive method is proposed to address the problem of incomplete filling of fixed time-domain information for some models due to model activation and discarding. A new interaction rule is also proposed to address the problem of non-interaction caused by different start times for the corresponding time domains of each model. Furthermore, the model set of this method changes with the actual maneuver of the target, reducing estimation errors caused by models that do not match the current maneuver.

[0055] This invention employs the following technical solution: a variable structure interactive multiple model estimation method based on rolling time-domain estimation (VSIMM-MHE), comprising the following steps:

[0056] Step 1: Determine the system model set. The system model set consists of multiple different motion models, each model corresponding to a specific maneuver of the maneuvering target.

[0057] Step 2: A time-domain adaptive method is proposed. In each iteration cycle, for all models in the model set, we first provide a fixed time domain, the length of which is denoted as N. e_max Then, find the last moment in the direction from the beginning to the end of the fixed time domain where no information exists, and use the historical information from its next moment to the current moment as the new time domain for the model.

[0058] Step 3: Propose a new interaction rule that allows models with the same time domain length to interact, while models with different time domain lengths do not interact. If there is only one model with a certain time domain length, output it directly as the interaction value.

[0059] Step 4: From the model set To model set Interactive multi-model estimation based on rolling time-domain estimation The mixture estimates and mixture covariance matrix obtained in step three are used as prior estimates and covariances of the initial state for each MHE. These are then combined with measured values ​​to calculate the estimates for each model. Subsequently, the model probabilities are updated, and the state estimates from each model are fused using these probabilities to obtain the model set. The estimated values ​​and covariance of each model at the current time.

[0060] Step 5: Design a new model classification method to compare the variables calculated from model probabilities and residual information. Based on the relationship between the thresholds t1 and t2, models are classified into primary models, important models, and unlikely models.

[0061] Step Six: Determine if there are any models adjacent to the main model. If not, proceed to the next step. If they exist, find those models that do not belong to the model set. This part is denoted as the model set. Perform from model set To model set The IMM-MHE estimation yields the state estimates, covariance, and model probabilities of the newly activated model.

[0062] Step 7: Fusion of Model Sets and model set Obtain the global fusion state estimate, covariance matrix at the current time step, and the initial state estimates, covariance matrices, and model probabilities for each model at the next time step. Update model set

[0063] Step 8: Define the set of discardable models This model set consists of unlikely models that are not adjacent to any primary model. If the set of discardable models is empty, proceed to the next step. Otherwise, let... here This is for Those model sets with lower probabilities ensure There are at least K models remaining.

[0064] Step Nine: Output And return to step two.

[0065] To explain in detail the technical content, structural features, and objectives of this invention, the following is a comprehensive explanation of the invention in conjunction with the accompanying drawings:

[0066] This invention proposes a variable structure interactive multiple model estimation method (VSIMM-MHE) based on rolling time-domain estimation. For example... Figure 1 As shown, during the iteration process, the model set is first obtained using time-domain adaptive methods. The time domain of each model is determined, and then interaction is performed based on whether the models have the same time domain length, followed by IMM-MHE estimation. Afterwards, the obtained model probabilities and residual information are used to calculate the variables. The model is categorized into primary, important, and unlikely models. Models adjacent to the primary model are activated, while the primary and important models are retained, and the unlikely models are discarded. Finally, the impact of thresholds t1 and t2 on the VSIMM-MHE method is analyzed. This method not only reduces the dependence on model probability accuracy but also fully utilizes historical time-stamp information, achieving high accuracy. Furthermore, it reduces estimation errors caused by models that do not match the current maneuver.

[0067] The specific steps are as follows:

[0068] Step 1: Determine the system model set. The system model set consists of multiple different models. Given the discretized state equations of these models:

[0069]

[0070] In the formula, It is the state variable of the system at time k, and the superscript i indicates the model corresponding to the system, F i This is the state transition matrix, where the subscript i indicates the corresponding model. It is the external input of the system. It is process noise.

[0071] The observation equation of the model is:

[0072]

[0073] In the formula, H i It is a measurement matrix, y k These are system measurements. It measures noise.

[0074] Step 2: A temporal adaptive method is proposed. In each iteration, for all models in the model set, we first give a fixed time domain, the length of which is denoted as N. e_max Then, find the last moment in the direction from the beginning to the end of the fixed time domain where no information exists, and use the historical information from its next moment to the current moment as the new time domain for that model. If no information exists at the last moment of the entire time domain, it means that the model did not participate in the estimation. This method yields the time domain for each model in the model set. The corresponding time domain lengths are different. It's worth noting that, due to the existence of time domain adaptation, there's no need to consider the fact that the current time k is less than the time domain length in the rolling time domain estimation. The part.

[0075] Step 3: Propose a new interaction rule to calculate the mixture estimate and its covariance matrix at the start time in the time domain. Models with the same time domain length will interact, while models with different time domain lengths will not interact. If there is only one model with a certain time domain length, its output will be used as the interaction value. The formula for interaction between models with the same time domain length is as follows:

[0076]

[0077]

[0078] In the formula, and These are the state estimates and covariance matrices of each model at the initial time in the time domain. and These are the mixture estimates and mixture covariance matrix of the models after interaction. Here, N in the subscript... e It corresponds to the time domain of model i. M represents the number of models involved in the estimation. It is model m (j) Transition to model m (i) The conditional model probability.

[0079] Conditional model probability The calculation formula is:

[0080]

[0081] In the formula, It is the model probability from the previous time step. It is the normalization factor, π ji ∈[0,1] is model m (j) To model m (i) The transition probability, if π ji If > 0, it means that model m (j) and model m (i) Adjacent to each other.

[0082] Step 4: From the model set To model set Interactive multi-model estimation based on rolling time-domain estimation, denoted as

[0083] 4.1 Filtering: The estimated values ​​for each model are computed in parallel using a rolling time-domain estimation algorithm. The estimation problem can be summarized as the following optimization problem:

[0084]

[0085] In the formula, It is the initial state. It is the prior estimate of the initial state. Q is the covariance, and Q and R are parameter matrices. The former represents confidence in the accuracy of the model, and the latter represents confidence in the accuracy of the measurement.

[0086] Formula (6) can be used to calculate The estimated state at the current time can then be obtained using the following formula.

[0087]

[0088]

[0089]

[0090] covariance matrix and It can be obtained through multiple iterations of formula (9).

[0091] in addition, and It can be calculated using the following formula:

[0092]

[0093]

[0094] 4.2 Model Probability Update: Utilizing Residuals Covariance Matrix Calculate the likelihood function of each model at the current time. and model probability The formula is as follows:

[0095]

[0096]

[0097] 4.3 Estimation Fusion: The estimated values ​​obtained from each model are combined. Covariance Matrix Based on model probability Fusion to obtain the estimated value The covariance matrix P k The formula is as follows:

[0098]

[0099]

[0100] Step 5: Using model probabilities and residual Calculate the variables The formula is as follows:

[0101]

[0102] in accordance with The relationship between the thresholds t1 and t2, and the model set. The models in the model can be divided into: main models Important Models and unlikely models

[0103] Step Six: Definition The model set consists of all models adjacent to the main model. If there is no main model, then the model set is... If the set is empty, proceed to the next step. Otherwise, obtain the model set. here It is a model set The complement of . Run the algorithm. Obtain the state estimates, covariance, and model probabilities of the newly activated model: Indicates the model set To model set The IMM-MHE estimation corresponds to formulas (3)-(15). It is worth noting that the model set... All models in the system are models to be activated, utilizing only historical information from the previous time step, and all models participate in the interaction.

[0104] Step 7: Fusion of Model Sets and model set Obtain the global fusion state estimate, covariance matrix at the current time step, and the initial state estimates, covariance matrices, and model probabilities for each model at the next time step. Update model set

[0105] Step 8: Define the set of discardable models This model set consists of unlikely models that are not adjacent to any primary model. If the set of discardable models is empty, proceed to the next step. Otherwise, let... here This is for Those model sets with lower probabilities ensure There are at least K models remaining.

[0106] Step Nine: Output And return to step two.

[0107] Threshold Analysis: Thresholds t1 and t2 are crucial factors in the algorithm, significantly impacting its performance. To reduce computational burden and minimize errors from models that don't fit the current maneuvering pattern, threshold t2 should be decreased. However, decreasing threshold t2 can lead to difficulties in activating models that match the current maneuvering pattern, also resulting in estimation errors. Similarly, decreasing threshold t1 might cause more models to be classified as unlikely and removed. Increasing threshold t1 might leave models that should have been removed still in the model set for estimation, increasing computational burden. Both scenarios exacerbate estimation errors. Therefore, thresholds t1 and t2 should be adjusted based on specific circumstances to find their respective "balance points."

[0108] The effects of this invention can be illustrated by the following simulation examples:

[0109] Accurately obtaining the status information of the target vehicle is of great significance for safe driving. Here, the VSIMM-MHE method proposed in this invention is used to track the target vehicle.

[0110] 1. Model Set: The model set consists of three models: the CA model, the lateral position design of the road centerline, and three other models corresponding to three maneuvers in daily driving: changing lanes to the left, going straight, and changing lanes to the right. These three models track the lane centerline, and their discrete kinematics formulas are as follows:

[0111]

[0112] y k =Hx k +υ k (18)

[0113] In the formula, T > 0 is the sampling period, ω k It is the system's process noise, allowing for tracking changes in the lateral position of the lane centerline, υ k The system's measurement noise is H, where H is the measurement matrix, and u is the measurement noise. k To control the input, x k =[p lon,k p lat,k v x v y a x a y ] T These are the system's state vectors, namely, longitudinal position, lateral position, longitudinal velocity, lateral velocity, longitudinal acceleration, and lateral acceleration.

[0114] An LQR controller is selected as the control input:

[0115] u k =-K LQR (x k-[0 p ref,k 0 0 0 0] T (18)

[0116] In the formula, K LQR It is the control gain, p ref,k It is the lateral position of the lane centerline.

[0117] 2. Simulation software: The simulation models of the estimator and the estimated object were built using MATLAB / Simulink and CarSim software. The driving environment was provided by SCANeR studio, with versions MATLAB R2019b, CarSim 2016.1 and SCANeR studio 1.9 respectively. The simulation step size was set to 0.1s.

[0118] 3. Simulation content and results:

[0119] To verify the effectiveness of the VSIMM-MHE estimation method proposed in this invention, a dual-line-shifting simulation experiment was conducted using MATLAB / Simulink, CarSim, and SCANeR studio, and compared with the possible model set method (LMS).

[0120] Depend on Figure 2 It is evident that VSIMM-MHE can reduce the computational burden. When the vehicle is traveling in a straight line, three models are involved in the estimation to ensure a rapid response to left lane changes, straight-ahead maneuvers, or right lane changes at the next moment. However, when the vehicle is changing lanes to the left or returning to its original lane to the right, the number of models involved in the estimation becomes only one, namely the model that best matches the maneuver, which greatly reduces the computational burden. Figure 3 This is the maximum probability model diagram for VSIMM-MHE and LMS under the double lane change scenario. The vertical axis 1 to 3 represent Model 1, Model 2, and Model 3, respectively, corresponding to left lane change, straight ahead, and right lane change. Model probability The larger the value, the better the corresponding model matches the current maneuver. The results show that, with the same prior information such as the Markov transition matrix, compared with the LMS method that only relies on model probabilities, VSIMM-MHE can more accurately classify the model by introducing residual information, and thus effectively identify the maneuvers corresponding to the vehicle's double lane change process, namely, straight ahead, left lane change, straight ahead, right lane change, and straight ahead.

[0121] Table 1 Comparison of Tracking Accuracy

[0122]

[0123] As shown in Table 1, VSIMM-MHE achieves higher estimation accuracy by fully utilizing historical information about the tracked target. Its mean absolute error (MAE), root mean square error (RMSE), and mean absolute percentage error (MAPE) are all smaller than those of the LMS method.

Claims

1. A variable structure interacting multiple model estimation method based on rolling horizon estimation, characterized in that: The steps are: S1, determine the system model set, the model set of the system is composed of multiple different models, and the state equation of the model discretization is given as: wherein is the state variable of the system at time k, the upper index i denotes the model corresponding to the system, F i is the state transition matrix, the lower index i denotes the corresponding model, is the external input of the system, is the process noise; The observation equation of the model is: where H is a measurement matrix, y is a measurement vector, x is a system state vector, and n is a measurement noise vector. i is a measurement matrix, y k is a measurement vector, is a measurement noise vector. S2, in each iteration cycle, for all models in the model set, a fixed time domain is given, the length of the time domain is denoted as N e_max ; then find the last time point without information from the beginning of the fixed time domain to the end of the fixed time domain, and the historical information from the next time point to the current time point is taken as the new time domain of the model; S3, if there is only one model with a certain time domain length, it is directly output as the interaction value, and the formula for interaction between models with the same time domain length is as follows: wherein and are the state estimate and covariance matrix of each model at the time-domain start time, and are the mixed estimate and mixed covariance matrix of each model after interaction, where N e is the time-domain M represents the number of models participating in estimation, is the model m (j) conditional model probability of transition to model m (i) . The formula of model probability is: wherein, is the model probability at the previous time step, is a normalization factor, π ji ∈ [0, 1] is the transition probability from model m (j) to model m (i) if π ji > 0, indicating that model m (j) and model m (i) are adjacent. S4, from the model set to the model set an interacting multiple model estimation based on a rolling horizon estimation, denoted as S4.1 filtering: the estimated value of each model is calculated in parallel through the rolling time domain estimation algorithm, and the estimation problem is as follows: wherein is the initial state, is the initial state, is the covariance, Q and R are parameter matrices, the former representing the confidence in the model accuracy and the latter the confidence in the measurement accuracy; Using equation (6) can be calculated Further, the current state estimate is obtained by the following equation covariance matrix and is obtained by iterating equation (9) several times. In addition, and By the following formula: S4.2 Model probability update: Utilize the residual and the covariance matrix Compute the likelihood function for each model at the current time and the model probability The formula is as follows: S4.3 Estimate fusion: combine the estimates from each model and covariance matrix according to model probabilities to obtain the fused estimate and covariance matrix P k with the formula: S5, by model probability and residual Compute variable The formula is as follows: According to and the size relationship of the threshold values t1, t2, the models in the model set are divided into: main models important models and less likely models S6、Definition the model set consisting of all models adjacent to the primary model, if the primary model exists, the model set is empty, go to next step; Otherwise, obtain the model set Here is the complement of the model set Run the algorithm Obtain the state estimates, covariances, and model probabilities of the new active models: denotes the IMM-MHE estimate from the model set to the model set corresponding to equations (3)-(15). S7, fusion model set and model set obtain the global fusion state estimate, covariance matrix at the current time, the initial state estimate, covariance matrix of each model at the next time, and model probability update the model set S8, define the discardable model set The model set consists of the unlikely models that are not adjacent to any primary model. If the discardable model set is empty, go to the next step; otherwise, let Here is the set of models with smaller probabilities in , ensuring that at least K models remain. S9, output And return to step two.

Images