Adaptive Trajectory Tracking Algorithm for the Aerospace Vehicle Based on Improved T-MPSP

Abstract

To deal with the uncertainty and disturbance that exist in the tracking system of an aerospace vehicle, an adaptive trajectory-tracking method based on a novel tracking model predictive static programming (T-MPSP) is proposed. Firstly, to make the proposed method more adaptive to uncertain parameter deviations, an extended Kalman filter (EKF) parameter correction strategy is designed. Then, the control constraints are considered to form a novel T-MPSP algorithm. By combining the parameter correction strategy with the improved T-MPSP algorithm, a novel adaptive tracking guidance scheme is presented. Finally, simulations are carried out to demonstrate the effectiveness of the proposed method.

1. Introduction

For the past few decades, aerospace vehicles have been applied in both military and civilian fields, and their performance has been very compelling. For missions involving aerospace vehicles, a good trajectory-tracking ability is the essential prerequisite for the successful application of the vehicles [1]. In a traditional trajectory-tracking process for an aerospace vehicle, the dynamic model of the vehicle and the desired trajectory satisfying all the constraints are given in advance. Then, the control methods are designed to guide the vehicle to track the desired trajectory. However, the trajectories of aerospace vehicles usually cover a wide range of altitudes, which may lead to a dramatic change in the atmospheric environment during the trajectory-tracking process. In addition to the disturbance brought about by the dramatic change in the external environment, there are also uncertainties about the dynamic system of the vehicles [2,3]. Thus, the precise model information is not always available, and the traditional trajectory-tracking method cannot achieve satisfactory performance under these circumstances. As a result, the design of the novel trajectory-tracking control method to cope with disturbance and uncertainty is very important.

One of the most commonly used control architectures for trajectory-tracking control is the proportional-integral-derivative (PID) controller and its variants [4,5,6,7], which are known for the simplicity of their framework. To improve the tracking performance, many modern control theories, including intelligent optimization [4], feedback control [5], and fuzzy control [6,7], have been applied to the trajectory-tracking method design process. Another popular control architecture for trajectory-tracking control derives from the sliding mode control theory. In [8], the sliding mode control theory is applied to make the tracking error converge to zero in a finite time period. As an improvement of the method in [8], the fixed-time control theory was combined with the sliding mode control theory. The robustness of the resulting tracking laws was further enhanced [9,10]. Recently, the neural network and adaptive updating laws were applied to the sliding mode control framework. The convergence of the tracking error was ensured by the sliding mode control approach, and any system uncertainties were handled by the neural network [11].

In addition, the nonlinear model predictive control (NMPC) was applied to the trajectory-tracking control [12,13,14,15]. However, several issues exist in the implementation of the NMPC-based tracking methods, which are summarized as follows: (1) a universal and exact model of the whole tracking system is difficult to acquire, (2) the calculation speed of the NMPC cannot meet the requirement in practice. A feasible way to cope with these issues is to use model predictive static programming (MPSP) [16], which combines the characteristics of approximate dynamic programming [17] and NMPC. The MPSP has been proven to be an effective method to cope with two-point boundary value problems with terminal constraints, which has significant advantages: (1) the terminal constraints are transformed into linear equality constraints with only the control variable to be optimized, (2) the closed analytical expression of the appropriate objective function is acquired from the static algorithm, (3) the sensitivity matrix of the algorithm can be calculated skillfully using recursion, which improves the calculation speed. The MPSP method has been used in many applications, such as in the trajectory control of launch vehicles, reentry guidance, cooperative control, etc. [18,19,20]. Typically, the MPSP method aims to improve the terminal accuracy by iteratively discretizing and updating. For a trajectory-tracking problem, only the terminal-tracking accuracy can be guaranteed using the MPSP method, and its computing time increases exponentially with the number of discrete nodes that are tracked. To deal with this drawback, the T-MPSP is put forward to track a trajectory over a receding horizon window. Meanwhile, the predicted time horizon can be set manually to balance the computational efficiency and the precision, which is another advantage of the T-MPSP algorithm [21,22].

Some trajectory-tracking methods are based directly on the input and output data of the system. For example, a robust model-free controller for trajectory-tracking control is proposed in Ref. [23]. No dynamic model information of the controlled system is needed, and the resulting controller is a combination of the PID controller and the sliding mode control. As an improvement of the work in Ref. [23], a forecasting-based data-driven model-free adaptive sliding mode attitude control method is proposed for the post-capture combined spacecraft with unknown inertial properties and external disturbances [24]. To cope with the unknown dynamics of the system, a model-free control method is proposed via the time-varying compensation of the un-modeled system [25]. An iterative sliding mode control technique is utilized to design the adaptive model-free trajectory-tracking method in Ref. [26]. Although these model-free control methods can deal with model uncertainties, the operation data require a huge memory size, and the computing burden is heavy.

Hence, considering the uncertainty and disturbance that exist in the tracking system of the aerospace vehicle, an adaptive trajectory-tracking method based on the novel T-MPSP is proposed. Firstly, to cope with the uncertain parameter deviations, an EKF parameter correction strategy is designed. Then, the control constraints are considered to form a novel T-MPSP algorithm. By combining the parameter correction strategy with the improved T-MPSP algorithm, an adaptive tracking guidance scheme is presented. Finally, simulations are carried out under various deviation conditions to verify the reliability and robustness of the proposed method.

The main contributions of this paper are summarized as follows:

(1)

