PDF  PubReader

Hu , Liu , and Yang: Improved Gauss Pseudospectral Method for UAV Trajectory Planning with Terminal Position Constraints

Qingquan Hu , Ping Liu and Jinfeng Yang

Improved Gauss Pseudospectral Method for UAV Trajectory Planning with Terminal Position Constraints

Abstract: Trajectory planning is a key technology for unmanned aerial vehicles (UAVs) to achieve complex flight missions. In this paper, a terminal constraints conversion-based Gauss pseudospectral trajectory planning optimization method is proposed. Firstly, the UAV trajectory planning mathematical model is established with considering the boundary conditions and dynamic constraints of UAV. Then, a terminal constraint handling strategy is presented to tackle terminal constraints by introducing new penalty parameters so as to improve the performance index. Combined with Gauss-Legendre collocation discretization, the improved Gauss pseudospectral method is given in detail. Finally, simulation tests are carried out on a four-quadrotor UAV model with different terminal constraints to verify the performance of the proposed method. Test studies indicate that the proposed method performances well in handling complex terminal constraints and the improvements are efficient to obtain better performance indexes when compared with the traditional Gauss pseudospectral method.

Keywords: Gauss Pseudospectral Optimization , Terminal Constraints , Trajectory Planning , UAV

1. Introduction

In recent years, unmanned aerial vehicle (UAV) has been favored by many researchers [1]. Due to its excellent characteristics of strong mobility, rapid takeoff and landing etc., quadrotor UAV has been widely applied in both commercial and military areas [2,3], for instance, monitoring and inspection, power inspection, traffic monitoring, post-disaster rescue, express delivery logistics [4], etc. Therefore, it is of great significance to study the trajectory planning methods for four-quadrotor UAVs to achieve the above goals.

With the development of computer science [5-7], optimization methods have been widely used for UAV trajectory planning. Based on the previous classification, these optimization methods can be divided into three categories: indirect approaches [8], direct methods [9], and intelligent algorithms [10]. By analyzing the references, it can be concluded that the characteristic of indirect methods is solving the optimal control problem by using the transformed two-point boundary value problem (TBVP) [11]. However, the direct methods solve the optimal control problem mainly through the nonlinear pro- gramming (NLP) problem obtained by using discrete strategies [12]. Different with the above two categories, intelligent algorithms employ the intelligent ways to optimize the discretization problems. As one of the most efficient numerical dynamic optimization approaches, pseudospectral algorithm has the advantages in terms of convergence domain and convergence speed when compared with other traditional direct methods [13]. Therefore, pseudospectral method becomes popular in UAV optimal control applications [14,15]. However, most of the flight path planning methods regard the quadrotor UAV as a mass point, without considering the flight time and the motion model of the UAV itself [2]. Meanwhile, high precision trajectory optimization methods are still needed for complex flight missions. In this paper, an improved Gauss pseudospectral method (GPM) is proposed to obtain high-precision trajectory planning for UAVs with big difference terminal position constraints. Firstly, a terminal constraints conversion strategy is presented to transform the state variables into the performance index. Then, an optimization frame based on GPM is further given with the use of the transform strategy. Correspondingly, the implementation steps of the proposed method are introduced in detail. Simulation tests are carried out on a four-quadrotor UAV model with comprehensively considering the boundary and variable constraints. Furthermore, the traditional GPM is also employed for comparison.

This paper is organized as follows: Section 2 discusses the UAV trajectory planning model; Section 3 introduces the improved Gauss pseudospectral optimization method, including Gauss-Legendre collocation discretization, terminal constraints conversion-based Gauss pseudospectral optimization strategy and algorithm implementation. Section 4 gives the parameters of simulation model and carries out the cases tests; Section 5 summarizes the work of this paper.

2. UAV Trajectory Planning Model

In this paper, the four-quadrotor UAV is studied, and two coordinate systems are employed to establish the model of this process. Fig. 1 shows the structure diagram of quadrotor UAV. Meanwhile, two coordinate systems are employed to establish the trajectory planning model.

Fig. 1.

Structure diagram of the four-quadrotor UAV.

(1) Body coordinate system: the origin of coordinates coincides with the centroid of the quadrotor UAV, which is used to determine the flight attitude of the drone in the air.

(2) Inertial coordinate system: the origin of coordinates coincides with the take point of aircraft on the ground, which is used to determine the space position of the aircraft.

