Real-Time Trajectory Planning for Hypersonic Entry Using Adaptive Non-Uniform Discretization and Convex Optimization

Abstract

This paper introduces an improved sequential convex programming algorithm using adaptive non-uniform discretization for the hypersonic entry problem. In order to ensure real-time performance, an inverse-free precise discretization based on first-order hold discretization is adopted to obtain a high-accuracy solution with fewer temporal nodes, which would lead to constraint violation between the temporal nodes due to the sparse time grid. To deal with this limitation, an adaptive non-uniform discretization is developed, which provides a search direction for purposeful clustering of discrete points by adding penalty terms in the problem construction process. Numerical results show that the proposed method has fast convergence with high accuracy while all the path constraints are satisfied over the time horizon, thus giving potential to real-time trajectory planning.

1. Introduction

With the increasing demand for spacecraft autonomy, not only the accuracy and optimality of the result trajectory but also the stability and efficiency of the solving algorithm are of crucial importance for real-time guidance. For the hypersonic entry problem, trajectory optimization is a popular method due to its extensive application prospect, yet it is still very challenging because of the highly nonlinear and complicated dynamics and constraints involved [1,2,3,4,5].

For the trajectory optimization problem, the existing methods can be classified as indirect methods and direct methods [6]. Based on Pontryagin’s maximum principle [7], indirect methods derive the necessary conditions of optimality and solve a two-point boundary value problem (TPBVP) to obtain the result trajectory [8,9]. Due to the inherent nonlinearity in dynamics of the hypersonic entry problem, the solution of TPBVP presents significant challenges and falls short of real-time requirements [10]. Despite theoretical guarantees on the optimality of the solution, practical considerations limit its feasibility. The direct methods convert the original problem into an approximated finite parameter optimization problem and use a nonlinear programming (NLP) algorithm to solve it [11,12,13]. Although NLP-based methods have been successful in many applications, the time-consuming solution process and the lack of convergence guarantee are major challenges [10]. Furthermore, an appropriate initial guess should be selected to achieve a high-quality solution, especially for the hypersonic entry problem.

In recent years, the application of convex optimization in trajectory optimization has shown great potential in many problem, including powered descent guidance [14] and spacecraft rendezvous and proximity operations [15]. If a problem can be formulated in a convex form, it can be solved in polynomial time with a strong convergence guarantee while obtaining the global optimal solution [16,17]. There are many state-of-the-art solvers based on the interior-point method (IPM) [18], such as GUROBI [19], MOSEK [20], and ECOS [21], to solve convex optimization problems efficiently. However, the majority of the trajectory optimization problems are non-convex. To deal with the nonlinearity in dynamics and constraints, the sequential convex programming (SCP) technique is developed to solve a sequence of convex subproblems to approximate the original problem [22]. The sub-problems are formulated by linearization about a reference trajectory and subsequently solved via iterative refinement of the reference trajectory until convergence of the solution is achieved. In order to ensure the convex approximation is accurate, a trust region is imposed to make sure that the solution is not far from the reference trajectory. There are both hard and soft methods to address the trust region [23,24]. Recently, this SCP technique has been applied to hypersonic re-entry problems and has shown its effectiveness to obtain high-accuracy solutions. Liu formulated the entry problem as a second-order cone programming (SOCP) problem and applies the successive convexification method to solve it [25]. Wang and Grant proposed an improved SCP algorithm and introduced a new control input for the entry problem [26,27]. Wang and Cui developed a rapid trajectory optimization algorithm with the pseudospectral method [28]. However, multiple hundreds of temporal nodes are usually required to maintain the accuracy due to the nonlinearity of the hypersonic entry problem and the long flight duration. Achieving a balance between real-time performance and accuracy represents a significant challenge, as reducing the number of discrete points may result in a loss of precision.

To deal with this issue, Kamath and Açıkmeşe et al. [29,30,31] propose an inverse-free precise discretization based on first-order hold (FOH) discretization. Considering the consistency of the original non-convex dynamics with the reference trajectory and addition of the stitching condition, this discretization would guarantee high accuracy with few temporal nodes and has been effectively applied to various problems, including powered descent guidance [29], multi-phase rocket landing [30], and hypersonic entry guidance [31]. In [31], the amount of the temporal nodes is only 40 to achieve the commensurate accuracy in [27], which necessitated an excess of 200 nodes. Nevertheless, for uniform discretization, a reduced number of temporal nodes would generate a sparse time grid, which may lead to constraint violation between the temporal nodes since the constraints are only imposed at discrete points in the SCP subproblems, resulting in a new issue. In [30], an non-uniform discretization with additional time interval dilation variables is introduced in multi-phase rocket landing to adaptively decide the turning points of different phases. A similar idea is extended to the hypersonic entry problem and the penalized trust region (PTR) algorithm, a soft trust region method, is used to construct the SCP process [31]. However, the resulting trajectory would still experience constraint violation. In addition, in our experiment for the hypersonic entry problem, both hard and soft trust region methods with additional time interval variables showed worse convergence compared with those with uniform discretization. One of the reasons is that the dynamics are time-sensitive, and the other is that an effective search direction should be given to achieve a purposeful distribution of discrete points.

In this paper, we propose a novel adaptive non-uniform discretization method to handle the above issues.

• An inverse-free precise discretization is adopted to obtain high accuracy with few temporal nodes for real-time performance.
• An adaptive non-uniform discretizaiton is proposed to construct the SCP subproblem with additional penalty terms. This would give the solver a search direction to cluster the temporal nodes more purposefully and the propagated trajectory would satisfy all the path constraints as a result, which is the main contribution of this paper.
• The validity of proposed method is substantiated through a numerical experiment compared with other SCP methods.

