Modeling and trajectory extrapolation method for nonlinear dynamic system with drop point constraint

Abstract

The invention relates to the technical field of trajectory modeling and target tracking, and discloses a nonlinear dynamic system modeling and trajectory extrapolation method with drop point constraint. According to the method, an optimal state decomposition structure is obtained by adopting a projection mode through a drop point constraint condition, and drop point constraint information is naturally fused into an unconstrained nonlinear dynamic system by utilizing a recursive relationship between states, so that reconstruction of a nonlinear dynamic system model with drop point constraint is realized; the UKF filter estimation and trajectory extrapolation method suitable for the nonlinear dynamic system with the drop point constraint is designed by using the proposed reconstruction model, and the influence of the drop point constraint effect on state evolution is further enhanced on the basis of keeping the advantages of the UKF filter estimation, so that the filtering and extrapolation results are more accurate. According to the method, the prediction and control performance of a nonlinear dynamic system model is enhanced, and the flexibility, accuracy and stability of dynamic system modeling and trajectory extrapolation of an actual moving target are improved.

Description

Technical Field

[0001] The present invention relates to the technical field of trajectory modeling and target tracking, and in particular to a nonlinear dynamic system modeling and trajectory extrapolation method with landing point constraints. Background Art

[0002] The modeling of dynamic systems has always been a hot topic of research. In practical applications, many dynamic systems operate under certain constraints. For example, in satellite launches, the launch vehicle needs to stably fly to the predetermined orbit within a specific time; in air traffic control at the airport, each arriving flight needs to fly in the direction of the runway. The constraints contain the intention of movement and provide prediction information of the system state, which can be used to obtain a better system model and a more accurate state estimation. In practice, the constraints can be equalities or inequalities, linear or nonlinear, time-varying or time-invariant, etc. Using these constraints in a dynamic system requires a good system model and accurate state estimation. Constraints, as prior information, should be incorporated into the dynamic model to ensure that the system state automatically satisfies the constraints at every moment, thereby enhancing the model's prediction and control capabilities. With the rapid development of industrial technology, the increase in the complexity of environmental information, and the development of high-speed maneuverable aircraft, the motion trajectory of the target often presents complex characteristics such as large scale, nonlinearity, and time-varying. In this context, simple linear models and related filtering algorithms can no longer meet the increasing demand for estimation accuracy. In addition, during target monitoring, the target may be obscured or temporarily leave the monitoring range. Trajectory extrapolation can predict the position of the target when it is invisible, so as to maintain continuous tracking. Combining motion intention or constraint information and building a suitable extrapolation model can effectively improve the accuracy of trajectory extrapolation. Therefore, establishing a robust landing point constraint nonlinear dynamic system model and trajectory extrapolation algorithm is still an urgent problem to be solved.

[0003] One of the keys to the modeling and trajectory extrapolation of nonlinear dynamic systems with landing point constraints is how to naturally embed the landing point constraint information into the nonlinear dynamic system model and design an effective trajectory extrapolation algorithm. Compared with linear dynamic systems, nonlinear systems have higher complexity and uncertainty. It is difficult for nonlinear systems to achieve optimal state estimation in a closed form. Therefore, the study of nonlinear dynamic system models and filtering algorithms faces more challenges. Li Xiaorong and others, a well-known expert in the field of international information fusion, constrained the motion state of the target and proposed a dynamic system modeling method with state constraints. By decomposing the state of the nonlinear dynamic system and incorporating the state constraint information into the state update process with the help of the recursive relationship between the states, a dynamic system with state constraints was established. However, if the constraints on the landing point position of the moving target and the problem of continuous tracking under invisible conditions are considered, this method cannot obtain an effective nonlinear dynamic system model with landing point constraints, and it is difficult to ensure the performance of the dynamic system model (such as state estimation, trajectory extrapolation or control, etc.).

[0004] The main idea of ​​this method is: k x k =r k , k=0,1,2,..., where and Represent the known matrix and vector respectively. Consider the following nonlinear dynamic system model in is the state of the moving target at time k (including the position, velocity, acceleration and other information of the moving target), f k (·) is the vector-valued state transfer function, is a known control input, is a function with a mean of 0 and a covariance matrix of The initial state of the moving target is obtained by the Gram-Schmidt orthogonalization method. Perform state decomposition, that is in is the known covariance matrix. Then, the Gram-Schmidt orthogonalization method is used again to calculate the target state at time k Decompose, that is in Finally, with the help of the recursive relationship between states, the state constraint C k x k =r k Embedded in nonlinear dynamic system models From this, we can obtain the state-constrained nonlinear dynamic system model: This is the modeling idea of ​​state-constrained dynamic systems.

[0005] Terminology explanation:

[0006] Modeling of nonlinear dynamic systems with impact point constraints: It mainly refers to the establishment of a nonlinear dynamic system model with impact point constraints. The modeling process involves specific conditions or constraints that need to be met for the evolution of the system state, such as constraints on attributes such as position, speed, and heading. Taking ballistic missiles as an example, they are affected by factors such as gravity and air resistance, and the flight trajectory needs to be precisely controlled to ensure that they hit the intended target. Optimization techniques, control theory, and numerical simulation are usually used to solve such problems. These methods help determine the target state trajectory to meet the constraints. In engineering practice, considering the impact of impact point constraints is crucial to the control performance, safety, and stability of dynamic systems.

[0007] Unscented Kalman Filter (UFK): It is a recursive algorithm for state estimation of nonlinear systems. It is an extension of the Kalman filter that specifically handles nonlinear dynamic systems and measurement models. Since the traditional extended Kalman filter (EKF) handles nonlinear problems by linearizing nonlinear functions, this may lead to a decrease in estimation accuracy. Unlike UKF, the algorithm uses an unscented transformation to handle nonlinear problems and does not require linearization of nonlinear functions. The core idea of ​​the unscented Kalman filter is to capture the statistical characteristics of the state distribution using a set of carefully selected sample points (called Sigma points), which are propagated through nonlinear functions to more accurately estimate new states and covariance matrices. UKF has higher accuracy and robustness when dealing with highly nonlinear systems and is widely used in navigation, tracking, and robotics.

[0008] Trajectory extrapolation: It refers to a technology that uses a known target trajectory data to track the target stably for a period of time using appropriate dynamic models and filters, and then predicts or forecasts the state of the target at a future time point. This technology is widely used in many fields, including traffic management, autonomous driving, motion analysis, and aerospace. Summary of the invention

[0009] In view of the above problems, the purpose of the present invention is to provide a nonlinear dynamic system modeling and trajectory extrapolation method with drop point constraints, which can overcome the problems of unstable performance or failure of the system model caused by the difficulty in characterizing the drop point constraint effect, the complexity of nonlinear dynamic system calculation, and the uncertainty caused by predicting the target state in an invisible target environment in practical applications, thereby enhancing the prediction and control performance of the nonlinear dynamic system model. The technical solution is as follows:

[0010] A nonlinear dynamic system modeling and trajectory extrapolation method with a drop point constraint comprises the following steps:

[0011] Step 1: According to the original nonlinear dynamic system model of the moving target and the original landing point constraints, the original state of the moving target at time k is Perform state projection operation on the point constraint space to obtain the original state An optimal solution of ; Based on the real constraint conditions, an optimal decomposition form of the real constraint state xk of the moving target at time k is obtained;

[0012] Step 2: Based on the optimal decomposition form of the true constraint state xk, the weight block matrix B in the state projection operation k The conditions satisfied and the real constraint states x at time k and time N k With x N The recursive relationship between them is used to perform basic algebraic operations to obtain a nonlinear dynamic system model with drop point constraints;

[0013] Step 3: Using the nonlinear dynamic system model with landing point constraint obtained in step 2, a state filtering estimation algorithm of the nonlinear dynamic system model with landing point constraint is designed based on the observation model of the real constraint state of the moving target;

[0014] Step 4: Based on the nonlinear dynamic system model with landing point constraints obtained in step 2, the real constraint state x of the moving target at time k-1 is given k-1 As well as the corresponding model parameters, the optimal estimate of the true constraint state xk of the moving target at time k is calculated using the state filter estimation algorithm of the nonlinear dynamic system model with landing point constraints in step 3 Based on the trajectory extrapolation expression of the moving target state, the trajectory extrapolation result of the moving target at time k+1 is finally obtained

[0015] Furthermore, the step 1 specifically includes:

[0016] The original nonlinear dynamic system model:

[0017]

[0018]

[0019] in, They represent the original states of the moving target at time k and time k+1 respectively, and n is the dimension of the original state; represents the observed value of the moving target at time k, m is the dimension of the observed value; f k (·) and h k (·) are the nonlinear state transfer function and observation function respectively; Indicates the original state The process noise is 0 and has a covariance of Gaussian distribution of Indicates the original state The observation noise is 0 and the covariance is Gaussian distribution; N is the total motion time of the moving target;

[0020] The original landing point constraint condition is:

[0021] The original state of the moving target when it reaches the landing point at time N Constraints are made to satisfy the following equation:

[0022]

[0023] Among them, the row full rank constraint matrix and vector All are known;

[0024] The landing point constraint space refers to the original state when the moving target reaches the landing point position at time N Satisfied equality constraint space: S N ={γ|Aγ=a}, γ is the original state The value when the equality constraint is satisfied;

[0025] The state projection operation is to use the Lagrange multiplier method to solve the following convex optimization problem:

[0026]

[0027]

[0028] Among them, B k is the weight block matrix, and is a known positive definite matrix, l = 1, 2, 3, is the weight sub-matrix, and satisfies B k，2 >0 and A(B N，1 -B N，2 )A T =0, 0 is a zero matrix of appropriate dimension; T is the transpose symbol; B N，1 and B N，2 They are the weight block matrices corresponding to N moments respectively The weight sub-matrix in ;