To our best knowledge, no existing methods have applied the EKF with the T-MPSP to solve the trajectory problems of aerospace vehicles.

(2)

Compared with the MPSP method in [18,19,20], the proposed method has a fast computing speed and high accuracy.

(3)

Compared with [21,22,23], the proposed scheme can cope with the control constraints.

The rest of this paper is organized as follows: Section 2 presents the model of the aerospace vehicle. Section 3 presents the online parameter identification method and the improved T-MPSP algorithm. The simulations are presented in Section 4. Finally, the conclusion is presented in Section 5.

2. Model of the Aerospace Vehicle

In this section, first, the dynamic model describing the motion of the aerospace vehicle is presented. Then, the constraints that should be satisfied are introduced.

2.1. Dynamic Equations

The three-dimensional mass point dynamic equations [22,27] of the aircraft in the longitudinal plane are

$$\left\{\begin{array}{l}\frac{dh}{dt}=v\sin \gamma \\ \frac{dv}{dt}=\frac{T\cos \alpha -D}{m}-g\sin \gamma \\ \frac{d\gamma }{dt}=\frac{T\sin \alpha +L}{mv}-\frac{g}{v}\cos \gamma +\frac{v}{r}\cos \gamma \\ \frac{dm}{dt}=-\frac{T}{g_0{I}_{\mathrm{sp}}}\end{array}\right.$$

(1)

where $h$, $v$, $\gamma$, $\alpha$ and $m$ represent the altitude, velocity, flight path angle, angle of attack (AOA), and mass, respectively. $g$ and $g_0$ represent the gravitational acceleration at the current altitude and on the earth’s surface, respectively. $I_{sp}$ represents the specific impulse of the engine. T, L and D denote the engine thrust, aerodynamic lift, and drag, respectively, which are defined as

$$\left\{\begin{array}{l}T=0.029\varphi I_{sp}\rho g_{0}v{C}_T{A}_C\\ L=\frac{1}{2}\rho v^2{S}_{\mathrm{ref}}{C}_L(\mathrm{Ma},\alpha )\\ D=\frac{1}{2}\rho v^2{S}_{\mathrm{ref}}{C}_D(\mathrm{Ma},\alpha )\end{array}\right.$$

(2)

where $C_T$, $C_L$, and $C_D$ denote the thrust, lift and drag coefficients, respectively. $\varphi$ denotes the throttle. $\rho$, $S_{\mathrm{ref}}$ and $\mathrm{Ma}$ denote the atmospheric density, reference area, and Mach number, respectively.

2.2. Flight Constraints

To ensure flight safety, the dynamic pressure constraint is considered, which is defined as

$$q=\frac{1}{2}\rho v^2\le q_{\max }$$

(3)

In addition, the aircraft must satisfy the terminal constraints, which are described as

$$\left\{\begin{array}{l}\left|h_f-h_{{}_f}^{*}\right|\le {\epsilon }_h\\ \left|v_f-v_{{}_f}^{*}\right|\le {\epsilon }_v\\ \left|{\gamma }_f-{\gamma }_{{}_f}^{*}\right|\le {\epsilon }_{\gamma }\end{array}\right.$$

(4)

where the subscript $f$ refers to the final value and the superscript $*$ refers to the desired value. ${\epsilon }_h$, ${\epsilon }_v$, and ${\epsilon }_{\gamma }$ represent the deviation thresholds of the terminal altitude, velocity, and flight path angle, respectively.

The control variables considered are $\alpha$ and the throttle $\varphi$, the constraints of which are as follows:

$$\left\{\begin{array}{l}{\alpha }_{\min }\le \alpha \le {\alpha }_{\max }\\ {\varphi }_{\min }\le \varphi \le {\varphi }_{\max }\end{array}\right.$$

(5)

Additionally, to prevent the control variable from changing too drastically, the rate of AOA must satisfy the following constraint:

$${\dot{\alpha }}_{\min }\le \dot{\alpha }\le {\dot{\alpha }}_{\max }$$

(6)

3. The Trajectory-Tracking Strategy

In this section, we will introduce an online parameter identification method and an improved T-MPSP algorithm. By combining these two methods, an adaptive trajectory-tracking guidance algorithm based on an improved T-MPSP will be presented.

3.1. Online Parameter Identification Method

Since an accurate model can improve the trajectory-tracking accuracy, a parameter correction strategy based on the EKF is designed in this subsection. In the actual tracking process, there are deviations in the parameters of atmospheric density, thrust coefficient, lift coefficient, and drag coefficient, which will make the model inaccurate. We define these parameters as

$$\rho ={\rho }^{*}\left(1+\Delta \rho \right),C_T=C_{{}_T}^{*}\left(1+\Delta C_T\right),C_L=C_{{}_L}^{*}\left(1+\Delta C_L\right),C_D=C_D^{*}\left(1+\Delta C_D\right)$$

(7)

where ${\rho }^{*}$, $C_{{}_T}^{*}$, $C_{{}_L}^{*}$ and $C_D^{*}$ are the desired values. $\rho$, $C_T$, $C_L$ and $C_D$ are the actual values. $\Delta \rho$, $\Delta C_T$, $\Delta C_L$, and $\Delta C_D$ respectively represent the unknown deviation of each parameter. These unknown deviations are written in an overall form as

$$\beta ={\left[\Delta \rho \hspace{1em}\Delta C_T\hspace{1em}\Delta C_L\hspace{1em}\Delta C_D\right]}^T$$

(8)

Defining $x=\left[\begin{array}{cccc}h& v& \gamma & m\end{array}\right]$, the following dynamic equations are obtained from (1) and (7) as

$$\dot{x}=f\left(x,u,\beta \right)$$

(9)

in which

$$f\left(x,u,\beta \right)=\left[\begin{array}{c}v\sin \gamma \\ \frac{T^{*}\left(1+\Delta \rho \right)\left(1+\Delta C_T\right)\cos \alpha -D^{*}\left(1+\Delta \rho \right)\left(1+\Delta C_D\right)}{m}-g\sin \gamma \\ \frac{T^{*}\left(1+\Delta \rho \right)\left(1+\Delta C_T\right)\sin \alpha +L^{*}\left(1+\Delta \rho \right)\left(1+\Delta C_L\right)}{mv}-\frac{g}{v}\cos \gamma +\frac{v}{r}\cos \gamma \\ -\frac{T^{*}\left(1+\Delta \rho \right)\left(1+\Delta C_T\right)}{g_0{I}_{sp}}\end{array}\right]$$

(10)

Since the EKF algorithm [28] needs to augment the unknown parameters into the state variables of the system, the new augmented dynamic equation is defined as

$${\dot{x}}_a=f_a\left(x_a,u\right)+{\omega }_a=\left[\begin{array}{c}f\left(x,u,\beta \right)\\ 0\end{array}\right]+{\omega }_a$$

(11)

where

$$\left\{\begin{array}{l}{x}_a={\left[h\hspace{1em}v\hspace{1em}\gamma \hspace{1em}m\hspace{1em}\Delta \rho \hspace{1em}\Delta C_T\hspace{1em}\Delta C_L\hspace{1em}\Delta C_D\right]}^T\\ u={\left[\begin{array}{cc}\alpha & \varphi \end{array}\right]}^T\end{array}\right.$$

(12)

${\omega }_a$ represents the uncorrelated zero-mean white Gaussian noise. To identify the unknown parameters, the EKF algorithm also requires the measurement information of the unknown parameters. The measurement equation is expressed as

$$y=\left[\begin{array}{c}\begin{array}{c}\begin{array}{l}{x}_m\\ a_{xm}\end{array}\\ a_{zm}\end{array}\\ q_m\end{array}\right]=h\left(x_a,u\right)+v_a=\left[\begin{array}{c}\begin{array}{c}x\\ \frac{\rho v^2}{2m}{C}_x+\frac{T}{m}\\ \frac{\rho v^2}{2m}{C}_z\end{array}\\ \frac{\rho v^2}{2}\end{array}\right]+v_a$$

(13)

where $x_m$, $a_{xm}$, $a_{zm}$ and $q_m$ denote the state values, axial acceleration, normal acceleration and dynamic pressure measured by the sensor, respectively. $v_a$ represents the uncorrelated zero-mean Gaussian white noise. $C_x$ and $C_z$ refer to axial and normal force coefficients, respectively. The calculation formulas of $C_x$ and $C_z$ are

$$\left\{\begin{array}{l}{C}_x=C_L\sin \alpha -C_D\cos \alpha \\ C_z=-C_L\cos \alpha -C_D\sin \alpha \end{array}\right.$$

(14)

Then, the two-step online parameter identification method will be introduced as follows:

• Prediction

First, the prior estimate of the state at the current time instant $k$ is

$${\widehat{x}}_{a_k}^{-}={\widehat{x}}_{a_{k-1}}+f\left({\widehat{x}}_{a_{k-1}}\right)\Delta t+\frac{F_{k}f\left({\widehat{x}}_{a_{k-1}}\right)}{2}\Delta t^2$$

(15)

where $F_k$ represents the Jacobian matrix of the augmented state equation $f_a\left({\widehat{x}}_{a_{k-1}}\right)$ to estimate the state ${\widehat{x}}_{a_{k-1}}$ at the previous time instant and $\Delta t$ represents the sampling time interval. Then, the error covariance matrix of the current time instant $k$ according to the equation is acquired as

$$P_k^{-}={\Phi }_k{P}_{k-1}{\Phi }_k^T+Q_k$$

(16)

where ${\Phi }_k=I+F_k\Delta t$ represents the state transition matrix, and $Q_k$ represents the noise covariance matrix.

b.

Update

Using the error covariance matrix $P_k^{-}$, we update the Kalman filter gain coefficient $K_k$ at the current time instant $k$ as

$$K_k=P_k^{-}{H}_k{}^T{\left(H_k{P}_k^{-}{H}_k^T+V_k\right)}^{-1}$$

(17)

where $V_k$ represents the measurement noise covariance matrix, and $H_k$ represents the Jacobian matrix of the prior estimate ${\widehat{x}}_{a_k}^{-}$ of the state from the measurement equation $y\left({\widehat{x}}_{a_k}^{-}\right)$. Subsequently, the error covariance matrix is updated as

$$P_k=\left(I-K_k{H}_k\right)P_{{}_k}^{-}{\left(I-K_k{H}_k\right)}^T+K_k{R}_k{K}_k{}^T$$

(18)

The measurement correction is updated as

$$\Delta y_k=y_k-y\left({\widehat{x}}_{a_k}^{-}\right)$$

(19)

where $y_k$ represents the actual measurement value.

Finally, we update the posterior estimate of the state at the current time instant $k$ using

$${\widehat{x}}_{a_k}={\widehat{x}}_{a_k}^{-}+K_k\Delta y_k$$

(20)

${\widehat{x}}_{a_k}$ of the augmented state has been obtained through the EKF algorithm, from which the estimated value of the corresponding unknown parameter $\beta$ is also obtained. Thus, the new model that accounts for uncertain derivations is obtained by substituting the estimated value of $\beta$ into (10).

Remark 1.

An imprecise model will make it difficult for the trajectory-tracking method design, and uncertain parameter deviations will affect the accuracy of the model. Hence, a parameter correction strategy based on the EKF is designed in this subsection. By applying the EKF algorithm, the augmented state ${\widehat{x}}_{a_k}$ is obtained, from which the estimated value of the corresponding unknown parameter $\beta$ is also obtained. Then, the exact model that considers uncertain derivations is obtained.

3.2. Improved T-MPSP Algorithm

According to (1), the state variables are $X={\left[h,v,\gamma ,m\right]}^T$, the control variables are $U={\left[\alpha ,\varphi \right]}^T$, and the output variables are $Y={\left[h,v,\gamma \right]}^T$. The discrete form of the state equation and output equation of a continuous nonlinear system is expressed as

$$X_{k+1}^i=F_k\left(X_k^i,U_k^i\right)$$

(21)

$$Y_k^i=h\left(X_k^i\right)$$

(22)

where $X\in {ℝ}^n$, $U\in {ℝ}^m$ and $Y\in {ℝ}^p$ denote the state, control, and output variables, respectively. $k=1,2,\cdots ,N_y$ represents the $k$th sampling point, and $N_y$ represents the total length of the prediction time domain. $k=1$ and $k=N_y$ represent the starting and ending points of the prediction time domain, respectively, and $i$ represents the number of iterations.

The main objective of the trajectory-tracking algorithm is to find the suitable control variables $U_k^{i+1}$ to make the output $Y_k^{i+1}$ as close to the desired output $Y_k^{*}$$(k=2,3,\cdots ,N_y)$ as possible at each sampling time; that is, $Y_k^{i+1}\to Y_k^{*}$, where $U_k^i$ represents the control variables at the current $i$th iteration, and $U_k^{i+1}$ represents the control variables for the next iteration.

Similarly, $Y_k^i$ is the current output, and $Y_k^{i+1}$ is the output of the next iteration. We should also add a performance index that minimizes the deviation of the control variables to avoid overly drastic changes in the control variables. Therefore, the following objective function is proposed:

$$J=\frac{1}{2}{\displaystyle \sum _{k=2}^{N_y}{\left(Y_k^{i+1}-Y_k^{*}\right)}^T{Q}_k^i\left(Y_k^{i+1}-Y_k^{*}\right)}+\frac{1}{2}{\displaystyle \sum _{k=1}^{N_y-1}{\left(U_k^{i+1}-U_k^i\right)}^T{R}_k^i\left(U_k^{i+1}-U_k^i\right)}$$

(23)

where $Q_k^i$ and $R_k^i$ are both the positive definite weight matrix at the $i$th iteration.

The deviation vectors between two consecutive iterations at the same time are defined as follows:

$$Y_k^{i+1}=Y_k^i+\Delta Y_k^i$$

(24)

$$X_k^{i+1}=X_k^i+\Delta X_k^i$$

(25)

$$U_k^{i+1}=U_k^i+\Delta U_k^i$$

(26)

By expanding the Taylor series of $\Delta Y_k^i$ and ignoring its higher-order terms, we obtain

$$\Delta Y_k^i\approx d{Y}_k^i=\left[\frac{\partial Y_k}{\partial X_k}\right]d{X}_k^i$$

(27)

Similarly, we obtain

$$\Delta X_{k+1}^i\approx d{X}_{k+1}^i=\left[\frac{\partial F_k}{\partial X_k}\right]d{X}_k^i+\left[\frac{\partial F_k}{\partial U_k}\right]d{U}_k^i$$

(28)

where $d{X}_k^i$ and $d{U}_k^i$ represent deviations in the state variables and control variables at the $k$th time instant, respectively. By substituting (27) into (26), we obtain

$$d{Y}_k^i=\left[\frac{\partial Y_k}{\partial X_k}\right]\left[\frac{\partial F_{k-1}}{\partial X_{k-1}}\right]d{X}_{k-1}^i+\left[\frac{\partial Y_k}{\partial X_k}\right]\left[\frac{\partial F_{k-1}}{\partial U_{k-1}}\right]d{U}_{k-1}^i$$

(29)

where $d{X}_{k-1}$ is also expanded into an equation composed of $d{X}_{k-2}$ and $d{U}_{k-2}$, and the corresponding equation is substituted into (29). By successively substituting $d{X}_k,d{X}_{k-1}\dots ,d{X}_2$ into the expression of $d{Y}_k^i$, we obtain

$$d{Y}_k^i={\left[A^k\right]}^{i}d{X}_1^i+{\left[B_1^k\right]}^{i}d{U}_1^i+{\left[B_2^k\right]}^{i}d{U}_2^i+\cdots +{\left[B_{k-1}^k\right]}^{i}d{U}_{k-1}^i$$

(30)

It is obvious that $d{X}_1^i=0$. Then, (30) is simplified to

$$d{Y}_k^i={\displaystyle \sum _{j=1}^{k-1}{\left[B_j^k\right]}^{i}d{U}_j^i}$$

(31)

$${\left[B_j^k\right]}^i=\left[\frac{\partial Y_k}{\partial X_k}\right]\left[\frac{\partial F_{k-1}}{\partial X_{k-1}}\right]\cdots \left[\frac{\partial F_{j+1}}{\partial X_{j+1}}\right]\left[\frac{\partial F_j}{\partial U_j}\right] ,k=2,3,\cdots ,N_y$$

(32)

where ${\left[B_j^k\right]}^i$ is called the sensitivity matrix. Then, the deviations in the output variables and control variables at each sampling time are formed into a linear equation, in which each $d{U}_j^i$ is a variable to be optimized. (32) is calculated recursively in the following way:

$$\begin{array}{l}{\left[A_k^k\right]}^i=I_{n\times n}\\ {\left[A_j^k\right]}^i={\left[A_{j+1}^k\right]}^i\left[\frac{\partial F_j}{\partial X_j}\right]\\ {\left[B_j^k\right]}^i=\left[\frac{\partial Y_k}{\partial X_k}\right]{\left[A_{j+1}^k\right]}^i\left[\frac{\partial F_j}{\partial U_j}\right]\end{array}$$

(33)

where $k=2,3,\dots ,N_y$, $j=\left(k-1\right),\left(k-2\right),\dots ,1$, when $j\ge k$, ${\left[B_j^k\right]}^i=0_{p\times m}$.

According to (24) and (26), and considering the small approximation $\Delta Y_k^i\approx d{Y}_k^i$, $\Delta U_k^i\approx d{U}_k^i$, we obtain

$$\left\{\begin{array}{l}\left(Y_k^{i+1}-Y_k^{*}\right)=\Delta Y_k^i+\left(Y_k^i-Y_k^{*}\right)=\Delta Y_k^i-\Delta Y_k^{*i}=d{Y}_k^i-\Delta Y_k^{*i}\\ \left(U_k^{i+1}-U_k^i\right)=\Delta U_k^i=d{U}_k^i\end{array}\right.$$

(34)

Hence, the objective function in (23) is rewritten as

$$J=\frac{1}{2}{\displaystyle \sum _{k=2}^{N_y}{\left(d{Y}_k^i-\Delta Y_k^{*i}\right)}^T{Q}_k^i\left(d{Y}_k^i-\Delta Y_k^{*i}\right)}+\frac{1}{2}{\displaystyle \sum _{k=1}^{N_y-1}{\left(d{U}_k^i\right)}^T{R}_k^i\left(d{U}_k^i\right)}$$

(35)

In the traditional T-MPSP algorithm, the increment of the control variables is added to the performance index to indirectly constrain the control variables. However, this method cannot strictly constrain the control variables, which may fail to satisfy the constraints. Therefore, we propose an improved T-MPSP algorithm by adding control variable constraints.

The control variable constraints (5) and (6) are expressed as

$$\begin{array}{l}{U}_{\min }\le U_k^{i+1}\le U_{\max }\\ {\dot{U}}_{\min }\le {\dot{U}}_k^{i+1}=\frac{U_{k+1}^{i+1}-U_k^{i+1}}{h}\le {\dot{U}}_{\max }\end{array}$$

(36)

where $h$ represents the sampling step size, $U\in {ℝ}^m$. According to (34), we obtain

$$U_{\min }\le U_k^i+d{U}_k^i\le U_{\max }\Rightarrow \left\{\begin{array}{l}d{U}_k^i\le U_{\max }-U_k^i\\ -d{U}_k^i\le U_k^i-U_{\min }\end{array}\right.$$

(37)

which is simplified to

$$\left\{\begin{array}{l}d{U}_k^i\le U_{\max }-U_k^i=C{1}_k^i\\ -d{U}_k^i\le U_k^i-U_{\min }=C{2}_k^i\end{array}\right.$$

(38)

The control magnitude constraint is thus transformed into a linear inequality constraint with the only unknown $d{U}_k^i$. Then, ${\dot{U}}_k^{i+1}$ is converted into

$${\dot{U}}_k^{i+1}=\frac{U_{k+1}^{i+1}-U_k^{i+1}}{h}=\frac{\left(U_{k+1}^i+d{U}_{k+1}^i\right)-\left(U_k^i+d{U}_k^i\right)}{h}$$

(39)

Thereby,

$${\dot{U}}_{\min }\le \frac{\left(U_{k+1}^i+d{U}_{k+1}^i\right)-\left(U_k^i+d{U}_k^i\right)}{h}\le {\dot{U}}_{\max }\Rightarrow \left\{\begin{array}{l}-d{U}_k^i+d{U}_{k+1}^i\le h{\dot{U}}_{\max }-U_{k+1}^i+U_k^i\\ d{U}_k^i-d{U}_{k+1}^i\le -h{\dot{U}}_{\min }+U_{k+1}^i-U_k^i\end{array}\right.$$

(40)

which is simplified to

$$\left\{\begin{array}{l}\left[\begin{array}{cc}-I_m& I_m\end{array}\right]\left[\begin{array}{c}d{U}_k^i\\ d{U}_{k+1}^i\end{array}\right]\le h{\dot{U}}_{\max }-U_{k+1}^i+U_k^i=C{3}_k^i\\ \left[\begin{array}{cc}{I}_m& -I_m\end{array}\right]\left[\begin{array}{c}d{U}_k^i\\ d{U}_{k+1}^i\end{array}\right]\le -h{\dot{U}}_{\min }+U_{k+1}^i-U_k^i=C{4}_k^i\end{array}\right.$$

(41)

where $I_m$ is an identity matrix of order $m\times m$. In this way, the change rate constraint is transformed into linear inequality constraints with unknowns $d{U}_k^i$ and $d{U}_{k+1}^i$. In order to facilitate the subsequent solution, (41) is rewritten in the following form:

$$\left\{\begin{array}{l}Pd{U}^i=C{3}^i\\ -Pd{U}^i=C{4}^i\end{array}\right.$$

(42)

where $P=\left[\begin{array}{ccccc}-I_m& I_m& 0& \cdots & 0\\ 0& -I_m& I_m& \cdots & 0\\ 0& 0& \ddots & \ddots & 0\\ 0& 0& 0& -I_m& I_m\end{array}\right]$,$d{U}^i=\left[\begin{array}{c}d{U}_1^i\\ d{U}_2^i\\ ⋮\\ d{U}_{N-2}^i\\ d{U}_{N-1}^i\end{array}\right]$,$C{3}^i=\left[\begin{array}{c}C{3}_1^i\\ C{3}_2^i\\ ⋮\\ C{3}_{N-2}^i\\ C{3}_{N-1}^i\end{array}\right]$,$C{4}^i=\left[\begin{array}{c}C{4}_1^i\\ C{4}_2^i\\ ⋮\\ C{4}_{N-2}^i\\ C{4}_{N-1}^i\end{array}\right]$.

Therefore, combining (35) with the inequality constraints in (38) and (42) yields

$$\begin{array}{l}\min J=\frac{1}{2}{\displaystyle \sum _{k=2}^{N_y}{\left(d{Y}_k^i-\Delta Y_k^{*i}\right)}^T{Q}_k^i\left(d{Y}_k^i-\Delta Y_k^{*i}\right)}+\frac{1}{2}{\displaystyle \sum _{k=1}^{N_y-1}{\left(d{U}_k^i\right)}^T{R}_k^i\left(d{U}_k^i\right)}\\ \mathrm{s}.\mathrm{t}.\left\{\begin{array}{l}d{U}_k^i\le C{1}_k^i\\ -d{U}_k^i\le C{2}_k^i\end{array}\right., \left\{\begin{array}{l}Pd{U}^i=C{3}^i\\ -Pd{U}^i=C{4}^i\end{array}\right.\end{array}$$

(43)

In this paper, the Lagrangian multiplier method and the penalty function are applied to solve the NLP problem; thus, we have

$$\begin{array}{ll}L& =\frac{1}{2}{\displaystyle \sum _{k=2}^{N_y}{\left({\displaystyle \sum _{j=1}^{k-1}{B}_j^{k}d{U}_j}-\Delta Y_k^{*}\right)}^T{Q}_k}\left({\displaystyle \sum _{j=1}^{k-1}{B}_j^{k}d{U}_j}-\Delta Y_k^{*}\right)\\ & +\frac{1}{2}{\displaystyle \sum _{k=1}^{N_y-1}{\left(d{U}_k\right)}^T{R}_k}\left(d{U}_k\right)\\ & +\frac{1}{2}{\displaystyle \sum _{k=1}^{N_y-1}{\left(d{U}_k-C{1}_k\right)}^T\sigma 1_k}\left(d{U}_k-C{1}_k\right)\\ & +\frac{1}{2}{\displaystyle \sum _{k=1}^{N_y-1}{\left(-d{U}_k-C{2}_k\right)}^T\sigma 2_k}\left(-d{U}_k-C{2}_k\right)\\ & +\frac{1}{2}{\displaystyle \sum _{k=1}^{N_y-2}{\left({\displaystyle \sum _{j=1}^{N_y-1}{P}_{kj}d{U}_j}-C{3}_k\right)}^T\sigma 3_k}\left({\displaystyle \sum _{j=1}^{N_y-1}{P}_{kj}d{U}_j}-C{3}_k\right)\\ & +\frac{1}{2}{\displaystyle \sum _{k=1}^{N_y-2}{\left({\displaystyle \sum _{j=1}^{N_y-1}-P_{kj}d{U}_j}-C{4}_k\right)}^T\sigma 4_k}\left({\displaystyle \sum _{j=1}^{N_y-1}-P_{kj}d{U}_j}-C{4}_k\right)\end{array}$$

(44)

where $P_{kj}$ represents the element of the $k$th row and the $j$th column of the matrix $P$. The data in the following equations are all in the same iteration $i$, so the superscript $i$ is omitted for the convenience of subsequent derivation. According to the necessary conditions for first-order optimality, we obtain

$$\begin{array}{l}\frac{\partial L}{\partial d{U}_l}=0\\ \Rightarrow R_{l}d{U}_l+{\displaystyle \sum _{k=2}^{N_y}\left({\left(B_l^k\right)}^T{Q}_k{\displaystyle \sum _{j=1}^{k-1}{B}_j^{k}d{U}_j}\right)}-{\displaystyle \sum _{k=2}^{N_y}{\left(B_l^k\right)}^T{Q}_k\Delta Y_k^{*}}+\sigma 1_l\left(d{U}_l-C{1}_l\right)+\\ \sigma 2_l\left(d{U}_l+C{2}_l\right)+{\displaystyle \sum _{k=2}^{N_y-2}\left({\left(P_{kl}\right)}^T\sigma 3_k{\displaystyle \sum _{j=1}^{N_y-1}{P}_{kj}d{U}_j}\right)}-{\displaystyle \sum _{k=2}^{N_y-2}{\left(P_{kl}\right)}^T\sigma 3_{k}C{3}_k}+\\ {\displaystyle \sum _{k=2}^{N_y-2}\left({\left(P_{kl}\right)}^T\sigma 4_k{\displaystyle \sum _{j=1}^{N_y-1}{P}_{kj}d{U}_j}\right)}+{\displaystyle \sum _{k=2}^{N_y-2}{\left(P_{kl}\right)}^T\sigma 4_{k}C{4}_k}=0\end{array}$$

(45)

where $\sigma 1_k$, $\sigma 2_k$, $\sigma 3_k$ and $\sigma 4_k$ are all penalty factors. By changing the positions of the terms in Equation (45), we obtain

$$\begin{array}{l}\left(R_l+\sigma 1_l+\sigma 2_l\right)d{U}_l+{\displaystyle \sum _{k=2}^{N_y}\left({\left(B_l^k\right)}^T{Q}_k{\displaystyle \sum _{j=1}^{k-1}{B}_j^{k}d{U}_j}\right)}+\\ {\displaystyle \sum _{k=2}^{N_y-2}\left({\left(P_{kl}\right)}^T\left(\sigma 3_k{\displaystyle \sum _{j=1}^{N_y-1}{P}_{kj}d{U}_j}+\sigma 4_k{\displaystyle \sum _{j=1}^{N_y-1}{P}_{kj}d{U}_j}\right)\right)}\\ ={\displaystyle \sum _{k=2}^{N_y}{\left(B_l^k\right)}^T{Q}_k\Delta Y_k^{*}}+{\displaystyle \sum _{k=2}^{N_y-2}{P}_{kl}{}^T\left(\sigma 3_{k}C{3}_k-\sigma 4_{k}C{4}_k\right)}+\sigma 1_{l}C{1}_l-\sigma 2_{l}C{2}_l\end{array}$$

(46)

All $d{U}_l(l=1,2,\dots ,N_y-1)$ in (46) are expressed in a matrix form as

$$\left[\begin{array}{cccc}{M}_{11}+T_1& \cdots & M_{1(N_y-2)}& M_{1(N_y-1)}\\ M_{21}& M_{22}+T_2& \cdots & M_{2(N-1)}\\ ⋮& \cdots & \ddots & ⋮\\ M_{(N_y-1)1}& \cdots & \cdots & M_{(N_y-1)(N_y-1)}+T_{N_y-1}\end{array}\right]\left[\begin{array}{c}d{U}_1\\ d{U}_2\\ ⋮\\ d{U}_{N_y-1}\end{array}\right]=\left[\begin{array}{c}{b}_1\\ b_2\\ ⋮\\ b_{N_y-1}\end{array}\right]$$

(47)

where

$$\left\{\begin{array}{l}{M}_{ij}={\displaystyle \sum _{l=\left(j+1\right)}^{N_y}{B}_i^l{}^T{Q}_l{B}_j^l}+{\displaystyle \sum _{k=2}^{N_y-2}\left(P_{ki}{}^T\sigma 3_k{P}_{kj}+P_{ki}{}^T\sigma 4_k{P}_{kj}\right)}\\ b_i=\sigma 1_{i}C{1}_i-\sigma 2_{i}C{2}_i+{\displaystyle \sum _{l=2}^{N_y}{B}_i^l{}^T{Q}_l\Delta Y_l^{*}}+{\displaystyle \sum _{k=2}^{N_y-2}{P}_{ki}{}^T\left(\sigma 3_{k}C{3}_k-\sigma 4_{k}C{4}_k\right)}\\ T_i=R_i+\sigma 1_i+\sigma 2_i\end{array}\right.$$

(48)

By solving (47), we obtain the corrections for all control variables $dU=\left[d{U}_1,d{U}_2,\dots ,d{U}_{N_y-1}\right]$. Finally, the updated control variables are calculated as

$$U^{i+1}=U^i+dU$$

(49)

3.3. Overall Structure and Operating Steps

In this subsection, we combine the online parameter identification method with the improved T-MPSP algorithm to propose an adaptive trajectorytracking guidance algorithm based on an improved T-MPSP. By applying the first-order Euler method, we obtain

$$X_{k+1}=X_k+hf\left(X,U\right)=F_k\left(X_k,U_k\right)$$

(50)

where $h$ represents the sampling step size. $X_k={\left[h_k,v_k,{\gamma }_k,m_k\right]}^T$, $U_k={\left[{\alpha }_k,{\varphi }_k\right]}^T$ and $Y_k={\left[h_k,v_k,{\gamma }_k\right]}^T$ denote the state, control, and output variables of the aircraft, respectively. $f\left(X,U\right)$ represents the state differential equation of (1). The comprehensive block diagram of the proposed method is presented in Figure 1, and the operating steps of the adaptive tracking guidance algorithm based on the improved T-MPSP are as follows:

Figure 1. The block diagram of the proposed method.

Step 1: Parameter initialization, such as initializing the prediction time domain, sampling step size, EKF parameters, etc.

Step 2: Use the EKF algorithm to identify the parameter deviation online, which is used to correct the prediction model for the T-MPSP algorithm.

Step 3: In the improved T-MPSP algorithm, use the revised model to update the control variables until they converge or the algorithm reaches the maximum number of iterations.

Step 4: Use the first control values of the prediction time-domain window of the T-MPSP algorithm as the control variables of the current time.

Step 5: Integrate the dynamic equations using fourth-order Runge-Kutta to the next time instant.

Step 6: Determine whether the terminal time is reached. If it is reached, stop the operation. Otherwise, go to step 2 and continue with the same steps.

4. Simulations

In this section, simulations are carried out to show the effectiveness of the proposed method. First, comparison simulations against the existing T-MPSP tracking method are presented. Then, several extreme combinations of parameter deviations are considered in the simulation. Finally, the Monte Carlo simulations are carried out to verify the robustness of the proposed method.

The control parameters are initialized as

$$\left\{\begin{array}{l}{N}_y=7\\ Q_k=\left[\begin{array}{ccc}1.11& 0& 0\\ 0& 1.96& 0\\ 0& 0& 5000\end{array}\right]\\ R_k=\left[\begin{array}{c}1000\\ 90000\end{array}\right]\end{array}\right.$$

(51)

And the constraints considered in this simulation are tabulated in Table 1.

Table 1. Constraints.

Process constraints	$q_{\max }$	150 (kPa)
Control constraints	$[{\alpha }_{\min },{\alpha }_{\max }]$	$[-3,21]$ (°)
	$[{\varphi }_{\min },{\varphi }_{\max }]$	$[0,2]$
	$[{\dot{\alpha }}_{\min },{\dot{\alpha }}_{\max }]$	$[-5,5]$ (${}^{\circ }/s$)
Terminal constraint	${\epsilon }_h$	500 (m)
	${\epsilon }_v$	50 (m/s)
	${\epsilon }_{\gamma }$	0.5 (°)

4.1. Comparison Simulations

To demonstrate the effectiveness of the proposed method, both the open-loop tracking method and the MPSP tracking law presented in [22] are considered as comparison methods in this subsection. The detailed simulation results are presented in Table 2 and Figure 1.

Table 2. Comparison results.

	Terminal Height Deviations (m)	Terminal Velocity Deviations (m/s)	Terminal Flight Path Angle Deviations (°)
Proposed method	32.95	−12.53	0.279
Open-loop tracking method	−444.49	272.40	1.124
T-MPSP method	−242.36	−15.48	0.025

In Figure 2, the solid lines in blue represent the tracking results of the proposed method, dash lines in blue refer to the tracking results of the open-loop control, dot lines in pink denote the tracking results of the comparison T-MPSP method, and the reference curves are presented in dash-dot form.

Figure 2. Comparison results.

For the open-loop control scenario, the tracking performance is far from satisfactory. Moreover, the maximum dynamic pressure violates the constraint, which will cause damage to the vehicle. As for the comparison T-MPSP method, the terminal accuracy is acceptable, while the overall tracking performance is not so good. In the proposed method, the terminal deviations are much smaller than in the comparison law, which confirms the effectiveness and superiority of the proposed method. The optimization process of the proposed method and the T-MPSP method requires about 1.7 s on our laptop with an AMD 1.8 GHz CPU. However, it needs about 2.8 s for the MPSP tracking method.

4.2. The Monte Carlo Simulations

In this subsection, trajectory tracking is carried out under 300 sets of deviations sampled through the LHS method. Furthermore, the distribution intervals of the four uncertain conditions are as follows:

$$\left\{\begin{array}{l}\Delta \rho \in \left[-0.1,0.1\right]\\ \Delta C_T\in \left[-0.05,0.05\right]\\ \Delta C_L\in \left[-0.1,0.1\right]\\ \Delta C_D\in \left[-0.1,0.1\right]\end{array}\right.$$

(52)

The terminal state deviations under the 300 groups of random deviations are presented in Figure 3, Figure 4 and Figure 5. The statistical maximum deviations in the terminal states are recorded in Table 3.

Figure 3. Terminal state deviation distributions.

Figure 4. Terminal state deviation distributions (left view).

Figure 5. Terminal state deviation distributions (vertical view).

Table 3. Maximum tracking deviations.

Terminal Height Deviations (m)	Terminal Velocity Deviations (m/s)	Terminal Flight Path Angle Deviations (°)	Dynamic Pressure (kPa)
148.73	47.26	0.313	142.12

The maximum tracking deviation in the terminal height is less than 150 m, which is far lower than the requirements of terminal height accuracy. At the same time, the maximum tracking deviation in the terminal speed is less than 50 m/s, which also meets the requirements. In addition, the absolute value of terminal flight path angle deviation is also within the allowable range of 0.5°, and the maximum dynamic pressure also meets the constraints. These simulation results show that the novel T-MPSP method proposed in this paper has a good anti-interference ability against coefficient deviations caused by complex flight environments.

5. Conclusions

An adaptive trajectory-tracking method based on a novel tracking model predictive static programming (T-MPSP) was proposed. The proposed method had advantages in computing efficiency and tracking accuracy. Further, it was more adaptive to uncertain parameter deviations due to the parameter correction strategy. Firstly, a parameter correction strategy was designed. Then, the control constraints were considered to form a novel T-MPSP algorithm. By combining the parameter correction strategy with the improved T-MPSP algorithm, an adaptive tracking guidance scheme was presented. Finally, simulations were carried out to show the effectiveness of the proposed method. In related future research, the saturation of the input should be considered in the design of the trajectory-tracking method.