Method for trajectory planning of hypersonic vehicle based on N-K iteration convex optimization algorithm

Abstract

The application relates to a hypersonic vehicle trajectory planning method based on an N-K iterative convex optimization algorithm, and relates to the field of trajectory planning.The application is used to solve the problem that the existing vehicle trajectory planning method cannot accurately plan a vehicle trajectory.The application comprises the following steps: obtaining a dimensionless hypersonic gliding stage dynamics model;obtaining a linearized hypersonic gliding stage dynamics model;discretizing parameters in the linearized hypersonic gliding stage dynamics model to obtain a parameter-discretized hypersonic gliding stage dynamics model;obtaining control quantity constraints, process constraints and a trust region constraint in hypersonic vehicle trajectory optimization;constructing a trajectory planning problem based on iterative convex optimization by using the parameter-discretized hypersonic gliding stage dynamics model and the control quantity constraints, the process constraints and the trust region constraint;and solving the trajectory planning problem based on iterative convex optimization to obtain a hypersonic vehicle trajectory planning result.The application is used for planning a vehicle trajectory.

Description

A Hypersonic Vehicle Trajectory Planning Method Based on the NK Iterative Convex Optimization Algorithm Technical Field

[0001] This invention relates to the field of trajectory planning, and in particular to a trajectory planning method for hypersonic vehicles based on the NK iterative convex optimization algorithm. Background Technology

[0002] Hypersonic vehicles primarily refer to winged or wingless aircraft that fly in near-space, relying on aerodynamics to maintain long-distance, extended-duration flight within that space, and achieving speeds exceeding Mach 5. Currently, the development of hypersonic vehicles mainly focuses on two directions: one is cruise vehicles using scramjet engines to achieve hypersonic flight; the other is gliders that maneuver from outside or near-space and re-enter, relying on their high lift-to-drag ratio to achieve prolonged unpowered gliding within the atmosphere. Compared to traditional aircraft, boost-glide vehicles possess advantages such as higher speed, longer range, and greater maneuverability; they also have advantages such as simpler and more diverse launch platforms and relatively lower costs, making them highly strategically valuable in the military field.

[0003] The near-space environment in which hypersonic boost-glide vehicles operate is complex and atmospherically dense. During flight, their surfaces experience intense friction with the surrounding air, resulting in significant aerodynamic heating. Furthermore, structural and material limitations, along with the substantial impact of large maneuvers on terminal velocity and range, severely restrict the overload capacity of boost-glide vehicles during flight. Additionally, the complexities of real-world combat environments and the presence of anti-missile interception systems pose significant threats to the flight safety of hypersonic boost-glide vehicles. Therefore, planning the flight trajectory of boost-glide vehicles requires consideration of not only initial and terminal state constraints, but also process constraints such as dynamic pressure, overload, thermal flux, and control amplitude constraints, as well as no-fly zone constraints imposed by interception systems. This further complicates trajectory planning. Consequently, optimizing the trajectory design of hypersonic vehicles under complex constraints remains a pressing and challenging problem.

[0004] Aircraft trajectory optimization plays a crucial role in the overall design of aircraft and is one of the key technologies in the overall design process. Essentially, it is a highly nonlinear optimal control problem. That is, to find a trajectory that simultaneously satisfies various constraints and optimizes performance indicators to describe the aircraft's motion in space. Common performance indicators include: shortest arrival time, maximum terminal velocity, optimal fuel consumption, and maximum range. Trajectory optimization problems are currently solved primarily using numerical methods. Numerical methods are further divided into direct methods and indirect methods. The principle of indirect methods is to use Pondrigen's maximum principle to transform the optimization problem into a two-point boundary value problem, and then obtain the optimal solution by deriving the relationship between the control variables, state variables, and co-state variables. Commonly used indirect methods include the target-shooting method, the finite difference method, and the sequential gradient repair method. Direct methods first transform the optimal control problem into a nonlinear programming problem using parameterization, and then solve the transformed nonlinear programming problem using numerical methods. However, these methods suffer from high computational complexity, difficulty in handling constraints, the existence of only local optima, and poor convergence. Convex optimization methods have become a hot topic in trajectory optimization research in recent years due to their advantages such as low requirements for the accuracy of initial guesses, the ability to solve the problem in a finite time, the fact that the local optimum of a convex problem is also the global optimum, and the ability to provide clear criteria for infeasibility within a finite number of iterations. However, standard convex optimization problems only have a global optimum and are insensitive to initial values. Therefore, existing techniques use linear approximation and add confidence region constraints, which means that sequential convex optimization methods can only search for the optimum near the expansion point. This makes them very sensitive to initial values ​​and cannot guarantee obtaining a global optimum, resulting in low trajectory planning accuracy. Secondly, the subproblems after linearization are not equivalent to the original problem, and the approximation accuracy is closely related to the choice of the expansion point. If the expansion point is not chosen properly, the subproblems may deviate significantly from the original problem, or even fail to obtain a feasible solution, and it cannot be guaranteed that the solutions to the subproblems in the solution sequence are convergent, thus making trajectory planning impossible. Therefore, traditional convex optimization-based aircraft trajectory planning methods still have the problem of difficulty in accurately planning aircraft trajectories. Summary of the Invention

[0005] The purpose of this invention is to solve the problem that existing aircraft trajectory planning methods are still difficult to accurately plan aircraft trajectories, and to propose a hypersonic aircraft trajectory planning method based on the NK iterative convex optimization algorithm.

[0006] The hypersonic vehicle trajectory planning method based on the NK iterative convex optimization algorithm is as follows:

[0007] Step 1: Establish a dynamic model of the hypersonic glide segment and make the hypersonic glide segment dynamic model dimensionless to obtain a dimensionless hypersonic glide segment dynamic model;

[0008] Step 2: Linearize the hypersonic glide dynamics model obtained from the dimensionless processing in Step 1 to obtain the linearized hypersonic glide dynamics model;

[0009] Step 3: Discretize the parameters in the linearized hypersonic gliding dynamics model to obtain the parameter discretized hypersonic gliding dynamics model;

[0010] Step 4: Obtain the control constraints, process constraints, and trust region constraints in the hypersonic vehicle trajectory optimization.

[0011] Step 5: Using the hypersonic glide segment dynamic model after parameter discretization obtained in Step 3 and the control constraints, process constraints and trust region constraints obtained in Step 4, construct a trajectory planning problem based on iterative convex optimization, and solve the trajectory planning problem based on iterative convex optimization to obtain the trajectory planning results of the hypersonic vehicle.

