Rocket recovery trajectory planning method based on improved model prediction static planning algorithm

Abstract

The application discloses a rocket recovery trajectory planning method based on an improved model prediction static programming algorithm, the rocket dynamics equation is calculated by using a second-order Pic iteration format, so that state quantities at different moments are independent of each other and state quantity expressions explicitly contain control quantities; non-equidistant scattering is carried out based on Chebyshev polynomials to improve the scattering precision; the MPSP algorithm is used to iteratively solve the optimal control problem after scattering, and a projection method is introduced in the solving process to process the thrust constraint. The application significantly improves the calculation efficiency of the MPSP algorithm and can solve the online trajectory planning problem of the rocket recovery containing complex constraints.

Description

Technical Field

[0001] This invention belongs to the field of guidance technology, specifically relating to a rocket recovery trajectory planning method based on an improved model prediction static programming algorithm. Background Technology

[0002] Reusable launch vehicles can significantly reduce launch costs and launch cycles, making them an important research direction for future aircraft. Vertical rocket recovery is a crucial method for achieving launch vehicle reuse, and guidance and control algorithms are one of the key technologies for rocket recovery. The vertical rocket recovery problem can generally be described as a complex, multi-constraint optimal control problem. The recovery of rocket stages requires both deceleration and adjustment under thrust constraints, and also must meet the state constraints (position, velocity, attitude) for a vertical, fixed-point landing. Solution methods for this type of optimal control problem can generally be divided into two categories: indirect methods and direct methods. Indirect methods utilize variational methods and Pontryagin's maximum (minimum) principle to derive the first-order necessary conditions of the optimal control problem, thus transforming it into a two-point or multi-point boundary value problem for solution. The advantages of indirect methods are high accuracy and fast solution speed, but they also suffer from being extremely sensitive to initial conjectures and having cumbersome derivations of the first-order necessary conditions for complex models. The direct method transforms the optimal control problem into a nonlinear programming problem through discrete methods. Since it does not require the derivation of first-order necessary conditions and is easy to handle complex constraint problems, it is widely used in trajectory planning problems.

[0003] In recent years, the model predictive static programming (MPSP) algorithm in the direct method has demonstrated good computational efficiency and accuracy in various scenarios such as missile terminal guidance, reentry guidance, and launch vehicle ascent phase. However, this method still has some drawbacks: 1) As the number of discrete points increases, the dimensionality of the variables to be solved increases, significantly reducing the computational speed; 2) The dynamic equations in the MPSP algorithm are discretized using the Euler method, which reduces the discretization accuracy; 3) The MPSP algorithm involves solving the Jacobian matrix, and the recursive solution of the Jacobian matrix and the stepwise update of the state sequence both increase the computational time. Therefore, the computational efficiency of the MPSP algorithm still needs further improvement. Summary of the Invention

[0004] To overcome the shortcomings of existing technologies, this invention provides a rocket recovery trajectory planning method based on an improved model predictive static programming algorithm. This invention employs a second-order Picard iterative scheme to calculate the rocket dynamics equations, ensuring that the state variables at different times are independent and that the state variable expressions explicitly include control variables. It uses Chebyshev polynomials for non-equidistant discretization to improve discretization accuracy. The MPSP algorithm is used iteratively to solve the discretized optimal control problem, and a projection method is introduced during the solution process to handle thrust constraints. This invention significantly improves the computational efficiency of the MPSP algorithm and can solve online trajectory planning problems for rocket recovery with complex constraints.

[0005] The technical solution adopted by this invention to solve its technical problem includes the following steps:

[0006] Step 1: Establish the fixed coordinate system and three-degree-of-freedom dynamic equations for recovery;

[0007] Step 1-1: The origin of the recovery fixed coordinate system is fixed to the landing point O. The Ox axis of the recovery fixed coordinate system points to the initial position of the rocket in the horizontal plane of the landing point and is positive. The Oy axis of the recovery fixed coordinate system is perpendicular to the horizontal plane of the landing point and points upward. The Oz axis of the recovery fixed coordinate system is perpendicular to the xOy plane of the recovery fixed coordinate system and forms a right-handed coordinate system.

[0008] Steps 1-2: Assuming the Earth's surface is a plane during rocket recovery, and neglecting entrainment acceleration and Coriolis acceleration, the dynamic equations are as follows:

[0009]