This paper is organized as follows. Section 2 presents the model of the hypersonic entry trajectory optimization problem including the dynamics and constraints. Section 3 introduces the adaptive non-uniform discretization and constructs the SCP subproblem and iteration process. Simulation and results analysis are shown in Section 4. The conclusions are summarized in Section 5.

2. Problem Formulation

In this section, we consider a typical entry trajectory problem for an unpowered hypersonic vehicle with multiple path constraints.

2.1. 3-DoF Entry Dynamics

The dimensionless dynamic equations over a spherical, rotating Earth can be modeled as follows. More details can be referred to in [27].

$$\dot{x}=f(x,u)=\left\{\begin{array}{cc}\hfill \dot{r}& =Vsin\gamma \hfill \\ \hfill \dot{\theta }& =\frac{Vcos\gamma sin\psi }{rcos\varphi }\hfill \\ \hfill \dot{\varphi }& =\frac{Vcos\gamma cos\psi }{r}\hfill \\ \hfill \dot{V}& =-D-\frac{Lsin\gamma }{r^2}+{\mathrm{\Omega }}_V\hfill \\ \hfill \dot{\gamma }& =\frac{Lcos\sigma }{V}+\frac{(V^2-1/r)cos\gamma }{Vr}+{\mathrm{\Omega }}_{\gamma }\hfill \\ \hfill \dot{\psi }& =\frac{Lsin\sigma }{Vcos\gamma }+\frac{Vcos\gamma sin\psi tan\varphi }{r}+{\mathrm{\Omega }}_{\psi }\hfill \end{array}\right.$$

(1)

where the state vectors are $x=\left[r,\theta ,\varphi ,V,\gamma ,\psi \right]$, representing the orbital radius, longitude, latitude, relative velocity, flight path angle, and heading angle, respectively. The Earth rotation-dependent terms, ${\mathrm{\Omega }}_V,{\mathrm{\Omega }}_{\gamma },{\mathrm{\Omega }}_{\psi }$, and the lift and drag accelerations, $L,D$, in (1) are shown below.

$$\begin{array}{cc}\hfill {\mathrm{\Omega }}_V& ={\mathrm{\Omega }}^{2}rcos\varphi \left(sin\gamma cos\varphi -cos\gamma sin\varphi cos\psi \right)\hfill \\ \hfill {\mathrm{\Omega }}_{\gamma }& =2\mathrm{\Omega }cos\varphi sin\psi +{\mathrm{\Omega }}^{2}rcos\varphi \left(cos\gamma cos\varphi +sin\gamma sin\varphi cos\psi \right)/V\hfill \\ \hfill {\mathrm{\Omega }}_{\psi }& =-2\mathrm{\Omega }\left(tan\gamma cos\psi cos\varphi -sin\varphi \right)+{\mathrm{\Omega }}^{2}rsin\varphi cos\varphi sin\psi /(Vcos\gamma )\hfill \\ \hfill L& =R_0\rho V^2{S}_{ref}{C}_L/\left(2m\right)\hfill \\ \hfill D& =R_0\rho V^2{S}_{ref}{C}_D/\left(2m\right)\hfill \end{array}$$

(2)

where $\mathrm{\Omega }$ is the Earth self-rotation rate, $\rho ={\rho }_0{e}^{(-h/hs)}$ is the atmospheric density depending on the altitude h, $S_{ref}$, m is the reference area and mass of the vehicle, and $C_L,C_D$ are the aerodynamic lift and drag coefficients related to Mach number and the attack angle $\alpha$.

As in [27], the control variable is restricted to bank angle $u=\sigma$. The attack angle $\alpha$ is pre-specified as a function of Mach number, as described in Section 4. All the variables are dimensionless and the dimensionless factors are shown in

Table 1. The dimensionless factors’ values.

Variable	Unit	Value
Time	s	$\sqrt{R_0/g_0}$
Distance	m	$R_0$
Velocity	m/s	$\sqrt{R_0{g}_0}$
Acceleration	$\mathrm{m}/{\mathrm{s}}^2$	$g_0$
Angle	rad	1
Angle rate	rad/s	$\sqrt{g_0/R_0}$

, where $R_0=6378.0$ km and $g_0=9.81$ m/s² represent the Earth’s radius and the acceleration of gravity, respectively.

2.2. State, Control, and Path Constraints

The inital and terminal conditions are

$$\begin{array}{c}\hfill x\left(t_0\right)=x_0\\ \hfill x\left(t_f\right)=x_f\end{array}$$

(3)

The control bounds and control rate constraints are given as follows:

$$\begin{array}{cc}\hfill -{\sigma }_{max}\le & \sigma \le {\sigma }_{max}\hfill \\ \hfill -d{u}_{max}\le & \dot{\sigma }\le d{u}_{max}\hfill \end{array}$$

(4)

where ${\sigma }_{\max }$ and $d{u}_{\max }$ are the bounds of the bank angle and its rate, respectively.

Three typical path constraints, including heat rate, dynamic pressure, and normal load, are considered:

$$\begin{array}{cc}\hfill \dot{Q}& =p_1(r,V)=k_Q\sqrt{\rho }{V}^{3.15}\le {\dot{Q}}_{max}\hfill \\ \hfill q& =p_2(r,V)=0.5\rho V^2\le q_{max}\hfill \\ \hfill n& =p_3(r,V)=\sqrt{L^2+D^2}\le n_{max}\hfill \end{array}$$

(5)

In this paper, no-fly zone constraints are considered as well, which are defined as cylinder zones with center $({\theta }_{NFZ},{\varphi }_{NFZ})$, radius $R_{NFZ}$, and infinite altitude. Thus, the no-fly zone constraints are expressed as

$${(\theta -{\theta }_{NFZ})}^2+{(\varphi -{\varphi }_{NFZ})}^2\ge R_{NFZ}^2$$

(6)

2.3. Nonconvex Optimal Control Problem

The maximum terminal velocity hypersonic entry trajectory optimization problems with fixed flight time are considered in this paper, which is the same as [27]. The nonconvex optimal control problem is shown in Problem 1.

Problem 1.

$$\begin{array}{cc}\hfill \underset{x,u}{min}& J=-V\left(t_f\right)\hfill \\ \hfill s.t.& \left(1\right),\left(3\right)-\left(6\right)\hfill \end{array}$$

(7)

3. Improved SCP Method with Adaptive Non-Uniform Discretization

In this section, we introduce the improved SCP algorithm using adaptive non-uniform discretization for the hypersonic entry problem. In the interest of completeness, we provide a brief introduction to the non-uniform scheme and precise discretization technique, both of which, as in [31], are utilized in the proposed method. Further details will be presented subsequently. In order to seek an appropriate search direction and achieve a purposeful distribution of temporal nodes, additional penalty terms with respect to the nonlinear term of path constraints and the distance term from the trajectory to the no-fly zone center are considered in the SCP subproblem construction.

3.1. Time Interval Dilation

To introduce the non-uniform discretization, we consider the original nonlinear dynamics in the sub-interval $[t_k,t_{k+1}),k=1,\dots ,N-1$,

$$\begin{array}{c}\hfill \dot{x}\left(t\right)=f(t,x\left(t\right),u\left(t\right)),t\in [t_k,t_{k+1})\end{array}$$

(8)

where $t_0=t_1<t_2<\cdots <t_N=t_f$, and define an affine map to normalize the orginal time interval (may not be equal) to a fixed interval, $[0,1)$:

$$\begin{array}{c}\hfill {\tau }_k\left(t\right)=\frac{t-t_k}{s_k},t\in [t_k,t_{k+1})\end{array}$$

(9)

where $s_k=t_{k+1}-t_k$ is the length of the $k$th time interval and can be referred to as the time interval dilation [30]. So far, the dynamics Equation (8) can be rewritten with respect to the normalized time ${\tau }_k$:

$$\begin{array}{c}\hfill \dot{x}\left({\tau }_k\right)=s_{k}f({\tau }_k,x\left({\tau }_k\right),u\left({\tau }_k\right))=F({\tau }_k,x\left({\tau }_k\right),u\left({\tau }_k\right),s_k),{\tau }_k\in [0,1)\end{array}$$

(10)

By treating $s_k$ as additional decision variables, the solver is allowed to decide the adaptive time grids rather than a uniform temporal grid.

What is more, some exact constraints should be added in practical implementation to ensure physical meaning:

$$0<{\mathrm{\Delta }}_{min}\le s_k\le {\mathrm{\Delta }}_{max}$$

(11)

to ensure the time order $t_0=t_1<t_2<\cdots <t_N=t_f$ and adjacent temporal nodes are not far away, and

$$\sum _{k=1}^N{s}_k=t_f$$

(12)

to ensure the fixed flight time.

3.2. Convexification and Discretization

A convex approximation of the dynamic (10) can be obtained by the first-order Tylor expansion about a reference trajectory $(\overline{x},\overline{u},\overline{s})$. The approximate equation is a linear time-varing (LTV) system as follows:

$$\dot{x}\left({\tau }_k\right)\approx A\left({\tau }_k\right)x\left({\tau }_k\right)+B\left({\tau }_k\right)u\left({\tau }_k\right)+S\left({\tau }_k\right)s_k+d\left({\tau }_k\right)$$

(13)

where $A\left({\tau }_k\right),B\left({\tau }_k\right),S\left({\tau }_k\right)$ are the Jacobians of the dynamics with state control and time dilation, respectively.

$$\begin{array}{cc}\hfill A\left({\tau }_k\right)& \triangleq {\nabla }_{x}F({\tau }_k,\overline{x}\left({\tau }_k\right),\overline{u}\left({\tau }_k\right),{\overline{s}}_k)\hfill \\ \hfill B\left({\tau }_k\right)& \triangleq {\nabla }_{u}F({\tau }_k,\overline{x}\left({\tau }_k\right),\overline{u}\left({\tau }_k\right),{\overline{s}}_k)\hfill \\ \hfill S\left({\tau }_k\right)& \triangleq {\nabla }_{s_k}F({\tau }_k,\overline{x}\left({\tau }_k\right),\overline{u}\left({\tau }_k\right),{\overline{s}}_k)\hfill \\ \hfill d\left({\tau }_k\right)& \triangleq F({\tau }_k,\overline{x}\left({\tau }_k\right),\overline{u}\left({\tau }_k\right),{\overline{s}}_k)\hfill \\ \hfill & -A\left({\tau }_k\right)\overline{x}\left({\tau }_k\right)-B\left({\tau }_k\right)\overline{u}\left({\tau }_k\right)-S\left({\tau }_k\right){\overline{s}}_k\hfill \end{array}$$

(14)

For discretization, a precise inverse-free discretization technique based on first-order hold (FOH) is adopted [29,30,31]. In the FOH case, the control input signal is considered as a piecewise affine function; thus, the control variables are only defined at the discrete time nodes and the control signal in the sub-interval can be parameterized as follows:

$$u\left({\tau }_k\right)=(1-{\tau }_k)u_k+{\tau }_k{u}_{k+1},k=1,\dots ,N-1$$

(15)

where $t\in [t_k.t_{k+1})$ and ${\tau }_k\in [0,1)$ as given in (9). Thus, The LTV dynamics (13) can be easily rewritten with respect to the deviations from the reference trajectory.

$$\mathrm{\Delta }\dot{x}\left({\tau }_k\right)=A\left({\tau }_k\right)\mathrm{\Delta }x\left({\tau }_k\right)+B\left({\tau }_k\right)(1-{\tau }_k)\mathrm{\Delta }{u}_k+B\left({\tau }_k\right){\tau }_k\mathrm{\Delta }{u}_{k+1}+S\left({\tau }_k\right)\mathrm{\Delta }{s}_k$$

(16)

where $\overline{\square }$ denotes the reference quantity, $\mathrm{\Delta }\square$ denotes the deviations from the reference trajectory, i.e., $\mathrm{\Delta }\square =\square -\overline{\square }$ and $\mathrm{\Delta }\dot{x}\left({\tau }_k\right)=\dot{x}\left({\tau }_k\right)-F({\tau }_k,\overline{x}\left({\tau }_k\right),\overline{u}\left({\tau }_k\right),{\overline{s}}_k)$, and the coefficient matrixes $A,B,S$ are the same as (14). It can be considered that the reference trajectory in the sub-interval $[t_k,t_{k+1})$ is in accordance with the original dynamics (10), rather than the convex approximation (13), like the typical FOH discretization in [32].

According to the knowledge of the linear system [33], the unique solution of (16) for $t\in [t_k.t_{k+1})$ and ${\tau }_k\in [0,1)$ is

$$\mathrm{\Delta }x\left({\tau }_k\right)=A_k\left({\tau }_k\right)\mathrm{\Delta }x\left(0\right)+B_k^{-}\left({\tau }_k\right)\mathrm{\Delta }{u}_k+B_k^{+}\left({\tau }_k\right)\mathrm{\Delta }{u}_{k+1}+S_k\mathrm{\Delta }{s}_k$$

(17)

where

$$\begin{array}{cc}\hfill A_k\left({\tau }_k\right)& =\mathit{\Phi }({\tau }_k,0),\hfill \\ \hfill B_k^{-}\left({\tau }_k\right)& =A_k\left({\tau }_k\right){\int }_0^{{\tau }_k}{\Phi }^{-1}(\zeta ,0)B\left(\zeta \right)(1-\zeta )\mathrm{d}\zeta ,\hfill \\ \hfill B_k^{+}\left({\tau }_k\right)& =A_k\left({\tau }_k\right){\int }_0^{{\tau }_k}{\Phi }^{-1}(\zeta ,0)B\left(\zeta \right)\zeta \mathrm{d}\zeta ,\hfill \\ \hfill S_k\left({\tau }_k\right)& =A_k\left({\tau }_k\right){\int }_0^{{\tau }_k}{\Phi }^{-1}(\zeta ,0)S\left(\zeta \right)\mathrm{d}\zeta ,\hfill \end{array}$$

(18)

where $\Phi ({\tau }_k,0)$ is called the state transition matrix (STM) with the following properties: $\Phi (0,0)=I$, $\dot{\Phi }({\tau }_k,0)=A\left({\tau }_k\right)\Phi ({\tau }_k,0)$, and ${\Phi }^{-1}(\tau ,\eta )=\Phi (\eta ,\tau )$.

In order to eliminate the inversion operation to avoid numerical problems, the $B_k^{\mp },S_k$ in (18) have the closer forms, as shown in Thereom 1, which is not proven in [29,30,31].

Theorem 1.

The coefficient matrixes (18) of the linear time-varying system (17) have the inverse-free form: $\forall {\tau }_k\in [0,1)$:

$$\begin{array}{cc}\hfill B_k^{-}\left({\tau }_k\right)& ={\int }_0^{{\tau }_k}A\left(\zeta \right)B_k^{-}\left(\zeta \right)+(1-\zeta )B\left(\zeta \right)\mathrm{d}\zeta ,\hfill \\ \hfill B_k^{+}\left({\tau }_k\right)& ={\int }_0^{{\tau }_k}A\left(\zeta \right)B_k^{-}\left(\zeta \right)+\zeta B\left(\zeta \right)\mathrm{d}\zeta ,\hfill \\ \hfill S_k\left({\tau }_k\right)& ={\int }_0^{{\tau }_k}A\left(\zeta \right)S_k\left(\zeta \right)+S\left(\zeta \right)\mathrm{d}\zeta \hfill \end{array}$$

(19)

Proof. 

Choosing the $B_k^{-}$ as an example, then taking the derivative and invoking the chain rule yields

$$\begin{array}{cc}\hfill \frac{\mathrm{d}}{\mathrm{d}{\tau }_k}{B}_k^{-}\left({\tau }_k\right)& =\frac{\mathrm{d}}{\mathrm{d}{\tau }_k}{A}_k\left({\tau }_k\right){\int }_0^{{\tau }_k}{\Phi }^{-1}(\zeta ,0)B\left(\zeta \right)(1-\zeta )\mathrm{d}\zeta \hfill \\ \hfill & +A_k\left({\tau }_k\right){\Phi }^{-1}({\tau }_k,0)B\left({\tau }_k\right)(1-{\tau }_k)\hfill \\ \hfill & =A\left({\tau }_k\right)\Phi ({\tau }_k,0){\int }_0^{{\tau }_k}{\Phi }^{-1}(\zeta ,0)B\left(\zeta \right)(1-\zeta )\mathrm{d}\zeta \hfill \\ \hfill & +I·B\left({\tau }_k\right)(1-{\tau }_k)\hfill \\ \hfill & =A\left({\tau }_k\right)B_k^{-}\left({\tau }_k\right)+B\left({\tau }_k\right)(1-{\tau }_k)\hfill \end{array}$$

(20)

The first and second equal signs come from the properties of STM, while the last one is a simplification of the original form of $B_k^{-}$ from (18). With $B_k^{-}\left(0\right)=0$, the inverse-free form of $B_k^{-}\left({\tau }_k\right)$ is obtained as shown in (19). $B_k^{+}$ and $S_k$ can be obtained by the same process.    □

For simplicity, we define $0_k$ and $1_k$ as 0 and 1, respectively, which denote that t is in the sub-interval $[t_k,t_{k+1})$. Then evaluating the LTV system (13) at ${\tau }_k=1_k^{-}$, we obtain

$$\mathrm{\Delta }x\left(1_k^{-}\right)=A_k\mathrm{\Delta }x\left(0_k\right)+B_k^{-}\mathrm{\Delta }{u}_k+B_k^{+}\mathrm{\Delta }{u}_{k+1}+S_k\mathrm{\Delta }{s}_k$$

(21)

Since the reference trajectory may not satisfy the original dynamics (1) in the sub-interval $[t_k,t_{k+1})$, Equation (22) would give $N-1$ trajectory segments, which makes a discontinuity occur in the temporal time nodes $t_2,\dots ,t_N$. The stitching condition is introduced to obtain a continuous trajectory over the time horizon, as shown in Figure 1.

$$\mathrm{\Delta }x\left(1_k^{-}\right)+\overline{x}\left(1_k^{-}\right)=\mathrm{\Delta }x\left(1_k\right)+\overline{x}\left(1_k\right)$$

(22)

As a result, the discretized dynamics in terms of the deviations are as follows:

$$\mathrm{\Delta }{x}_{k+1}=A_k\mathrm{\Delta }{x}_k+B_k^{-}\mathrm{\Delta }{u}_k+B_k^{+}\mathrm{\Delta }{u}_{k+1}+S_k\mathrm{\Delta }{s}_k+x_{k+1}^{\mathrm{prop}}-{\overline{x}}_{k+1}$$

(23)

where $\mathrm{\Delta }{x}_k=\mathrm{\Delta }x\left(0_k\right)$, ${\overline{x}}_{k+1}=\mathrm{\Delta }x\left(1_k\right)$ and $x_{k+1}^{\mathrm{prop}}=\overline{x}\left(1_k^{-}\right)={\overline{x}}_k+{\int }_{0_k}^{1_k^{-}}F(\zeta ,\overline{x}\left(\zeta \right),\overline{u}\left(\zeta \right),\overline{s})\mathrm{d}\zeta$, which denotes the integration of the original dynamic in $[t_k,t_{k+1})$ from the reference trajectory.

Thus the discretized dynamics with respect to absolute variables are recovered from Equation (23):

$$x_{k+1}=A_k{x}_k+B_k^{-}{u}_k+B_k^{+}{u}_{k+1}+S_k{s}_k+x_{k+1}^{\mathrm{prop}}-(A_k{\overline{x}}_k+B_k^{-}{\overline{u}}_k+B_k^{+}{\overline{u}}_{k+1}+S_k{\overline{s}}_k)$$

(24)

With the idea of consistency of the original dynamics of the reference trajectory in sub-intervals and the addition of the stitching condition, the accuracy of the solution would be improved even over a sparse time grid.

3.3. Additional Penalty Terms in SCP Subproblem

In our experiments for the non-uniform scheme, it is observed that regardless of a hard trust region method with additional fixed constraints or a soft trust region method by augmenting the objective function with penalty terms, the method may not converge or converge very slowly, while the resulting propagated trajectory may violate the path constraints between temporal nodes as well.

As mentioned above, one reason is that an effective search direction should be given. Inspired by the PTR algorithm [24], we augment the objective function with additional penalty terms with respect to the nonlinear term of path constraints and the distance term of the no-fly zone, which would give a more purposeful direction to distribute the temporal nodes. As the path constraints exhibit high levels of nonlinearity, the logarithm transformation is used to mitigate this issue.

3.3.1. Log-Tranforms of Path Constraints

Consider the typical path constraints (5) of the hypersonic entry problem: heat rate, dynamic pressure, and normal load. Since the Tylor expansion of the original path constraints (5) would obtain complicated nonlinear terms, we take the logarithm transformation of both sides of (5):

$$\begin{array}{cc}\hfill ln\left(\dot{Q}\right)& =ln\left(k_Q\right)+0.5ln\left({\rho }_0\right)-\frac{R_0}{2h_s}(r-1)+3.15ln\left(V\right)\hfill \\ & \le ln\left({\dot{Q}}_{max}\right)\hfill \\ \hfill ln\left(q\right)& =ln\left(0.5\right)+ln\left({\rho }_0\right)-\frac{R_0}{h_s}(r-1)+2ln\left(V\right)\hfill \\ & \le ln\left(q_{max}\right)\hfill \\ \hfill ln\left(n\right)& =0.5ln(C_L^2+C_D^2)+ln\left(\frac{R_0{\rho }_0{S}_{ref}}{2m}\right)-\frac{R_0}{h_s}(r-1)+2ln\left(V\right)\hfill \\ \hfill & \le ln\left(n_{max}\right)\hfill \end{array}$$

(25)

Due to the monotonicity of logarithmic transformations, Equation (25) is equivalent to (5), while the transformed constraints are linear to orbital radius r and only nonlinear to velocity V. Thus, the Tylor expansion of the transformed constraints has a simpler nonlinear term $ln\left(V\right)$ than that of the original constraints.

Consider the second-order Tylor series expansion of $ln\left(V\right)$:

$$ln\left(V\right)=ln\left(\overline{V}\right)+\frac{1}{\overline{V}}(V-\overline{V})-\frac{1}{2{\overline{V}}^2}{(V-\overline{V})}^2$$

(26)

Then we replace $ln\left(V\right)$ in (25) with the primary term $ln\left(\overline{V}\right)+\frac{1}{\overline{V}}(V-\overline{V})$ as the linearization of the transformed constraints and augment the objective function with the nonlinear term $\frac{1}{{\overline{V}}^2}{(V-\overline{V})}^2$, since the quadratic term is convex. With the variable time-step scheme and additional nonlinear penalty term, this would give the optimizer a target or direction to incentivize the temporal nodes to cluster around the highly nonlinear region.

3.3.2. Distance Penalty Term of No-Fly Zone

The propagated trajectory between temporal nodes could be within the no-fly zone because of the sparse time interval, as shown in Figure 2. In order to address the above issue, prior work is to set a dense time grid around the no-fly zone in advance. Instead of that, our method can allow discrete points to adaptively cluster around the no-fly zone during iteration, which would take full advantage of the non-uniform scheme.

The nonlinear no-fly zone constraints (6) can be linearized with first-order Tylor expansion:

$$2(\overline{\theta }-{\theta }_{NFZ})\theta +2(\overline{\varphi }-{\varphi }_{NFZ})\varphi \ge d$$

(27)

where $d=R_{NFZ}^2-{(\overline{\theta }-{\theta }_{NFZ})}^2-{(\overline{\varphi }-{\varphi }_{NFZ})}^2+2(\overline{\theta }-{\theta }_{NFZ})\overline{\theta }+2(\overline{\varphi }-{\varphi }_{NFZ})\overline{\varphi }$.

We penalize the distance from the trajectory to the center of the no-fly zone $\sum {(\theta -{\theta }_{NFZ})}^2+{(\varphi -{\varphi }_{NFZ})}^2$ in the objective function, which would give the solver a search direction with physical significance. With the linear no-fly zone constraints (27), the discrete points would tend to cluster around the no-fly zone and disperse beyond it.

Note that the penalty term of the no-fly zone should be restricted to several temporal nodes to prevent all nodes from gathering around the no-fly zone.

3.4. Discrete Convex Subproblem and Iteration Algorithm

After the appeal discussion, we summarize the discrete convex sub-problem as shown in Problem 2. The control difference is regarded as the control rate constraint because of the FOH approximation of control. A hard trust region on time dilation is enforced in constraints, while the objective function is augmented with trust region terms of state and control, and two additional penalty terms: the nonlinear term of path constraints and distance penalty term of the no-fly zone. The trust term of time dilation can be omitted since the penalty terms of path constraints and the no-fly zone have shown a good iterative performance in our experiment. Thus, the SCP iteration process is given in Algorithm 1 as follows:

Algorithm 1: Improved SCP algorithm with adaptive non-uniform discretization

Problem 2.

Discrete convex subproblem in SCP iteration

$$\begin{array}{cc}\hfill \underset{x,u}{min}J& =-V_N+\underset{J_{tr,1}}{\underbrace{{\omega }_{tr,1}\sum _{k=1}^N\parallel x_k-{\overline{x}}_k{\parallel }_2^2+{\parallel u_k-{\overline{u}}_k\parallel }_2^2}}+\underset{J_{tr,2}}{\underbrace{\left({\omega }_{tr,2}\sum _{k=1}^{N-1}{\parallel s_k-{\overline{s}}_k\parallel }^2\right)}}\hfill \\ \hfill & +\underset{J_{nl}}{\underbrace{{\omega }_{nl}\sum _{k=1}^N\frac{\parallel V_k-{\overline{V}}_k{\parallel }^2}{{\overline{V}}_k^2}}}+\underset{J_{NFZ}}{\underbrace{{\omega }_{NFZ}\sum _{k=i_j^1}^{i_j^n}\parallel {\theta }_k-{\theta }_{NFZ}{\parallel }^2|+\parallel {\varphi }_k-{\varphi }_{NFZ}{\parallel }^2}}\hfill \\ \hfill s.t.& \forall k=1,\dots ,N\hfill \\ \hfill & x_{k+1}=A_k{x}_k+B_k^{-}{u}_k+B_k^{+}{u}_{k+1}+S_k{s}_k+d_k\hfill \\ \hfill & x_1=x_0\hfill \\ \hfill & x_N=x_f\hfill \\ \hfill & -{\sigma }_{max}\le u_k\le {\sigma }_{max}\hfill \\ \hfill & -d{u}_{max}\le \frac{u_{k+1}-u_k}{{\overline{s}}_k}\le d{u}_{max}\hfill \\ \hfill & {\overline{p}}_{1,k}-\frac{R_0}{2h_s}r+\frac{3.15}{{\overline{V}}_k}(V-{\overline{V}}_k)\le ln\left({\dot{Q}}_{max}\right)\hfill \\ \hfill & {\overline{p}}_{2,k}-\frac{R_0}{h_s}r+\frac{2}{{\overline{V}}_k}(V-{\overline{V}}_k)\le ln\left(q_{max}\right)\hfill \\ \hfill & {\overline{p}}_{3,k}-\frac{R_0}{h_s}r+\frac{2}{{\overline{V}}_k}(V-{\overline{V}}_k)\le ln\left(n_{max}\right)\hfill \\ \hfill & 2({\overline{\theta }}_k-{\theta }_{NFZ}){\theta }_k+2({\overline{\varphi }}_k-{\varphi }_{NFZ}){\varphi }_k\ge d_{NFZ}\hfill \\ \hfill & {\mathrm{\Delta }}_{min}\le s_k\le {\mathrm{\Delta }}_{max}\hfill \\ \hfill & \sum _{k=1}^{k-1}{s}_k=t_f\hfill \\ \hfill & -\mathrm{\Delta }{T}_{max}\le s_k-{\overline{s}}_k\le \mathrm{\Delta }{T}_{max}\hfill \end{array}$$

(28)

where ${\overline{p}}_{i,k},i=1,2,3$ and $d_{NFZ}$ can be obtained from (25) and (27), and $i_j^1,\dots i_j^n$ are the temporal nodes set to cluster around the no-fly zone. Note that the augmented objective with $J_{tr,1}$ and $J_{tr,2}$ is the general PTR algorithm for non-uniform scheme.

4. Numerical Results

In this section, the effectiveness of the proposed method is verified compared with a different SCP algorithm, as shown in

Table 2. Comparative SCP methods.

Name	Method	Reference
SCP1	non-uniform precise discretization	[31]
SCP2	uniform precise discretization	[29,32]
SCP3	uniform FOH discretization	[27]

. Two cases are considered to focus on distinct instances of constraint violation. Case 1 does not include the no-fly zone constraint and focuses on potential violations of path constraints as the number of discrete points decreases. With an additional no-fly zone constraint, Case 2 focuses on the phenomenon that the propagated trajectory may pass through the no-fly zone.

The reference area and the mass of the vehicle are $S_{ref}=391.22$ m² and $m=104,305.0$ kg. The aerodynamic coefficients depend on the attack angle $\alpha$ (in degrees), while the angle-of-attack profile depends on the vehicle’s velocity:

$$\begin{array}{cc}\hfill C_L& =-0.041065+0.016292\alpha +0.0002602{\alpha }^2\hfill \\ \hfill C_D& =0.080505-0.03026C_L+0.86495C_L^2\hfill \end{array}$$

(29)

$$\alpha =\left\{\begin{array}{cc}40,\hfill & \hfill \mathrm{if}V>4570\mathrm{m}/\mathrm{s}\\ 40-0.20705{(V-4570)}^2/{340}^2,\hfill & \hfill \mathrm{else}\end{array}\right.$$

(30)

The remaining simulation parameters are shown in

Table 3. Parameters for entry problem.

Parameter	Value	Parameter	Value
$t_f$	1600 s
$h_0$	100 km	$h_f$	25 km
${\theta }_0$	0 deg	${\theta }_f$	12 deg
${\varphi }_0$	0 deg	${\varphi }_f$	70 deg
$V_0$	7450 m/s	${\gamma }_f$	−10 deg
${\gamma }_0$	0 deg	${\psi }_f$	90 deg
${\psi }_0$	0 deg	${\sigma }_{max}$	80 deg
$d{u}_{max}$	10 deg/s	$k_Q$	1.65 × 10−4
${\dot{Q}}_{max}$	1500 $\mathrm{kW}/{\mathrm{m}}^2$	$q_{max}$	18,000 $\mathrm{N}/{\mathrm{m}}^2$
$n_{max}$	2.5 g	${\mathrm{\Delta }}_{min}$	10 s
${\mathrm{\Delta }}_{max}$	150 s	$\mathrm{\Delta }{T}_{max}$	50 s
No-fly zone used in Case 2
${\theta }_{NFZ}$	5 deg	${\varphi }_{NFZ}$	50 deg
$R_{NFZ}$	5.5 deg

, which is the same as [27].

The subproblem is constructed in YALMIP [34], a MATLAB modeling toolbox, and solved by ECOS [21], an open-source convex optimization solver. All of the numerical simulations are running on a personal desktop with an Intel Core i9 3.1 GHz processor.

The initial reference is the trajectory obtained by integrating the original dynamics (1) with the given initial control, as in [27]. The convergence condition is selected as ${\mathrm{\Delta }}_x=\underset{1\le i\le N}{max}|{\widehat{x}}_i^k-{\overline{x}}_i|\le {ϵ}_x=[1000\mathrm{m},1\mathrm{deg},1\mathrm{deg},100\mathrm{m}/\mathrm{s},1\mathrm{deg},1\mathrm{deg}]$ and ${\mathrm{\Delta }}_s=\underset{1\le i\le N-1}{max}|{\widehat{s}}_i^k-{\overline{s}}_i|\le {ϵ}_s=5\mathrm{s}$, where $\widehat{x}$ and $\widehat{s}$ are the solution of the subproblem.

4.1. Iterative Performance

Comparisons of the state and control profiles for Case 1 and Case 2 are shown in Figure 3, Figure 4, Figure 5, Figure 6, Figure 7 and Figure 8. The account of temporal nodes for the proposed method are as follows. SCP1 and SCP2 are each 40, in which case the constraint violation is observed, and SCP3 is 300 in order to maintain the same accuracy as the above methods.

It can be seen that the solutions of the proposed method are similar to those from SCP2 and SCP3. Note that the solutions of SCP1 are quite different due to its poor convergence. As shown in Figure 5 and Figure 8, one shortcoming of the proposed method is that the control jitter in the segments where points cluster is obvious because of the dense time grid around those points.

The iterative performance of the proposed method and comparative methods is shown in Figure 9. The iteration number of the proposed method is less than the comparative methods. Note that the comparative methods would require more iterations to meet the convergence condition when the objective function is near the optimal value, while the proposed method required fewer iterations, demonstrating its fast convergence. What is more, for the non-uniform scheme, SCP1 with light penalty weighting ${\omega }_{tr,2}<1$ in $J_{tr,2}$ reached the maximum number of iterations, while that with heavy weighting ${\omega }_{tr,2}>1$ would show negligible alterations for the distribution of temporal nodes; thus, the addition of time interval dilation is not necessary in this case.

For the non-uniform scheme, the discrete point distribution and the change of time dilation, $max|s-\overline{s}|$, with iterations are shown in Figure 10. It can be observed that there is no obvious clustering rule for SCP1, while the position of discrete points always changes with the number of iterations. In contrast, the discrete points of the proposed method would cluster after a few iterations. In addition, the results of time interval change with iterations show that the proposed method has stable convergence performance, since the change between two adjacent iterations decreases progressively, which means the result trajectory would become increasingly similar.

In order to guarantee that the result trajectory of the SCP process is feasible to meet the original dynamics, the residual error between the optimized results and the trajectory obtained by integrating the original dynamics is measured. The residual error results for Case 1 and Case 2 are shown in

Table 4. Residual error with different temporal nodes for Case 1.

Method	Temporal Node	Iteration	CPU Time (s)	$\mathrm{\Delta }\mathit{r}$ (m)	$\mathrm{\Delta }\mathit{\theta }$ (deg)	$\mathrm{\Delta }\mathit{\varphi }$ (deg)	$\mathrm{\Delta }\mathit{V}$ (m/s)	$\mathrm{\Delta }\mathit{\gamma }$ (deg)	$\mathrm{\Delta }\mathit{\psi }$ (deg)
Proposed	40	8	1.409	22.577	0.044	0.011	1.910	0.022	0.072
	50	8	1.452	18.457	0.042	0.010	1.509	0.018	0.046
	60	10	1.758	3.798	0.035	0.005	1.249	0.019	0.027
SCP1	40	30	5.247	814.272	0.323	0.128	25.132	1.196	0.203
	50	30	5.790	627.624	0.218	0.105	28.145	0.288	1.544
	60	30	6.293	587.483	0.203	0.097	26.329	0.279	1.416
SCP2	40	14	2.085	5.201	0.005	0.002	0.236	0.001	0.010
	50	14	2.2370	21.356	0.011	0.006	0.851	0.004	0.055
	60	14	2.548	11.769	0.002	0.001	0.172	0.040	0.051
SCP3	200	15	5.5930	95.028	0.001	0.001	0.496	0.147	0.282
	300	19	9.857	42.689	0.004	0.002	0.048	0.034	0.085
	400	20	14.833	27.497	0.005	0.002	0.287	0.005	0.018

and

Table 5. Residual error for entry problem for Case 2.

Method	Temporal Node	Iteration	CPU Time (s)	$\mathrm{\Delta }\mathit{r}$ (m)	$\mathrm{\Delta }\mathit{\theta }$ (deg)	$\mathrm{\Delta }\mathit{\varphi }$ (deg)	$\mathrm{\Delta }\mathit{V}$ (m/s)	$\mathrm{\Delta }\mathit{\gamma }$ (deg)	$\mathrm{\Delta }\mathit{\psi }$ (deg)
Proposed	40	8	1.262	14.279	0.017	0.006	0.076	0.015	0.083
	50	8	1.403	9.201	0.025	0.007	0.426	0.022	0.032
	60	9	1.730	9.907	0.030	0.005	0.813	0.012	0.006
SCP1	40	30	4.234	1045.231	0.445	0.280	86.537	0.994	3.329
	50	30	4.532	654.915	0.192	0.111	29.263	0.279	1.672
	60	30	5.054	516.706	0.175	0.084	22.669	0.258	1.145
SCP2	40	11	1.437	5.142	0.002	0.004	0.234	0.002	0.015
	50	15	1.771	5.453	0.001	0.001	0.164	0.004	0.016
	60	15	1.932	5.285	0.001	0.0004	0.126	0.017	0.026
SCP3	200	11	3.891	97.585	0.002	0.001	0.682	0.224	0.573
	300	13	6.760	47.728	0.002	0.002	0.462	0.094	0.281
	400	15	10.334	36.177	0.005	0.006	0.586	0.057	0.182

, averaged over 50 simulation runs.

It can be observed that for SCP2 and SCP3, the precise discretization can guarantee commensurate accuracy with fewer temporal nodes, while more than 200 nodes are needed to achieve the same result in [27], which demonstrates the effectiveness of the precise discretization. Note that, with the non-uniform scheme, SCP1 showed worse convergence performance and low accuracy as mentioned above, which means that the feasibility of the result trajectory is not guaranteed. In contrast, the proposed method overcomes the shortcomings, maintains the advantage of the precise discretization, and shows better convergence performance, as shown in Table 4 and Table 5.

4.2. Constraint Satisfaction Performance

As mentioned above, for the uniform precise discretization, the propagated trajectory may violate the path constraints between temporal nodes due to the sparse time grid. This phenomenon was observed to occur when the account of temporal nodes decreased to 40. Thus, we choose the case of 40 nodes for presentation. The path constraints of the proposed method for Case 1 are shown in Figure 11 and contrasted with SCP1 and SCP2.

It can be observed that the heat load would touch the boundary during the initial state of flight. The constraint violations occur for the propagated trajectory of SCP1 and SPC2 due to the sparse time grid. In contrast, the propagated trajectory of the proposed method satisfies the path constraints over the time horizon. In addition, the discrete points would cluster around the peak value of heat load, as shown in Figure 11.

Furthermore, the same phenomenon may occur for the no-fly zone constraints as well. Case 2 focuses on the no-fly zone constraint violation, and the numerical results are shown in Figure 12. In this case, all the path constraints were satisfied for all methods in our experiments. It can be observed that the propagated trajectories of SCP1 and SCP2 both pass through the no-fly zone. Note that, for the non-uniform scheme, the discrete points of SCP1 do not cluster around the no-fly zone. The 3D trajectory of tbe proposed method is shown in Figure 12b; the points in red are set to cluster around the no-fly zone. The propagated trajectory of the proposed method skimmed over the no-fly zone, while the time interval between adjacent red points is the minimum set in (11), which indicated that the point distribution of the proposed method is better than SCP2.

5. Conclusions

This paper proposes an improved SCP algorithm for the hypersonic entry problem using a novel adaptive non-uniform discretization. The proposed method has advantages in performance of path constraint satisfaction and convergence. Firstly, the proposed method employs an inverse-free precise discretization to ensure high accuracy and real-time performance. Then, an adaptive non-uniform scheme is developed to distribute discrete points adaptively by adding additional penalty terms in the SCP subproblem, which would guarantee constraint satisfaction. Finally, numerical results show that the proposed method achieves a fast convergence while maintaining high accuracy with few temporal nodes. More importantly, the discrete points of the proposed method would cluster around the segment where the constraints may be violated, and the propagated trajectory satisfies all the path constraints over the time horizon even for a small number of discrete points.

Future work will focus on the following points: (1) Due to the non-uniform scheme, a similar idea can extend to the hypersonic entry problem with the waypoint constraint and other problems; (2) The simulation will be carried out on an embedded platform to verify the effectiveness of the proposed method for a limited-power environment; (3) High-performance solvers are considered to further improve the solving speed, such as the proportional integral projected gradient method [35], a first-order method for the conic convex problem.