A MPSP trajectory design method based on generalized Newton iteration

Abstract

A MPSP trajectory design method based on generalized Newton iteration belongs to the field of aircraft control technology. The method is as follows: establish a dynamic model, transform the continuous time optimization problem into a discrete finite dimensional problem; generate the initial state vector and initial control vector of the guessed value, and set the jump-out condition; calculate the approximate value of the state vector and the terminal output error, calculate the state vector error; determine whether the terminal output error and the state vector error both meet the jump-out condition; if satisfied, the trajectory design is completed, otherwise the control vector is updated with the control vector update formula; the state vector is updated with the state vector update formula. The present invention solves the problem that the traditional trajectory planning method relies on manual experience to adjust parameters and cannot meet the online automatic solution of the trajectory, reduces the algorithm complexity, improves the robustness of convergence, and has a good application prospect in the trajectory planning of energy management missiles.

Description

Technical Field

[0001] The invention relates to an MPSP trajectory design method based on generalized Newton iteration, belonging to the technical field of aircraft control. Background Art

[0002] Reusable aircraft will be subject to strong aerodynamic interference in the energy management section, causing the aircraft to deviate from the nominal trajectory, so it is necessary to plan the trajectory online. The energy management section trajectory needs to meet multiple constraints such as terminal height, terminal speed, terminal track angle, terminal heading angle, terminal position, etc., which makes the solution of this section of the trajectory become an extremely complex multi-constraint nonlinear problem. In addition, traditional design methods rely on manual experience to adjust the parameters in the trajectory, which makes it difficult to achieve automatic design. Summary of the invention

[0003] In order to solve the problems existing in the background technology, the present invention provides an MPSP trajectory design method based on generalized Newton iteration.

[0004] To achieve the above object, the present invention adopts the following technical solution: a MPSP trajectory design method based on generalized Newton iteration, the method comprising the following steps:

[0005] S1: Establish the dynamic model of the nonlinear system and discretize the state vector and control vector using Chebyshev discretization to transform the continuous time optimization problem into a discrete finite dimensional problem;

[0006] S2: Generate the initial state vector X of the guess value 0 and the initial control vector U 0 , and set the number of nodes N and the exit conditions: output error δy and state error δX;

[0007] S3: Calculate the state vector approximation and terminal output error Calculate the state vector error

[0008] S4: Determine the terminal output error and the state vector error Whether all the exit conditions are met, that is, and If both are satisfied, the trajectory design is completed and the final state vector X is output. k And the final control vector U k , otherwise proceed to S5;

[0009] S5: Update the control vector using the control vector update formula;

[0010] S6: Update the state vector using the state vector update formula;

[0011] S7: Repeat S4.

[0012] Compared with the prior art, the present invention has the following beneficial effects:

[0013] The present invention solves the problem that the traditional trajectory planning method relies on manual experience to adjust parameters and cannot meet the requirements of online automatic trajectory calculation. The continuous-time trajectory planning problem is transformed into a discrete root-finding problem by using Chebyshev polynomials to approximate the dynamic equations and constraint equations, and then solved by Newton's iteration method. The state quantity and control quantity in the trajectory design process are used as variables to be solved, and Newton's method is used to solve them simultaneously, which reduces the algorithm complexity and improves the robustness of convergence. It has good application prospects in energy management missile trajectory planning. BRIEF DESCRIPTION OF THE DRAWINGS

[0014] Figure 1 It is a flow chart of the present invention. DETAILED DESCRIPTION

[0015] The technical solution of the present invention will be clearly and completely described below in conjunction with the drawings in the embodiments of the present invention. Obviously, the described embodiments are only part of the embodiments of the invention, rather than all of the embodiments. Based on the embodiments of the present invention, all other embodiments obtained by ordinary technicians in this field without making creative work are within the scope of protection of the present invention.

[0016] A MPSP trajectory design method based on generalized Newton iteration, MPSP is model predictive static programming, MPSP, model predictive static programming. The method comprises the following steps:

[0017] S1: Establish the dynamic model of the nonlinear system and discretize the state vector and control vector using Chebyshev discretization to transform the continuous time optimization problem into a discrete finite dimensional problem;