[0012] Furthermore, the establishment of the hypersonic gliding dynamic model in step one, and the dimensionless transformation of the hypersonic gliding dynamic model to obtain the dimensionless hypersonic gliding dynamic model, specifically involves:

[0013]

[0014]

[0015] Where g0 is the gravitational acceleration, R0 is the Earth's radius, r is the dimensionless distance from the Earth's center, v is the dimensionless velocity of the spacecraft, γ is the track angle, λ is longitude, ψ is the heading angle, φ is latitude, and Ω is the velocity. v Ω γ Ω ψ It is an intermediate variable, ω e It is the dimensionless angular velocity of Earth's rotation. ρ is the Earth's rotational angular velocity before dimensionless calculation, ρ is the atmospheric density, m is the mass of the spacecraft, S is the cross-sectional area of ​​the spacecraft, and L is the aerodynamic lift of the spacecraft after dimensionless calculation. It is the aerodynamic lift of the spacecraft before dimensionless designation, C L It is the aerodynamic coefficient of aerodynamic lift. It is the aerodynamic drag of the aircraft after being dimensionless. C is the dimensionless aerodynamic drag of the aircraft. D It is the aerodynamic coefficient of aerodynamic drag. t' is the time before dimensionless transformation, t' is the time after dimensionless transformation, and σ is the aircraft's tilt angle. It is the velocity of the aircraft before it is dimensionless. It is the geocentric distance before dimensionless conversion.

[0016] Furthermore, in step two, the hypersonic gliding dynamics model obtained by the dimensionless processing in step one is linearized to obtain a linearized hypersonic gliding dynamics model, specifically as follows:

[0017] Step 2.1: Design the first control variable u:

[0018] u = [u1, u2] T

[0019]

[0020] Where u1 is the climb force coefficient and u2 is the turning force coefficient;

[0021] Step 22: Introduce the first control variable u designed in Step 21 to obtain the aircraft dynamics model, specifically as follows:

[0022]

[0023] a 14 =sinγ,a 15 =vcosγ

[0024]

[0025]

[0026] Where x = [r,λ,φ,v,γ,ψ], x is the state variable of the aircraft, and δx is the differential of x. yes The derivative of f(x,u) is an intermediate function. It is a partial derivative sign, δu is the differential of u, g(x), B(x), h(x) are intermediate variables, and a 14 a 15 a 21 a 23 a 24 a 25 a 26 a 31 a 34 a 35 a 36 a 41 a 44 a 45 a 51 a 54 a 55 a 61 a 63 a 64 a65 a 65 It is an intermediate variable. L r L v h is an intermediate variable. s It is an altitude constant;

[0027] Step Two and Three: Based on the aircraft dynamics model obtained in Step Two and Two, introduce the second independent variable τ' and the third control quantity u. t Thus, a linearized hypersonic gliding dynamics model is obtained.

[0028] Furthermore, the intermediate variables g(x), B(x), and h(x) are specifically as follows:

[0029]

[0030]

[0031] in, It is the aerodynamic drag of the aircraft after dimensionless scaling.

[0032] Furthermore, in steps two and three, the aircraft dynamics model obtained in step two and two introduces a second independent variable τ' and a third control quantity u. t Thus, a linearized hypersonic gliding dynamics model is obtained, specifically:

[0033] First, define the second independent variable τ′∈[0,1] and the duration u of the trajectory. t =t f -t0, reduces the time complexity of the trajectory optimization problem from the interval [t0, t... f Mapped to the interval [0,1], time t takes the following form:

[0034] t = t0 + (t f -t0)τ'

[0035] Where t0 is the starting time of trajectory planning, t f It is the time when the trajectory planning ends;

[0036] Then, the extended-dimensional state variables are obtained using time t. The aircraft dynamics model is rewritten as follows:

[0037]

[0038] The aircraft dynamics model is further rewritten as follows:

[0039]

[0040]

[0041] in, B1 and B2 are intermediate variables. yes The derivative with respect to time t, 0 1×2 It is a zero matrix;

[0042] Finally, based on the further rewritten aircraft dynamics model, a linearized hypersonic glide dynamics model is obtained:

[0043]

[0044] in, yes The differential, yes The differential, It is an intermediate variable.

[0045] Furthermore, the parameter discretization process in step three of the linearized hypersonic gliding dynamics model to obtain the parameter discretized hypersonic gliding dynamics model is specifically as follows:

[0046] Step 3: 1. Reduce the time interval of the trajectory optimization problem from [t0, t...] f Mapping the trajectory to the interval [-1, 1], we obtain the time after the trajectory mapping:

[0047]

[0048] Where τ is the time to map to the interval [-1, 1];

[0049] Step 3.2: Insert N time nodes within the time interval mapped to the [-1,1] interval. Use the time nodes and the end boundary point as interpolation points to obtain N+1 interpolation points. Use the Lagrange interpolation polynomial to approximate the state variables corresponding to each interpolation point to obtain approximate values ​​of the aircraft state variables, specifically:

[0050]

[0051] Where x(τ) is the approximate value of the spacecraft state quantity at the current time τ, and X(τ) is the Lagrange interpolation polynomial value at the current time τ. n ) is the time τ corresponding to the nth interpolation point. n The Lagrange interpolation polynomial value, L n(τ) is an intermediate variable, n = 0, ..., N, where n is the interpolation point number and N+1 is the total number of interpolation points. m τ is the time corresponding to the m-th interpolation point. n It is the time corresponding to the nth interpolation point;

[0052] Step 33: Based on the approximate values ​​of the aircraft state variables obtained in Step 32, obtain the discretized hypersonic gliding segment dynamic model.

[0053] Furthermore, in step three-three, the discretized hypersonic gliding dynamics model is obtained based on the approximate values ​​of the aircraft state variables obtained in step three-two, specifically as follows:

[0054] First, by differentiating the approximate values ​​of the aircraft's state variables, we obtain the following formula:

[0055]

[0056] Where j = 0, ..., N-1, j is the time node number, τ i τ is the time corresponding to the i-th interpolation point. j It is the time corresponding to the j-th time node, where i and l are the labels of the interpolation points, and D jn (τ j ) is an intermediate variable;

[0057] Then, the dynamic model of the hypersonic gliding segment after discretization is obtained:

[0058]

[0059] Where, f(x(τ) j ),u(τ j )) is an intermediate variable, u(τ) j ) is a control variable At time node τ j The value at that point, x(τ) n ), x(τ) j ) is an intermediate variable;