[0010] In the formula: r is the position vector, V is the velocity vector, m is the mass, and I is the velocity vector. sp For specific impulse, g0 is the gravitational acceleration at sea level; T is the thrust vector, aligned with the rocket's longitudinal axis; F A For axial aerodynamic force, g(r) is the gravitational acceleration;

[0011] Steps 1-3: The aerodynamic forces acting on the rocket during flight are represented in the rocket body coordinate system, with the rocket's center of mass as the origin. The Ox axis of the rocket body coordinate system points along the body axis towards the rocket's nose. The Oy axis of the rocket body coordinate system is perpendicular to the Ox axis of the rocket body coordinate system in the rocket's principal plane of symmetry. The Oz axis of the rocket body coordinate system is perpendicular to the xOy plane of the rocket body coordinate system, forming a right-handed coordinate system. Only the axial aerodynamic force F is considered. A Its numerical expression is:

[0012]

[0013] In the formula: ρ is the atmospheric density, v is the velocity magnitude, S is the reference area, and C A Normal aerodynamic coefficient;

[0014] Gravitational acceleration is expressed as:

[0015]

[0016] In the formula: μ is the Earth's gravitational constant, and r is the distance from the rocket to the Earth's center;

[0017] During recovery, the thrust vector T is subject to amplitude constraints:

[0018] T min ≤T≤T max (4)

[0019] In the formula, T min T max These represent the lower and upper limits of the thrust amplitude, respectively.

[0020] Meanwhile, the terminal constraints of the recycling process are:

[0021]

[0022] In the formula: r f V f These are the position and velocity of the rocket's flight terminal, respectively. Let m be the desired terminal position and velocity, respectively. f For the mass of the flight terminal, m dry For the dry weight of the rocket;

[0023] Steps 1-4: Equations (1) to (5) describe the optimal control problem as follows:

[0024]

[0025] in For state variables, To control the quantity, ψ is the pitch angle; y is the yaw angle, which adjusts the thrust direction; η is the throttle ratio, which adjusts the thrust magnitude. For output quantity, For the desired terminal state, t f For the terminal moment of rocket flight, g(x(t),u(t)) represents the dynamic equation of the velocity term;

[0026] Step 2: Set the real time t∈[t0,t... f Transform to virtual time τ∈[-1,1], and establish the optimal control problem P0 for rocket recovery trajectory planning;

[0027] Let the real time t∈[t0,t f The conversion to virtual time τ∈[-1,1] is specifically formulated as follows:

[0028]

[0029] In the formula: τ is the virtual time range;

[0030] The optimal control problem P0 is defined as follows:

[0031]

[0032] In the formula: κ is the time transformation parameter for transforming the dynamic equations to the virtual time domain. Represents the state quantity at the initial moment;

[0033] Step 3: Using Chebyshev polynomials as basis functions, perform non-equidistant discretization on the Picard iterative scheme of dynamic equation (1) to obtain the discretized optimal control problem P1;

[0034] Step 3-1: Obtain the natural second-order system Picard iterative scheme of the rocket recovery dynamics equations;

[0035] Based on equation (8), the equivalent integral equation of the rocket recovery dynamics equation is:

[0036]

[0037] In the formula: These are the initial position and initial velocity, respectively.

[0038] The Picard iteration scheme for the k-th iteration of the natural second-order system of equation (9) is:

[0039]

[0040] Step 3-2: Introduce Chebyshev polynomials as basis functions, select CGL points as collocation points, approximate the integrand term on the right side of the velocity equation, and then integrate term by term to achieve the discretization of equation (10);

[0041] Select N+1 discrete points and define the discrete time series as τ=[τ0,τ1,…,τ N ] T The discrete state sequence is Discrete control sequence is The discretized dynamic equations are then expressed as:

[0042]

[0043] In the formula: The velocity vector is obtained after the k-th iteration of discretization. Let X be the position vector obtained after the k-th iteration of discretization. k Let κ = (t) be the discrete state sequence of the k-th iteration, i = 0, 1, ..., N be the collocation sequence, k = 1, 2, ... be the iteration number, and κ = (t)f -t0) / 2 is the time transformation parameter; A=RKCL, R,K,C,L all represent matrix parameters of the discretization process, C is determined by the basis function Chebyshev polynomial, C [i+1,·] This represents the (i+1)th row of the matrix; when the number of discrete points is determined, C and A are constant matrices; G(X) k U) represents the matrix form of the dynamic equations at N+1 discrete nodes, specifically:

[0044]