[0018] S101: Define the dynamics of a nonlinear system as follows:

[0019]

[0020] In formula (1):

[0021] is the first-order derivative of X(t), is the state vector;

[0022] is the control vector;

[0023] t0 is the initial time;

[0024] t fis the end time;

[0025] X = [xhzv γ ψ];

[0026] x is the x coordinate of the aircraft in the airport coordinate system;

[0027] h is the height of the aircraft;

[0028] z is the z coordinate of the aircraft in the airport coordinate system;

[0029] v is the speed of the aircraft;

[0030] γ is the flight path angle of the aircraft;

[0031] ψ is the heading angle of the aircraft;

[0032] U = [ασ];

[0033] α is the angle of attack of the aircraft;

[0034] σ is the roll angle of the aircraft;

[0035] And define the dynamic initial state of the nonlinear system as follows:

[0036] X(t0)=X 0 (2)

[0037] In formula (2):

[0038] X 0 is the initial state vector;

[0039] S102: Establish the dynamic model of the nonlinear system as follows:

[0040]

[0041] In formula (3):

[0042] L is the aerodynamic lift;

[0043] D is the aerodynamic drag;

[0044] Aerodynamic lift and aerodynamic drag are both determined by the angle of attack α and the velocity v;

[0045] g is the acceleration due to gravity;

[0046] m is the mass of the aircraft;