[0060] Finally, the dynamic model of the hypersonic gliding segment after discretization is simplified to the following form:

[0061]

[0062] Where Δt is the length of the equidistant interval, and D is D jn (τ j A matrix composed of ) It is the first state variable at time τ. It is an intermediate variable.

[0063] Furthermore, the acquisition of control constraints, process constraints, and trust region constraints in the hypersonic vehicle trajectory optimization in step four specifically includes:

[0064] Step 41: Introduce auxiliary variable u3 to obtain the control constraints in the hypersonic vehicle trajectory optimization, as follows:

[0065] (1) Introduce the auxiliary variable u3, and obtain the amplitude constraint of u3 as follows:

[0066] (u3) min ≤u3≤(u3) max

[0067]

[0068] Among them, (u3) min It is the lower bound of u3, (u3) max It is the upper bound of u3, u1 is the climb force coefficient, and u2 is the turning force coefficient;

[0069] The constraints on the climb force coefficient u1 are as follows:

[0070] u3cosσ max ≤u1≤u3cosσ min

[0071] The constraints on the turning force coefficient u2 are as follows:

[0072]

[0073] (2) The constraint on the rate of change of the tilt angle is as follows:

[0074]

[0075] in, The rate of change of the tilt angle, It is the minimum rate of change of the tilt angle. It is the minimum rate of change of the tilt angle, u1(τ) n ) is the interpolation point τ n The lift coefficient, u2(τ) n ) is the interpolation point τ n The turning force coefficient;

[0076] Step 42: Obtain process constraints in hypersonic vehicle trajectory optimization:

[0077] (1) Geocentric distance constraint:

[0078]

[0079] In the formula, For the first The lower bound of the geocentric distance corresponding to the spacecraft state variables obtained in the next iteration; According to the first The upper bound of the geocentric distance corresponding to the spacecraft state variables obtained in the next iteration;

[0080] (2) No-fly zone constraints:

[0081]

[0082] in, It is the first The state variable obtained in the iteration corresponds to the first... The coordinates of the no-fly zone, p w R represents the coordinates of the center point of the no-fly zone. w N is the radius of the no-fly zone. p It refers to the number of no-fly zones. It is a no-fly zone designation;

[0083] Step 43: Set trust region constraints:

[0084]

[0085] Where η is the trust region radius, and η has the same dimension as x. It is the first The aircraft state variables obtained in the next iteration, where x is the selected aircraft state variable.

[0086] Furthermore, in step five, the hypersonic gliding dynamics model obtained in step three after parameter discretization, and the control constraints, process constraints, and trust region constraints obtained in step four, are used to construct a trajectory planning problem based on iterative convex optimization. The trajectory planning result for the hypersonic vehicle is then obtained by solving the iterative convex optimization-based trajectory planning problem. Specifically:

[0087] First, let the optimization index function be the linear deviation index function J1, and solve the trajectory planning problem based on iterative convex optimization to obtain the aircraft state variables. and the first control quantity

[0088] The trajectory planning problem based on iterative convex optimization is as follows:

[0089]

[0090] Then, determine whether the iteration stopping condition J1 < ε1 is met. If the iteration stopping condition is met, stop the iteration and output. and If the stopping iteration condition is not met, then check if the preset number of iterations has been reached. If the preset number of iterations has been reached, then stop the iteration and output the result. and If the preset number of iterations has not been reached, then update. and Continue iterating;

[0091] renew and The following formula is used:

[0092]

[0093] in, It is the iteration number label. Here, ε is the total number of iterations, and ε is the iteration convergence evaluation metric. Indicates the first The state variables of the hypersonic aircraft in the next iteration. It is the first In the next iteration, the hypersonic vehicle state variables are δx, which is the differential of x, and Δt, which is the time difference. Indicates the first and The first control variable in the next iteration, δu, is the differential of u. This is the initial state of a hypersonic aircraft. x is the terminal state quantity of the hypersonic vehicle, and x0 is the initial state quantity of the hypersonic vehicle. f It is the final state quantity of a hypersonic vehicle, x min It is the lower bound of the aircraft state variables, x max It is the upper bound of the aircraft state variables, u2(τ) n+1 ) is τ n+1 The lift coefficient value at point u2(τ) n ) is τ n The lift coefficient value at point u1(τ) n ) is τ n The u1 value at that location, ε1 is the preset slack variable value, and ε1 is the first iteration convergence evaluation index value;

[0094] Then, the spacecraft state variables and the first control quantity We first set an initial value, then set the optimization index function to the state deviation index function J2, and then solve the trajectory planning problem based on iterative convex optimization to obtain the trajectory planning result of the hypersonic vehicle.

[0095] Furthermore, the optimization index function is set to the state deviation index function J2, and then the trajectory planning problem based on iterative convex optimization is solved, specifically as follows:

[0096] spacecraft state variables and the first control quantity As an optimization of J2 and We first determine the initial value, and then solve the following trajectory planning problem based on iterative convex optimization:

[0097]

[0098] Then, it checks whether the iteration stopping condition J2 < ε2 is met. If the stopping condition is met, the iteration stops and the solution is output. If the stopping condition is not met, it checks whether the preset number of iterations has been reached. If the preset number of iterations has been reached, the iteration stops. If the preset number of iterations has not been reached, the solution is updated. and Continue iterating:

[0099]

[0100] In the formula, As preset slack variable values, c1, c2, and q are constant coefficients. For SP2 The state variables obtained from the second iteration of optimization and SP1 are used to obtain the state variables of the first iteration. The difference in state variables obtained from each iteration, u t ε is the trajectory duration, and ε2 is the convergence evaluation index value of the first iteration.

[0101] The beneficial effects of this invention are as follows:

[0102] This invention first uses the Newton-Cantorovich method to linearize the hypersonic gliding dynamics equations; second, it uses Gaussian interpolation to discretize the hypersonic gliding dynamics equations, transforming an infinite-dimensional nonlinear programming problem into a finite-dimensional linear programming problem; third, it convexizes the process constraints; finally, it proposes a convex optimization method based on Newton-Cantorovich iteration and uses it to solve the trajectory optimization problem. This invention eliminates initial value sensitivity, can quickly converge to a feasible solution of the original problem, and can guarantee that the solutions obtained from solving the subproblems in the sequence are convergent. Therefore, this invention can quickly and accurately plan the trajectory of the aircraft. Attached Figure Description