Based on above coordinate systems definition, it can be found that the motion of a four-rotor UAV includes linear motion along three axes and rigid motion around three axes. Therefore, 12 state variables are defined as: position [TeX:] $$(x, y, z)^{\mathrm{T}}$$, speed [TeX:] $$\left(v_x, v_y, v_z\right)^{\mathrm{T}}$$, attitude [TeX:] $$(\phi, \theta, \psi)^{\mathrm{T}}$$ and angular [TeX:] $$(p, q, r)^{\mathrm{T}}$$. Meanwhile, the UAV is assumed as a rigid body with uniform symmetry in mass and shape. Furthermore, in order to facilitate the design of a trajectory and control system, it is assumed that the quadrotor UAV hovers or flies with a low speed. Specially, the near-ground effect caused by rotor airflow is ignored. Under the assumptions of small angle, the angular velocity is approximately expressed as:

[TeX:] $$\omega=\left[\begin{array}{lll} p & q & r \end{array}\right]^T=\left[\begin{array}{lll} \dot{\phi} & \dot{\theta} & \psi \end{array}\right]^T,$$

where [TeX:] $$\phi, \theta, \text{ and } \psi$$ represent the unmanned aerial vehicle attitude angles. Finally, the nonlinear dynamic process of the four-rotor UAV is briefly stated as follows:

[TeX:] $$F(x(t), U(t), t)=\left\{\begin{array}{l} \dot{x}=v_x, \dot{y}=v_y, \dot{z}=v_z \\ \dot{v}_x=\left[U_1(\cos \phi \sin \theta \cos \psi+\sin \phi \sin \psi)\right] / M \\ \dot{v}_y=\left[U_1(\cos \phi \sin \theta \sin \psi-\sin \phi \cos \psi)\right] / M \\ \dot{v}_z=\left[U_1 \cos \phi \cos \theta-M g\right] / M \\ \dot{\phi}=p, \dot{\theta}=q, \dot{\psi}=r \\ \dot{p}=\left[l U_2+q r\left(I_y-I_z\right)\right] / I_x, \dot{q}=\left[l U_3+p r\left(I_z-I_x\right)\right] / I_y \\ \dot{r}=\left[U_4+p q\left(I_x-I_y\right)\right] / I_z \end{array},\right.$$

where [TeX:] $$I_x, I_y, \text{ and } I_z$$ represent the inertia forces of four-rotor UAV corresponding to the three axes, l represents the action point of lift, M is the mass of UAV, U denotes the control input and g is the gravitational acceleration. Meanwhile, the four control inputs are obtained by using the follow equation:

[TeX:] $$U(t)=\left[\begin{array}{l} U_1 \\ U_2 \\ U_3 \\ U_4 \end{array}\right]=\left[\begin{array}{cccc} k_1 & k_1 & k_1 & k_1 \\ 0 & -k_1 & 0 & k_1 \\ -k_1 & 0 & k_1 & 0 \\ k_2 & -k_2 & k_2 & -k_2 \end{array}\right]\left[\begin{array}{c} \omega_1^2 \\ \omega_2^2 \\ \omega_3^2 \\ \omega_4^2 \end{array}\right],$$

where [TeX:] $$k_1$$ represents the lift coefficient and [TeX:] $$k_2$$ represents the torque coefficient.

Generally, the trajectory optimization objective of a UAV is to maximize or minimize a specific performance index under initial and limitation constraints. In this work, the trajectory planning optimization model mainly considers path and terminal position constraints. Therefore, the UAV trajectory planning problem is defined as follows.

[TeX:] $$$$ \min _{U(t)} J=\Phi_0\left(x\left(t_f \mid U(t)\right)\right)+\int_{t_0}^{t_f} L_0(x(t \mid U(t)), U(t)) d t $$ s.t. Dynamic process: Eq. (2), Initial conditions: $x(0)=x_0$ Path constraints: $\mathrm{g}(\mathrm{x}(t), \mathrm{U}(t), t) \geq 0$ Terminal position constraints: $\mathrm{x}\left(t_f\right)=x_{t_f}$$$

where J is the performance index, g(x(t), U(t), t) is the inequality path constraints, [TeX:] $$\mathrm{x}_0 \text { and } x_{t_f}$$ are the initial and terminal conditions, respectively.

3. Improved Gauss Pseudospectral Optimization Method

It is obvious that problem (4) is an optimal control problem. Generally, GPM has been widely used to solve this kind of problems in recent years [9,16-18]. According to previous references [19-21], it can be concluded that the Gaussian pseudospectral algorithm converges faster and has a larger convergence domain when compared with control variable parameterization (CVP) method [22] and iterative dynamic programming (IDP) method [23]. Thus, GPM can be a candidate strategy for solving problem (4). However, terminal constraints with big magnitude differences will influence the optimization precision of GPM. To tackle this issue, an improved GPM is proposed in this paper and is stated below.

3.1 Gauss-Legendre Collocation Discretization

Firstly, introduce a new intermediate variable into problem (4) and then use the following transformation formula for time scale transformation:

[TeX:] $$t=\frac{t_f-t_0}{2} T+\frac{t_f+t_0}{2}.$$

Thus, the time interval of the trajectory optimization problem is [TeX:] $$\left[\mathrm{T}_0, \mathrm{~T}_f\right]=[-1,1].$$ Then, problem (4) is converted into the following standard Bolza problem:

[TeX:] $$\begin{aligned} \operatorname{Min}_{U(\tau)} & J=\Phi_0\left(x(-1), t_0, x(1), t_f\right)+\frac{t_f-t_0}{2} \int_{-1}^1 L_0\left(x(T), U(T), T, t_0, t_f\right) d T \\ & \frac{d x}{\mathrm{dT}}=\frac{t_f-t_0}{2} F\left(x(T), U(T), T, t_0, t_f\right) \\ & E\left(x(-1), t_0, x(1), t_f\right)=0 \\ & g(x(T), U(T), T) \geq 0, T \in[-11] \end{aligned}$$

Next, Lagrange interpolation is used to discretize the state variables. Suppose the function values [TeX:] $$\left\{\left(\mathrm{T}_1, y_1\right),\left(\mathrm{T}_2, y_2\right), \cdots,\left(\mathrm{T}_{\mathrm{N}+1}, y_{\mathrm{N}+1}\right)\right\}$$ are known and N-degree polynomials are then used for interpolation as follows:

[TeX:] $$y(T) \approx Y(T)=\sum_{i=1}^{N+1} L_i(T) y_i,$$

where the interpolation polynomial [TeX:] $$L_i(T)$$ is:

[TeX:] $$L_i(T)=\prod_{j=1, j \neq i}^{N+1} \frac{T-T_j}{T_i-T_j}.$$

Based on Eq. (8), the following property can be obtained:

[TeX:] $$L_i\left(T_j\right)=\delta_{i j}=\left\{\begin{array}{l} 1, i=j \\ 0, i \neq j \end{array}\right..$$

Furthermore, it is necessary to obtain the value of [TeX:] $$y_i$$ by selecting appropriate discrete points. By replacing the nodes in the new time interval using the zeros of the Legendre polynomial defined below, discrete points can be determined.

[TeX:] $$\begin{aligned} & G_{n+1}(x)=\left(x-a_n\right) G_n(x)-b_n^2 G_{n-1}(x), n=0,1, \ldots \\ & G_0(x)=1, \quad G_{-1}(x)=0 \end{aligned}$$

Based on the discussion of Liu et al. [24], Eq. (10) can be solved by using the following Theorem 1. Meanwhile, the obtained discrete points are named as LG collocation points in this paper.

Theorem 1. Suppose the Legendre polynomial is known as follows:

[TeX:] $$\begin{aligned} & G_{n+1}(\tau)=\left(\tau-a_n\right) G_n(\tau)-b_n^2 G_{n-1}(\tau), n=0,1, \ldots \\ & G_0(\tau)=1, \quad G_{-1}(\tau)=0 \end{aligned}$$

The eigenvalues of the following matrix R are the roots of Eq. (11).

[TeX:] $$R=\left(\begin{array}{llllll} a_0 & b_1 & & & & \\ b_1 & a_1 & b_2 & & & \\ & & & \ddots & & \\ & & & & a_{n-2} & b_{n-1} \\ & & & & b_{n-1} & a_{n-1} \end{array}\right) \text {. }$$

Remark 1. It should be noted that Theorem 1 has been proved by Liu et al. [24], the proof is thus omitted in this work. By solving Eq. (12), the roots can be obtained, and the collocation points are then calculated by using these roots [24].

3.2 Terminal Constraints Conversion-based Gauss Pseudospectral Optimization

By using the obtained LG collocation points, the control and state variables can be simultaneously approximated by Legendre polynomials. Assuming that the discretization points are N+1 LG nodes, then the Lagrange interpolation approximation is employed to discretize both the control and state variables. Correspondingly, the following expression is obtained:

[TeX:] $$x(T) \approx X(T)=\sum_{i=1}^{N+1} L_i(T) X\left(T_i\right)=\sum_{i=1}^{N+1} L_i(T) X_i,$$

[TeX:] $$U(T) \approx \sum_{i=1}^{N+1} L_i(T) U\left(T_i\right)=\sum_{i=1}^{N+1} L_i(T) U_i,$$

where [TeX:] $$X_i \text { and } U_i$$ are the values of state vector and control vector at LG collocation point [TeX:] $$T_i.$$

Therefore, based on Eq. (13), the derivative of x at LG discretization points can be easily obtained by the following equation:

[TeX:] $$\dot{x}(T) \approx \dot{X}(T)=\sum_{i=1}^{N+1} \dot{L}_i(T) X_i.$$

In this work, [TeX:] $$L_i(\mathrm{T})$$ LG collocation point is defined as follows and calculated by using the following formula,

[TeX:] $$\dot{x}\left(T_k\right) \approx \dot{X}\left(T_k\right)=\sum_{i=1}^{N+1} \dot{L}_i\left(T_k\right) X_i=\sum_{i=1}^{N+1} B_{k, i} X_i,$$

[TeX:] $$B_{k, i}=\dot{L}_i\left(T_k\right)=\left\{\begin{array}{c} \frac{\dot{b}\left(T_k\right)}{\dot{b}\left(T_i\right)\left(T_k-T_i\right)}, k \neq i \\ \frac{\ddot{b}\left(T_k\right)}{2 \dot{b}\left(T_k\right)}, k=i \end{array}\right. \text {, }$$

[TeX:] $$b\left(T_k\right)=\prod_{i=1}^{N+1}\left(T_k-T_i\right).$$

Thus, the following formula is a substitute of Eq. (2):

[TeX:] $$\sum_{i=1}^{N+1} B_{k, i} X_i=\frac{t_f-t_0}{2} F\left(X_k, U_k, T_k ; t_0, t_f\right), k=1,2, \ldots, N+1.$$

On this basis, the integral term can be obtained through the following formula:

[TeX:] $$\int_{-1}^1 L_0\left(x(T), U(T), T, t_0, t_f\right) d T \approx \sum_{k=1}^N w_k L_0\left(x\left(T_k\right), U\left(T_k\right), T_k, t_0, t_f\right),$$

where [TeX:] $$w_k$$ is the integral weight and can be obtained based on the study by Liu et al. [24]. By using the above discretization strategy, problem (6) then is transformed into an NLP problem stated below.

[TeX:] $$\begin{array}{ll} \operatorname{Min}_{X_k, U_k} & J=\Phi_0\left(X_0, t_0, X_f, t_f\right)+\frac{t_f-t_0}{2} \sum_{k=1}^{N+1} w_k L_0\left(X_k, U_k, T_k, t_0, t_f\right) \\ \text { s.t. } & \sum_{i=1}^{N+1} B_{k, i} X_i=\frac{t_f-t_0}{2} F\left(X_k, U_k, T_k ; t_0, t_f\right), k=1,2, \ldots, N+1 \\ & \boldsymbol{E}\left(X_0, t_0, X_f, t_f\right)=0 \\ & g\left(X_k, U_k, T\right) \geq 0 \\ & X_f-X_0-\sum_{i=1}^{N+1} X_i \sum_{k=1}^N w_k B_{k, i}=0 . \end{array}$$

It can be found that constraints in problem (6) are discretized simultaneously and are transformed into distributed constraints at LG points. Especially, the terminal constraints are also tackled as equality constraints in NLP problem. However, Zhang et al. [25] pointed out that when the terminal values of constraints are very big, the terminal constraints will not be absolutely satisfied. To tackle this issue, this work proposes a terminal constraints conversion-based strategy. The improvement idea of this strategy is to convert the state variables with big order of magnitude differences into the performance index to make the optimization results more accurate.

Firstly, denoting penalty parameters [TeX:] $$\rho_j, j=1, \ldots, M$$ to record the difference values of state variables. In this paper, penalty parameters represent the order of magnitude between the final values of different state variables and are defined as:

[TeX:] $$\rho_j=x_{i, t_f} / x_{j, t_f}, \quad i \neq j, x_{j, t_f} \neq 0.$$

where [TeX:] $$x_{i, t_f}$$ denotes the final value of the i-th state variable, and [TeX:] $$x_{i, t_f}\lt x_{j, t_f}$$.

To obtain accurate performance index, [TeX:] $$\rho_j$$ is chosen by the order of magnitude when the final values of two state variables differ greatly. The handling rules of the proposed strategy are stated as follows.

Case 1: if [TeX:] $$\rho_j \geq 0.1,$$ it means that the difference between the terminal constraints is not big enough, terminal constraints do not need to be tackled. Thus, problem (21) is employed to obtain the optimization results.

Case 2: if [TeX:] $$0\lt \rho_j\lt 0.1 \text {, }$$ it means that the difference between terminal constraints passes the threshold, terminal constraints should be tackled. In this work, the terminal constraint of [TeX:] $$x_{j, t_f}$$ is expanded into the objective function by using the penalty parameter [TeX:] $$\rho_j.$$ Based on this strategy, problem (21) is transformed into the following optimization problem:

[TeX:] $$\begin{aligned} & \left.\min _U=\Phi_0\left(X_0, t_0, X_f, t_f\right)+\frac{t_f-t_0}{2} \sum_{k=1}^{N+1} w_k L_0\left(X_k, U_k, T_k ; t_0, t_f\right)+\sum_{j=1}^M \rho_j X_{j, N}-X_{j, t_f}\right) \\ & \text { s.t. } \quad \sum_{i=1}^{N+1} B_{k, i} X_i=\frac{t_f-t_0}{2} F\left(X_k, U_k, T_k ; t_0, t_f\right), k=1,2, \ldots, N+1 \\ & \quad \boldsymbol{E}\left(X_0, t_0, X_f, t_f\right)=0 \\ & \quad g\left(X_k, U_k, T\right) \geq 0 \\ & X_{i, t_f}-X_0-\sum_{i=1}^{N+1} X_i \sum_{k=1}^N w_k B_{k, i}=0 . \end{aligned}$$

where [TeX:] $$X_{i, t_f} \text { and } X_{j, t_f}$$ denote the terminal values of terminal constraints. By using the proposed strategy, it can be seen that the terminal constraints in problem (23) are decreased.

3.3 Implementation Steps

Based on above specific descriptions, the detailed implementation steps of the proposed improved GPM are given. Meanwhile, Fig. 2 shows the flow chart of this method. Specifically, the main steps of the proposed algorithm are stated below.

Step 1: Establish a dynamic model of the quadrotor UAV and obtain the optimization problem (4).

Step 2: Use time transformation to transform optimization problem (4) into Bolza optimization problem (6).

Step 3: Employ Gauss-Legendre collocation discretization method to calculate the LG collocation points. Then, combine the obtained LG collocation points with Legendre polynomials to convert state equations into equality discrete equations.

Step 4: Discretize control variables and state variables, transform Bolza optimization problem (6) into NLP problem (21).

Step 5: Terminal constraints conversion check. If [TeX:] $$0\lt \rho_j\lt 0.1$$, introduce penalty parameter [TeX:] $$\rho_j$$ to obtain problem (23), otherwise, reserve problem (21) and go to Step 6.

Step 6: Use gradient-based NLP solver to solve the transformed NLP problem.

Step 7: Output the obtained optimization results.

Fig. 2.

Flow chart of the improved Gauss pseudospectral method.

4. Simulation Tests

The simulation tests are carried out on a quadrotor UAV model to verify the effectiveness of the algorithm. In addition, the traditional Gaussian pseudospectral method is also employed for comparison. All simulation tests are implemented in the MATLAB software by using a personal computer with Intel CORE i5/1.7 GHz CPU processor and DDR3L/1600 MHz 8 GB memory.

4.1 Parameters of Four-Quadrotor UAV

In this test, the performance index of UVA trajectory optimization is to minimize the running time. The corresponding parameters of the UAV model are given in Table 1. Meanwhile, the position, speed, attitude, and angular velocity constraints are shown in Table 2. The initial and terminal conditions are given in Table 3. Furthermore, the optimization parameters of the two methods are set the same.

Table 1.

Parameters of four-quadrotor UAV
Item Value
M 0.99 kg
l 0.23 m
g [TeX:] $$9.8 \mathrm{~m} / \mathrm{s}^2$$
[TeX:] $$I_x$$ [TeX:] $$0.01 \mathrm{~kg} / \mathrm{m}^2$$
[TeX:] $$I_y$$ [TeX:] $$0.011 \mathrm{~kg} / \mathrm{m}^2$$
[TeX:] $$I_z$$ [TeX:] $$0.0206 \mathrm{~kg} / \mathrm{m}^2$$
[TeX:] $$k_1$$ [TeX:] $$5.5 \times 10^{-6} \mathrm{~N} /\left(\mathrm{rad}^2 / \mathrm{s}^2\right)$$
[TeX:] $$k_2$$ [TeX:] $$3.65 \times 10^{-7} \mathrm{~N} \cdot \mathrm{m} /\left(\mathrm{rad}^2 / \mathrm{s}^2\right)$$

Table 2.

Constraints of four-quadrotor UAV
Item Constraints
Position [TeX:] $$|x| \leq 120 \mathrm{~m}$$
[TeX:] $$|y| \leq 120 \mathrm{~m}$$
[TeX:] $$|z| \leq 120 \mathrm{~m}$$
Attitude [TeX:] $$|\phi| \leq \pi / 2$$
[TeX:] $$|\theta| \leq \pi / 2$$
[TeX:] $$|\psi| \leq \pi / 2$$
Speed [TeX:] $$\left|v_x\right| \leq 10 \mathrm{~m} / \mathrm{s}$$
[TeX:] $$\left|v_y\right| \leq 10 \mathrm{~m} / \mathrm{s}$$
[TeX:] $$\left|v_z\right| \leq 10 \mathrm{~m} / \mathrm{s}$$
Angular velocity [TeX:] $$|p| \leq 1.7 \mathrm{rad} / \mathrm{s}$$
[TeX:] $$|q| \leq 1.7 \mathrm{rad} / \mathrm{s}$$
[TeX:] $$|r| \leq 1.7 \mathrm{rad} / \mathrm{s}$$

Table 3.

Parameters of initial and terminal conditions
Item Position (m) ϕ (rad) θ (rad) ψ (rad) V (m/s)
Initial conditions (0,0,0) 0 0 0 0
Terminal condition I (2,100,100) free free free 0
Terminal condition II (2,300,300) free free free 0
4.2 Cases Test

4.2.1 Terminal condition I test

In this test, 30 LG collocation points are chosen and the terminal position condition I (2,100,100) is employed. Table 4 shows the simulation results of the proposed procedure and the traditional GPM method. It can be found that the performance index of the proposed method is 13.4264 seconds, which is 0.16 seconds smaller than that of traditional GPM, revealing that the improvement strategy is efficient. The optimization position curves of the two methods are shown in Fig. 3. Both methods satisfy the terminal conditions, while the running time of the proposed method is smaller. Fig. 4 reveals the corresponding speed curves of the two methods. It is obvious that the speed curves of two methods are different. Correspondingly, Fig. 5 gives the trajectory curve comparison of the two methods. The proposed method has better flight trajectory.

Table 4.

Performance indexes of the two methods for terminal condition I test
Method LG points NLP solver min J (s)
Traditional GPM 30 FMINCON (Interior-point method) 13.5908
Proposed 30 FMINCON (Interior-point method) 13.4264

Fig. 3.

Position curves of (a) the traditional GPM and (b) the proposed methods (Terminal condition I test).

Fig. 4.

Speed curves of (a) the traditional GPM and (b) the proposed methods (Terminal condition I test).

Fig. 5.

Trajectory curve comparison of the two methods (Terminal condition I test).

4.2.2 Terminal condition II test

To further verify the performance of the proposed method, terminal condition II test is carried out. During the test, terminal position condition II (2,300,300) is used. Similarly, the traditional GPM is employed to make comparisons. Table 5 presents the performance indexes of the two methods. Test results show that the performance indexes are 36.9176 and 36.4276 seconds, respectively. It can be found that the proposed method still obtains a better result than that of GPM, further showing that the improvement is excellent. The obtained optimization position curves of the two methods are revealed in Fig. 6. The two methods satisfy the terminal conditions well. Compared with Terminal condition I test, it can be also noted that when the terminal position changes bigger, more flight time is needed. Figs. 7 and 8 show the corresponding speed curves and trajectory curves of the two methods, respectively. Simulation results reveal the potential application of the proposed method for UAVs with complex flight missions.

Table 5.

Performance indexes of the two methods for terminal condition II test
Method LG points NLP solver min J (s)
Traditional GPM 30 FMINCON (Interior-point method) 36.9176
Proposed 30 FMINCON (Interior-point method) 36.4276

Fig. 6.

Position curves of (a) the traditional GPM and (b) the proposed methods (Terminal condition II test).

Fig. 7.

Speed curves of (a) the traditional GPM and (b) the proposed methods (Terminal condition II test).

Fig. 8.

Trajectory curve comparison of the two methods (Terminal condition II test).

5. Conclusion

In this paper, an improved trajectory planning optimization method is proposed for quadrotor UAVs. The complex boundary conditions and dynamic constraints are taken into consideration simultaneously. Under the frame of numerical optimization approach, a terminal constraints-handling strategy combined with Gauss pseudospectral optimization is proposed for tacking terminal constraints with big differences. Simulation tests show that the proposed method performances well in handling UAV trajectory planning problems with complex terminal constraints. Meanwhile, compared with the traditional GPM, the superiority of the improved method is verified. Test results show that better performance indexes can be obtained by the proposed method, revealing the potential application value of improvement for UAV trajectory planning. Extensions of the proposed method are to develop energy recovery velocity planning methods for four-quadrotor UAVs with complex path constraints.


Qingquan Hu

He received his bachelor’s degree in 2015 and is now studying for a master’s degree in control engineering from the School of Automation, Chongqing University of Posts and Telecommunications, Chongqing, China. His current research interests include optimization control and trajectory optimization.


Ping Liu

He received his B.S. degree from North China Electric Power University in 2012 and the Ph.D. degree in control science and engineering from Zhejiang University, China, in 2017. He is currently an associate professor in the College of Automation, Chongqing University of Posts and Telecommunications, Chongqing, China. His research interests include optimal control and dynamic optimization of complex systems, trajectory optimization.


Jinfeng Yang

She received her M.S. degree from Chongqing Jiaotong University in 2015. She is currently a lecture in Department of Track and Electrical Engineering, Chongqing Jianzhu College, Chongqing, China. Her research interests include intelligent transpor-tation and control theory.


  • 1 F. Santoso, M. A. Garratt, and S. G. Anavatti, "Hybrid PD-fuzzy and PD controllers for trajectory tracking of a quadrotor unmanned aerial vehicle: autopilot designs and real-time flight tests," IEEE Transactions on Systems, Man, and Cybernetics: Systems, vol. 51, no. 3, pp. 1817-1829, 2021. https://doi.org/10.1109/TSMC. 2019.2906320doi:[[[10.1109/TSMC.2019.2906320]]]
  • 2 B. Zhang, Q. Zong, H. Lu, and S. Shao, "Trajectory optimization of quand⁃rotor UAV formation Using hp⁃adaptive pseudospectral method," Scientia Sinica Technologica, vol. 47, no. 3, pp. 239-248, 2017. https://doi.org/10.1360/N092016-00198doi:[[[10.1360/N09-00198]]]
  • 3 M. Wen, J. Park, and K. Cho, "A scenario generation pipeline for autonomous vehicle simulators," Humancentric Computing and Information Sciences, vol. 10, article no. 24, 2020. https://doi.org/10.1186/s13673020-00231-zdoi:[[[10.1186/s13673020-00231-z]]]
  • 4 J. H. Park, M. M. Salim, J. H. Jo, J. C. S. Sicato, S. Rathore, and J. H. Park, "CIoT-Net: a scalable cognitive IoT based smart city network architecture," Human-centric Computing and Information Sciences, vol. 9, article no. 29, 2019. https://doi.org/10.1186/s13673-019-0190-9doi:[[[10.1186/s13673-019-0190-9]]]
  • 5 M. J. Ding, S. Z. Zhang, H. D. Zhong, Y . H. Wu, and L. B. Zhang, "A prediction model of the sum of container based on combined BP neural network and SVM," Journal of Information Processing Systems, vol. 15, no. 2, pp. 305-319, 2019. https://doi.org/10.3745/JIPS.04.0107doi:[[[10.3745/JIPS.04.0107]]]
  • 6 Y . Wang, H. Tao, and Z. Ma, "Symbiotic organisms search for constrained optimization problems," Journal of Information Processing Systems, vol. 16, no. 1, pp. 210-223, 2020. https://doi.org/10.3745/JIPS.01.0049doi:[[[10.3745/JIPS.01.0049]]]
  • 7 S. S. Alresheedi, S. Lu, M. Abd Elaziz, and A. A. Ewees, "Improved multiobjective salp swarm optimization for virtual machine placement in cloud computing," Human-centric Computing and Information Sciences, vol. 9, article no. 15, 2019. https://doi.org/10.1186/s13673-019-0174-9doi:[[[10.1186/s13673-019-0174-9]]]
  • 8 O. Cots, J. Gergaud, and D. Goubinat, "Direct and indirect methods in optimal control with state constraints and the climbing trajectory of an aircraft," Optimal Control Applications and Methods, vol. 39, no. 1, pp. 281-301, 2018. https://doi.org/10.1002/oca.2347doi:[[[10.1002/oca.2347]]]
  • 9 M. Khaksar‐e Oshagh and M. Shamsi, "Direct pseudo‐spectral method for optimal control of obstacle problem: an optimal control problem governed by elliptic variational inequality," Mathematical Methods in the Applied Sciences, vol. 40, no. 13, pp. 4993-5004, 2017. https://doi.org/10.1002/mma.4366doi:[[[10.1002/mma.4366]]]
  • 10 C. Kang, S. Wang, W. Ren, Y . Lu, and B. Wang, "Optimization design and application of active disturbance rejection controller based on intelligent algorithm," IEEE Access, vol. 7, pp. 59862-59870, 2019. https://doi.org/10.1109/ACCESS.2019.2909087doi:[[[10.1109/ACCESS..2909087]]]
  • 11 R. Rishel, "Application of an extended Pontryagin principle," IEEE Transactions on Automatic Control, vol. 11, no. 2, pp. 167-170, 1966. https://doi.org/10.1109/TAC.1966.1098286doi:[[[10.1109/TAC.1966.1098286]]]
  • 12 A. Zanelli, A. Domahidi, J. Jerez, and M. Morari, "FORCES NLP: an efficient implementation of interiorpoint methods for multistage nonlinear nonconvex programs," International Journal of Control, vol. 93, no. 1, pp. 13-29, 2020. https://doi.org/10.1080/00207179.2017.1316017doi:[[[10.1080/00207179.2017.1316017]]]
  • 13 B. Luo, Y . Yang, D. Liu, and H. N. Wu, "Event-triggered optimal control with performance guarantees using adaptive dynamic programming," IEEE Transactions on Neural Networks and Learning Systems, vol. 31, no. 1, pp. 76-88, 2020. https://doi.org/10.1109/TNNLS.2019.2899594doi:[[[10.1109/TNNLS.2019.2899594]]]
  • 14 G. H. Lu, J. Xia, and R. Zhou, "4D tactical trajectory tracking of UAV based on autonomous planning," Missiles & Space V ehicles, vol. 2018, no. 4, pp. 56-64, 2018.custom:[[[-]]]
  • 15 L. Xiao, J. Ying, X. Liu, and L. Ma, "An effective simultaneous approach with variable time nodes for dynamic optimization problems," Engineering Optimization, vol. 49, no. 10, pp. 1761-1776, 2017. https://doi.org/10.1080/0305215X.2016.1270276doi:[[[10.1080/0305215X.2016.1270276]]]
  • 16 M. E. Dennis, W. W. Hager, and A. V . Rao, "Computational method for optimal guidance and control using adaptive Gaussian quadrature collocation," Journal of Guidance, Control, and Dynamics, vol. 42, no. 9, pp. 2026-2041, 2019. https://doi.org/10.2514/1.G003943doi:[[[10.2514/1.G003943]]]
  • 17 Y . M. Agamawi and A. V . Rao, "CGPOPS: a C++ software for solving multiple-phase optimal control problems using adaptive gaussian quadrature collocation and sparse nonlinear programming," ACM Transactions on Mathematical Software (TOMS), vol. 46, no. 3, article no. 25, 2020. https://doi.org/10.1145/3390463doi:[[[10.1145/3390463]]]
  • 18 M. Nategh, B. Vaferi, and M. Riazi, "Orthogonal collocation method for solving the diffusivity equation: application on dual porosity reservoirs with constant pressure outer boundary," Journal of Energy Resources Technology, vol. 141, no. 4, article no. 042001, 2019. https://doi.org/10.1115/1.4041842doi:[[[10.1115/1.4041842]]]
  • 19 Y . X. Li, X. Li, S. B. Liu, and P . Kang, "Application of gauss pseudo-spectral method in trajectory optimization of variable trust missile," Modern Defence Technology, vol. 47, no. 3, pp. 71-77, 2019.custom:[[[http://open.oriprobe.com/articles/57088018/Application_of_Gauss_Pseudo_Spectral_Method_in_Tra.htm]]]
  • 20 J. Huang, Z. Liu, Z. Liu, Q. Wang, and J. Fu, "A pk-adaptive mesh refinement for pseudospectral method to solve optimal control problem," IEEE Access, vol. 7, pp. 161666-161679, 2019. https://doi.org/10.1109/ ACCESS.2019.2952139doi:[[[10.1109/ACCESS.2019.295]]]
  • 21 B. Zhang, Q. Zong, L. Dou, B. Tian, D. Wang, and X. Zhao, "Trajectory optimization and finite-time control for unmanned helicopters formation," IEEE Access, vol. 7, pp. 93023-93034, 2019. https://doi.org/10.1109/ ACCESS.2019.2927817doi:[[[10.1109/ACCESS.2019.2927817]]]
  • 22 Z. Ren, C. Xu, Z. Wu, and X. Liu, "Optimal tracking control of flow velocity in a one-dimensional magnetohydrodynamic flow," Engineering Optimization, vol. 51, no. 1, pp. 1-21, 2019. https://doi.org/ 10.1080/0305215X.2018.1426759doi:[[[10.1080/0305215X.2018.1426759]]]
  • 23 S. Li, Y . Ge, and Y . Shi, "An iterative dynamic programming optimization based on biorthogonal spatialtemporal Hammerstein modeling for the enhanced oil recovery of ASP flooding," Journal of Process Control, vol. 73, pp. 75-88, 2019. https://doi.org/10.1016/j.jprocont.2018.12.008doi:[[[10.1016/j.jprocont.2018.12.008]]]
  • 24 P . Liu, Y . Hu, J. Liao, L. Fan, X. Li, and X. Liu, "Optimization operation of electric locomotive based on twostage adaptive Gauss re-collocation pseudospectral approach," Acta Automatica Sinica, vol. 45, no. 12, pp. 2344-2354, 2019.doi:[[[10.16383/j.aas.c190211]]]
  • 25 M. Zhang, Y . Sun, and G. Duan, "Reentry trajectory optimization of hypersonic vehicle with enhancing parametrization method," in Proceedings of 2010 3rd International Symposium on Systems and Control in Aeronautics and Astronautics, Harbin, China, 2010, pp. 40-45. https://doi.org/10.1109/ISSCAA.2010.563 3024doi:[[[10.1109/ISSCAA.2010.5633024]]]