Time-varying formation dynamics modeling and constrained trajectory optimization of multi-quadrotor UAVs

Abstract

The formation of multi-quadrotor UAVs (QUAVs) in long flights will encounter many constraints and obstacles in the flight process, so it is necessary to change the formation shape to avoid these constraints. When the flight path of QUAVs is individually optimized, multiple individual dynamic problems will be faced, making the solution complicated and spending a long time. In this paper, as a whole instead of individuals considering the QUAVs, the formation is regarded as a rigid body using the Voronoi graph theory method in the flight process. The constraints of the followed QUAVs are transformed into the constraints of leader QUAV. Therefore, the formation is transformed for different constraints by changing the virtual rigid body structure or shape. A time-varying model is established to facilitate the use of optimization. The energy consumption of the leader UAV within the specified time is minimized as the trajectory optimization objective. The optimization improvement of the end heading and height constraints is proposed. A trajectory optimization method for the leader QUAV based on Gauss pseudospectral method is proposed, transforming the original optimal control problem into the nonlinear programming one. The simulation results demonstrate and verify the proposed method.

1 Introduction

The cooperative path planning of multi-UAVs is the crucial technology of UAVs' cooperative combat mission planning system [1,2,3,4,5]. The problem of constraints of multi-UAVs collaborative path planning needs to concern both the physical performance of a single UAV and the cooperative relationship among all UAVs. Besides, it relates to achieving the target constraint and minimum safe distance constraint among multi-UAVs [6,7,8]. When the environment changes, the formation of multi-UAVs should be adjusted in real time to ensure the survival of UAVs [9, 10].

In the past, the research on the path planning of multi-UAVs has mainly focused on assembling multi-UAVs and the respective path planning of multi-UAVs. Turpin et al. [11] considered the problem of controlling a group of Mavs to rapidly move in a 3D environment while maintaining a tight formation. They independently planed its trajectory for each UAV based on the local information of its neighboring UAVs [12]. Shanmugavel et al. [13] proposed a three-stage collaborative path planning method using Dubins paths with clothoid arcs. However, the flight paths of all UAVs were independently planned for the research above, and each UAV considered its constraints to establish an optimization model without considering the topological constraints relationship among them.

Ref. [14, 15] used the genetic algorithm to realize the trajectory planning problem of multiple UAVs. Radmanesh et al. [16] studied the collision-free 3D trajectories for multi-UAVs and proposed a decentralized method based on a partial differential equation. Chen et al. [17] solved multi-quadrotor UAVs’ formation path planning problem (QUAVs) by introducing a virtual rate rigid body and updating the artificial potential field method. However, when multi-UAVs need to keep a certain formation flight or transformation through the obstacles to the formation, these methods can no longer meet the requirements.

Reference [18], using the neural network control method, the Nonlinear AutoRegressive eXogenous (NARX) model of UAV was obtained. Altan et al. [19] established the mathematical model of the UAV three-axis frame system based on the traditional Newton–Euler method. The linear and nonlinear modeling were focused on achieving target tracking. In Ref. [20], the inherent nonlinear dynamic characteristics of the UAV were linearized. The model of UAV under external disturbance was built by the system identification method.

From the literature review, we found that most of the formation methods for multi-UAVs independently plan the fight body, rather than consider them as a whole. The innovative way is to convert the information of followed quadrotor UAVs (QUAVs) into the constraints of the leader QUAV in the formation. The QUAVs formation can be regarded as a virtual rigid body, and the formation can be changed by varying the rigid body structure before different constraints. After pass one constraint, the QUAVs formation remains unchanged or changes to the specified formation. It can ensure the maintenance of multi-QUAVs formation and optimize multiple performance indexes simultaneously.

Formation problems with constraints in cooperative path planning of multi-UAVs should be an optimization problem. At present, Gauss pseudospectral method (GPM) has many applications in trajectory optimization because of its higher precision and faster convergence speed compared to other algorithms in solving the optimal control problems. Zhang et al. [21] combined the takeoff mass with the trajectory optimization simultaneously by treating them as a nonlinear control problem. When the scopes of initial and terminal qualities and speeds are specified, the optimization problem is solved by using the GPM. In Ref. [22], an optimization method based on GPM was proposed based on the characteristics of the propulsion system. In Ref. [23], with the minimum of time and energy consumption as the optimal goal, the GPM method was used to optimize the landing trajectory of a tilt rotor. However, few pieces of literature using the GPM studied the QUAVs’ path optimization of variable formation under the constraint of three-dimensional space.

Aiming at QUAVs formation with obstacles in three-dimensional space, this paper innovatively proposes a formation method for cooperative trajectory planning of multi-UAVs. The novelties and contributions are as follows:

• (1) Constraints of the followed QUAVs are transformed into constraints of leader QUAV in QUAVs formation. The QUAVs formation is regarded as a virtual rigid body so that the movement of each flight follows the movement of the leader.

• (2) The optimization method of the end heading and height constraints is improved. It makes the UAV meet the goal of flying straight in the horizontal direction to the destination and gradually decreasing from a certain height in the final process of flight.

• (3) The GPM is adopted to transform the original optimal control problem into the nonlinear programming problem (NLP). The GPM can specify the final flight direction and accurately determine the time to reach the destination. The proposed method can use all the information given.

The rest of this article is organized as follows. Section 2 introduces the model of a QUAV attitude system. The dynamic model of multiple QUAVs is established to analyze the constraints and formation maintenance during its flight. Then, the problem summary faced in the flight trajectory optimization model for QUAVs formation is given. In Sect. 3, the principle of GPM is used to solve the problems in Sect. 2. Section 4 conducts simulation examples. Section 5 draws some conclusions.

2 Dynamics model of multiple QUAV

2.1 Dynamics model of QUAV

The QUAV is a kind of aircraft with four rotors, having two types of X-type distribution and cross-type distribution [24, 25]. In this paper, the X-type is used, and the schematic diagram of the QUAV is shown in Fig. 1 [26, 27].

Fig. 1

Schematic view of the QUAV system

The quadrotor UAV can achieve various motions by changing the rotor speed. In the figure, $E = [x_{{\text{e}}} ,\,y_{{\text{e}}} ,\,z_{{\text{e}}} ]$ is the north-east-down inertial coordinate system (NED), $B = [x_{{\text{b}}} ,\,y_{{\text{b}}} ,\,z_{{\text{b}}} ]$ is the body coordinate system, the origin of which coincides with the mass center of the body, and the coordinate axis is fixedly connected with the QUAV. The x-axis is the forward direction of the UAV, and $F_{i} (t),\,i = 1,\,2,\,3,\,4$ is the thrust produced by the four motors, respectively. In addition, $[x(t),\,y(t),\,z(t)]$ is the position of the QUAV in the local NED frame $\varepsilon$, and $[\phi (t),\,\theta (t),\,\psi (t)]$ the Euler angle of the QUAV, $\phi$ the roll angle, $\theta$ the pitch angle, and $\psi$ the yaw angle of the QUAV, respectively.

The following assumptions are made:

• (1) The QUAV is regarded as a rigid body and is completely symmetrical.

• (2) The origin of body coordinate O_b is completely coincident with the mass center of QUAV.

• (3) The propeller of the QUAV is a rigid body without considering its structure and elastic deformation.

The nonlinear dynamic model of the QUAV in the coordinate of $\varepsilon$ is given as follows [28, 29]:

$$ \left\{ {\begin{array}{*{20}l} {\ddot{x} = - (\cos \psi \sin \theta \cos \phi + \sin \psi \sin \phi )\frac{{U_{1} }}{m} + \frac{{K_{1} \dot{x}}}{m},} \hfill \\ {\ddot{y} = - (\sin \psi \sin \theta \cos \phi - \cos \psi \sin \phi )\frac{{U_{1} }}{m} + \frac{{K_{2} \dot{y}}}{m},} \hfill \\ {\ddot{z} = - (\cos \theta \cos \phi )\frac{{U_{1} }}{m} + g + \frac{{K_{3} \dot{z}}}{m},} \hfill \\ {\ddot{\phi } = \frac{1}{{I_{x} }}\left[ {lU_{2} + \dot{\theta }\dot{\psi }(I_{y} - I_{z} ) - K_{4} \dot{\phi }} \right],} \hfill \\ {\ddot{\theta } = \frac{1}{{I_{y} }}\left[ {lU_{3} + \dot{\phi }\dot{\psi }(I_{z} - I_{x} ) - K_{5} \dot{\theta }} \right],} \hfill \\ {\ddot{\psi } = \frac{1}{{I_{z} }}\left[ {U_{4} + \dot{\theta }\dot{\phi }(I_{x} - I_{y} ) - K_{6} \dot{\psi }} \right],} \hfill \\ \end{array} } \right. $$

(1)

where $x,\,y,\,z$ are the positions of the QUAV, $I_{x} ,\,I_{y} ,\,I_{z}$ the rotational inertias of the QUAV, respectively; because of the symmetry, $I_{x} = I_{y}$; in the inertial frame of reference, $\dot{\phi },\;\dot{\theta }$ and $\dot{\psi }$ are the roll, pitching, and yaw angle velocities, respectively; $l$ is the length between the center of aircraft and the rotor; $U_{1} ,\;U_{2} ,\;U_{3}$ and $U_{4}$ are vertical, rolling, pitch and yaw motion control quantities, respectively. $K_{1} ,\;K_{2} ,\;K_{3}$ are the air resistance coefficient of three axes, $K_{4} ,\;K_{5} ,\;K_{6}$ the air resistance moment coefficient of three axes. In addition, $[v_{x} ,\;v_{y} ,\;v_{z} ]$ and $[\omega_{\phi } ,\;\omega_{\theta } ,\;\omega_{\psi } ]$ are defined as linear velocity components and angular velocity components around $x,\;y,\;z$ coordinate in the NED coordinate system, where $v_{x} ,\;v_{y}$ and $v_{z}$ are the forward, lateral, and falling velocities, respectively, $\omega_{\phi } ,\;\omega_{\theta }$ and $\omega_{\psi }$ the roll, pitch, and yaw angular velocities, respectively.

The expression of the control quantity is expressed as follows: the control quantity directly controls the motor speed, and the actual implementation form is the PWM wave to control the motor.

$$ \left[ {\begin{array}{*{20}c} {U_{1} } \\ {U_{2} } \\ {U_{3} } \\ {U_{4} } \\ \end{array} } \right] = \left[ {\begin{array}{*{20}c} {K_{T} (\Omega_{1}^{2} + \Omega_{2}^{2} + \Omega_{3}^{2} + \Omega_{4}^{2} )} \\ {\frac{\sqrt 2 }{2}K_{T} (\Omega_{1}^{2} - \Omega_{2}^{2} - \Omega_{3}^{2} + \Omega_{4}^{2} )} \\ {\frac{\sqrt 2 }{2}K_{T} (\Omega_{1}^{2} + \Omega_{2}^{2} - \Omega_{3}^{2} - \Omega_{4}^{2} )} \\ {K_{M} (\Omega_{1}^{2} - \Omega_{2}^{2} + \Omega_{3}^{2} - \Omega_{4}^{2} )} \\ \end{array} } \right], $$

(2)

where $\Omega_{i} \;(i = 1,\;2,\;3,\;4)$ represents the rotation speed of each propeller, $K_{T}$ the propeller lift coefficient, $K_{M}$ the inverse torque coefficient of the propeller.

2.2 Description of flight problems of multiple QUAVs

The formation of QUAVs is shown in Fig. 2.

Fig. 2

Formation of QUAVs

The formation of QUAVs follows the leader (UAV1), tracking the desired trajectory $x_{1} (t)$. Each UAV is controlled to maintain the desired shape denoted by $s_{i,j}$. In this paper, the formation is composed of N QUAVs. Due to turning or other situations during flying, if multiple QUAVs keep the formation unchanged, their speeds may be inconsistent. Therefore, the following assumptions are defined in this paper:

• (1) When the formation is not turning, all UAVs move at a constant speed. When it is flying in formation, the speed of each UAV is not consistent. It does not take into account the energy consumption of the turn.

• (2) The UAVs can communicate reliably to prevent collisions among them.

• (3) The UAVs formation can be changed according to environmental constraints. After passing the obstacle, the formation recovers and continues to fly at the same speed and direction.

• (4) The formation of QUAVs is regarded as a virtual rigid body.

The basic idea of the virtual structure method is to regard the QUAV formation system as a virtual structure of a rigid body. Each QUAV is considered as a fixed point on the virtual structure. When the QUAVs' formation moves, the QUAV can follow the corresponding fixed point on the rigid body. If the formation must be transformed to avoid obstacles, and then keep fixed. The transformed formation is also called the new virtual rigid body.

In this paper, the consistency theory based on the Voronoi graph theory [11, 30, 31] defines the formation-keeping problem among the QUAVs. The formation system is transformed into a network topology.

According to the information communication among the formation formed by $N$ QUAVs, the formation is modeled as a directed graph $G$, as shown in Fig. 2. The direction of information flow is represented by the direction of arrows in the directed graph $G$. The node $p_{1}$ represents the leader, and the remaining nodes are followers $p_{i} (t) = \left[ {x_{i} (t),\;y_{i} (t),\;z_{i} (t),\;\psi_{i} (t),\;\theta_{i} (t),\;\phi_{i} (t)} \right]^{T}$, $(i = \, 2, \ldots ,N)$. The leader position is the equilibrium point of the formation consistency algorithm. The expected position deviation between the leader and followers is defined as the relative position deviation. The leader sends its state to followers. The topology of information among the QUAVs can be any directed graph. The shape vectors are 6 × 1 vectors relating positions of pairs of UAVs. For UAV $i$ and $j$,

$$ p_{i,j} (t) = p_{j} (t) - p_{i} (t) = \left[ {\begin{array}{*{20}c} {x_{j} (t) - x_{i} (t)} \\ {y_{j} (t) - y_{i} (t)} \\ {z_{j} (t) - z_{i} (t)} \\ \begin{gathered} \psi_{j} (t) - \psi_{i} (t) \hfill \\ \theta_{j} (t) - \theta_{i} (t) \hfill \\ \phi_{j} (t) - \phi_{i} (t) \hfill \\ \end{gathered} \\ \end{array} } \right]. $$

(3)

We assume UAV1 is specified as the leader, $g(t) = x_{1}$. The overall shape can be prescribed by a $(N - 1) \times (N - 1) \,$ vector-valued, skew-symmetric matrix [11].

Considering the change of the topology with the time change, let $G = \left\{ {\left. {G_{1} ,\;G_{2} ,\; \ldots \;,\;G_{m} } \right\}} \right.,\;m \ge 1$ represents the possible formation set and $m$ the maximum number.

Definition 1

[32] Time-varying formation can be represented by a set of functions $h = [h_{1} (t), \ldots ,\;h_{N} (t)]^{T}$. For a given formation $h_{i} (t)$, if the formation system of UAVs satisfies in any given initial situation

$$ \mathop {\lim }\limits_{t \to \infty } \;\left( {\xi_{i} (t) - h_{i} (t) - c(t)} \right) = 0,\;i = 1,\;2,\; \ldots ,\;N, $$

(4)

then it is said that the QUAVs achieve formation $h_{i} (t)$, where $h_{i} (t) = \left[ {h_{ix} (t),h_{iy} (t),h_{iz} (t)} \right]$ represents the information for each UAV in the formation, $c(t)$ is called a certain trajectory function of a formation center, $\xi_{i} (t) = \left[ {x_{i} (t),\;y_{i} (t),\;z_{i} (t)} \right]$ represents the real-time position in the three-dimensional space of the i-th UAV.

According to Ref. [32], if the formation in this paper meets Definition 1, the time-varying formation transformation of QUAVs in this paper can be realized. In this way, the research is meaningful.

When a QUAV formation encounters an obstacle, it sometimes needs to change its formation to get through the obstacle. Formation transformation can be regarded as reassembly of QUAVs, namely formation reconstruction. The QUAVs formation is regrouped from formation A into formation B, as shown in Fig. 3. In formation transformation, minimum energy consumption or time is required [33]. In this paper, the minimum time is taken as the required target of formation transformation.

Fig. 3

Formation transformation

QUAVs have the advantages of hovering and maneuvering in all directions, so formation change can be made by methods of formation rotation, formation reduction, or new formation.

2.2.1 Rotating

To keep the formation rigidity, the formation can be rotated at the same angle around a centerline or a center point to keep the relative position of each UAV unchanged. Suppose the position of its rotation center is $[x_{r} ,\;y_{r} ,\;z_{r} ]$, and the rotation angle in 3D space is $\alpha$, then the rotation path length of the i-th QUAV is an arc with length

$$ l_{i} = \alpha \left[ {(x_{i} - x_{r} )^{2} + (y_{i} - y_{r} )^{2} + (z_{i} - z_{r} )^{2} } \right]^{1/2} . $$

(5)

For the minimum rotation time $t$, corresponding to the QUAV with the maximum rotation length $l_{\max }$, Eq. (5) be minimized, i.e.,

$$ J_{1} = t_{\min } = \min \left( {\max \left( {\frac{{l_{i} }}{{v_{i} }}} \right)} \right),\;i = 1,\;2, \ldots ,\;N. $$

(6)

Then, the rotation speed of other QUAVs is

$$ v_{i} = \frac{{l_{i} }}{{t_{\min } }},\;i = 1,\;2, \ldots ,\;N. $$

(7)

For the QUAV with the maximum rotation path with its maximum flight speed $v_{\max }$, the minimum time can also be obtained

$$ t_{\min } = \frac{{l_{\max } }}{{v_{\max } }}. $$

(8)

All QUAVs can rotate synchronously. After passing the obstacle, the formation rotates back to the original formation at an angle $- \,\alpha$ and continues to fly, or keeps the formation unchanged.

2.2.2 Formation shrinking

The formation of QUAVs draws close to the center of formation in proportion to shrink formation and pass obstacles, as shown in Fig. 4 [33].

Fig. 4

Reduction diagram of formation

Formation shrinking can be regarded as a translational movement. In this paper, formation shrinking transformation is requested with minimum time. The distance between the formation center $[x_{r} ,\;y_{r} ,\;z_{r} ]$ and the physical center $[x_{f} ,\;y_{f} ,\;z_{f} ]$ of the QUAV that is farthest from the formation center is

$$ l_{\max } = \left[ {(x_{f} - x_{r} )^{2} + (y_{f} - y_{r} )^{2} + (z_{f} - z_{r} )^{2} } \right]^{{{1 \mathord{\left/ {\vphantom {1 2}} \right. \kern-\nulldelimiterspace} 2}}} . $$

(9)

Namely,

$$ J_{2} = t_{\min } = \min \left( {\frac{{l_{f} }}{{v_{f} }}} \right). $$

(10)

At this time, the rotation speed of other UAVs is

$$ v_{i} = \frac{{l_{i} }}{{t_{\min } }},\;i = 1,\;2,\; \ldots ,\;N. $$

(11)

If the UAV is allowed to fly at its maximum speed $v_{\max }$, we can also get

$$ t_{\min } = \frac{{l_{\max } }}{{v_{\max } }}. $$

(12)

That is, all UAVs can rotate synchronously.

After passing the obstacle, the formation recovers and continues to fly at the same speed and direction, or continues to fly with the formation unchanged.

2.2.3 New formation

When the UAVs need to be transformed into a new formation, the UAV formation can be regarded as regrouping without obstacles with minimum time. Here, if all UAVs are required to fly at a maximum constant speed $v_{\max }$, the time $t_{i}$ is proportional to its distance, namely,

$$ t_{i} = \frac{{l_{i} }}{{v_{\max } }}. $$

(13)

It can be based on the existing formation regrouping methods, such as literature [2, 6, 7, 33], taking the following as the objective function to solve the optimal value:

$$ J_{3} = t_{\min } = \min \left\{ {\left. {\max t_{i} } \right\}} \right.,\;i = 1,\;2,\; \ldots \;,\;N. $$

(14)

After $t_{\min }$ is obtained, the UAVs can fly at a constant speed $v_{i} = {{l_{i} } \mathord{\left/ {\vphantom {{l_{i} } {t_{\min } }}} \right. \kern-\nulldelimiterspace} {t_{\min } }}$ again to realize synchronous formation transformation.

2.3 Constraints of leader QUAV

• (1) Initial boundary condition constraint

  The initial condition needs to meet for the UAV at $t = 0$, namely,

  $$ \begin{aligned} X(t_{0} ) = &\, X_{0} ,\;Y(t_{0} ) = Y_{0} ,\;Z(t_{0} ) = Z_{0} ,\;\theta (t_{0} ) \\ & = \theta_{0} ,\;\phi (t_{0} ) = \phi_{0} ,\;\psi (t_{0} ) = \psi_{0} . \\ \end{aligned} $$

  (15)

• (2) Terminal condition constraint

  The terminal position and terminal attitude constraints of the UAV at $t = t_{f}$ are,

  $$ \begin{aligned} X(t_{f} ) = & \,X_{f} ,\;Y(t_{f} ) = Y_{f} ,\;Z(t_{f} ) = Z_{f} ,\;\theta (t_{f} ) \\ & = \theta_{f} ,\;\phi (t_{f} ) = \phi_{f} ,\;\psi (t_{f} ) = \psi_{f} . \\ \end{aligned} $$

  (16)

• (3) Minimum turning radius

  Because the flight physical property of the UAV determines the value of its turning radius, the limitation of the turning radius should be taken into account when path planning is carried out. Besides, a larger turning radius prevents the UAVs from colliding with each other when they change formation. The minimum turning radius refers to the circular radius of the UAV when it moves in a circular motion with extreme acceleration at the horizontal plane. In general, this radius should meet the following requirement:

  $$ \frac{{v_{1}^{2} }}{{R_{1} \cdot g}} \le n_{1\max } , $$

  (17)

  where $v_{1}$ is the speed of the leader QUAV, $R_{1}$ its turning radius, $g$ the gravitational acceleration, and $n_{1\max }$ its maximum normal acceleration. We can obtain the minimum transition radius of the leader QUAV flight, meeting

  $$ R_{1} \ge \frac{{v_{1}^{2} }}{{n_{1\max } \cdot g}}. $$

  (18)

• (4) Control constraints

  $$ \begin{gathered} U_{{i\min }} \le U_{i} \le U_{{i\max }} ,\;U_{{i\max }} > 0, \hfill \\ U_{{i\min }} \le 0,\;i = 1,\;2,\;3,\;4, \hfill \\ \end{gathered} $$

  (19)

  where $U_{i\min }$ and $U_{i\max }$ are the corresponding minimum and maximum of $U_{i}$.

• (5) Flight altitude minimum constraint

  In general, UAV will encounter high buildings, mountains, or enemy fire range during flight. Therefore, the minimum height of the UAV flight should be set, satisfying

  $$ z_{1} (t) \ge z_{\min } (t),\;z_{\min } (t) > 0, $$

  (20)

  where $z_{1} (t)$ and $z_{\min } (t)$ are the real-time and minimum flying altitude of the leader QUAV.

• (6) Horizontal constraint

  In this paper, at a certain flying altitude, the horizontal obstacles are represented by the disk and the constraints are as follows:

  $$ \begin{gathered} \left( {x_{1} (t) - x_{j} } \right)^{2} + \left( {y_{1} (t) - y_{j} } \right)^{2} > R_{j} , \hfill \\ j = 1,\;2,\; \ldots ,\;n, \hfill \\ \end{gathered} $$

  (21)

  where $R_{j}$ represents the radius of the $j$-th no-fly zone disk, and $n$ the total number of no-fly zones.

2.4 Constraints of followed QUAVs

The path optimization of the leader QUAV in multi-QUAVs formation is only considered in this paper, and the followed QUAVs exist as a constraint condition. The constraint conversion from followed QUAVs to leader QUAV is dealt with as follows:

• (1) According to the position information of the followers and leader in the formation, the position of the followers in the inertial coordinate system can be expressed as follows:

  $$ \left\{ {\begin{array}{*{20}l} {x_{i} (t) = x_{1} (t) - x_{i1} ,} \hfill \\ {y_{i} (t) = y_{1} (t) - y_{i1} ,} \hfill \\ {z_{i} (t) = z_{1} (t) - z_{i1} ,} \hfill \\ \end{array} } \right. $$

  (22)

  where $[x_{i1} ,\;y_{i1} ,\;z_{i1} ]$ is the relative position between the i-th follower and the leader in the formation, as shown in Fig. 5.

  Fig. 5

  

  Relative position in formation between followed QUAVs and leader QUAV

  In this case, the follower constraints can be transformed into constraints on the leader QUAV.

• (2) Flight altitude minimum

  $$ \left\{ {\begin{array}{*{20}l} {z_{i} (t) \ge h_{\min } (t),\;\;z_{i} (t) = z_{1} (t) - z_{i1} } \hfill \\ { \Rightarrow z_{1} (t) \ge h_{\min } (t) + z_{i1} ,\;\;h_{\min } (t) > 0.} \hfill \\ \end{array} } \right. $$

  (23)

• (3) Horizontal constraint

  $$ \begin{aligned} \left( {x_{i} (t) - x_{j} } \right)^{2} + \left( {y_{i} (t) - y_{j} } \right)^{2} = & \left( {x_{1} (t) - x_{{i1}} - x_{j} } \right)^{2} \\ & + \left( {y_{1} (t) - y_{{i1}} - y_{j} } \right)^{2} \\ & > R_{j} ,\;j = 1,\;2,\; \cdots \;,\;m. \\ \end{aligned} $$

  (24)

• (4) Turning radius minimum

  Like the leader QUAV, the turning radius of the followers should also meet the requirement

  $$ \frac{{v_{i}^{2} }}{{R_{i} \cdot g}} \le n_{i\max } , $$

  (25)

  where $v_{i}$ is the speed of the i-th follower, $R_{i}$ the corresponding turning radius, $g$ the gravitational acceleration, and $n_{i\max }$ the maximum normal acceleration. For this purpose, we obtain a minimum flight transition radius $R_{i} = {{v_{i}^{2} } \mathord{\left/ {\vphantom {{v_{i}^{2} } {(n_{i\max } \cdot g)}}} \right. \kern-\nulldelimiterspace} {(n_{i\max } \cdot g)}}$ of the i-th follower. In formation flight, the leader and the followers rotate around the same center, that is, they fly around the same center of a circle. The speed relationship between $v_{i}$ and $v_{1}$ has the formula

  $$ \frac{{v_{i} }}{{v_{1} }} = \frac{{\frac{1}{2}\pi R_{i}^{2} }}{{\frac{1}{2}\pi R_{1}^{2} }} = \frac{{R_{i}^{2} }}{{R_{1}^{2} }} \Rightarrow v_{i} = \frac{{R_{i}^{2} }}{{R_{1}^{2} }}v_{1} . $$

  (26)

According to Fig. 5,

$$ \begin{aligned} R_{i} = & \sqrt {x_{i}^{2} + y_{i}^{2} + z_{i}^{2} } \\ & = \sqrt {(x_{1} - x_{i1} )^{2} + (y_{1} - y_{i1} )^{2} + (z_{1} - z_{i1} )^{2} } . \\ \end{aligned} $$

(27)

We can get

$$ \frac{{v_{i}^{2} }}{{R_{i} \cdot g}} = \frac{{\left( {\frac{{R_{i}^{2} }}{{R_{1}^{2} }}v_{1} } \right)^{2} }}{{R_{i} \cdot g}} = \frac{{R_{i}^{3} }}{{R_{1}^{4} }}v_{1}^{2} . $$

(28)

That is,

$$ \frac{{\left( {\sqrt {(x_{1} - x_{i1} )^{2} + (y_{1} - y_{i1} )^{2} + (z_{1} - z_{i1} )^{2} } } \right)^{3} }}{{R_{1}^{4} }}v_{1}^{2} \le n_{i\max } . $$

(29)

Remark 1

From Eqs. (22–29), all the constraints of followed QUAVs are converted to the minimum turning radius, speed, and position constraints of the leader QUAV.

2.5 Improvement of performance index function

2.5.1 Energy consumption index for total flight distance

When formation transformation is not taken into account in the formation flight, the minimum energy consumption performance index traditionally adopted is

$$ J_{4} = \min \left( {\sum\limits_{i = 1}^{N} {w_{i} } } \right), $$

(30)

where $w_{i} = \int\nolimits_{{t_{0} }}^{{t_{f} }} {w_{i} (t){\text{d}}t}$ denotes the total energy consumption of UAV_i, and $w_{i} (t)$ the real-time energy consumption. For all UAV_i, since the concept of virtual rigid formation is adopted in this paper, each UAV must reach the specified location at the same time $t_{f}$, so the speed and distance of all UAV_i are in direct proportion [33], namely

$$ v_{i} = \frac{{l_{i} }}{{t_{f} - t_{0} }}. $$

(31)

Ignoring the impacts of different UAV_i on air density, windward area, thermal effects, etc., the energy consumption generated by UAV_i is

$$ w_{i} = F_{i} l_{i} , $$

(32)

where $F_{i} = k_{f} v_{i}^{2}$ represents the air resistance during UAV_i flight, $k_{f}$ the air resistance coefficient with a constant value.

2.5.2 Analysis on the influence of formation transformation

The formation transformation takes the leader QUAV as the central axis for transformation. During formation transformation, the leader still follows its trajectory, and the followers make the formation change. The formation change or rotation can be ignored relative to the total flight distance. The time solution of formation change can be completed online quickly, and the influence on the overall flight optimization of the QUAVs can be ignored.

2.5.3 Terminal demand

Generally speaking, if a UAV formation is required to fly in a straight line from a certain horizontal direction at a certain altitude to the target point, to ensure that the UAV's terminal flight path is straight and reduce the control demand at the terminal stage, we introduce the following performance index function:

$$ J_{5} = \min \left( {Q(t)\left( {\psi_{1} (t) - \psi_{r} } \right)} \right), $$

(33)

$$ J_{6} = \min \left( {Q(t)\left( {z_{1} (t) - h_{r} } \right)} \right), $$

(34)

where $Q(t)$ is the weighting function. We finally adopt the performance index function as follows:

$$ J = a_{1} J_{4} + a{}_{2}J_{5} + a_{3} J_{6} , $$

(35)

where $a_{1} ,a{}_{2},a_{3}$ are the weighting coefficients which can be selected according to the actual requirements. In this paper, the energy consumption index of UAV takes up a significant proportion and $a_{1} = 0.8,\;a_{2} = 0.1,\;a_{3} = 0.1$.

The flight track diagram and control curve of the improved single QUAV are shown in Fig. 6, in which different weighting functions are added as follows, respectively [34]:

Fig. 6

x–y plane flight diagram

(1) $Q(t) = 1$; (2) $Q(t) = t^{2}$; (3) $Q(t) = 1/(t_{f} - t + 0.5)$; (4) $Q(t) = 1/(t_{f} - t + 0.5)^{2}$.

The following simulation is carried out: a single QUAV takes off from $(0,\;0,\;2)\;{\text{m}}$ and lands on $(1000,\;1000,\;2)\;{\text{m}}$, and the end requires the UAV to fly from 90° direction to the target point and gradually land from a higher altitude. Figures 6 and 7 show the x–y plane and the corresponding 3D flight diagrams, respectively.

Fig. 7

3D flight diagram

From Fig. 6, the four selected weighting functions can ensure that the UAV will turn into a flat state in the latter part of the flight. Differently, from the 3D flight diagram in Fig. 7, for $Q(t) = 1$, the altitude of UAV in the later period of flight changes a lot, and it cannot guarantee that the UAV lands from a higher position; Taking $Q(t) = t^{2}$ and $Q(t) = 1/(t_{f} - t + 0.5)^{2}$, the altitude is higher and the descent time is longer; Taking $Q(t) = 1/(t_{f} - t + 0.5)$, the altitude is not so high, and the variation of UAV flight is smooth. Therefore, comprehensively considering the simulation results, $Q(t) = 1/(t_{f} - t + 0.5)$ is selected as the time function.

Remark 2

From Eqs. (33–35), the four selected weighting functions can ensure that the UAV meets the goal of flying straight in the horizontal direction to the destination and gradually decreasing from a certain height in the final process of flight. Then, the altitude of UAV in the latter period of flight is not so high, and the variation of UAV flight is smooth.

2.6 Problem summary

We conclude the following flight path optimization model for formation flight of QUAVs: for Eq. (1) of the leader QUAV, finding the control quantity to make the performance index [Eq. (35)] optimal, satisfying boundary value constraints [Eqs. (15) and (16)], we process constraints [Eqs. (18–21, 23, 24, 29)].

Now, the faced optimization problems are

• (1) The optimal solution is a curved form and an infinite-dimensional problem.

• (2) The optimization model is a dynamic equation in differential form.

• (3) It is an objective function of integral form.

• (4) There are constraints.

To solve these problems, the Gauss pseudospectral method (GPM) principle [35, 36] is used.

• (1) The optimization problem of the curve is transformed into the optimization problem of several characteristic points on the curve, and the infinite-dimensional problem is reduced to a finite-dimensional problem.

• (2) The whole curve is fitted by the polynomial.

• (3) We transform the dynamic differential equation into algebraic constraint by sampling function.

• (4) The integral form of the objective function is transformed into an algebraic equation by using the Gauss quadrature formula.

In this way, the whole optimal control problem is put into solving the framework of nonlinear programming.

3 Principle of GPM

In GPM, control variables and state variables are dispersed first in a selected Gauss point series. Select the discrete points for the node to construct through Lagrange interpolation polynomial, and approximate state and control variables based on the polynomial, and then approximate the derivative of the state variable by taking the derivative of these global interpolation polynomials. The differential equation constraints can be converted into a set of algebraic equation constraints. For the integral term in the performance index, Gauss integral is used for discretization. The terminal state is obtained from the initial state and the integral of the right function, and Gauss integral is used for discretization calculation. After the above transformation, the original optimal control problem is transformed into a parametric optimization problem with a series of algebraic constraints, which is also called the nonlinear programming problem (NLP) [37,38,39].

3.1 Time-domain transformation

When GPM is adopted, the time interval $[t_{0} ,\;t_{f} ]$ needs to be transformed into $[ - 1,\;1]$, so the time $t$ is converted as:

$$ {\kern 1pt} t \in [t_{0} ,\;t_{f} ] \to \tau \in [ - 1,\;1],\;\tau = \frac{2t}{{t_{f} - t_{0} }} - \frac{{t_{f} + t_{0} }}{{t_{f} - t_{0} }}. $$

(36)

3.2 Approximation of state variables and control variables

The GPM uses $K$ order Legendre–Gauss (LG) points (the roots of $K$ order Legendre polynomials $P_{K} (\overline{\tau })$) and $\tau_{0} = - 1$ as the discrete node to construct $K + 1$ order Lagrange interpolation polynomials. The approximate expression of the state variable is

$$ {\kern 1pt} x(\tau ) \approx X(\tau ) = \sum\limits_{i = 0}^{K} {L_{i} (\tau )X(\tau_{i} )} {\kern 1pt} {\kern 1pt} {\kern 1pt} , $$

(37)

where

$$ {\kern 1pt} L_{i} (\tau ) = \prod\limits_{j = 0,j \ne i}^{K} {\frac{{\tau - \tau_{i} }}{{\tau_{i} - \tau_{j} }}} , $$

(38)

$$ {\kern 1pt} P_{K} (\overline{\tau }) = \frac{1}{{2^{K} K!}}\frac{{{\text{d}}^{K} }}{{{\text{d}}\overline{\tau }^{K} }}\left[ {\left( {\overline{\tau }^{2} - 1} \right)^{K} } \right],\;K = 1,\;2,\; \ldots . $$

(39)

Letting the approximate state value at the node in Eq. (37) equal to the actual state value, we get $X_{i} = X(\tau_{i} ) = x(\tau_{i} )$.

For the control variables, the same method is used to discretize, i.e.,

$$ u(\tau_{k} ) = U(\tau_{k} ),\;(k = 1,\; \ldots \;,\;K)\;, $$

(40)

namely,

$$ u(\tau ) \approx U(\tau ) = \sum\limits_{i = 1}^{K} {L_{i} (\tau )\;U(\tau_{i} )} . $$

(41)

3.3 Transformation of dynamic differential equation constraints

Take the derivative of Eq. (37)

$$ \dot{x}(\tau ) \approx \dot{X}(\tau ) = \sum\limits_{i = 0}^{K} {D_{ki} (\tau_{k} )\;X(\tau_{i} )} , $$

(42)

where the differential matrix $D \in R^{{K \times \left( {K + 1} \right)}}$ can be determined offline, namely,

$$ D_{ki} = \dot{L}_{i} (\tau_{k} ) = \left\{ {\begin{array}{*{20}l} {\frac{{(1 + \tau_{k} )\dot{P}_{K} (\tau_{k} ) + P_{K} (\tau_{k} )}}{{(\tau_{k} - \tau_{i} )[(1 + \tau_{i} )\dot{P}_{K} (\tau_{i} ) + P_{K} (\tau_{i} )]}}} \hfill & {i \ne k,} \hfill \\ {\frac{{(1 + \tau_{i} )\ddot{P}_{K} (\tau_{i} ) + 2\dot{P}_{K} (\tau_{i} )}}{{2[(1 + \tau_{i} )\dot{P}_{K} (\tau_{i} ) + P_{K} (\tau_{i} )]}}} \hfill & {i = k,} \hfill \\ \end{array} } \right. $$

(43)

where $\tau_{k} \;(k = 1,\; \ldots \;,\;K)$ is a point in the set $\kappa = \left\{ {\tau_{1} ,\;\tau_{2} ,\; \ldots \;,\;\tau_{K} } \right\}$, and $\overline{\tau }_{i} (i = 0,\; \ldots \;,\;K)$ belongs to the set $\kappa_{0} = \left\{ {\overline{\tau }_{0} ,\;\overline{\tau }_{1} ,\; \ldots \;,\;\overline{\tau }_{K} } \right\}$. By substituting Eq. (43) into the dynamic equation, the algebraic constraint equation that the state variable should satisfy on the discrete node can be obtained by

$$ \sum\limits_{i = 0}^{K} {D_{ki} X(\tau_{i} )} - \frac{{t_{f} - t_{0} }}{2}f\left( {X(\tau_{k} ),\;U(\tau_{k} ),\;\tau_{k} ;\;t_{0} ,\;t_{f} } \right) = 0, $$

(44)

where $k = \, 1,\; \ldots \;,\;K$.

3.4 Processing of terminal state constraints

The terminal moment node is not included in the approximate expression of the state variable, but the terminal state should also satisfy the constraint of the dynamic equation

$$ x(\tau _{f} ) = x(\tau _{0} ) + \int\limits_{{ - 1}}^{1} {f\left( {x(\tau ),\;u(\tau ),\;\tau } \right){\text{d}}\tau } . $$

(45)

Equation (45) is approximated by Gauss integral formula. We get

$$ X(\tau _{f} ) = X(\tau _{0} ) + \frac{{t_{f} - t_{0} }}{2}\sum {\lambda _{k} } f\left( {X(\tau _{k} ),\;U(\tau _{k} ),\;\tau ;\;t_{0} ,\;t_{f} } \right), $$

(46)

where $\lambda_{k} = \int_{ - 1}^{1} {L_{i} (\overline{\tau }){\text{d}}\tau }$ is Gauss weight, and $\tau$ is Legendre–Gauss point.

3.5 Approximation of integral term in the performance index function

The integral term in the performance index function is approximated by the Gauss integral formula

$$ \begin{aligned} J = & \Phi \;(X_{0} ,\;t_{0} ,\;X_{f} ,\;t_{f} ) \\ & + \frac{{t_{f} - t_{0} }}{2}\sum\limits_{k = 1}^{2} {\lambda_{k} g(X_{k} ,\;U_{k} ,\;\tau_{k} ;\;t_{0} ,\;t_{f} )} , \\ \end{aligned} $$

(47)

where $\Phi ( \cdot )$ is the no-integral term in the performance index function.

3.6 General description of GPM

Based on the above mathematical transformation, the principle of GPM to solve the optimal control problem can be described as follows:

• (1) Calculate state variables $X_{i} ,\;\;i = 0,\; \ldots \;,\;K$ and control variables $U_{k}$, $k = 1,\; \ldots \;,\;K$ and $t_{0}$, $t_{f}$ on the discrete nodes to make the minimum performance index [Eq. (47)].

• (2) Satisfy the dynamics equation of algebraic constraint equation (44), the terminal state constraint equation (46), and the boundary conditions of the original optimal control problem

  $$ \varphi \left( {X_{0} ,\;t_{0} ,\;X_{f} ,\;t_{f} } \right)\; = \;0, $$

  (48)

and process constraints

$$ C(X_{k} ,\;U_{k} ,\;\tau_{k} ;\;t_{0} ,\;t_{f} ) \le 0,\,\,k = \, 1,\; \ldots \;,\;K. $$

(49)

The original optimal control problem is transformed into a nonlinear programming problem (NLP), namely

$$ \begin{array}{*{20}l} {\min \,\,F(y),} \hfill & {y \in R^{M} ,} \hfill \\ {s.t.\,\,g_{j} (y) \ge 0,} \hfill & {j = \, 1,\;2,\; \ldots ,\;q,} \hfill \\ {h_{j} (y) = 0,} \hfill & {j = \, 1,\;2,\; \ldots ,\;r.} \hfill \\ \end{array} $$

(50)

where $y$ is the designed variable, including control variable, state variable, and two endpoint time.

At present, there are many effective methods to solve the nonlinear programming problem. The sequential quadratic programming algorithm (SQP) is widely used for its local superone-time convergence and global convergence, so the GPM uses the SQP algorithm to deal with the transformed NLP problem.

Remark 3

Through GPM, the infinite-dimensional problem is transformed into a finite-dimensional problem. It converts the dynamic differential equation to an algebraic constraint by using the sampling function, and the integral form of the objective function is transformed into an algebraic equation by using the Gaussian quadrature formula. And the optimization problem is transformed into a nonlinear problem.

4 Simulation verification of trajectory optimization

The simulation in this paper is aimed at the formation of six QUAVs. The type of UAVs used is the ZY F150 QUAV. The formation is formed by an equilateral triangle when taking off, and the formation-keeping strategy is the leader-followed method. The relevant parameters of the QUAVs used in the experiment are shown in Eq. (51) and Table 1.

$$ \left\{ {\begin{array}{*{20}l} {0 \le x_{i} \le 1500\;{\text{m}},\;0 \le y_{i} \le 1500\;{\text{m}},\;0 \le z_{i} \le 35\;{\text{m}},} \hfill \\ {\left| {v_{xi} } \right| \le 5\;{{\text{m}} \mathord{\left/ {\vphantom {{\text{m}} {\text{s}}}} \right. \kern-\nulldelimiterspace} {\text{s}}},\;\left| {v_{yi} } \right| \le 5\;{{\text{m}} \mathord{\left/ {\vphantom {{\text{m}} {\text{s}}}} \right. \kern-\nulldelimiterspace} {\text{s}}},\;\left| {v_{zi} } \right| \le 2\;{{\text{m}} \mathord{\left/ {\vphantom {{\text{m}} {\text{s,}}}} \right. \kern-\nulldelimiterspace} {\text{s,}}}} \hfill \\ {\left| {\phi_{i} } \right| \le \pi ,\;\left| {\theta_{i} } \right| \le {\pi \mathord{\left/ {\vphantom {\pi 3}} \right. \kern-\nulldelimiterspace} 3},\;\left| {\psi_{i} } \right| \le {\pi \mathord{\left/ {\vphantom {\pi {3,}}} \right. \kern-\nulldelimiterspace} {3,}}} \hfill \\ {\left| {p_{i} } \right| \le 32\,^{^\circ } /s,\;\left| {q_{i} } \right| \le 10\,^{^\circ } /s,\;\left| {r_{i} } \right| \le 5\,^{^\circ } /s,} \hfill \\ {0 \le U_{1i} \le 5.2\;{\text{N}},} \hfill \\ {\left| {U_{2i} } \right| \le 0.0501\;{\text{N}}\,{\text{m}},} \hfill \\ {\left| {U_{3i} } \right| \le 0.0501\;{\text{N}}\,{\text{m}},} \hfill \\ {\left| {U_{4i} } \right| \le 0.0209\;{\text{N}}\,{\text{m}}.} \hfill \\ \end{array} } \right. $$

(51)

Table 1 Simulation parameters of the ZY F150 QUAV

The terrain constraint model adopts the ground height generated by Eq. (52) and the mountain terrain data is generated by Eq. (53) [40, 41].

$$ \begin{aligned} z_{1} (x,y) = & {\text{abs}}\left( {\sin \;(y + a) + b \cdot \sin \;(x) + c \cdot \cos \left( {d \cdot \sqrt {x^{2} + y^{2} } } \right)} \right. \\ & \left. { + e \cdot \cos \;(y) + f \cdot \sin \left( {g \cdot \sqrt {x^{2} + y^{2} } } \right)} \right), \\ \end{aligned} $$

(52)

$$ z_{2} (x,y) = \sum\limits_{i = 1}^{n} {h_{i} \exp \left[ { - \left( {\frac{{x - x_{i} }}{{x_{si} }}} \right)^{2} - \left( {\frac{{y - y_{i} }}{{y_{si} }}} \right)^{2} } \right],} $$

(53)

where in Eq. (52), $x$ and $y$ are the point coordinates of the model projected on the horizontal plane, $z_{1}$ the ground surface elevation value corresponding to the horizontal surface point, $a$, $b$, $c$, $d$, $e$, $f$, $g$ the constant coefficients to control the rise and fall of the ground surface reference terrain in the digital map model; function ${\text{abs}}()$ is the absolute function, that is, all-terrain heights are above sea level, as the reference terrain of the formation environment of the QUAVs. This paper takes $x = y = [0,\;0.001,\;0.002,\; \ldots ,\;1500]$, $a = 10,\;b = 0.2,\;c = 0.1,\;d = 0.6,\;e = 1$, $f = 0.1,\;g = 0.1$. In Eq. (53), $z_{2}$ is the mountain elevation value given at the corresponding point $[x,\;y]$ on the map, $[x_{i} ,\;y_{i} ]$ the central coordinate of the i-th mountain, $h_{i}$ the terrain parameter to control the height, $x_{si}$ and $y_{si}$ the attenuation along the x-axis and y-axis, respectively, which controls the slope, and n the total number of mountains. By setting the above parameters, different peaks with different heights and slopes can be achieved. This paper takes $h_{i} = [55,\;20,\;40,\;30,\;48,\;25,\;31]$, $x_{i} = [500,\;300,\;300,\;1300,\;760,\;700,\;1300]$, $y_{i} = [1100,\;410,\;800,\;450,\;750{,}\;200,\;1300]$, $x_{s} = [50,\;40{,}\;30,\;86,\;87{,}\;60{,}\;30]$, $y_{s} = [50{,}\;30,\;20,\;79{,}\;83{,}\;90{,}\;30]$, so there are seven peaks.

In the formation of QUAVs with a leader-followed strategy, the reference path of the leader QUAV should be specified first, and the proposed method is used to generate the reference path of the leader QUAV. The path generated by GPM is given in the form of nodes. In this paper, the B-spline smoothing strategy is adopted to smooth the process and limit the maximum value.

4.1 Simulation 1: trajectory planning of UAV formation with $z_{i} \in [0,\;35]\;{\text{m}}$

When we limit the maximum height that a QUAV can fly to as its maximum height $z_{i} = 35\;{\text{m}}$, the task to be accomplished by the leader QUAV is to start from the specified starting point [x₀, y₀, z₀] = [2, 1, 3] m, with the initial velocity vector [v_x0, v_y0, v_z0] = [2, 2, 0] m/s and the initial attitude angle $[\psi_{0} ,\;\theta_{0} ,\;\phi_{0} ] = [45^{^\circ } ,\; - 1^{^\circ } ,\;0^{^\circ } ]$. It passes through the non-flat area distributed over a number of mountains and arrives at the specified location $[1200,\;1000,\;2]\;{\text{m}}$ with the specified velocity vector $[v_{xf} ,\;v_{yf} ,\;v_{zf} ] \, = { [}0,\;0,\;0]\;{\text{m/s}}$ and attitude angle $[\psi_{f} ,\;\theta_{f} ,\;\phi_{f} ] = [45^{^\circ } ,\;0^{^\circ } ,\;0^{^\circ } ]$ at the specified time $t = 275\;{\text{s}}$, and gradually descends from the high altitude to the specified height ${2}\;{\text{m}}$. The takeoff positions of the five followed QUAVs are $[0,\;0,\;3]\;{\text{m}}$, $[0,\;2,\;3]\;{\text{m}}$, $[1,\;0.5,\;3]\;{\text{m}}$,$[0,\;0.5,\;3]\;{\text{m}}$, $[1,\;1.5,\;3]\;{\text{m}}$, respectively. The takeoff velocity vector, attitude angle, terminal velocity vector, and attitude angle are the same as the leader QUAVs. The simulation results of the proposed method are shown in Figs. 8, 9, 10, 11, 12, 13, and 14.

Fig. 8

3D view of the optimized path

Fig. 9

$x - y$ plane of the optimized path

Fig. 10

Curves of pitch angle and roll angle of the leader QUAV

Fig. 11

Formation of UAVs

Fig. 12

Curves of $U_{1}$ of leader QUAV

Fig. 13

Curves of $U_{2}$, $U_{3}$ and $U_{4}$ of leader QUAV

Fig. 14

Curves of $v_{x} ,$ $v_{y}$ and $v_{z}$ of leader QUAV

Figures 8 and 9 show the flight path optimized by the method presented in this paper from different visual angles of the formation of UAVs in the environment with multiple obstacles distributed. The UAVs do not collide with any obstacles and can reach the endpoint from the starting point smoothly, and all of which meet the constraints. Due to the introduction of terminal constraint weight $Q(t) = 1/(t_{f} - t + 0.5)$ in the performance index function [Eqs. (33) and (34)], the purpose is to make the UAV meet the goal of flying straight in the horizontal direction to the destination and gradually decreasing from a certain height in the final process of flight. From Figs. 8 and 9, the planned trajectory fully meets these two requirements. Due to the large maximum flying height of the ZY F150 QUAV, the takeoff point nearest obstructions can be directly overflown. The UAVs’ formation maintains the same formation, and then keeps flying at a certain height, doesn't need to turn to evade, which can prevent again to evade other obstacles or avoid formation changes. Since we have allowed a longer time to arrive at the destination in the followed flight process, the UAVs formation by changing the flight direction and height consumes the excess flight time to arrive at the specified time.

The general optimization performance index function includes the terminal position to minimize the error of the optimized terminal position result. However, in the proposed method, we treat the terminal position as a constraint (i.e., Eq. 16). The planned terminal position of the UAV flight path is consistent with the required position without any error, indicating that the proposed method has strong applicability. From Fig. 9, the heading angle of UAVs’ formation changes smoothly with the height of obstacles, indicating that the trajectory of the planning satisfies the motion law of the QUAV using the proposed method. This is a highlight of this article.

Moreover, due to the constraint of the minimum turning radius (i.e., Eq. 18), the planned optimized path does not appear where the turning radius is very small, and the path is smooth. When leader QUAV flight at a certain height, the same height of peaks in a horizontal position need to have a distance with it, and the followed QUAVs also need to meet this requirement. The constraint equations (23) and (24) are converted into position constraints of the leader QUAV, so the distance between the trajectory of the leader QUAV and mountain will have a bigger space, for the use of QUAV formation flight. This is the second highlight of this article.

From Fig. 10, we can see that the change of pitch angle and roll angle of the leader QUAV is small during the entire flight; this is also related to the characteristics of the QUAV. Changing the heading angle, ascent, or descent of a QUAV, does not need to change its pitch angle. Because the whole flight process is in higher airspace, there is no need to roll to avoid obstacles or change formation, so the roll angle of the whole flight process is also small. Moreover, according to the UAV dynamics model [Eq. (1)], the changes of roll angle and pitch angle are controlled by the control quantity $U_{2}$ and $U_{3}$. Due to the limitation of the control quantity in Eq. (19), its change is not likely to appear a big jump. Figure 11 is the zoomed formation, showing the formation of QUAVs during the flying process, where the dots represent six QUAVs.

Figures 12 and 13 show the curves of variation of control variables $U_{1}$, $U_{2}$, $U_{3}$ and $U_{4}$, respectively. Due to the limitation of the control quantity in Eq. (19), the change of $U_{1}$, $U_{2}$, $U_{3}$ and $U_{4}$ is not likely to appear a big jump. From Eq. (2), $U_{1}$ is larger than $U_{2}$, $U_{3}$ and $U_{4}$ in general, because $U_{1}$ is the sum of the lift forces of all four propellers, which is used to change the flight speed of the QUAV, and the attitude angular velocity of QUAV cannot change too dramatically, so its control quantity $U_{2}$, $U_{3}$ and $U_{4}$ are small. From Fig. 12, $U_{1}$ is used to change the speed of the three spatial directions to realize the change of the trajectory of leader QUAV, to avoid obstacles or consume extra time for the leader QUAV. And, from Fig. 13, roll angle control quantity $U_{2}$ keeps a small value in the entire process of flight, showing that the roll angle of the leader QUAV almost does not change during the flight. The pitch angle control quantity $U_{3}$ is also small, although it is up and down; due to the special structure of QUAV, the pitch angle does not need to be greatly changed. But the change of yaw angle control quantity $U_{4}$ is bigger, because it needs to change the flight direction of leader QUAV. This is consistent with the above attitude angle analysis.

Figure 14 shows the curves of variation of linear velocity variables $v_{x}$, $v_{y}$ and $v_{z}$, respectively. Since we are using the NED coordinate system, the change of the z-axis direction is the opposite of the normal situation. Due to the realization of the QUAV to avoid obstacles in the corresponding $x - y$ plane or consume extra flight time, when the vertical velocity $v_{z}$ changes, the horizontal velocity can be changed through the change of $v_{x}$ and $v_{y}$. Due to the need to leap over obstacles, rise and descend, the vertical velocity $v_{z}$ of the QUAV varies. Besides, the requirement that the terminal flight needs to land from a certain height (Eq. 34), the vertical velocity $v_{z}$ at the end of the flight is a process of slowly rising and then landing. This is consistent with the first highlight of this paper.

4.2 Simulation 2: trajectory planning of UAV formation with $z_{i} \in [0,\;15]\;{\text{m}}$

When we limit the maximum height that the QUAVs can fly as $z_{i} { = }15\;{\text{m}}$, the task to be accomplished by the leader QUAV is to start from the initial attitude angle $[\psi_{0} ,\;\theta_{0} ,\;\phi_{0} ] = [60^{{\text{o}}} ,\; - 18^{{\text{o}}} ,\;0^{{\text{o}}} ]$ and arrive at the attitude angle $[\psi_{f} ,\;\theta_{f} ,\;\phi_{f} ] = [ - 60^{^\circ } ,\;0^{^\circ } ,\;0^{^\circ } ]$ at the specified time $t = 350\;{\text{s}}$, other parameters are the same as in Simulation 1. The simulation results are shown in Figs. 15, 16, 17, 18, 19, and 20.

Fig. 15

3D view of the optimized path

Fig. 16

$x - y$ plane of the optimized path

Fig. 17

Curves of pitch angle and roll angle of the leader QUAV

Fig. 18

Curves of $v_{x} ,$ $v_{y}$ and $v_{z}$ of the leader QUAV

Fig. 19

Curves of $U_{1}$ of the leader QUAV

Fig. 20

Curves of $U_{2}$, $U_{3}$ and $U_{4}$ of the leader QUAV

From Fig. 15, when the flying maximum height is limited as $z_{i} { = }15\;{\text{m}}$, which is less than the height of the mountain, the QUAV cannot fly over the mountain and has to make a detour process. Compared with the 3D view of the optimized path in Fig. 8 in Simulation 1, the flight trajectory has no large ascending and descending parts. Although the optimized flight path of the leader QUAV in Fig. 15 is lower, due to the highly volatile terrain generated by terrain generation [Eqs. (52) and (53)], and the limitation of type (20), the optimal planned path cannot contact the ground. The leader QUAV cannot leap high terrain, but according to the limitation of Eq. (21), at a certain height, there is a position constraint on its horizontal position at the same height of the mountain, so the optimal planned path of the leader QUAV can well bypass the mountain without colliding with it. Besides, like Simulation 1, since the height and horizontal position of the followed QUAV are also converted by Eqs. (23) and (24) to the height and horizontal position of the leader QUAV, there is a large space between the optimal planned path of the leader QUAV and the ground and mountains for the flight of the QUAVs formation.

From the optimization path in $x - y$ plane graph in Fig. 16, we can see that the UAV formation needs to change flight heading angle between obstacles, and ultimately to head to the target point, which is significantly different from Simulation 1. However, as we stipulate that the UAV formation should reach the target point at the specified time of $350\;{\text{s}}$, the UAV formation needs to make some detours in the flight process to consume the extra flight time, which is also automatically considered in the optimization process. Of course, we can also consider time optimization as the optimization goal, rather than a set time task. However, the path planned by the method in this paper is still smooth and suitable for QUAV flight.

From Fig. 17, because of the characteristics of the QUAV, the change of pitch angle of the leader is also small during the whole flight. Because there are some turns to avoid obstacles or to change formations during the whole flight process, the roll angle of the whole flight process is a little larger than the value in Simulation 1. However, due to the existence of the minimum turning radius in Eqs. (18) and (29), the turning radius in the optimized path of the leader QUAV is not too small, otherwise, it will cause the failure of the leader QUAV structure.

Figure 18 shows the curves of variation of linear velocity variables $v_{x}$, $v_{y}$ and $v_{z}$, respectively. The QUAV does not have to make major altitude changes to fly over the mountains, as we reduce the maximum flying height of the UAV. The change of $v_{z}$ in the z-axis direction is also smaller than that in Simulation-1. However, due to many changes in the flight direction, the changes of $v_{x}$ and $v_{y}$ in the horizontal direction are even bigger, and are not consistent with Simulation 1. This also leads to frequent changes in the control quantity $U_{1}$, as shown in Fig. 19.

Figures 19 and 20 show the curves of variation of control variables $U_{1}$, $U_{2}$, $U_{3}$ and $U_{4}$, respectively. Due to the long flight time of QUAV formation, the value of $U_{1}$ is smaller than $U_{1}$ in Simulation-1, but the control quantities $U_{2}$, $U_{3}$ and $U_{4}$ used to change the attitude are a little bit bigger than $U_{2}$, $U_{3}$ and $U_{4}$ in Simulation 1.

From these two simulations above, we conclude:

Remark 4

The proposed method, transforming the constraint information of the followed QUAVs into the constraint information of the leader QUAV, can plan a smooth flight path. It meets the time agreement and can change according to the maximum flight height of the UAV, and meet the requirement that the whole UAV formation does not collide with obstacles.

4.3 Comparative simulation analysis of the trajectory optimization

The standard particle swarm optimization (PSO) algorithm [42,43,44] is adopted to compare with the proposed method. The simulation data is the same as Simulation 2, and the simulation constraints are the same as the leader QUAV and the followed QUAVs listed in this paper. The simulation results are shown in Figs. 21 and 22. When the maximum flight altitude of QUAV is 15 m, the QUAV formation needs to detour to cross the mountains. From the optimized path on the $x - y$ plane of Fig. 22, the UAV formation changes the heading angle between the mountain peaks to flight to the aim point, which is close to Simulation 2. The proposed method over the PSO method has three advantages:

• (1) Short optimization time. Compared with the proposed method, the used optimization time is 21.83 s (simulation conditions: Lenovo laptop, model × 1, Matlab version: 2008b) faster than the used optimization time of the proposed method that is 25.32 s. However, the PSO algorithm only can deal with one QUAV's flight trajectory planning at a time, and then take the planned flight path as the constraint to optimize the next QUAV's flight trajectory. Overall, its optimization time is longer than the simulation time of the proposed method.

• (2) The PSO is a time-optimal trajectory optimization method, but the algorithm can not specify the final flight direction and accurately determine the time to reach the destination.

• (3) The PSO cannot get the flight speed information, attitude information, attitude angular velocity information, and control information of each path-point of the optimized path optimized by the PSO algorithm. But the proposed method can get all the information, like Simulation 1 and Simulation 2, this is also the third highlight of this article.

Fig. 21

3D view of the optimized path

Fig. 22

$x - y$ plane of the optimized path

5 Conclusion

In this paper, the trajectory optimization problem of multiple QUAVs formation with obstacles in three-dimensional space was studied. The formation system was transformed into a network topology, and then the formation was treated as a time-varying virtual rigid body by using the virtual structure method. By analyzing the constraint information of the followed QUAVs in the formation, the information of the followed QUAVs in the formation was converted to constraints of the leader QUAV. Therefore, the formation is transformed for different constraints by changing the virtual rigid body structure. The formation trajectory optimization problem is stand-alone to simplify the complex multi-QUAVs formation trajectory optimization problem. The requirement of the performance index was analyzed. For the situation that the formation flies in a straight line from a certain horizontal direction at a certain altitude to the target point, the terminal flight requirement was added, and the traditional performance index has been improved. The Gauss pseudospectral method has been applied to transform the optimal control problem of path optimization into the solving framework of nonlinear programming. Two simulations for different maximum flight heights were conducted. It was verified that the method transforming the constraint information of the followed QUAVs into the constraint information of the leader QUAV can plan a smooth flight path. It also meets the constraint requirements and the whole UAV formation does not collide with obstacles. A comparative simulation of the PSO was given and demonstrated the advantages of the proposed method over the PSO method.