[0103] Figure 1 shows the ground trajectory curve of the aircraft;

[0104] Figure 2 shows the velocity variation curve;

[0105] Figure 3 shows the height variation curve;

[0106] Figure 4 shows the angle of attack variation curve;

[0107] Figure 5 shows the curve of the change in the tilt angle;

[0108] Figure 6 shows the trajectory angle variation curve;

[0109] Figure 7 shows the dynamic pressure variation curve;

[0110] Figure 8 shows the heat flow variation curve;

[0111] Figure 9 shows the overload variation curve;

[0112] Figure 10 shows the comparison curves between the integral and the planned trajectory;

[0113] Figure 11 shows the curves comparing the integral and the planned height. Detailed Implementation

[0114] Specific Implementation Method 1: The specific process of the hypersonic vehicle trajectory planning method based on the NK iterative convex optimization algorithm in this implementation method is as follows:

[0115] Step 1: Establish a dynamic model for the hypersonic gliding segment and make the hypersonic gliding segment dynamic model dimensionless to obtain a dimensionless hypersonic gliding segment dynamic model, specifically as follows:

[0116] First, when using convex optimization methods for trajectory optimization, the influence of the J2 perturbation term is ignored. The dynamic model of the aircraft in the half-velocity frame can then be written as follows:

[0117]

[0118] in, All are about time The derivative of It represents the distance between the Earth's centers, λ is longitude, and φ is latitude. γ is the unit speed, γ is the track angle, and ψ is the heading angle. R0 = 6378 km is the Earth's rotational angular velocity, R0 = 6378 km is the Earth's radius, ρ is the atmospheric density, ρ0 is the atmospheric density at standard sea level, and h is the current altitude of the spacecraft. s =7110m is the altitude constant. It is the aerodynamic drag of the aircraft. σ is the aerodynamic lift of the aircraft, S is the roll angle of the aircraft, and C is the cross-sectional area of ​​the aircraft. L It is the aerodynamic coefficient of lift, C D C is the aerodynamic coefficient for aerodynamic drag. L C DIt can be expressed as a function of angle of attack and Mach number.

[0119] Then, to ensure calculation accuracy, the dynamic equations for the unpowered segment were dimensionless, using the geocentric distance before dimensionless variable transformation. velocity before dimensionless Earth's rotational angular velocity before dimensionless measurement Time before dimensionless After dimensionless processing, the dynamic model formula (1) of the aircraft is rewritten as follows:

[0120]

[0121] Where g0 is the gravitational acceleration, R0 = 6378 km is the Earth's radius, r is the dimensionless geocentric distance, v is the dimensionless spacecraft velocity, γ is the track angle, λ is longitude, ψ is the heading angle, φ is latitude, and Ω is the velocity. v Ω γ Ω ψ It is an intermediate variable, ω e It is the dimensionless angular velocity of Earth's rotation. ρ is the Earth's rotational angular velocity before dimensionless calculation, ρ is the atmospheric density, m is the mass of the spacecraft, S is the cross-sectional area of ​​the spacecraft, and L is the aerodynamic lift of the spacecraft after dimensionless calculation. It is the aerodynamic lift of the spacecraft before dimensionless designation, C L It is the aerodynamic coefficient of aerodynamic lift. It is the aerodynamic drag of the aircraft after being dimensionless. C is the dimensionless aerodynamic drag of the aircraft. D It is the aerodynamic coefficient of aerodynamic drag. t' is the time before dimensionless transformation, and t' is the time after dimensionless transformation. It is the velocity of the aircraft before it is dimensionless. It is the geocentric distance before dimensionless conversion.

[0122] Step 2: Linearize the hypersonic gliding dynamics model obtained from the dimensionless processing in Step 1 to obtain the linearized hypersonic gliding dynamics model, including the following steps:

[0123] Most algorithms for solving nonlinear differential equations are currently based on Newton's method or its variants. When solving a set of nonlinear differential equations, discretization and polynomial approximation are typically used first to obtain a set of equivalent algebraic equations. Then, Newton's method or its variants are used iteratively from given initial values ​​until a numerical solution with a certain accuracy is obtained. Using Newton's method requires calculating the Jacobian matrix of the equation system, which is tedious and time-consuming. To reduce the complexity of calculating the Jacobian matrix, this invention directly applies Newton's method to the original set of differential equations, rather than to the algebraic equations obtained using discretization and polynomial approximation. This method of directly applying Newton's method to differential equations and iteratively linearizing nonlinear differential equations is called the "Newton-Kontorovich (NK)" method.

[0124] Without loss of generality, assume the existence of a first-order differential equation:

[0125]

[0126] Suppose there exists an iteration variable s (k) It is an approximation of the true solution s of the differential equation. Then, by performing a Taylor expansion of equation (15) at s, we can obtain:

[0127]

[0128] Among them, f s This represents the derivative of the function f(s) with respect to s;

[0129] The recurrence relation for the next iteration is:

[0130]

[0131] Where f and f s Both are s = s (k) The value at time;

[0132] Define the iteration variable s (k) The difference between the solution s and the true solution s of the equation is δs:

[0133] δs=ss (k) (18)

[0134] The Frechet derivative is defined as:

[0135]

[0136] in, It is a scaling factor, F(s) is a nonlinear operator containing differentials or partial differentials, Fs δs is the Frechet derivative of the nonlinear operator F(s) with respect to s in the δs direction. s It is a linear operator;

[0137] Based on the above definition, the scaling factor can be... The Frechet derivative shifts from complex functional analysis to the simpler domain of solving calculus. Its importance lies in giving the linear terms in the generalized Taylor expansion of any nonlinear operator F(s), as follows:

[0138] F(s+δs)=F(s)+F s (s)δs+o[(δs) 2 (20)

[0139]

[0140] Combining equations (19), (20), and (21), we get:

[0141]

[0142] In the unpowered flight phase, angle of attack and bank angle are the control variables in the trajectory optimization problem. Observation reveals that the angle of attack is implicitly contained within the aerodynamic forces in the dynamic equations for the unpowered phase, while the bank angle exists as a trigonometric function. Therefore, if the angle of attack and bank angle are directly used as control variables, the corresponding control matrices will inevitably be highly nonlinear. This significantly increases the difficulty of linearizing the dynamic equations, and the solved control variables are prone to chattering after linearization. To simplify the problem, we consider introducing other variables as new control variables. The newly selected control variables should meet the following two conditions: 1) The control matrix of the new control variable can be written in a simple linear form with respect to the state variables; 2) There is a unique mapping relationship between the new control variable and the original control variables (angle of attack and bank angle), which can effectively reflect changes in the angle of attack and bank angle.