[0029] The original state An optimal solution of is:

[0030]

[0031] The real constraint condition is: the real constraint state x when the moving target reaches the landing point at time N N Constraints are made to satisfy the following equation:

[0032] Ax N =a.

[0033] Among them, the row full rank constraint matrix and vector All are known;

[0034] The real constraint state x k An optimal decomposition form of is:

[0035] x k =x k -B k，1 A T (AB k，2 A T ) -1 Ax N +B k，1 A T (AB k，2 A T ) -1 a

[0036] Furthermore, the step 2 specifically includes:

[0037] The real constraint state x at time k and time N k With x N The recursive relationship between them is:

[0038]

[0039] in, represents the nonlinear composite calculation function corresponding to time k, and the superscript (Nk) represents the total number of composite calculations required by the composite function; w k is the real constraint state x k process noise;

[0040] The basic algebraic operations are:

[0041] By setting the real constraint state x at time k and time N k With x N Substitute the recursive formula between into the optimal decomposition form of the true constraint state xk obtained in step 1, and perform a recursive operation backward for one time step, that is,

[0042]

[0043] Among them, n k+1，1 and B k+1，2is the weight sub-matrix;

[0044] The nonlinear dynamic system model with drop point constraints is:

[0045]

[0046] Furthermore, the step 3 specifically includes:

[0047] Step 3.1: State prediction step, use the unscented transformation method and the point-constrained nonlinear dynamic system model to calculate the state prediction of one step And the forecast error covariance matrix ∑ k+1|k ;

[0048] The specific implementation process of the traceless transformation method is as follows:

[0049] Step 3.1.1: Use the distribution of variable x to select some appropriate points x i (i=0,1,…,2n x ), that is, the σ point, and its calculation formula is

[0050]

[0051] The weight vector corresponding to the σ point is calculated as

[0052] c 0 = k / (nx+k),

[0053] c i =1 / (2(n x + k)), i = 1, ..., 2n x ,

[0054] in, and∑ x is the mean and covariance matrix of the known distribution, k represents a natural number, and its value varies with the distribution of x, n x Represents the dimension of variable x; is the ith row or ith column of the square root of the matrix; c 0 and c i represents the weight vector of point σ;

[0055] Step 3.1.2: Substitute the point σ into the nonlinear function f(·) to obtain the nonlinear function value point set:

[0056] y i =f(x i ), i = 0, 1, ..., 2n x

[0057] Using the nonlinear function value point set {y i}Calculate the mean of the sample and the covariance matrix

[0058] Step 3.1.3: Calculate the one-step state forecast And the forecast error covariance matrix ∑ k+1|k :

[0059] Update the state for a given k time and∑ k|k , determine the σ point by unscented transformation calculation Recalculate

[0060]

[0061]

[0062]

[0063] in, is the function value point set of the state at time k+1, Qk is the covariance matrix of the process noise distribution;

[0064] Step 3.2: Observation prediction step, based on unscented transformation and observation model z k =h k (x k )+υ k To calculate the one-step forecast of the observation And the gain matrix K k+1 ; Among them, υ k is the real constraint state x k The observation noise of z k is the real constraint state x k Observed value of

[0065] Compute one-step forecast of observations And the gain matrix K k+1 , using the value calculated in step 3.1 and∑ k+1|k , the σ point is determined by unscented transformation calculation Recalculate

[0066]

[0067]

[0068]

[0069] in, is the set of function value points observed at time k+1;

[0070]

[0071]

[0072] Among them, R k is the covariance matrix of the observation noise distribution;

[0073] Step 3.3: State update step, using the state forecast obtained in step 3.1 and the forecast error covariance matrix ∑ k+1|k , and the one-step forecast of the observation obtained in step 3.2 And the gain matrix K k+1 , according to the observed value z k+1 Update to get status x k+1 The best estimate of and the estimated error covariance matrix ∑ k+1|k+1 ;

[0074] The state x k+1 The best estimate of and the estimation error covariance matrix ∑ k+1|k+1 The calculation formula is:

[0075]

[0076] Furthermore, the step 4 specifically includes:

[0077] The corresponding model parameters are: the nonlinear function f at time k-1 k-1 (·) and h k-1 The specific expression of (·), process noise w k-1 and observation noise υ k-1 The probability distribution of weight sub-matrix B k，1 and B k，2 , row full rank constraint matrix A and vector a;

[0078] The trajectory extrapolation expression of the moving target state is:

[0079]

[0080]

[0081] in, is the trajectory extrapolation result of the moving target at time k+1.

[0082] The beneficial effects of the present invention are:

[0083] 1) The present invention proposes a method for modeling and trajectory extrapolation of a nonlinear dynamic system with a landing point constraint, so as to overcome the problem that in actual application scenarios, the state estimation and prediction performance of the constructed nonlinear dynamic system model with a landing point constraint is reduced or even fails due to the uncertainty of trajectory extrapolation under the condition that the nonlinear dynamic system is subject to the landing point constraint and the moving target is invisible.