[0045] Based on the discretized dynamic equation (12), the optimal control problem P1 is:

[0046]

[0047] Step 4: For problem P1, the state sequence X k Determined by the control sequence U, its constraints are ultimately transformed into terminal nonlinear constraints. Based on this, the control sequence U is solved with respect to the control sequence U in the kth iteration. k Linearization yields an approximate optimal control problem P2;

[0048] Step 4-1: In problem equation (13), the state variables are calculated in parallel using the discretized dynamic equations. The state variable x at the terminal time is... N It is represented by equation (11), and the state sequence X k The unknowns are the control sequence U and the terminal time t. f Therefore, terminal constraints Transform into:

[0049] Φ(U,t f )=0 (14)

[0050] Equation (14) is about the variable to be solved, U,t f Highly nonlinear equations;

[0051] Step 4-2: Solve equation (14) for the kth iteration. k , Linearization is performed at this point, resulting in the linearized solution equation:

[0052]

[0053] In the formula, Φ′(U k ), These are the terminal constraints with respect to the control sequence U and the terminal time t, respectively. f The Jacobian matrix is ​​defined as:

[0054]

[0055] Therefore, problem P0 is ultimately transformed into problem P2:

[0056]

[0057] Step 5: Solve equation (17) using the model-based predictive static programming algorithm, and derive the Jacobian matrix Φ′(U k and the variable to be solved U k+1 ,

[0058] Step 5-1: In step 4, the problem of rocket recovery is finally transformed into solving problem (17). For equation (17), the model prediction static programming algorithm is used to solve it.

[0059] Considering the L2 norm of the controllable change and the adjustability of the terminal time, the following performance indicators are selected:

[0060]

[0061] In the formula, W is the weight matrix; under the condition of performance index (18), the update iteration of control quantity and terminal time is as follows:

[0062]

[0063]

[0064] The updated expression for the variable to be solved is:

[0065]

[0066] Step 5-2: In each iteration of the model prediction static programming algorithm, a projection method is introduced to handle the thrust inequality constraint, improving computational efficiency. The thrust amplitude constraint is actually a constraint on the throttling ratio η that adjusts the thrust magnitude, i.e.:

[0067]

[0068] Step 6: Set the initial guess X 0 U 0 The process involves iterative updates until the terminal error condition is met, thereby generating guidance commands and completing the trajectory planning and solution task for rocket recovery.

[0069] Furthermore, R, K, C, L are constant matrices, and the specific calculation formulas are equations (23) to (26):

[0070]

[0071]

[0072]

[0073]

[0074] In the formula: the formula for calculating the elements of the first row of matrix K is:

[0075]

[0076] In the formula: T i (τ) and τ i The calculation formula is:

[0077] τ i =-cos(iπ / N),i=0,1,…,N (28)

[0078] T i (τ)=cos(i·cos -1 (τ)), i=0,1,…,N (29)

[0079] Furthermore, W is a weight matrix, specifically:

[0080]

[0081] When N is even

[0082]

[0083] When N is odd

[0084]

[0085] Furthermore, the specific process of iterative update in step 6 is as follows:

[0086] Step 6-1: Let the number of iterations k = 0, given the initial conjecture X of the state sequence and control sequence. 0 U 0 , And let X k =X 0 U k =U 0 ,

[0087] Step 6-2: During the k-th iteration, based on equations (19) to (20), calculate the increment dU of the control quantity and the terminal time. k ,

[0088] Step 6-3: Update control variable and terminal time U k+1 , Introduce thrust constraints (22) and calculate the updated values.

[0089] Step 6-4: Determine if the convergence condition is met: If the condition is met, the iteration stops, and the current solution is the desired one; if the condition is not met, return to step 6-2 and iterate until the condition is met.

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

[0091] This invention combines Picard iteration with model predictive static programming. Through second-order Picard iteration and discretization using Chebyshev as the basis function, the performance indicators and dynamic equations are transformed to obtain a discrete-form optimal control problem for rocket recovery. Throughout the process, the state variables at discrete points are independent, simplifying both the state sequence update process and the Jacobian matrix calculation, thus improving the computational efficiency of the model predictive static programming algorithm. Based on this, the model predictive static programming algorithm is used iteratively to solve the linearized optimal control problem, and a projection method is employed to constrain the throttling ratio η, achieving thrust inequality constraints. According to the algorithm of this invention, explicit expressions for the variables to be solved can be obtained, making the iterative formula both concise and efficient, and capable of handling thrust boundary constraints, thereby significantly improving computational efficiency. Attached Figure Description

[0092] Figure 1 This is a flowchart of the method of the present invention.

[0093] Figure 2 The curves showing the change of the rocket's position in each direction over time are shown in the embodiments of the present invention.

[0094] Figure 3 The curves showing the velocity of the rocket in each direction over time are shown in the embodiment of the present invention.

[0095] Figure 4 The image shows the thrust of a rocket as a function of time in an embodiment of the present invention.

[0096] Figure 5 This is a curve showing the change in position error as a function of the number of discrete points in an embodiment of the present invention.

[0097] Figure 6 This is a curve showing the change in speed error with the number of discrete points in an embodiment of the present invention.

[0098] Figure 7 The curve showing the change in calculation time as a function of the number of discrete points in an embodiment of the present invention.

[0099] Figure 8 The curve showing the change in calculation time as a function of the number of discrete points in some embodiments of the present invention is shown. Detailed Implementation

[0100] The present invention will be further described below with reference to the accompanying drawings and embodiments.

[0101] The technical problem solved by this invention is to provide a rocket recovery trajectory optimization method based on an improved model prediction static programming algorithm to address the shortcomings of existing technologies. This method significantly improves the efficiency of optimization algorithms with multiple constraints and nonlinear dynamic equation constraints.

[0102] A rocket recovery trajectory planning method based on an improved model-predictive static programming algorithm includes the following steps:

[0103] 1. Establish the fixed coordinate system and three-degree-of-freedom dynamic equations for recovery;

[0104] The origin of the fixed coordinate system is fixed to the landing point O. The Ox axis points to the initial position of the rocket in the horizontal plane of the landing point and is positive. The Oy axis is perpendicular to the horizontal plane of the landing point and points upward. The Oz axis is perpendicular to the xOy plane and forms a right-handed coordinate system.

[0105] The flight distance and time range during rocket recovery are relatively small. Therefore, it is assumed that the Earth's surface is a flat plane throughout the entire process, and entrainment acceleration and Coriolis acceleration are ignored. The dynamic equations are as follows:

[0106]

[0107]

[0108]

[0109] In the formula: r is the position vector, V is the velocity vector, m is the mass, and I is the velocity vector. sp Let g be the specific impulse, and g0 be the gravitational acceleration at sea level. The thrust vector T is aligned with the rocket's longitudinal axis. The aerodynamic forces acting on the rocket during flight are represented in the rocket body coordinate system, with the rocket's center of mass as the origin. The Ox axis points along the body axis towards the rocket's nose, the Oy axis is perpendicular to the Ox axis in the rocket's principal plane of symmetry, and the Oz axis is perpendicular to the xOy plane, forming a right-handed coordinate system. Because the rocket is at a small angle of attack during landing, only the axial aerodynamic force F is considered. A Its numerical expression is:

[0110]

[0111] In the formula: ρ is the atmospheric density, V is the velocity magnitude, S is the reference area, and C A Here is the normal aerodynamic coefficient. Gravitational acceleration is expressed as:

[0112]

[0113] During recovery, the thrust T is subject to amplitude constraints:

[0114] T min ≤T≤T max

[0115] Meanwhile, the terminal constraints of the recycling process are:

[0116]

[0117] In the formula: These represent the desired terminal position and velocity, respectively, in m. dry This refers to the dry weight of the rocket.

[0118] The optimal control problem can be simply described as follows:

[0119]

[0120]

[0121]

[0122] T min ≤T≤T max

[0123] in For state variables, To control the quantity, ψ is the pitch angle, y is the yaw angle (adjusting the thrust direction), and η is the throttle ratio (adjusting the thrust magnitude). As output quantities, the expressions for the terminal output quantities are independent of each other. This represents the desired terminal state.

[0124] 2. Considering that the terminal time is adjustable, the real time t∈[t0,t... f Transform to virtual time τ∈[-1,1], and establish the optimal control problem P0 for rocket recovery trajectory planning;

[0125] Let the real time t∈[t0,t f The conversion to virtual time τ∈[-1,1] is specifically formulated as follows:

[0126] t=κ1+κ2τ

[0127]

[0128]

[0129] The optimal control problem P0 is defined as follows:

[0130] findu(τ)

[0131]

[0132]

[0133]

[0134] T min ≤T≤T max

[0135] The constraint equations in the above equation include: dynamic equations, initial state and terminal state constraint equations.

[0136] 3. Based on the Pika iterative scheme of the dynamic equations, and using Chebyshev polynomials as basis functions, the discrete optimal control problem P1 is obtained by non-equidistant discretization.

[0137] 3.1 The natural second-order system Picard iterative scheme for the rocket recovery dynamics equations is obtained;

[0138] The equivalent integral equation based on the rocket recovery dynamics equation is:

[0139]

[0140]

[0141] In the formula: These represent the initial position and initial velocity, respectively.

[0142] The Picard iteration scheme for the k-th iteration of the above natural second-order system is:

[0143]

[0144]

[0145] 3.2 Introducing Chebyshev multiple similarity as a basis function, selecting CGL points as collocation points, approximating the integrand term on the right-hand side of the velocity equation, and then integrating term by term to achieve discretization;

[0146] Assuming N+1 discrete points are selected, the discrete time series is defined as τ=[τ0,τ1,…,τ N ] T The discrete state sequence is Discrete control sequence is The discretized dynamic equations can then be expressed as:

[0147]

[0148]

[0149] In the formula: i = 0, 1, ..., N is the sequence of collocations, k = 1, 2, ... is the number of iterations, κ = (t f-t0) / 2 is the time transformation parameter. A = RKCL, C is determined by the Chebyshev polynomials of the basis functions, C [i+1,·] This represents the (i+1)th row of the matrix. When the number of discrete points is determined, C and A are then determined to be constant matrices. G(X) k U) represents the matrix form of the dynamic equations at N+1 discrete nodes, specifically:

[0150]

[0151] In the formula: matrices R, K, C, L are constant matrices.

[0152]

[0153]

[0154]

[0155]

[0156] In the formula: the formula for calculating the elements of the first row of matrix K is:

[0157]

[0158] In the formula: T i (τ) and τ i The calculation formula is:

[0159] τ i = -cos(iπ / N), i = 0, 1, ..., N

[0160] T i (τ)=cos(i·cos -1 (τ)), i = 0, 1, ..., N

[0161] Based on the discretized dynamic equations, the optimal control subproblem P1 for the k-th iteration is:

[0162] findU,t f

[0163]

[0164]

[0165] F(x N ) = 0

[0166] T min ≤T≤T max

[0167] 4. For problem P1, the state sequence X kDetermined by the control sequence U, its constraints can ultimately be transformed into terminal nonlinear constraints. Based on this, the control sequence U is expressed with respect to U. k Linearization yields the approximate optimal control problem P2; 4.1 In this problem, the state variables can be computed in parallel using the discretized dynamic equations. Terminal constraints can be... It can be transformed into:

[0168] Φ(U,t f ) = 0

[0169] The above equation is about the variables to be solved, U and t. f The highly nonlinear equations.

[0170] 4.2 Solution U in the kth iteration k , Linearization is performed at this point, resulting in the linearized solution equation:

[0171]

[0172] In the formula, Φ′(U k ), These are the terminal constraints with respect to the control sequence U and the terminal time t, respectively. f The Jacobian matrix is ​​defined as:

[0173]

[0174] Therefore, P0 is ultimately transformed into problem P2:

[0175] findU

[0176]

[0177] T min ≤T≤T max

[0178] 5. Based on the model-based predictive static programming algorithm, derive the Jacobian matrix Φ′(U k ) and the update amount dU for each iteration of the control quantity.

[0179] 5.1 In step 4, the problem of rocket recovery is finally transformed into problem P2, performance indicators are introduced, and the model predictive static programming algorithm is used to solve it.

[0180] Considering the L2 norm of the controllable change and the adjustability of the terminal time, the following performance indicators are selected:

[0181]

[0182] In the formula, W is the weight matrix, specifically:

[0183]

[0184] When N is even

[0185]

[0186]

[0187] When N is odd

[0188]

[0189]

[0190] Under performance index-based conditions, the update iterations of control variables and terminal time are as follows:

[0191]

[0192]

[0193] submatrix They have explicit expressions and are independent of each other, therefore the Jacobian matrix Φ′(U k It can be computed in parallel:

[0194]

[0195] As previously mentioned, both thrust and aerodynamic force are represented in the arrow system and require the aid of... ψ is transformed into a landing-fixed relationship. Therefore, the specific expression for g(x,u) is:

[0196]

[0197] In the formula:

[0198]

[0199] Then the updated variable U to be solved k+1 , for:

[0200]

[0201] 5.2 For each iteration of the model prediction static programming algorithm, a projection method is introduced to handle the thrust inequality constraint, thereby improving computational efficiency. The thrust amplitude constraint is actually a constraint on the throttling ratio η that adjusts the thrust magnitude, i.e.:

[0202]

[0203] 6. Set the initial guess X 0 U0 The system iterates and updates until the terminal error condition is met, thereby generating guidance commands and achieving rocket recovery.

[0204] The specific process of iterative update is as follows:

[0205] 6.1 Let the number of iterations k = 0, given the initial conjecture X of the state sequence and control sequence. 0 U 0 , And let X k =X 0 U k =U 0 ,

[0206] 6.2 During the k-th iteration, calculate the increment dU of the control quantity and the terminal time. k ,

[0207] 6.3 Update control variables and terminal time U k+1 , Introducing a projection method to handle thrust constraints, and calculating the updated...

[0208] 6.4 Determine if the convergence condition is met: If the condition is met, the iteration stops, and the current solution is the desired one; if not, return to step 6.2 and iterate until the condition is met. The specific flowchart is as follows: Figure 1 As shown. Specific implementation examples:

[0210] This embodiment performs rocket recovery trajectory planning simulation. All simulations are executed on a desktop computer equipped with an Intel i5-7200U processor with a main frequency of 2.50GHz. All programs are compiled and run in the MATLAB environment.

[0211] Rocket reference area S ref =10.51m 2 Specific impulse I sp =282s, maximum thrust T max =845.2kN, total weight m0=38.963t, dry weight m dry = 27.215t. Throughout the landing process, the throttle ratio η varies within the range of 0.6 to 1. Aerodynamic forces primarily consider axial drag, and C is assumed to be... A =1, atmospheric density adopts the standard atmospheric model. Normalized standard value: (r e (where the radius is the Earth's radius) (g0 is the acceleration due to gravity), (m0 is the total weight of the rocket).

[0212] The rocket's initial state upon recovery from solid-state: Position: x0 = 1512m, y0 = 4119m, z0 = 324m; Velocity: V x0 = -149m / s, V y0 = -259m / s, V z0 = -30m / s. Rocket terminal state: Terminal position: x f =0,y f =1m,z f =0, terminal speed: V xf =0,V yf = -1m / s, V zf =0. The vertical components of the terminal position and velocity are set to non-zero to achieve terminal attitude constraints for rocket landing.

[0213] Thirty discrete points were selected, and the convergence thresholds for position and velocity were set to 1m and 0.1m / s, respectively.

[0214] Based on the above steps and parameters (the specific process is as follows) Figure 1 As shown in the figure, the rocket recovery trajectory planning method based on the Picka iteration-model prediction static programming algorithm designed in this invention has completed the numerical simulation of the rocket recovery trajectory planning task. This invention completes the solution after 22 iterations, with the total flight time from the initial position to the desired recovery position being 32.71s and fuel consumption being 6.3644t. The position change curve during this process is shown in the figure. Figure 2 As shown, the velocity change curve is as follows: Figure 3 As shown, the thrust variation curve is as follows: Figure 4 As shown, the terminal position and velocity both meet the expectations. Simultaneously, the thrust vector directions at the terminal moments are 88.9722° and 89.6458°, respectively. The x and z-axis components of the thrust at the terminal moment are close to zero, with the thrust component concentrated along the y-axis, which is consistent with the terminal attitude requirements. The above results demonstrate the effectiveness of this invention for rocket recovery trajectory planning.

[0215] To verify the computational accuracy of this invention, terminal error statistical analysis was performed using different numbers of discrete points. Interpolation was performed on the control variables in the optimized solution, and numerical integration was performed on the original nonlinear dynamic equation using ode45 with a maximum step size of 0.01 s. The terminal position and velocity errors were statistically analyzed (average of 50 calculations). The curve showing the change in terminal position error as the number of discrete points increased from 31 to 101 is shown below. Figure 5 As shown, the curve of the terminal speed error variation is as follows: Figure 6 As shown. From Figure 5 , Figure 6It can be seen that as the number of discrete points increases, the terminal error generally decreases. The terminal position error decreases from 1.5567m to 1.0373m, and the terminal velocity error decreases from 0.0564m / s to 0.0326m / s. The above results demonstrate that the calculation accuracy of this invention is high.

[0216] To verify the computational time of this invention, statistical analysis of the time consumption was performed using different numbers of discrete points. On one hand, the time taken to solve the trajectory optimization problem of rocket recovery (the average time of 50 solutions) was statistically analyzed using different numbers of discrete points, and the variation curves are shown below. Figure 7 As shown in the figure, it can be seen that when the number of discrete points increases from 31 to 101, the solution time increases from 25ms to 100ms. On the other hand, the time consumption for each iteration, state integral, and Jacobian matrix calculation during the solution process is statistically analyzed (average over 50 solutions), and the variation curves are shown in the figure. Figure 8 As shown, although the time required to solve the state integral and Jacobian matrix increases with the number of discrete points, it remains within 1ms, and the time for a single iteration is within 5ms. These results demonstrate the high computational efficiency of this invention.

[0217] In summary, this invention provides a rocket recovery trajectory planning method based on an improved model predictive static programming algorithm. The differential dynamic equations of rocket recovery are solved using second-order Picard iteration, and the continuous dynamic system is discretized using Chebyshev polynomials as basis functions. This approach improves the MPSP algorithm, making the state variables at different times independent, eliminating the need for recursive calculations, and improving discretization accuracy. Furthermore, in the specific iterative solution process, a projection method is used to handle thrust constraints, obtaining explicit expressions for the updated variables, making the iterative process of the MPSP algorithm more efficient.

Claims

1. A rocket recovery trajectory planning method based on an improved model predictive static programming algorithm, characterized in that, Includes the following steps: Step 1: Establish the fixed coordinate system and three-degree-of-freedom dynamic equations for recovery; Step 1-1: Retrieve the origin and landing point of the fixed coordinate system Fixed connection, recycling of fixed coordinate system The axis pointing in the horizontal plane at the landing point towards the rocket's initial position is positive; this is the coordinate system fixed during recovery. The axis is perpendicular to the horizontal plane of the landing point and points upwards, recovering the fixed coordinate system. The axis and the fixed coordinate system of recycling The planes are perpendicular and form a right-handed coordinate system; Steps 1-2: Assuming the Earth's surface is a plane during rocket recovery, and neglecting entrainment acceleration and Coriolis acceleration, the dynamic equations are as follows: （1） In the formula: For position vectors, It is a velocity vector. For quality, For the purpose of comparison, The gravitational acceleration at sea level; The thrust vector is aligned with the rocket's longitudinal axis. Axial aerodynamic force, It is gravitational acceleration; Steps 1-3: The aerodynamic forces acting on the rocket during flight are represented in the rocket body coordinate system, with the rocket's center of mass as the origin. The axis points along the body axis towards the head of the arrow body, in the arrow body coordinate system. The axis lies in the principal plane of symmetry of the rocket and is perpendicular to the rocket's coordinate system. Axis, the coordinate system of the rocket body The axis and the coordinate system of the rocket body The planes are perpendicular and form a right-handed coordinate system; only axial aerodynamic forces are considered. Its numerical expression is: （2） In the formula: Atmospheric density, For speed magnitude, For reference area, Normal aerodynamic coefficient; Gravitational acceleration is expressed as: （3） In the formula: The gravitational constant is the constant of gravity. The distance from the rocket to the Earth's core; During recovery, thrust vector There are amplitude constraints: （4） In the formula, , These represent the lower and upper limits of the thrust amplitude, respectively. Meanwhile, the terminal constraints of the recycling process are: （5） In the formula: , These are the position and velocity of the rocket's flight terminal, respectively. These represent the desired terminal position and velocity, respectively. For the quality of the flight terminal, For the dry weight of the rocket; Steps 1-4: Equations (1) to (5) describe the optimal control problem as follows: （6） in For state variables, To control the quantity, The pitch angle; The yaw angle is used to adjust the thrust direction; Adjust the thrust to achieve the throttling ratio; For output quantity, For the desired terminal state, For the final moment of rocket flight, The dynamic equation representing the velocity term; Step 2: Set the actual time Convert to virtual time Establish the optimal control problem P0 for rocket recovery trajectory planning; Real time Convert to virtual time The specific conversion formula is as follows: （7） In the formula: This is a virtual time range; The optimal control problem P0 is defined as follows: （8） In the formula: These are the time transformation parameters for converting the dynamic equations to the virtual time domain. Represents the state quantity at the initial moment; Step 3: Using Chebyshev polynomials as basis functions, perform non-equidistant discretization on the Picard iterative scheme of dynamic equation (1) to obtain the discretized optimal control problem P1; Step 3-1: Obtain the natural second-order system Picard iterative scheme of the rocket recovery dynamics equations; Based on equation (8), the equivalent integral equation of the rocket recovery dynamics equation is: （9） In the formula: These are the initial position and initial velocity, respectively. The second-order natural system of equation (9) The iteration format for the next iteration of the pickup truck is: （10） Step 3-2: Introduce Chebyshev polynomials as basis functions, select CGL points as collocation points, approximate the integrand term on the right side of the velocity equation, and then integrate term by term to achieve the discretization of equation (10); Select Given discrete points, define the discrete time series as follows: The discrete state sequence is The discrete control sequence is The discretized dynamic equations are then expressed as: （11） In the formula: For the discretized first The velocity vector obtained from the next iteration For the discretized first The position vector obtained from the next iteration For the first The discrete state sequence of the next iteration. For the collocation sequence, For the number of iterations, For time conversion parameters; , All represent matrix parameters of the discretization process. Determined by the Chebyshev polynomials of the basis functions. Represents the matrix of the first Okay; when the number of discrete points is fixed, It is determined to be a constant matrix; The dynamic equations are expressed in The matrix form at each discrete node is as follows: （12） Based on the discretized dynamic equation (12), the optimal control problem P1 is: （13） Step 4: For problem P1, the state sequence By control sequence The decision is made, and its constraints are ultimately transformed into terminal nonlinear constraints. Based on this, the control sequence is... Regarding the first The control sequence to be solved next Linearization yields an approximate optimal control problem P2; Step 4-1: In problem equation (13), the state variables are calculated in parallel using the discretized dynamic equations. The state variables at the terminal time are... It is represented by equation (11), and the state sequence By control sequence The decision is made, therefore the unknowns are the control sequence. and terminal time ; Therefore, terminal constraints Transform into: （14） Equation (14) is about the variable to be solved Highly nonlinear equations; Step 4-2: Apply equation (14) in the first... Solution of the next iteration Linearization is performed at this point, resulting in the linearized solution equation: （15） In the formula, These are terminal constraints with respect to the control sequence. Terminal time The Jacobian matrix is ​​defined as: （16） Therefore, problem P0 is ultimately transformed into problem P2: （17） Step 5: Solve equation (17) using the model prediction static programming algorithm to derive the Jacobian matrix. and variables to be solved ; Step 5-1: In step 4, the problem of rocket recovery is finally transformed into a problem (17). For equation (17), the model prediction static programming algorithm is used to solve it. Consider controlling the amount of change The norm and terminal time are adjustable; the performance metrics selected are as follows: （18） In the formula Let be the weight matrix; under the conditions of performance index (18), the update iterations of the control quantity and the terminal time are as follows: （19） （20） The updated expression for the variable to be solved is: （21） Step 5-2: In each iteration of the model prediction static programming algorithm, a projection method is introduced to handle the thrust inequality constraint, thereby improving computational efficiency; the thrust amplitude constraint is actually the throttling ratio that adjusts the thrust magnitude. The constraints, namely: （22） Step 6: Set the initial guess The process involves iterative updates until the terminal error condition is met, thereby generating guidance commands and completing the trajectory planning and solution task for rocket recovery.

2. The rocket recovery trajectory planning method based on an improved model prediction static programming algorithm according to claim 1, characterized in that, The For a constant matrix, the specific calculation formulas are equations (23) to (26): （23） （24） （25） （26） Where: matrix The formula for calculating the first row of elements is: （27） In the formula: The calculation formula is: （28） （29）。 3. The rocket recovery trajectory planning method based on an improved model prediction static programming algorithm according to claim 1, characterized in that, The The weight matrix is ​​as follows: （30） when When it is even, （31） when When it is an odd number, （32）。 4. The rocket recovery trajectory planning method based on an improved model prediction static programming algorithm according to claim 1, characterized in that, The specific process of iterative update in step 6 is as follows: Step 6-1: Let the number of iterations be... Given the initial conjecture of the state sequence and control sequence and order ; Step 6-2: In the first During the step-by-step iteration, based on equations (19) to (20), the increments of the control quantity and the terminal time are calculated. ; Step 6-3: Update control variables and terminal time Thrust constraints (22) are introduced, and the updated values ​​are calculated. ; Step 6-4: Determine if the convergence condition is met: If the condition is met, the iteration stops and the current step is the solution; if the condition is not met, the iteration returns to step 6-2 until the condition is met.

Images