[0143] Step 2.1: Introduce the first control variable u and its components: lift coefficient u1 and turning force coefficient u2.

[0144] u = [u1, u2] T (twenty three)

[0145]

[0146] Step 22: Obtain the aircraft dynamics model based on the first control variable u introduced in Step 21:

[0147] The aircraft dynamics model is as follows:

[0148]

[0149]

[0150] Where g(x), B(x), and h(x) are intermediate variables, f(x,u) is an intermediate function, and x = [r,λ,φ,v,γ,ψ] is the aircraft state variable;

[0151] make Based on the derivation, we have:

[0152] F(x+δx,u+δu)=F(x,u)+F x (x,u)δx+F u (x,u)δu(26)

[0153]

[0154] Where δu is the differential of u, and δx is the differential of x. yes The derivative of F x (x,u)δx、F u (x,u)δu is an intermediate variable, and f(x+λδx,u) and f(x,u+λδu) are obtained through formula (25). It is the scaling factor;

[0155] Substituting equations (27) and (28) into equation (26), the aircraft dynamics model can be rewritten as follows:

[0156]

[0157]

[0158] in, L r L v It is an intermediate variable.

[0159] The trajectory optimization problem studied in this invention is considered a time-free two-point boundary value problem with a defined initial and final state. If control of the final time is required, the dimension of the dynamic equations can be extended, treating time as a new state variable and the final time as a new control variable.

[0160] Steps two and three: Introduce a new second independent variable τ' and a third control variable u. t Thus, a linearized hypersonic gliding dynamics model is obtained:

[0161] First, define a new second independent variable τ′∈[0,1] and the duration u of the trajectory. t =t f -t0, reduces the time complexity of the trajectory optimization problem from the interval [t0, t... f Mapped to the interval [0,1], time t takes the following form:

[0162] t = t0 + (t f -t0)τ'(30)

[0163] Where t0 is the starting time of trajectory planning, t f It is the time when the trajectory planning ends;

[0164] Then, the transformed time t is used as a new state variable to expand the dimension of the system equations, obtaining the expanded state variables. At this point, the aircraft dynamics model can be written as:

[0165]

[0166] Similarly, the dynamic model of an aircraft can also be written in the following form:

[0167]

[0168] in, B1 and B2 are intermediate variables. yes The derivative with respect to time t, 0 1×2 It is a zero matrix;

[0169] Thus, the linearized hypersonic gliding dynamics model is obtained, as shown in the following equation:

[0170]

[0171]

[0172] in, yes The differential, yes The differential, It is an intermediate variable.

[0173] Step 3: Discretize the parameters in the linearized hypersonic gliding dynamics model to obtain the parameter discretized hypersonic gliding dynamics model, including the following steps:

[0174] Step 3: 1. Reduce the time interval of the trajectory optimization problem from [t0, t...] f Mapping the trajectory to the interval [-1, 1], we obtain the time after the trajectory mapping:

[0175]

[0176] Where τ is the time mapped to the interval [-1, 1]; τ = -1 corresponds to the start time t0 of the trajectory, and τ = 1 corresponds to the end time t0 of the trajectory. f ;

[0177] Step 3.2: Insert N time nodes (LG nodes) within the time interval mapped to the [-1,1] interval to obtain N+1 interpolation points (time nodes and end boundary points). Use the Lagrange interpolation polynomial to approximate the state variables corresponding to each interpolation point to obtain approximate values ​​of the aircraft state variables, specifically:

[0178]

[0179] Where x(τ) is the approximate value of the spacecraft state quantity at the current time τ, and X(τ) is the Lagrange interpolation polynomial value at the current time τ. n ) is the time τ corresponding to the nth interpolation point. n The Lagrange interpolation polynomial value, L n (τ) is an intermediate variable, n is the interpolation point number, N+1 is the total number of interpolation points, and τ m τ is the time corresponding to the m-th interpolation point. n It is the time corresponding to the nth interpolation point, where n = 0, ..., N;

[0180] Step 33: Based on the approximate values ​​of the aircraft state variables obtained in Step 32, obtain the discretized hypersonic gliding phase dynamic model, specifically as follows:

[0181] First, differentiating formula (35) yields the following formula:

[0182]

[0183] Where j = 0, ..., N-1, j is the time node number, τ i τ is the time corresponding to the i-th interpolation point. j It is the time corresponding to the j-th time node, where i and l are the labels of the interpolation points, and D jn (τ j ) is an intermediate variable;

[0184] Then, the dynamic model of the hypersonic gliding segment after discretization is obtained:

[0185]

[0186] Formula (37) can be simplified to the following form:

[0187]

[0188] Among them, u(τ j ) is a control variable At time node τ j The value at point D, where Δt is the length of the equidistant interval (the time difference of each small segment), and D is D jn (τ j The matrix composed of x(τ) n ), x(τ) j The intermediate variable is obtained from formula (35). It is the first state variable at time τ, f(x(τ) j ),u(τ j )), f(x(τ), Obtained according to formula (25);

[0189] The control variables exist directly in the kinematic equations and do not have differential forms, so there is no need to use interpolation polynomials for approximation.

[0190] Step 4: Convexify the control and process constraints in the hypersonic vehicle trajectory optimization, and introduce trust region constraints:

[0191] Step 41: Introduce auxiliary variable u3 to obtain control constraints in hypersonic vehicle trajectory optimization:

[0192] Introducing auxiliary variables The auxiliary variable u3 is only related to the angle of attack and the aircraft's state, and is independent of the roll angle. Based on the range of values ​​for the state variables corresponding to the trajectory at the interpolation node and the angle of attack, the range of values ​​for u3 is determined; that is, the amplitude constraint of u3 can be expressed as:

[0193] (u3) min ≤u3≤(u3) max (38)

[0194] Among them, (u3) min It is the lower bound of u3, (u3) max It is the upper bound of u3, u1 is the climb force coefficient, and u2 is the turning force coefficient;

[0195] The range of values ​​for u1 can be determined based on the range of values ​​for the auxiliary variable u3 and the tilt angle, that is:

[0196] u3cosσmax ≤u1≤u3cosσ min (39)

[0197] Where, σ max It is the maximum tilt angle in the constraint, σ min It is the minimum tilt angle in the constraints;

[0198] By employing relaxation techniques, constraints on u2 can be obtained:

[0199]

[0200] In the process of trajectory optimization, in addition to constraining the magnitudes of the angle of attack and the roll angle, it is usually also necessary to constrain the rate of change of the roll angle:

[0201] Taking the derivative of u2 with respect to time, we get:

[0202] u2′=(u3sinσ)′=u3cosσσ′=u1σ′ (41)

[0203] Where σ′ is the derivative of the tilt angle σ;

[0204] At this point:

[0205]

[0206] in, The rate of change of the tilt angle, It is the minimum rate of change of the tilt angle. It is the minimum rate of change of the tilt angle, u1(τ) n ) is the interpolation point τ n The lift coefficient, u2(τ) n ) is the interpolation point τ n The turning force coefficient;