[0084] 2) The present invention creatively uses the state projection technology to project the state of the moving target into the real landing point constraint space, realizes the optimal decomposition of the real constraint state, and naturally embeds the landing point constraint information into the state update process with the help of the recursive relationship between the states, thereby obtaining a nonlinear dynamic system model with a landing point constraint; at the same time, for this system model, by adding the landing point constraint condition to the prediction step of the state estimation, the state prediction model is corrected, and an unscented Kalman filter algorithm with a nonlinear dynamic system model with a landing point constraint is designed. In addition, based on the landing point constraint non-dynamic system model and the unscented Kalman filter result, the state trajectory extrapolation expression is obtained, and the trajectory can be extrapolated for a period of time in the future after the target has been stably tracked for a period of time, achieving the purpose of continuous tracking of the moving target in an invisible environment.

[0085] 3) The present invention provides a method for modeling and trajectory extrapolation of nonlinear dynamic systems with landing point constraints, which can solve the high complexity and uncertainty problems faced by traditional methods in dealing with constrained nonlinear system modeling and state trajectory extrapolation. This method is more in line with actual application scenarios, enhances the flexibility, accuracy and stability of dynamic system models, and is particularly effective for modeling, continuous tracking and prediction of nonlinear systems with landing point constraints on moving targets. This method has important application value in the fields of scientific engineering, autonomous driving, aerospace, etc. BRIEF DESCRIPTION OF THE DRAWINGS

[0086] Figure 1 The present invention is a flow chart of the modeling process of a nonlinear dynamic system model with a drop point constraint.

[0087] Figure 2 The flowchart of the algorithm of UKF filtering and trajectory extrapolation of the nonlinear dynamic system model with landing point constraints of the present invention.

[0088] Figure 3 It is a comparison diagram of the state trajectories of the J2 motion state model without constraints and with landing point constraints in the implementation example of the present invention.

[0089] Figure 4 This is a state trajectory diagram of the landing point constraint J2 motion state model under the same landing point conditions with different starting points in an implementation example of the present invention.

[0090] Figure 5It is a state trajectory diagram of the landing point constraint J2 motion state model under the same starting point but different landing point conditions in an embodiment of the present invention.

[0091] Figure 6 This is a graph of the root mean square error (RMSE) of the observation, 10, 20, and 50-step extrapolation, and filtering of the unconstrained J2 motion state model under the condition that the observation standard deviation is 100m in the implementation example of the present invention.

[0092] Figure 7 This is a graph of the root mean square error (RMSE) of the filter for 10, 20, and 50 steps of extrapolation under the condition that the observation standard deviation is 100m in the implementation example of the present invention, with the J2 motion state model with landing point constraint.

[0093] Figure 8 This is a graph of the root mean square error (RMSE) of the observation, 10, 20, and 50-step extrapolation, and filtering of the unconstrained J2 motion state model under the condition that the observation standard deviation is 200m in the implementation example of the present invention.

[0094] Fig. 9 This is a graph of the root mean square error (RMSE) of the filter for 10, 20, and 50 steps of extrapolation under the condition that the observation standard deviation is 200m in the implementation example of the present invention, with the J2 motion state model with a landing point constraint. DETAILED DESCRIPTION

[0095] The present invention is further described in detail below with reference to the accompanying drawings and specific embodiments.

[0096] like Figure 1 , 2 As shown, the present invention has a flowchart of a nonlinear dynamic system modeling with a point constraint and a UKF filtering and trajectory extrapolation algorithm, and the specific implementation steps of the algorithm are as follows:

[0097] Step 1: Given the original nonlinear dynamic system model of the moving target and the original landing point constraints, the original state at time k By performing state projection operation on the point constraint space, the original state can be obtained. An optimal solution of , based on the real constraints, and then obtains the real constraint state x k An optimal decomposition form of .

[0098] The original nonlinear dynamic system model is:

[0099]

[0100]

[0101] in, They represent the original states of the moving target at time k and time k+1 respectively. represents the observed value of the moving target at time k. k (·) and h k (·) are the nonlinear state transfer function and observation function respectively. represents process noise and has a mean of 0 and a covariance of Gaussian distribution, represents observation noise and has a mean of 0 and a covariance of Gaussian distribution.

[0102] The original landing point constraint condition is: the original state of the moving target when it reaches the landing point at time N (arbitrary vector value) is constrained to satisfy the following equation:

[0103]

[0104] Among them, the row full rank constraint matrix and vector All are known.

[0105] The landing point constraint space is: the original state of the target Satisfied equality constraint space S N ={γ|A γ =a}, γ is the original state The value when the equality constraint is satisfied.

[0106] The state projection operation is: Use the Lagrange multiplier method to solve the following convex optimization problem:

[0107]

[0108]