[0047] The terminal constraint guidance problem is generally described as determining an appropriate control vector U(t), t∈[t0,t f ], so that the final system output Y(t f )(It can be seen from the dynamics of the nonlinear system (1) from the initial time t0 to the end time t fThe integral is close to the expected end result y d ,Right now:

[0048] y(X(t f ))=y d (4)

[0049] The present invention also takes the system state vector X(t), t∈[t0,t f ] as the variable to be determined.

[0050] S103: Therefore, the constraint-guided problem to be solved is defined as:

[0051]

[0052] X(t0)=X 0 (6)

[0053] y(X(t f ))=y d (7)

[0054] In formula (7):

[0055] X(t f ) is the terminal state vector;

[0056] y d The desired end result;

[0057] S104: Integrate both sides of equation (5) to transform the dynamics of the nonlinear system into an integral form as follows:

[0058]

[0059] S105: According to the Chebyshev polynomial, the physical time domain t∈[t0,t f ] is scaled to the time domain τ∈[-1,1] as follows:

[0060]

[0061] Therefore, formula (8) can be rewritten as:

[0062]

[0063] S106: Introduce the Chebyshev polynomial approximation of the unknown trajectory X(τ) and the integral of equation (10) along the trajectory. In fact, with the help of Chebyshev polynomials, the integral can be easily obtained At the CGL node [τ1,τ2,…τ N ]. The value of the unknown trajectory X(τ) at the CGL node is as follows:

[0064] X=X0 +E·F(X,U,X 0 ) (11)

[0065] In formula (11):

[0066] is in discrete state vector form;

[0067] Represents the matrix form of the function f(X,U) at N+1 CGL nodes;

[0068] is the control vector form;

[0069] is a constant matrix, and

[0070]

[0071]

[0072] In formula (13):

[0073] The (k,j)th element is T j (τ k ) matrix;

[0074] I n×n is the n×n dimensional identity matrix;

[0075] The coefficient matrix A = S N ·T·V, is a diagonal matrix with diagonal elements {(1 / N),(2 / N),...,(2 / N),(1 / N)}, The (k,j)th item is T k (τ j ), T i (τ):=cos(i·cos -1 (τ)),i=0,1,…,N,S N is a constant matrix, and

[0076]

[0077] Therefore, the continuous dynamic equation of formula (3) is discretized into the algebraic equation of formula (11) at the CGL node; from formula (11), the terminal state X(τ N ):

[0078] X(τ N )=X 0 +E N ·F(X,U,X0 ) (15)

[0079] In formula (15):

[0080] is the terminal constant matrix;

[0081] S107: Substituting equation (15) into equation (7), the end constraint can be rewritten as:

[0082] y(X 0 +E N ·F(X,U,X 0 ))=y d (16)

[0083] S108: That is, convert the continuous time optimization problem P0 into a discrete finite dimensional problem P1, that is, find the discrete state vector form X and the control vector form U according to equation (11) and equation (16);

[0084] S109: The obviously discrete finite-dimensional problem P1 is a root-finding problem with unknown variables in the form of discrete state vectors X and control vectors U. The solution is defined as s, s = [X T ,U T ] T , rewrite the discrete finite-dimensional problem P1 into the finite-dimensional problem P2: the solution s satisfies the problem function Ψ(s) = 0,

[0085]

[0086] S2: Generate the initial state vector X of the guess value 0 and the initial control vector U 0 , and set the number of nodes N and the exit conditions: output error δy and state error δX;

[0087] Initial control vector U 0 It can be set according to the airport location, the initial control vector U 0 is the fixed angle of attack and roll angle throughout the process, that is Initial guess state vector X 0 It can be obtained by integrating formula (1).

[0088] The exit condition is set to output error δy = [10 100 10 50 0.1 0.1],

[0089] State Error

[0090] The number of nodes N=100.

[0091] S3: Calculate the state vector approximation and terminal output error Calculate the state vector error

[0092] S4: Determine the terminal output error and the state vector error Whether all the exit conditions are met, that is, and If both are satisfied, the trajectory design is completed and the final state vector X is output. k And the final control vector U k , otherwise proceed to S5;

[0093] S5: Update the control vector using the control vector update formula;

[0094] S501: Update the control vector. The Newton method iteration formula for solving equation (17) is:

[0095] s k+1 =s k +Δs k (18)

[0096]

[0097] In formula (18):

[0098] Δs k is the Newton step of the kth iteration;

[0099] is the Jacobian matrix;

[0100] S502: Determine the Newton step Δs of the kth iteration by solving the linear equation of formula (19): k ;

[0101] S503: Obtain the updated next generation variable solution s from equation (18) k+1 ;

[0102] S504: Iteratively solve equation (17) to obtain the Newton step equation:

[0103]

[0104]

[0105] In formula (20):

[0106] I is the identity matrix;

[0107] The derivative of the terminal output to the terminal state quantity;

[0108] is the derivative of the dynamics with respect to the state quantity;

[0109] is the derivative of the dynamics with respect to the control quantity;

[0110]

[0111]

[0112]

[0113] S505: Define X in equation (20) and equation (21) k+1 -X k =ΔX k , U k+1 -U k =ΔU k , therefore, the updated linear equation is formed. Solving equation (20) yields:

[0114]

[0115] In formula (25):

[0116]

[0117]

[0118] Equation (25) consists of two terms: the first term is evaluated by equation (26) and is completely dependent on the previous step, while the second term is related to the control vector U k+1 Linear correlation.

[0119] In formula (25):

[0120] C k is the control quantity coefficient matrix;

[0121] S506: Rewrite equation (20) into the following form:

[0122]

[0123] From equation (25), we can get node τ N The corresponding components, formula (28) can be simplified as:

[0124]

[0125] S507: Substitute equation (29) into equation (21) and after algebraic processing, we get:

[0126]

[0127] In formula (30):

[0128]

[0129] S508: The constraint given by equation (21) has been transformed into The linear equation of is shown in equation (30). In addition, take out the node τ from equation (25) N The corresponding component, then the updated terminal state vector can be rewritten as:

[0130]

[0131] S509: From equation (32), we can see that the updated terminal state vector X k+1 The updated terminal control vector U k +1 At the same time, the updated terminal state vector is constrained by equation (30). Therefore, applying equation (32) to equation (30), the discrete problem SP3 of determining the updated control vector can be expressed as:

[0132]

[0133] in:

[0134]

[0135] In formula (33):

[0136] The solution corresponding to the state The terminal output error;

[0137] B k is the intermediate matrix;

[0138] S5010: Equation (33) is an undetermined system, containing p equations and (N+1)m unknowns, (N+1)m>p. Therefore, an additional condition must be imposed to facilitate the solution. It can be obtained that the general performance index J is minimized, and

[0139]

[0140] In formula (35):

[0141] R(t) is the weight matrix under normal conditions;

[0142] R(τ) is the time domain from [t0,t f ] is converted to the weight matrix after [-1,1],

[0143] S5011: Using the ClenShaw-CurtiS orthogonal format, equation (35) can be expressed as:

[0144] J=U T RU (36)

[0145] In formula (36):

[0146] R is the orthogonal weight matrix, ω i (i=0,1,…,N) is a constant orthogonal coefficient;

[0147] S5012: Solve the constrained optimization problem of the objective function of equation (36) and the linear equality constraint of equation (33), and obtain the solution of the discrete problem SP3 as follows:

[0148] U k+1 =R -1 (B k ) T (B k R -1 (B k ) T ) -1 ·(ΔY+B k U k ) (37)

[0149] In formula (37):

[0150] B k is the intermediate matrix;

[0151] (B k ) T For B k The transpose of .

[0152] S6: Update the state vector using the state vector update formula;

[0153] The updated control vector U k+1 Substitute the solution into equation (25) to obtain the updated state vector X k+1 By repeating the above process, the updated control vector U can be subsequently updated k+1 and the updated state vector X k+1 The solution.

[0154] It will be apparent to those skilled in the art that the invention is not limited to the details of the exemplary embodiments described above and that the invention can be implemented in other forms without departing from the spirit or essential features of the invention. Therefore, the embodiments should be considered in all respects as exemplary and non-limiting, and the scope of the invention is defined by the appended claims rather than the foregoing description, and it is intended that all variations within the meaning and range of equivalents of the claims be included in the invention. Any reference numeral in a claim should not be considered as limiting the claim to which it relates.

[0155] In addition, it should be understood that although the present specification is described according to implementation modes, not every implementation mode contains only one independent technical solution. This description of the specification is only for the sake of clarity. Those skilled in the art should regard the specification as a whole. The technical solutions in each embodiment may also be appropriately combined to form other implementation modes that can be understood by those skilled in the art.

Claims

1. A MPSP trajectory design method based on generalized Newton iteration, characterized by: The method comprises the following steps: S1: Establish the dynamic model of the nonlinear system and discretize the state vector and control vector using Chebyshev discretization to transform the continuous time optimization problem into a discrete finite dimensional problem; S2: Generate the initial state vector X of the guess value 0 and the initial control vector U 0 , and set the number of nodes N and the exit conditions: output error δy and state error δX; S3: Calculate the state vector approximation and terminal output error Calculate the state vector error S4: Determine the terminal output error and the state vector error Whether all the exit conditions are met, that is, and If both are satisfied, the trajectory design is completed and the final state vector X is output. k And the final control vector U k , otherwise proceed to S5; S5: Update the control vector using the control vector update formula; The S5 comprises the following steps: S501: Update the control vector. The Newton method iteration formula for solving equation (17) is: s k+1 =s k +Δs k (18) Ψ(s k )+▽Ψ(s k )·Δs k =0 (19) In formula (18): Δs k is the Newton step of the kth iteration; ▽Ψ is the Jacobian matrix; S502: Determine the Newton step Δs of the kth iteration by solving the linear equation of formula (19): k ; S503: Obtain the updated next generation variable solution s from equation (18) k+1 ; S504: Iteratively solve equation (17) to obtain the Newton step equation: In formula (20): I is the identity matrix; The derivative of the terminal output to the terminal state quantity; is the derivative of the dynamics with respect to the state quantity; is the derivative of the dynamics with respect to the control quantity; S505: Solve equation (20) to obtain: In formula (25): In formula (25): C k is the control quantity coefficient matrix; S506: Rewrite equation (20) into the following form: From equation (25), we can get node τ N The corresponding components, formula (28) can be simplified as: S507: Substitute equation (29) into equation (21) and after algebraic processing, we get: In formula (30): S508: Updated terminal state vector can be rewritten as: S509: Apply equation (32) to equation (30), and determine the discrete problem SP3 of updating the control vector can be expressed as: in: In formula (33): The solution corresponding to the state The terminal output error; B k is the intermediate matrix; S5010: Minimize the general performance index J, and In formula (35): R(t) is the weight matrix under normal conditions; R(τ) is the time domain from [t0,t f ] is converted to the weight matrix after [-1,1], S5011: Using the ClenShaw-CurtiS orthogonal format, equation (35) can be expressed as: J=U T RU (36) In formula (36): R is the orthogonal weight matrix, ω i (i=0,1,...,N) is a constant orthogonal coefficient; S5012: Solve the constrained optimization problem of the objective function of equation (36) and the linear equality constraint of equation (33), and obtain the solution of the discrete problem SP3 as follows: U k+1 =R -1 (B k ) T (B k R -1 (B k ) T ) -1 ·(ΔY+B k U k ) (37) In formula (37): B k is the intermediate matrix; (B k ) T For B k The transpose of S6: Update the state vector using the state vector update formula; S7: Repeat S4.

2. The MPSP trajectory design method based on generalized Newton iteration according to claim 1, characterized in that: The S1 comprises the following steps: S101: Define the dynamics of a nonlinear system as follows: In formula (1): is the first-order derivative of X(t), is the state vector; is the control vector; t0 is the initial time; t f is the end time; X = [xhzv γ ψ]; x is the x coordinate of the aircraft in the airport coordinate system; h is the height of the aircraft; z is the z coordinate of the aircraft in the airport coordinate system; v is the speed of the aircraft; γ is the flight path angle of the aircraft; ψ is the heading angle of the aircraft; U = [α σ]; α is the angle of attack of the aircraft; σ is the roll angle of the aircraft; And define the dynamic initial state of the nonlinear system as follows: X(t0)=X 0 (2) In formula (2): X 0 is the initial state vector; S102: Establish the dynamic model of the nonlinear system as follows: In formula (3): L is the aerodynamic lift; D is the aerodynamic drag; g is the acceleration due to gravity; m is the mass of the aircraft; S103: Define the constraint-guided problem to be solved as: X(t0)=X 0 (6) and(X(t f ))=and d (7) In formula (7): X(t f ) is the terminal state vector; y d The desired end result; S104: Integrate both sides of equation (5) to transform the dynamics of the nonlinear system into an integral form as follows: S105: According to the Chebyshev polynomial, the physical time domain t∈[t0,t f ] is scaled to the time domain τ∈[-1,1] as follows: Therefore, formula (8) can be rewritten as: S106: The value of the unknown trajectory X(τ) at the CGL node is as follows: X=X 0 +E·F(X,U,X 0 ) (11) In formula (11): is in discrete state vector form; Represents the matrix form of the function f(X,U) at N+1 CGL nodes; is the control vector form; is a constant matrix, and In formula (13): The (k,j)th element is T j (τ k ) matrix; I n×n is the n×n dimensional identity matrix; The coefficient matrix A = S N ·T·V, is a diagonal matrix with diagonal elements {(1 / N),(2 / N),...,(2 / N),(1 / N)}, The (k,j)th item is T k (τ j ), T i (τ):=cos(i·cos -1 (τ)),i=0,1,…,N,S N is a constant matrix, and Therefore, the continuous dynamic equation of formula (3) is discretized into the algebraic equation of formula (11) at the CGL node; from formula (11), the terminal state X(τ N ): X(τ N )=X 0 +E N ·F(X,U,X 0 ) (15) In formula (15): is the terminal constant matrix; S107: Substituting equation (15) into equation (7), the end constraint can be rewritten as: y(X 0 +E N ·F(X,U,X 0 ))=y d (16) S108: That is, convert the continuous time optimization problem P0 into a discrete finite dimensional problem P1, that is, find the discrete state vector form X and the control vector form U according to equation (11) and equation (16); S109: define the solution to be solved as s, s = [X T ,U T ] T , rewrite the discrete finite-dimensional problem P1 into the finite-dimensional problem P2: the solution s satisfies the problem function Ψ(s) = 0,

Images