[0207] Through the above processing, the initial constraints on the angle of attack and the roll angle are transformed into constraints on the introduced new control variables. Furthermore, the constraints obtained through this process are all convex constraints.

[0208] Step 4.2: Perform convexity processing on the process constraints in the hypersonic vehicle trajectory optimization:

[0209] Dynamic pressure constraints, overload constraints, heat flux constraints, equilibrium gliding constraints, and no-fly zone constraints are all nonlinear functions of state variables, and none of them are convex functions. Therefore, when using convex optimization algorithms to solve trajectory optimization problems, convexification is required. Based on the altitude and velocity expressions of dynamic pressure constraints, overload constraints, and heat flux constraints, as well as the expression for equilibrium gliding constraints, we transform them into constraints on the distance from the Earth's center, which are abbreviated as:

[0210]

[0211] In the formula, For the first The lower bound of the geocentric distance corresponding to the spacecraft state variables obtained in the next iteration; According to the first The upper bound of the geocentric distance corresponding to the spacecraft state variables obtained in the next iteration;

[0212] The no-fly zone constraint is also non-convex, so it is linearized by transforming it into an affine function of the state variables. Taking a no-fly zone as an example, the specific form is:

[0213]

[0214] in, It is the first The state variable obtained in the iteration corresponds to the first... The coordinates of the no-fly zone, p w R represents the coordinates of the center point of the no-fly zone. w N is the radius of the no-fly zone. p It refers to the number of no-fly zones. It is a no-fly zone designation;

[0215] Step 4.3: To reduce the error after linearizing the dynamic equations, a trust region constraint is introduced, expressed as:

[0216]

[0217] Where η is the trust region radius, and η has the same dimension as x. It is the first The aircraft state variables obtained in the next iteration, where x is a manually selected aircraft state variable;

[0218] Step 5: Using the hypersonic gliding dynamics model obtained in Step 3 after parameter discretization and the constraints obtained in Step 4, construct a trajectory planning problem based on iterative convex optimization, and solve the trajectory planning problem based on iterative convex optimization to obtain the trajectory planning result of the hypersonic vehicle. Specifically:

[0219] Step 51: Perform a Taylor expansion on the hypersonic gliding dynamics model obtained in Step 3 after parameter discretization. Based on the hypersonic gliding dynamics model obtained in Step 3 after Taylor expansion and the constraints obtained in Step 4, construct a trajectory planning problem based on iterative convex optimization, specifically as follows:

[0220] When the condition is met Solve the following problem to minimize J:

[0221]

[0222] Then, determine whether the iteration stopping condition is met. If the iteration stopping condition is met, the iteration stops; if the stopping condition is not met, it is determined whether the preset number of iterations has been reached. If the preset number of iterations has been reached, the iteration stops; if the preset number of iterations has not been reached, the iteration continues according to the following update:

[0223]

[0224] in, It is the iteration number label. It is the total number of iterations. It is the first The changes in the state variables of the hypersonic aircraft in the next iteration, ε is a small quantity, is the evaluation index value for iterative convergence, and J is the optimization index function. Indicates the first The state variables of the hypersonic aircraft in the next iteration. It is the first The state variables of the hypersonic aircraft in the next iteration. It is the first In the next iteration, the hypersonic vehicle state variables are δx, which is the differential of x, and Δt, which is the time difference. Indicates the first and The first control variable in the next iteration, u is the first control variable, and δu is the derivative of u. This is the initial state of a hypersonic aircraft. x is the terminal state quantity of the hypersonic vehicle, and x0 is the initial state quantity of the hypersonic vehicle. f It is the final state quantity of a hypersonic vehicle, x min It is the lower bound of the aircraft state variables, x max It is the upper bound of the aircraft state variables, u2(τ) n+1 ) is τ n+1 The lift coefficient value at point u2(τ) n ) is τ n The lift coefficient value at point u1(τ) n ) is τ n The u1 value at that location.

[0225] Step 52: The main constraints to be considered for the aircraft during the gliding phase include: initial state constraints, terminal state constraints, dynamic pressure constraints, overload constraints, heat flux constraints, no-fly zone constraints, and control quantity constraints. Following the trajectory planning problem formula based on iterative convex optimization obtained in Step 51, and using time optimization as the optimization index, the trajectory optimization problem is solved to obtain the trajectory planning results for the hypersonic aircraft.

[0226] First, let the optimization index function be the linear deviation index function J1, and solve the trajectory planning problem based on iterative convex optimization to obtain... and Specifically as follows:

[0227]

[0228] Then, determine whether the iteration stopping condition J1 < ε1 is met. If the iteration stopping condition is met, stop the iteration and output x. (k) and If the stopping iteration condition is not met, check if the preset iteration count has been reached. If the preset iteration count has been reached, stop the iteration; otherwise, update. and Continue iterating:

[0229]

[0230] Then, the spacecraft state variables and the first control quantity Initial values ​​are set, then the optimization index function is set to the state deviation index function J2, and then the trajectory planning problem based on iterative convex optimization is solved to obtain the trajectory planning result of the hypersonic vehicle, specifically:

[0231]

[0232] Then, it checks whether the iteration stopping condition J2 < ε2 is met. If the stopping condition is met, the iteration stops and the solution is output. If the stopping condition is not met, it checks whether the preset number of iterations has been reached. If the preset number of iterations has been reached, the iteration stops. If the preset number of iterations has not been reached, the solution is updated. and Continue iterating:

[0233]

[0234] In the formula, As preset slack variable values, c1, c2, and q are constant coefficients. For SP2 The state variables obtained from the second iteration of optimization and SP1 are used to obtain the state variables of the first iteration. The difference in state variables obtained from each iteration, u t ε is the trajectory duration, and ε2 is the convergence evaluation index value of the first iteration.