[0109] in, and They represent the original states of the moving target at time k and time N respectively, and x k is the real constraint state of the moving target at time k, and the weight block matrix is a known positive definite matrix, is a weight sub-matrix that satisfies B k，2 >0 and A(B N，1 -B N，2 )A T =0, 0 is a zero matrix of appropriate dimension. N，1 and B N，2 They are the weight block matrices corresponding to N moments respectively The weight sub-matrix in .

[0110] Original state An optimal solution of is:

[0111]

[0112] The real constraint condition is: the real constraint state x when the moving target reaches the landing point at time N N (determined vector) is constrained to satisfy the following equation

[0113] Ax N =a,

[0114] Among them, the row full rank constraint matrix and vector All are known.

[0115] The actual constraint state x k An optimal decomposition form of is:

[0116] x k =x k -B k，1 A T (AB k，2 A T ) -1 Ax N +B k，1 A T (AB k，2 A T ) -1 a

[0117] Step 2: Based on the true constraint state x obtained in step 1 k The optimal decomposition form is based on the weight block matrix B k Satisfied condition A(B N，1 -B N，2 )A T = 0 and the true constraint state x k With x N By performing the recursive relationship between them and performing basic algebraic operations, we can obtain a nonlinear dynamic system model with drop point constraints.

[0118] The actual constraint state x k With x N The recursive relationship between them is: state x k With x N The recursive formula between:

[0119]

[0120] in represents the composite calculation function corresponding to time k, and (Nk) in the last equation represents the total number of composite calculations required by the composite function.

[0121] The basic algebraic operation is: By setting the true constraint state xk With x N Substitute the recursive formula between into the real constraint state x obtained in step 1 k The optimal decomposition form of and then recursively perform a time step backward, that is,

[0122]

[0123] The nonlinear dynamic system model with drop point constraints is:

[0124]

[0125] Step 3: According to the nonlinear dynamic system model with drop point constraints obtained in step 2, the model parameters at time k are input respectively, and the real constraint state x at time k+1 can be calculated. k+1 , based on the observation model z of the real constraint state of the moving target k =h k (x k )+υ k Get the observed value z of the state k Finally, the state filtering estimation of the dynamic system model with the drop point constraint is performed to obtain the real constraint state x k+1 The best estimate of

[0126] The model parameters are: the real constraint state x at time k k , nonlinear function f k (·) and h k The specific expression of (·), process noise w k and observation noise v k The probability distribution of weight sub-matrix B k+1,1 and B k+1,2 , state constraint matrix A, state constraint vector a.

[0127] The filter estimation is: Unscented Kalman Filter (UKF), which does not rely on Taylor expansion to linearize nonlinear systems, but uses unscented transformation to handle the transmission of mean and covariance in nonlinear processes. The algorithm approximates the probability density distribution of nonlinear functions by approximating the posterior probability density of the state through a series of deterministic samples. In this way, there is no need to approximate the nonlinear function or calculate the derivative of the Jacobian matrix.

[0128] The state filtering estimation algorithm of the dynamic system model with a drop point constraint is: a UKF algorithm of a nonlinear dynamic system model with a drop point constraint, which includes the following three steps:

[0129] Step 3.1: State prediction step, use the unscented transformation and the point-constrained nonlinear dynamic system model to calculate the state prediction of one step And the forecast error covariance matrix ∑ k+1|k ;

[0130] The specific implementation process of traceless transformation is as follows:

[0131] (a) Use the distribution of variable x to select some appropriate points x i (i=0,1,…,2n), namely σ point, its calculation formula is:

[0132]

[0133] The weight vector corresponding to the σ point is calculated as

[0134] c 0 = k / (n x + k),

[0135] c i =1 / (2(n x + k)), i = 1, ..., 2n x ,

[0136] in, and∑ x is the mean and covariance matrix of the x distribution, k indicates that the natural number can take different values ​​depending on the distribution of x, n x Represents the dimension of variable x; is the ith row or ith column of the matrix square root.

[0137] (b) Substitute the point σ in (a) into the nonlinear function f(·) to obtain the set of nonlinear function value points

[0138] y i =F(x i ), i = 0, 1, ..., 2n x