[0235] Example: To verify the beneficial effects of the present invention, the following simulation experiments were conducted:

[0236] The purpose of the first step SP1 is to ensure that the linearization error of the dynamic equation is relatively small, and to obtain a preliminary feasible solution that satisfies the constraints under a set of poor initial values.

[0237] The second step, SP2, introduces optimization metrics and ensures convergence speed. To guarantee continuity, the planned terminal state of the pull-up segment is used as the initial value for the gliding segment optimization. The simulation parameters are shown in Table 1.

[0238] Table 1 Simulation Parameters

[0239]

[0240] The confidence region constraint is set as follows:

[0241]

[0242] The convergence condition is set as follows:

[0243]

[0244] Simulation results are shown in Figures 1-11. The results demonstrate that the convex optimization method based on Newton-Kontorovich iteration can achieve trajectory optimization that satisfies complex constraints such as dynamic pressure constraints, overload constraints, thermal flux constraints, no-fly zone constraints, initial state constraints, and terminal state constraints. The initial guessed gliding time was 1156.8 s, while the optimized time was 1053 s, indicating that a time-optimal solution was obtained. Furthermore, the angle of attack and roll angle remained continuously changing throughout the process without abrupt changes, reducing the control complexity of the control system.

[0245] It can be seen that after interpolating the control quantity generated by the convex optimization method, the result obtained by integration can be in good agreement with the result generated by the convex optimization. Therefore, it can be proved that the accuracy of the result obtained by this method can be demonstrated.

Claims

1. A hypersonic vehicle trajectory planning method based on the NK iterative convex optimization algorithm, characterized in that... The specific process of the method is as follows: Step 1: Establish a hypersonic gliding segment dynamic model and make the hypersonic gliding segment dynamic model dimensionless to obtain a dimensionless hypersonic gliding segment dynamic model; Step 2: Linearize the hypersonic gliding segment dynamic model after the dimensionless processing in Step 1 to obtain a linearized hypersonic gliding segment dynamic model, specifically: Step 21: Design the first control variable. ： in, It is the lift coefficient. It is the turning force coefficient. It is the aerodynamic coefficient of aerodynamic lift. It is the aircraft's tilt angle; Step 22, introduce the first control variable designed in Step 21. Obtain the aircraft dynamics model, specifically: in, ， These are aircraft state variables. yes The differential, yes The derivative of It is an intermediate function. It is the partial derivative sign. yes The differential, 、 、 It is an intermediate variable. 、 、 、 、 、 、 、 、 、 、 、 、 、 、 、 、 、 、 、 、 、 It is an intermediate variable. 、 、 、 It is an intermediate variable. It is an altitude constant. It is the aerodynamic drag of the aircraft after being dimensionless. It is the dimensionless geocentric distance. It is the dimensionless speed of the aircraft. It is the track angle. It's longitude. It is the heading angle. It's latitude. It is the aerodynamic drag of the aircraft after being dimensionless. It is the dimensionless aerodynamic drag of the aircraft. It is the aerodynamic coefficient of aerodynamic drag. It is the aerodynamic coefficient of aerodynamic lift. It is the aerodynamic lift of the aircraft after dimensionless transformation. It is the Earth's radius. For the mass of the aircraft, It is the cross-sectional area of ​​the aircraft. It is atmospheric density. 、 、 It is an intermediate variable; Step 2 and 3: Based on the aircraft dynamics model obtained in Step 2, a second independent variable is introduced. and third control quantity Thus, a linearized hypersonic gliding dynamics model is obtained, specifically: First, the second independent variable is defined. Duration of the trajectory This reduces the time complexity of the trajectory optimization problem from intervals. Mapped to In the interval, time It is in the following form: in, It is the start time of trajectory planning. It is the time when the trajectory planning ends; then, time is used. Obtain extended-dimensional state variables The aircraft dynamics model is rewritten as follows: The aircraft dynamics model is further rewritten as follows: in, 、 、 、 、 、 It is an intermediate variable. yes The derivative with respect to time t, It is a zero matrix; finally, based on the further rewritten aircraft dynamics model, the linearized hypersonic gliding dynamics model is obtained: in, yes The differential, yes The differential, 、 These are intermediate variables; Step 3: Discretize the parameters in the linearized hypersonic glide segment dynamic model to obtain the parameter discretized hypersonic glide segment dynamic model; Step 4: Obtain the control constraints, process constraints, and trust region constraints in the hypersonic vehicle trajectory optimization; Step 5: Use the parameter discretized hypersonic glide segment dynamic model obtained in Step 3 and the control constraints, process constraints, and trust region constraints obtained in Step 4 to construct a trajectory planning problem based on iterative convex optimization, and solve the trajectory planning problem based on iterative convex optimization to obtain the hypersonic vehicle trajectory planning result.

2. The hypersonic vehicle trajectory planning method based on the NK iterative convex optimization algorithm according to claim 1, characterized in that: The step one, establishing a hypersonic gliding dynamic model and making it dimensionless to obtain a dimensionless hypersonic gliding dynamic model, specifically involves: in, It is gravitational acceleration. It is the Earth's radius. It is the dimensionless geocentric distance. It is the dimensionless speed of the aircraft. It is the track angle. It's longitude. It is the heading angle. It's latitude. 、 、 It is an intermediate variable. It is the dimensionless angular velocity of Earth's rotation. It is the Earth's rotational angular velocity before it was dimensionless. It is atmospheric density. For the mass of the aircraft, It is the cross-sectional area of ​​the aircraft. It is the aerodynamic lift of the aircraft after dimensionless transformation. It is the aerodynamic lift of the pre-dimensional undefined aircraft. It is the aerodynamic coefficient of aerodynamic lift. It is the aerodynamic drag of the aircraft after being dimensionless. It is the dimensionless aerodynamic drag of the aircraft. It is the aerodynamic coefficient of aerodynamic drag. It is time before it is dimensionless. It is the dimensionless time. It is the aircraft's tilt angle. It is the velocity of the aircraft before it is dimensionless. It is the geocentric distance before dimensionless conversion.