[0139] (c) Using the function value point set {y i The mean of the sample can be calculated and the covariance matrix where c i Represents the weight vector of the σ point.

[0140] The state forecast of the step And the forecast error covariance matrix ∑ k+1|k The calculation is: Update the state for a given k moment and ∑k|k, determine the σ point by unscented transformation calculation (i=0,1,…,2n x ), and then calculate

[0141]

[0142]

[0143]

[0144] in, is the function value point set of the state at time k+1, and Qk is the covariance matrix of the process noise distribution.

[0145] Step 3.2: Observation prediction step, based on unscented transformation and observation model z k =h k (x k )+υ k To calculate the one-step forecast of the observation And the gain matrix K k+1 ;

[0146] One-step forecast of the observations described And the gain matrix K k+1 The calculation of is: using the calculation obtained in step 3.1 and∑ k+1|k , the σ point is determined by unscented transformation calculation (i=0,1,…,2n x ), and then calculate:

[0147]

[0148]

[0149]

[0150] in, is the set of function value points observed at time k+1, c i is the weight vector of point σ,

[0151]

[0152]

[0153] Here R k is the covariance matrix of the observation noise distribution.

[0154] Step 3.3: State update step, using the state forecast obtained in step 3.1 and the forecast error covariance matrix Σ k+1|k , and the one-step forecast of the observation obtained in step 3.2 And the gain matrix K k+1 , according to the observed value z k+1 Update to get status x k+1 The best estimate of and the estimated error variance matrix ∑ k+1|k+1 ;

[0155] The state x k+1 The best estimate of and the estimation error covariance matrix ∑ k+1|k+1 The calculation formula is:

[0156]

[0157] Step 4: Based on the nonlinear dynamic system model with landing point constraints obtained in step 2, the real constraint state x of the moving target at time k-1 is given k-1 As well as the corresponding model parameters, the real constraint state x of the moving target at time k can be calculated by using the filtering estimation algorithm of the nonlinear dynamic system model with the landing point constraint in step 3. k The best estimate of Based on the trajectory extrapolation expression of the moving target state, the trajectory extrapolation result of the moving target at time k+1 can be obtained.

[0158] The corresponding model parameters are: the nonlinear function f at time k-1 k-1 (·) and h k-1 The specific expression of (·), process noise w k-1 and observation noise υ k-1 The probability distribution of weight sub-matrix B k，1 and B k，2 , row full rank constraint matrix A, vector a.

[0159] The trajectory extrapolation expression of the moving target state is:

[0160]

[0161] in, is the trajectory extrapolation result of the moving target at time k+1, the nonlinear composite function operator It can be obtained through numerical calculation.

[0162] This embodiment specifically describes the modeling of a nonlinear dynamic system with a landing point constraint and a trajectory extrapolation algorithm proposed in the present invention for a scenario of continuous tracking of a nonlinear dynamic system with a landing point constraint and an invisible target, and conducts a simulation experiment.

[0163] Problem description: Compare the state trajectories generated by the traditional unconstrained nonlinear dynamic system model and the nonlinear dynamic system model with drop point constraints. Analyze the impact of different starting and drop point positions on the state trajectory of the nonlinear dynamic system model with drop point constraints. Use the UKF filter to implement filter estimation for the unconstrained and drop point constrained nonlinear dynamic system models, and conduct trajectory extrapolation experiments of 10, 20, and 50 steps.

[0164] Parameter setting: This example considers a flying target in a three-dimensional scene with a total flight time of 200 seconds. The observation noise standard deviation is set to 100m and 200m. The weight submatrix in the nonlinear dynamic system model with landing point constraint is selected. Among them I i is a 6×6 identity matrix, given the matrix in the drop point constraint

[0165] and vector

[0166] In order to verify the effectiveness and superior performance of the proposed nonlinear dynamic system model with point constraints and trajectory extrapolation scheme, the unconstrained gravity acceleration (J2) motion model, which is widely used in practice, is used as the implementation object. Assume that the geocentric vector p = ||(x, y, z)′||, ||·|| represents l 2 -norm, μ = 3.986 × 10 14 is the gravitational constant, is a constant coefficient, J 2 =1.0826×10 -3 is Jeffery constant, r e =6378137 represents the radius of the earth, ω = 7.292×10 -5 represents the angular velocity of the Earth's rotation, if is the position and velocity vector of the 3D coordinate system, then the unconstrained J2 motion model has the following ODE form:

[0167]

[0168] In fact, although the above equations are in the form of an ODE, their numerical calculation can be expressed in the following discrete form:

[0169] X k+1 =g k (X k )+w k , k = 1, ..., N,

[0170] where g k (·) can be effectively solved by Runge-Kutta algorithm. Based on the above formula, the J2 motion model with landing point constraint can be obtained

[0171]

[0172] Algorithm implementation: It is divided into three scenarios:

[0173] (a) Set the starting point coordinates of the moving target (0m, 0m, r e m), where r e =6378137 represents the radius of the earth. The initial velocities of each component are (6Ma, 6Ma.2.9Ma), where Ma represents the speed of sound, i.e. 340m / s. The target moves for 200s before reaching the landing point, and the corresponding coordinates are (4×10 5 m,4×10 5 m, r e m), the state trajectory is generated by using the unconstrained J2 motion model and the J2 motion model with landing point constraints. The results are as follows: Figure 3 .

[0174] (b) Fix the target landing point, initial speed and movement time, randomly select 20 starting points at different positions, and use the J2 motion model with landing point constraint to generate trajectories. The results are shown in Figure 4 .

[0175] (c) Fix the starting position, initial speed and movement time of the target, randomly select 20 landing points at different positions, and use the J2 motion model with landing point constraints to generate trajectories. The results are shown in Figure 5 .

[0176] In addition, in scenario (a), when the standard deviation of the observation noise is set to 100m, the J2 motion model with no constraints and with landing point constraints is used to generate state trajectory data, and UKF filtering estimation and trajectory extrapolation are performed. The results are shown in Figure 2. Figure 6 , Figure 7 .

[0177] Similarly, in scenario (a), when the standard deviation of the observation noise is set to 200m, the J2 motion model with no constraints and with a landing point constraint is used to generate state trajectory data, and UKF filtering estimation and trajectory extrapolation of 10, 20, and 50 steps are performed. The results are shown in Figure 2. Figure 8 , Fig. 9 .

[0178] Effect analysis: According to Figure 3 It shows that when the starting point, landing point and parameter settings of the target are the same, the unconstrained J2 motion model cannot accurately reach the landing point, while the J2 motion model with landing point constraints can accurately reach it. Figure 4 It shows that when the landing point is fixed, the state trajectory generated by the J2 motion model with landing point constraint will pass through the specified landing point regardless of how the starting point changes. Figure 5It shows that the state trajectory generated by the J2 motion model with landing point constraint at the same starting point will pass through the respective landing point positions due to different landing point constraints. Figure 6-7 It can be seen that when the standard deviation of the observation noise is 100m, the RMSE of the UKF filter of the unconstrained J2 motion model is close to the observation error, and the RMSE of the 10-step, 20-step, and 50-step extrapolation are much larger than the observation error; while the RMSE of the UKF filter and 10-step extrapolation of the J2 motion model with landing point constraints is less than or close to the observation error, and the RMSE of the 20-step and 50-step extrapolation are much smaller than the unconstrained model. The overall performance of the constrained J2 motion model is significantly better than that of the unconstrained J2 motion model. Figure 8-9 It can be seen that when the standard deviation of the observation noise is 100m, the RMSE of the UKF filter of the unconstrained J2 motion model is close to the observation error, and the RMSE of the 10-step, 20-step, and 50-step extrapolation are all much larger than the observation error; while the RMSE of the UKF filter, 10-step, and 20-step extrapolation of the J2 motion model with landing point constraints is less than or close to the observation error, and the RMSE of the 50-step extrapolation is much smaller than that of the unconstrained model. The overall performance of the constrained J2 motion model is significantly better than that of the unconstrained J2 motion model. Figure 7-9 From the results, we can see that when the standard deviation of the observation error is 100m and 200m, the UKF filter tracking error: the J2 motion model with landing point constraint is significantly better than the unconstrained J2 motion model. At the same time, after the filter has been stably tracked for a period of time, the trajectory is extrapolated. In terms of extrapolation error, the trajectory extrapolation error of the J2 motion model with landing point constraint is much smaller than that of the unconstrained J2 motion model.

Claims

1. A nonlinear dynamic system modeling and trajectory extrapolation method with drop point constraints, characterized in that: The following steps are involved: Step 1: According to the original nonlinear dynamic system model of the moving target and the original landing point constraints, the original state of the moving target at time k is Perform state projection operation on the point constraint space to obtain the original state An optimal solution of ; Based on the real constraint conditions, the real constraint state x of the moving target at time k is obtained k An optimal decomposition form of ; Step 2: According to the real constraint state x k The optimal decomposition form is based on the weight block matrix B in the state projection operation k The conditions satisfied and the real constraint states x at time k and time N k With x N The recursive relationship between them is used to perform basic algebraic operations to obtain a nonlinear dynamic system model with drop point constraints; Step 3: Using the nonlinear dynamic system model with landing point constraint obtained in step 2, a state filtering estimation algorithm of the nonlinear dynamic system model with landing point constraint is designed based on the observation model of the real constraint state of the moving target; Step 4: Based on the nonlinear dynamic system model with landing point constraints obtained in step 2, the real constraint state x of the moving target at time k-1 is given k-1 As well as the corresponding model parameters, the state filter estimation algorithm of the nonlinear dynamic system model with landing point constraints in step 3 is used to calculate the real constraint state x of the moving target at time k k The best estimate of Based on the trajectory extrapolation expression of the moving target state, the trajectory extrapolation result of the moving target at time k+1 is finally obtained 2. The nonlinear dynamic system modeling and trajectory extrapolation method with landing point constraints according to claim 1 is characterized in that: The step 1 specifically includes: The original nonlinear dynamic system model: in, They represent the original states of the moving target at time k and time k+1 respectively, and n is the dimension of the original state; represents the observed value of the moving target at time k, m is the dimension of the observed value; f k (·) and h k (·) are the nonlinear state transfer function and observation function respectively; Indicates the original state The process noise is 0 and has a covariance of Gaussian distribution of Indicates the original state The observation noise is 0 and the covariance is Gaussian distribution; N is the total motion time of the moving target; The original landing point constraint condition is: The original state of the moving target when it reaches the landing point at time N Constraints are made to satisfy the following equation: Among them, the row full rank constraint matrix and vector All are known; The landing point constraint space refers to the original state when the moving target reaches the landing point position at time N Satisfied equality constraint space: S N ={γ|A γ =a}, γ is the original state The value when the equality constraint is satisfied; The state projection operation is to use the Lagrange multiplier method to solve the following convex optimization problem: Among them, B k is the weight block matrix, and is a known positive definite matrix, l = 1, 2, 3, is the weight sub-matrix, and satisfies B k，2 >0 and A(B N，1 -B N，2 )A T =0, 0 is a zero matrix of appropriate dimension; T is the transpose symbol; B N，1 and B N，2 They are the weight block matrices corresponding to N moments respectively The weight sub-matrix in ; The original state An optimal solution of is: The real constraint condition is: the real constraint state x when the moving target reaches the landing point at time N N Constraints are made to satisfy the following equation: Ax N =a, Among them, the row full rank constraint matrix and vector All are known; The real constraint state x k An optimal decomposition form of is: x k =x k -B k，1 A T (AB k，2 A T ) -1 Ax N +B k，1 A T (AB k，2 A T ) -1 a。 3. The nonlinear dynamic system modeling and trajectory extrapolation method with landing point constraints according to claim 2 is characterized in that: The step 2 specifically includes: The real constraint state x at time k and time N k With x N The recursive relationship between them is: in, k=1, ..., N-1 represents the nonlinear composite calculation function corresponding to time k, and the superscript (Nk) represents the total number of composite calculations required by the composite function; w k is the real constraint state x k process noise; The basic algebraic operations are: By setting the real constraint state x at time k and time N k Substitute the recursive formula between and xN into the actual constraint state x obtained in step 1 k The optimal decomposition form of , and then perform a recursive operation backward for one time step, that is, x k+1 =x k+1 -B k+1,1 A T (AB k+1,2 A T ) -1 A Among them, B k+1，1 and B k+1，2 is the weight sub-matrix; The nonlinear dynamic system model with drop point constraints is:

4. The nonlinear dynamic system modeling and trajectory extrapolation method with landing point constraints according to claim 3 is characterized in that: The step 3 specifically includes: Step 3.1: State prediction step, use the unscented transformation method and the point-constrained nonlinear dynamic system model to calculate the state prediction of one step And the forecast error covariance matrix ∑ k+1|k ; The specific implementation process of the traceless transformation method is as follows: Step 3.1.1: Use the distribution of variable x to select some appropriate points x i , i=0,1,…,2n x , that is, the σ point, its calculation formula is: The weight vector corresponding to the σ point is calculated as c0=κ / (n x +k), c i =1 / (2(n x +κ)),i=1,...,2n x , in, and∑ x is the mean and covariance matrix of the known distribution, κ represents a natural number, and its value varies with the distribution of x, n x Represents the dimension of variable x; is the ith row or ith column of the square root of the matrix; c0 and c i represents the weight vector of point σ; Step 3.1.2: Substitute the point σ into the nonlinear function f(·) to obtain the nonlinear function value point set: y i =f(x i ),i=0,1,…,2n x Using the nonlinear function value point set {y i }Calculate the mean of the sample and the covariance matrix Step 3.1.3: Calculate the one-step state forecast And the forecast error covariance matrix ∑ k+1|k : Update the state for a given k time and∑ k|k , determine the σ point by unscented transformation calculation Recalculate in, is the function value point set of the state at time k+1, Q k is the covariance matrix of the process noise distribution; Step 3.2: Observation prediction step, based on unscented transformation and observation model z k =h k (x k )+v k To calculate the one-step forecast of the observation And the gain matrix K k+1 ; where v k is the real constraint state x k The observation noise of z k is the real constraint state x k Observed value of Compute one-step forecast of observations And the gain matrix K k+1 , using the value calculated in step 3.1 and∑ k+1|k , the σ point is determined by unscented transformation calculation Recalculate in, is the set of function value points observed at time k+1; Among them, R k is the covariance matrix of the observation noise distribution; Step 3.3: State update step, using the state prediction of one step obtained in step 3.1 and the forecast error covariance matrix ∑ k+1|k , and the one-step forecast of the observation obtained in step 3.2 And the gain matrix K k+1 , according to the observed value z k+1 Update to get status x k+1 The best estimate of and the estimated error covariance matrix Σ k+1|k+1 ; The state x k+1 The best estimate of and the estimation error covariance matrix ∑ k+1|k+1 The calculation formula is:

5. The nonlinear dynamic system modeling and trajectory extrapolation method with landing point constraints according to claim 4 is characterized in that: The step 4 specifically includes: The corresponding model parameters are: the nonlinear function f at time k-1 k-1 (·) and h k-1 The specific expression of (·), process noise w k-1 and observation noise v k-1 The probability distribution of weight sub-matrix B k，1 and B k，2 , row full rank constraint matrix A and vector a; The trajectory extrapolation expression of the moving target state is: in, is the trajectory extrapolation result of the moving target at time k+1.

Images