3. The hypersonic vehicle trajectory planning method based on the NK iterative convex optimization algorithm according to claim 2, characterized in that: The discretization of parameters in the linearized hypersonic gliding dynamics model in step three, to obtain the parameter discretized hypersonic gliding dynamics model, specifically involves: Step three, firstly, changing the time of the trajectory optimization problem from an interval... Mapped to The interval is used to obtain the time after trajectory mapping. in, It is mapped to The time interval; Step 32, mapping to Insertion within the time interval Using time nodes and the end boundary point as interpolation points, we obtain... For each interpolation point, the state variables corresponding to that point are approximated using a Lagrange interpolation polynomial to obtain approximate values ​​for the aircraft's state variables. Specifically: in, It is the current moment. Approximate values ​​of the spacecraft's state variables at any given time. It is the current moment. The Lagrange interpolation polynomial value at time t. It is the time corresponding to the nth interpolation point. The Lagrange interpolation polynomial value, It is an intermediate variable. Where n is the interpolation point number, and N+1 is the total number of interpolation points. It is the time corresponding to the m-th interpolation point. It is the time corresponding to the nth interpolation point; Step 33: Based on the approximate values ​​of the aircraft state variables obtained in Step 32, obtain the discrete hypersonic gliding segment dynamic model.

4. The hypersonic vehicle trajectory planning method based on the NK iterative convex optimization algorithm according to claim 3, characterized in that: The step 33, which obtains the discretized hypersonic glide dynamics model based on the approximate values ​​of the aircraft state variables obtained in step 32, specifically involves: First, differentiating the formulas for the approximate values ​​of the aircraft state variables to obtain the following formula: in, ， It is a time node number. It is the time corresponding to the i-th interpolation point. It is the time corresponding to the j-th time node. 、 These are the labels of the interpolation points. These are intermediate variables; then, the discretized dynamic model of the hypersonic gliding segment is obtained: in, It is an intermediate variable. It is a control variable At the time point The value at that location, 、 These are intermediate variables; finally, the discretized hypersonic gliding dynamics model is simplified to the following form: in, It is the length of the equidistant interval. yes The matrix formed That was at that moment The first state quantity at time t. It is an intermediate variable.

5. The hypersonic vehicle trajectory planning method based on the NK iterative convex optimization algorithm according to claim 4, characterized in that: Step four, obtaining the control constraints, process constraints, and trust region constraints in the hypersonic vehicle trajectory optimization, specifically involves: Step four one, introducing auxiliary variables. To obtain the control constraints in the trajectory optimization of hypersonic vehicles, the following is done: (1) Introduce auxiliary variables , obtain The amplitude constraint is: in, yes The lower bound, yes The upper realm, It is the lift coefficient. It is the turning force coefficient; the climbing force coefficient. The constraints are as follows: Turning force coefficient The constraints are as follows: (2) The constraint on the rate of change of the tilt angle is as follows: in, The rate of change of the tilt angle, It is the minimum rate of change of the tilt angle. It is the maximum rate of change of the tilt angle. Interpolation point The climbing force coefficient, Interpolation point The turning force coefficient; Step 4.2, Obtaining process constraints in hypersonic vehicle trajectory optimization: (1) Geocentric distance constraint: In the formula, For the first The lower bound of the geocentric distance corresponding to the spacecraft state variables obtained in the next iteration; According to the first (2) No-fly zone constraints: The upper bound of the geocentric distance corresponding to the aircraft state variables obtained in the next iteration; in, It is the first The state variable obtained in the iteration corresponds to the first... The coordinates of the no-fly zone The coordinates of the center point of the no-fly zone are: The radius of the no-fly zone. It refers to the number of no-fly zones. It is the no-fly zone designation; Step 43, Set trust region constraints: in, It is the trust region radius. and Same dimensions It is the first The spacecraft state variables obtained in the next iteration It is the selected aircraft state quantity.

6. The hypersonic vehicle trajectory planning method based on the NK iterative convex optimization algorithm according to claim 5, characterized in that: Step five involves constructing a trajectory planning problem based on iterative convex optimization using the hypersonic gliding dynamics model discretized from the parameters obtained in step three, and the control constraints, process constraints, and trust region constraints obtained in step four. Solving this iterative convex optimization trajectory planning problem yields the trajectory planning result for the hypersonic vehicle. Specifically, the optimization index function is set to a linear deviation index function. The trajectory planning problem based on iterative convex optimization is solved to obtain the spacecraft state variables. and the first control quantity The trajectory planning problem based on iterative convex optimization is as follows: Then, determine whether the iteration stopping condition is met. If the iteration stopping condition is met, then stop the iteration and output. and ； If the stopping condition is not met, check if the preset number of iterations has been reached. If the preset number of iterations has been reached, stop the iteration and output the result. and ； If the preset number of iterations has not been reached, then update. and Continue iterating; update and The following formula is used: in, It is a constant coefficient. It is the iteration number label. Indicates the first The state variables of the hypersonic aircraft in the next iteration. It is the first The state variables of the hypersonic aircraft in the next iteration. yes The differential, It's a time difference. 、 Indicates the first and The first control variable in the next iteration. yes The differential, It is the initial state quantity of a hypersonic vehicle. It is the terminal state quantity of the spacecraft. For the initial state quantities of a hypersonic vehicle, It is the final state quantity of a hypersonic aircraft. It is the lower bound of the aircraft's state variables. It is the upper bound of the aircraft's state variables. yes The lift coefficient value at that location, yes The lift coefficient value at that location, yes place value, It is a preset slack variable value. This is the convergence evaluation index value of the first iteration; then, the spacecraft state variables... and the first control quantity As an optimization In and The initial value is then set, and the optimization index function is then let be the state deviation index function. Then, the trajectory planning problem based on iterative convex optimization is solved to obtain the trajectory planning results of the hypersonic vehicle.

7. The hypersonic vehicle trajectory planning method based on the NK iterative convex optimization algorithm according to claim 6, characterized in that: The optimization index function is described as the state deviation index function. Then, the trajectory planning problem based on iterative convex optimization is solved, specifically: the spacecraft state variables are... and the first control quantity As an optimization In and We first determine the initial value, and then solve the following trajectory planning problem based on iterative convex optimization: Then, determine whether the iteration stopping condition is met. If the iteration stopping condition is met, the iteration stops and the solution is output; if the stopping condition is not met, it checks if the preset number of iterations has been reached. If the preset number of iterations has been reached, the iteration stops; otherwise, the solution is updated. and Continue iterating: In the formula, To preset slack variable values, and The constant coefficient, For SP2 The state variables obtained from the second iteration of optimization and SP1 are used to obtain the state variables of the first iteration. The difference in state variables obtained from each iteration of optimization It is the duration of the trajectory. It is the convergence evaluation index value of the second iteration.

Images