A Hierarchical Cooperative Mission Planning Mechanism for Multiple Unmanned Aerial Vehicles

Abstract

In this paper, the cooperative multi-task online mission planning for multiple Unmanned Aerial Vehicles (UAVs) is studied. Firstly, the dynamics of unmanned aerial vehicles and the mission planning problem are studied. Secondly, a hierarchical mechanism is proposed to deal with the complex multi-UAV multi-task mission planning problem. In the first stage, the flight paths of UAVs are generated by the Dubins curve and B-spline mixed method, which are defined as “CBC)” curves, where “C” stands for circular arc and “B” stands for B-spline segment. In the second stage, the task assignment problem is solved as multi-base multi-traveling salesman problem, in which the “CBC” flight paths are used to estimate the trajectory costs. In the third stage, the flight trajectories of UAVs are generated by using Gaussian pseudospectral method (GPM). Thirdly, to improve the computational efficiency, the continuous and differential initial trajectories are generated based on the “CBC” flight paths. Finally, numerical simulations are presented to demonstrate the proposed approach, the designed initial solution search algorithm is compared with existing methods. These results indicate that the proposed hierarchical mission planning method can produce satisfactory mission planning results efficiently.

1. Introduction

UAVs have seen decades of successful deployment in military operations. These military UAVs are traditionally large and expensive. Recent technological advances have enabled small, inexpensive and commercially available UAVs. Small UAVs carrying cameras and other sensors have many civil uses, including agriculture research and management, search and rescue operations, surveillance, and communication relay [1]. UAVs are especially powerful in situations where ground vehicles cannot access an area, making UAVs useful in monitoring disaster scenarios in real-time. Unfortunately, a single UAV can only provide limited flight time and limited coverage [2]. In this paper, we investigate the problem of mission planning for multiple cooperating UAVs working together to monitor and survey multiple geographical points in an area of interest.

Different from the formation control in the work of W. Xing, etc. [3], multi-UAV cooperative task processing is suitable for loose formation structures. To guarantee that UAVs will be able to process intricate tasks cooperatively and efficiently, mission planning is one of the key problems, and combines the problems of task assignment and optimal trajectory planning for UAVs [4]. Task assignment consists of assigning tasks to UAVs with the aim of reducing the overall cost of task execution. Trajectory planning is the problem of planning optimal trajectories that satisfy the platform constraints and safety requirements of UAVs.

In multi-UAV multi-task scenarios, the number of UAVs is often less than the number of tasks. Therefore, one UAV may be assigned more than one task. Furthermore, the constraints of UAV platforms should be considered in order to obtain the trajectories from one position (entry, task positions) to another (other task positions, exit). In a real scenario, the rationality and validity of the task assignment depends on accurate path estimation (which should be part of trajectory planning), which would lead to a heavy computation load when the numbers of UAVs and tasks become large. Therefore, the task assignment problem in a multi-UAV multi-task scenario is much more difficult than that in a single-UAV scenario. The multi-UAV multi-task mission planning (MUMTMP) problem is a complicated combinatorial optimization problem.

In many studies, task assignment and path-planning are considered as two independent parts of mission planning and reasonable results have been achieved. Many researchers have studied the multi-agent task assignment problem. Kuncheva Li proposes a task assignment method based on the Hungarian algorithm [5]. Maddula T et al. apply the Dijkstra algorithm for task assignment [6]. Zhao Ming et al. propose the Differential Evolution algorithm to solve the task assignment problem for multiple UAVs [7]. Centralized optimization methods could generate efficient solutions. However, it takes too much time for computation and is sometimes not robust. To improve the computational efficiency, distributed methods have been studied. The max-mum method has been widely used to solve the task assignment problem for distributed multiple robots [8,9,10]. The auction-based algorithm has also been widely used in task assignment, but the communication loads among team members are usually too heavy. Moreover, in each round of auction, only one single task is solved, and it is easy for the solution to fall into local optima when a UAV performs multiple tasks [11,12]. The bionics-based optimization method could solve the task assignment of multiple UAVs quickly; however, it is mainly used in scenarios where each UAV performs a single task [8].

Many algorithms have been proposed to solve the trajectory planning problem of multiple UAVs. Bai Li et al. propose the centralized motion planning method based on the optimization algorithm, which generates satisfying planning results within sufficient time [13]. Yoshiaki Kuwata et al. propose a distributed receding horizon mixed integer linear programming method for multiple UAVs to generate trajectories [14]. Gu Xueqiang et al. propose a distributed receding horizon planning method [15], without considering the task assignment problem. Intelligent algorithms such as the genetic algorithm have been proposed to solve the multi-UAV trajectory planning problem [16,17]. However, the dynamic models of UAVs are oversimplified in these studies. The artificial potential field-based method has been proposed to deal with the multi-UAV trajectory planning problem [18], and could be used for short term planning and may lead to vibration of flight trajectories. To sum up, the mathematical programming method could generate satisfactory trajectories with consideration given to the maneuver constraints of UAVs. However, it is computationally inefficient if the MUMTMP problem is not reasonably decoupled. The intelligent optimization algorithm and the artificial potential field are computationally efficient, but due to oversimplification of the UAV platform and environment model, the acquired results may be inefficient.

Zhang Yu et al. propose a hierarchical planning mechanism to manage the single-UAV multi-task problem [19]. In the first layer, the single UAV multi-task mission planning problem is modeled as a traveling salesman problem (TSP) with consideration given to the dynamic constraints. When the costs of each flight path are known, the task sequence is determined by solving the TSP. In addition, then, the Gauss pseudospectral method (GPM) is used to create the final optimal flight trajectories. Since the computation is mostly conducted off-line, this is a prior-planning method. To deal with unexpected changes during the mission, they propose an online search strategy based on the offline planning results. However, such an online search strategy could not guarantee being able to find feasible solutions.

This paper studies the online MUMTMP problem. The original MUMTMP problem is a complex combinatorial optimization problem with heavy resolving load. The key issue of online MUMTMP is to find sub-optimal solutions within a limited time [4]. An appropriate method should be applied to efficiently acquire satisfactory estimations for the costs of trajectories.

Askari, A et al. propose constructing the flight path of the UAV by combining Dubins curves and Bezier curves, which could find a sub-optimal solution [20]. The UAV models are simplified reasonably by using the Dubins curve-based trajectory planning methods. Oren Gal et al. study the visible trajectories planning with a Dubins UAV model in 3D urban environments [21]. Suresh Jeyaraman et al. present formal techniques for the modeling and validation of a cooperative UAV team that uses the Dubins set for path planning [22]. Compared with Bezier curves, B-spline curves are advantageous in that the local adjustment would only affect a limited segment of the curves. Mohamed Elbanhawi et al. propose planning the obstacle avoidance paths of vehicles by using the B-spline curve [23]. We propose estimating the trajectory costs by using “CBC” curves, which mix the Dubins curve and the B-spline curve. “CBC” curves consider both the dynamic constraints and the obstacles in the environment. In addition, based on the features of the Dubins curve and the B-spline curve, the “CBC” curves could generate optimal flight curves.

A hierarchical planning strategy is proposed in this paper. First, the CBC flight paths are proposed to plan the flight paths, which could provide reasonable estimations for the costs of trajectories. Accordingly, a weighted directed graph is obtained, and the task assignment problem is modeled as a multi-base multi-traveling salesman problem (MBMTSP). Second, after the task assignment has been accomplished, the Gaussian pseudospectral method (GPM) is used to compute the optimal trajectories. As the efficiency of GPM relies on the initial solutions, the continuous-differential initial trajectories are designed by using the “CBC” curves. The performance of this approach is demonstrated by making a comparison with existing methods. Well-optimized solutions are obtained efficiently in complex scenarios.

The main contributions of this paper are as follows:

First, we present a hierarchical mechanism, which decomposes the MUMTMP problem into two relevant sub-problems.

Second, the Dubins curve and B-spline curve are mixed to generate flight paths of UAVs, with respect to the dynamic constraints and obstacle avoidance requirements. This gives a reasonable estimation of the cost of each flight trajectory.

Third, we design “CBC”-based continuous-differential initial trajectories to improve the computational efficiency of trajectory planning.

The rest of this paper is organized as follows. The MUMTMP problem formulation is given in Section 2. In Section 3, the hierarchical mission planning mechanism is proposed. In Section 4, the trajectory planning method is discussed. The experimental results are presented in Section 5, followed by the conclusion in Section 6.

2. MUMTMP Problem Formulation

2.1. Analysis of the UAV Dynamics Model

Assume there are $N_U$ UAVs and $N_T$ targets. As a dynamic motion system in three-dimensional space, the trajectory planning of UAVs should be based on appropriate dynamic and kinematics models.

In the ENU coordinate-system $o\text{-}xyz$, the particle kinematics equation of the UAV $U_i$, $i\in N_U$ is defined as:

$$\{\begin{array}{c}{\dot{x}}_i=V_{i}cos{\gamma }_{i}cos{\psi }_i\\ {\dot{y}}_i=V_{i}cos{\gamma }_{i}sin{\psi }_i\\ {\dot{h}}_i=V_{i}sin{\gamma }_i\end{array}$$

(1)

$P_i=(x_i,y_i,h_i)$ is the spatial coordinate system. $V_i$ is the velocity, ${\gamma }_i$ is the pitch angle, and ${\psi }_i$ is the course angle. In the flight path axis system $O^k\text{-}{x}^k{y}^k{z}^k$, the particle dynamics equation of $U_i$ is defined as:

$$\{\begin{array}{c}{\dot{V}}_i=\frac{L_{i}cos{\alpha }_i-D_i}{m_i}-gsin{\gamma }_i\\ {\dot{\gamma }}_i=\frac{(L+Tsin\alpha )cos{\mu }_i}{m{V}_i}-\frac{g}{V_i}cos{\gamma }_i\\ {\dot{\psi }}_i=\frac{(L_i+T_{i}sin{\alpha }_i)sin{\mu }_i}{mVcos{\gamma }_i}\end{array}$$

(2)

$m_i$ is the quantity of $U_i$; $g$ is the gravity, ${\alpha }_i$ is the angle of attack, ${\mu }_i$ is the roll angle, and $T_i,D_i,L_i$ are thrust, drag and lift power of engine. The states of $U_i$ is $X_i=[P_i,V_i,{\psi }_i,{\gamma }_i]$. The control variables are $u=({\alpha }_i,{\mu }_i,T_i)$. As the thrust is controlled by throttle ${\delta }_i$, the control tuple is turned into $u=({\alpha }_i,{\mu }_i,{\delta }_i)$

During the flight, the platform stability and safety of UAVs must be ensured [19]. The feasible speeds of UAVs should be in specific ranges at different altitudes. The dynamic constraints of UAVs are expressed as the following inequality equations when the flight altitudes of UAVs are clear:

$$\begin{array}{l}{h}_i^{max}\ge h_i\ge h_i^{min}, V_i^{max}\ge V_i\ge V_i^{min}\\ {\alpha }_i^{max}\ge {\alpha }_i\ge {\alpha }_i^{min}, {\mu }_i^{max}\ge {\mu }_i\ge {\mu }_i^{min}\end{array}$$

(3)

The constraints on maneuverability are as follows:

$${\dot{\alpha }}_i^{max}\ge {\dot{\alpha }}_i\ge {\dot{\alpha }}_i^{min},{\dot{\mu }}_i^{max}\ge {\dot{\mu }}_i\ge {\dot{\mu }}_i^{min},{\dot{\gamma }}_i^{max}\ge {\dot{\gamma }}_i\ge {\dot{\gamma }}_i^{min}$$

(4)

To improve the computational efficiency, the heading maneuver constraints are simplified as the minimum turning radius in horizontal and vertical plane in the path planning phase:

$$\begin{array}{cc}{R}_{min,i}^{horizontal}=\frac{v_i^2{cos}^2{\gamma }_i^{max}}{\eta gsin\left|{\phi }_i^{max}\right|},& R_{min,i}^{vertical}=\frac{v_i^2}{g(n_f-cos({\gamma }_i^{max}))}\end{array}$$

(5)

where ${\phi }_i^{max}$ is the maximum roll Angle, ${\gamma }_i^{max}$ is the maximum pitch Angle, $\eta ,g,n_f$ are simplified as constants.

2.2. MUMTMP Problem Formulation

The MUMTMP problem includes two relevant sub-problems: task assignment and trajectory optimization. Task assignment assigns tasks to UAVs in a specific sequence. Trajectory optimization generates the optimal trajectory segments of each UAV with consideration given to the constraints. The following constraints are considered to plan feasible trajectory of each UAV.

UAVs may encounter air threats or obstacles during the flight. To ensure the flight safety of UAVs, it is necessary to keep them away from air threats or obstacle during flight. The threats and obstacle regions are considered as non-fly zones. These zones are modeled as hemispherical regions. The flight constraints are modeled as:

$$\Vert P_i-P_o^j\Vert \ge R_o^i\hspace{1em}\hspace{1em}\forall i\in N_{\mathrm{U}},\forall j\in N_O$$

(6)

where $P_i$ is the position of $U_i$, $N_o$ is the number of obstacles in the local region, $P_o^j$ is the position of the $j$th obstacle, and $\Vert \Vert$ is 2-norm.

The starting and ending states of $U_i$ are constrained as:

$$X_i(0)=X_o^i,\hspace{1em}\hspace{1em}{X}_i(e)=X_e^i$$

(7)

Meanwhile, UAVs should maintain specific attitudes to execute tasks when they reach the task positions. The attitude constraint of $U_i$ in the $j$th task position is defined as:

$$X_i^{j_T}=X_j^T$$

(8)

The trajectory planning problem is complex, as (1), (2), (6), (7) and (8) are all nonlinear constraints. Hence reasonable estimations of the costs of potential trajectories are required. When the trajectory costs are obtained, the task assignment problem could be modeled as a multi-base multi-traveling salesman problem (MBMTSP) by using graph theory [15]. The entry points, task points and exit points are collectively referred to as nodes ${\overrightarrow{V}}_i,i=1\cdots 2N_U+N_T$. The directed edge from one node to another is denoted as $e({\overrightarrow{V}}_i,{\overrightarrow{V}}_j),i\ne j$, and the cost of the directed edge is $j_e=(x({\overrightarrow{V}}_i),x({\overrightarrow{V}}_j))$. The process of MBMTSP is as follows: UAVs start from their entry nodes, arrive at assigned task nodes sequentially, and leave the mission area from the exit nodes after all tasks are completed as shown in Figure 1.

The aim of task assignment is to minimize the cost of the MBMTSP. Suppose that the task execution cost of $U_i$ is $C_i$, and $C$ is the total costs of all the UAVs.

If the purpose of MBMTSP is only to minimize the overall cost, it could lead to workload unbalance among UAVs, as some UAVs may be assigned too many tasks, while others are assigned few or even no tasks. Considering the fairness of task assignment, the object of MBMTSP is defined as the weighted summation of two sub-objects in Equation (9). One is the summation of the costs of all UAVs and the other is the cost of the UAV with the maximum cost. The object function can effectively balance the cost of the whole UAV team and the cost of every UAV team member.

$$\mathrm{F}=\lambda {\displaystyle \sum _{i=1}^{N_U}{C}_i+(1-\lambda )}\underset{1\le j\le N_U}{max}{C}_j$$

(9)

In most cases, the goal of MBMTSP is to finish the task as soon as possible, while the trajectory of task execution and fuel consumption are the secondary objectives in the mission-centered scenario. This paper defines the task execution time as the optimization object.

To find the optimal task assignment, the cost of each trajectory should be acquired. The estimated trajectory should consider both the dynamic constraints of UAV and obstacle avoidance constraints in the environment. As mentioned above, the costs of $2N_U{N}_{\mathrm{T}}{+\mathrm{A}}_{N_{\mathrm{T}}}^2$ edges in the graph should be acquired. It would be impractical to plan fine trajectories for UAVs with consideration given to time consumption [19]. To improve the computational efficiency, the primary task is to reduce time consumption to obtain a reasonable cost estimation for every trajectory.

After the MBMTSP is solved, the sequential waypoints set of $U_i$ is obtained, which is defined as $S_w^i=\{X_w^{i,1},\dots ,X_w^{i,n_i^w}\}$. The fine trajectories that connect each waypoint and successors should be generated to guide each UAV. Assuming that the initial path of $U_i$ between each pair of waypoints $(X_w^{i,m},X_w^{i,n})$ is given as $c_{o,i}^{m,n}$, the objective in optimization-based trajectory planning is to find a feasible UAV trajectory that minimizes the time of flight. The hybrid optimal control problem is thus as follows. Given an initial condition $X_o^i$ and end condition $X_e^i$, find an input sequence u: [$t_0,t_f$]$\to U$, that fulfills the dynamics (2) and constraints (3) (4) (6), and minimizes the cost function $J(u)$. That is, the following hybrid optimal control problem:

$$\underset{u}{min}J(u)=\varphi (t_f,c_{o,i}^{m,n})+{\displaystyle \underset{t_0}{\overset{t_f}{\int }}L(x(t),u(t),c_{o,i}^{m,n})dt}$$

(10)

s.t:

$$\begin{array}{l}\begin{array}{l}{X}_i(t_0)=X_w^{i,m},X_i(t_f)=X_w^{i,n}\hfill \\ \begin{array}{l}{v}_i\in [v_i^{min},v_i^{max}],{\gamma }_i\in [{\gamma }_i^{min},{\gamma }_i^{max}]\\ {\alpha }_i\in [{\alpha }_i^{min},{\alpha }_i^{max}],{\mu }_i\in [{\mu }_i^{min},{\mu }_i^{max}]\end{array}\hfill \end{array}\\ {\Vert P_i-P_o^j\Vert }_2-R_o^j\ge 0\hspace{1em}\hspace{1em}\forall i\in N_{\mathrm{U}},\forall j\in N_O\end{array}$$

(11)

In Equation (10), $\varphi$ is referred to as the Mayer term, denoting a final cost, and the integral term in J is referred to as the Lagrange term, denoting a running cost. The final cost can be used to quantify the time consumption. The running cost can represent costs accumulated during the flight. The UAV trajectory planning problem is then modeled as the optimal control problem.

3. Mission Planning Problem Solving for Multiple Cooperative UAVs

This section discusses the problem of online MUMTMP mechanism based on the discussion in Section 2.

The mechanism is shown in Figure 2. First, the flight paths are planned by the Dubins curve and B-spline curve mixed method. We define this type of flight path as “CBC” curves. The objective is to minimize the length of each segment of the flight. By properly designing the coefficients of the “CBC” curves, it is possible to avoid obstacles in the environment and satisfy the platform maneuver constraints.

Second, the tasks of the UAVs are assigned in the task assignment stage. As the estimation cost of each flight path is determined, a weighted digraph is built. There are $2N_U{N}_{\mathrm{T}}{+\mathrm{A}}_{N_{\mathrm{T}}}^2$ edges in the digraph. The task assignment problem is modeled as a MBMTSP 16. The task assigning policy that minimizes the overall cost is solved by using existing methods, such as the LKH solver.

After the task sequence of each UAV is determined, the specific waypoints of each UAV are clear. The trajectory planning method will be used to generate the optimal trajectories of each UAV from each forward waypoint to its successor. GPM is adopted as the optimal control method, for which the objective is to minimize the trajectory cost. The constraints are the dynamics of UAVs and the obstacle avoidance requirements. The “CBC” curves are used to create the initial trajectories to enhance the computation efficiency.

4. Flight Path Planning Using the Dubins Curve and B-Spline Mixed Method

According to the analysis of the directed graph in Figure 1, the flight segments include the directed edges from the entry nodes to task nodes, from task nodes to exit nodes, and from one task node to another. As stated above, the number of possible flight paths is $A_{N_T}^2+2N_T{N}_U$. The control sequences of UAVs along the path do not need to be acquired in the flight path planning stage. Assuming that the speeds of UAVs are fixed, the optimal objective is transformed into finding the shortest flight path from the start position to the end position. The dynamic constraints of each UAV $U_i$, such as the starting and arriving states, and the maneuver constraints, should be considered to create the optimal feasible flight path. Dubins curves are used to produce the flight paths of UAVs [24]. According to the Dubins curves theory, the shortest path between two vectors consists of an arc and its tangent. The shape of the path includes “CLC”, “CCC” and “CCL”. The “CLC” path is composed by two arcs and the tangent line between the two arcs; the “CCL” path utilizes two arcs to satisfy the angle demand at first, and then points to the target; the “CCC” path consists of three circular arcs. An arc is the shortest rotation curve between two vectors with the least order in two- or three-dimensional space; meanwhile, a straight line is the path with the shortest distance between two points. If the radius of the arc satisfies the constraint of the minimum turning radius of one UAV, the path joining the arc and the tangent line is the shortest path with the least order [25]. In this paper, “CLC” curve planning is applied with state constraint and maneuvering constraint of UAVs.

Given two waypoints $(X_w^{i,m},X_w^{i,n})$ of UAV $U_i$, both $X_w^{i,m}$ and $X_w^{i,n}$ contain the constraints on ${\psi }_i$, ${\gamma }_i$, and ${\mu }_i$. The adjustment requirements of ${\gamma }_i$ and ${\psi }_i$ should be taken into account.

First, the Dubins curve-based path planning with the constraint of ${\psi }_i$ is discussed. Suppose the coordinate of $X_w^{i,m}$ is $P_i^m$ and the coordinate of $X_w^{i,n}$ is $P_i^n$. In the horizontal O-XY coordinates, we focus on three parameters $P_i^m=(x_i^m,y_i^m,{\psi }_i^m)$ and $P_i^n=(x_i^n,y_i^n,{\psi }_i^n)$. As shown in Figure 3, four different maneuver arcs could be chosen at $P_i^m$ and $P_i^n$, of which the centers are $C_1^r,C_1^l$ and $C_2^r,C_2^l$ respectively. When these choices are selected at $P_i^m$ and $P_i^n$, four different Dubins curves could be formed. Without loss of generality, the curve with centers $C_1^l$ and $C_2^r$ is chosen as the example. The coordinates of $C_1^l$ and $C_2^r$ could be determined based on the states of $X_w^{i,m}$ and $X_w^{i,n}$. By virtue of the horizontal minimum turning radius $R_{min,i}^{horizontal}$, the common tangent line of these two circles could be solved. Thus, a “CLC” path with consideration given to the constraints of $U_i$ at $P_i^m$ and $P_i^n$ could be figured out.

Because of the differences between ${\gamma }_i^m$ and ${\gamma }_i^n$, it is necessary to establish the vertical plane coordinate system $D_{xy}-H$, which is composed by the vertical coordinate axis $H$ and the projection of $P_i^m{P}_i^n$ in the $x-y$ plane. Assuming $P_i^m$ is the origin point of the local coordinate system, the motion states of $U_i$ at $P_i^m$ and $P_i^n$ are $(0,0,{\gamma }_i^m)$ and $(d_{xy}^i,d_h^i,{\gamma }_i^n)$. The length of a Dubins curve on the horizontal plane is $d_h^i=h_i^e-h_i^o$, as shown in Figure 4. In the vertical coordinate system $D_{xy}-H$, the projections of ${\gamma }_i^m$ and ${\gamma }_i^n$ are on the positive half axis of $D_{xy}$, which means ${\gamma }_i\in [-\pi /2,\pi /2]$. Since the value of ${\gamma }_i$ cannot exceed $[-\pi /2,\pi /2]$, it is necessary to discuss the feasibility of the planning strategy on the vertical plane. The angle adjustment considered path planning on the vertical plane is shown in Figure 4.

Additionally, flight safety should be considered. There may be fixed obstacles or threats in the local environment, and hence the problem of obstacles avoidance must be considered. It is difficult to acquire smooth conflict-free curves in virtue of Dubins curve. The B-spline is used to devise collision-free flight paths.

A description of B-splines can be found in Wang G G [26]. The B-spline curve is controlled by a set of knots. The knots are connected sequentially to form a knot vector, and the B-spline approximates the continuous curve of the knot vector. The B-spline curve has a high-degree continuous geometry character. For instance, the B-spline of order n has the degree n − 1. If the number of knots of the corresponding curve is n + 1, with coordinates $d_0\left(x_0, y_0, z_0\right)$,…, $d_n\left(x_n, y_n, z_n\right)$, and knot vector $U=\left\{t_0,\dots ,t_m\right\}$, the B-Spline curve can be written as Function (12):

$$p(u)={\displaystyle \sum _{i=0}^n{d}_i{B}_{i,k}(u)}$$

(12)

$d_i$$(i=0,1,\dots ,n)$ represents the coordinates of the i-th knots. $B_{i,k}$ $(i=0,1,\dots ,n)$ is the i-th blending function of degree k. The blending function can be written by means of the Cox-de Boor recursion Formula 13:

$$\begin{array}{l}{B}_{i,0}(u)=\{\begin{array}{ll}1\hfill & \mathrm{if} t_i\le u\le t_{i+n}\hfill \\ 0\hfill & otherwise\hfill \end{array}\\ B_{i,k}(u)=\frac{u-t_i}{t_{i+k}-t_i}{B}_{i,k}(u)+\frac{t_{i+k+1}-u}{t_{i+k+1}-t_{i+1}}{B}_{i+1,k}(u)\end{array}$$

(13)

The shape of the curve can be controlled by the position of the knots. The B-spline curve has the advantage of continuous curvature, and the curvature at the nodes of adjacent curve segments is also continuous, so it is characterized by local support. The shortest distance for the UAV to bypass obstacles is ensured by the knots. If the local constraints of the trajectory are not satisfied, the trajectory can be corrected by adjusting the corresponding knots without affecting other trajectory segments. This excellent quality has led to the B-spline curve being widely used in the field of path planning [16].

As a B-spline curve is determined when the coordinates of knots are known, we propose an obstacle avoidance path planning method by virtue of the B-spline curve.

The beginning point and the end point of the B-spline are $P_1$ and $P_2$ respectively, as shown in Figure 1 and Figure 4. Suppose that there are n knots with coordinates $P_k^1\left(x_k^1, y_k^1, z_k^1\right)$,…, $P_k^n\left(x_k^n, y_k^n, z_k^n\right)$ between $P_1$ and $P_2$, which are defined as the control variables. We sample the B-spline properly. Therefore, the sample point series $P_s=\{p_1,\dots ,p_m\}$ is generated, where $m$ is the number of sample points. The optimal collision-free B-spline flight paths could be acquired by solving the nonlinear programming problem. The objective function is:

$$\min {\displaystyle \sum _{i=1}^{m-1}{\Vert p_{i+1},p_i\Vert }_2}$$

(14)

s.t.

$$\begin{array}{l}{P}_s=f_{sample}(C_{b-spline}(P_k^1,\dots P_k^n))\\ {\Vert O_j,p_i\Vert }_2-R_o^j\ge 0,\forall i\in m-1,\forall j\in N_O\end{array}$$

(15)

where $f_{sample}$ is the sample method and $C_{b-spline}$ is the B-spline curve that is determined by $\{P_k^1,\dots P_k^n\}$.

This paper proposes mixing the B-spline and Dubins curves together to take advantage of their characteristics. Regarding the obstacle avoidance requirements, the “CLC” curve is upgraded by “CBC”, where the tangent line is replaced by the B-spline curve. The “CBC” path that is the mixture of the Dubins curves and the B-spline curve is shown in Figure 5.

As shown in Figure 3, there are four different adjustment policies from $X_w^{i,m}$ to $X_w^{i,n}$. We need to choose the adjustment mode according to the specific conditions. Two factors should be considered. First, the starting point $P_1$ and ending point $P_2$ of the middle B-spline curve segment should be kept as far from the obstacles as possible. If $P_1$ or $P_2$ are too close to obstacles in the environment, it may lead to excessive bending at $P_1$ or $P_2$, which may mean that the arc and the B-spline cannot connect together smoothly. Second, the shortest “CBC” curve should be chosen from the alternatives.

By using the “CBC” curve-based planning method, the costs for different UAVs to execute different tasks in different sequences could be estimated. In a further step, the task assignment of multiple UAVs is constructed as MUMBTSP, and the existing TSP solving method is applied to solving the problem.

5. Trajectory Planning by Using the Gauss Pseudospectral Method

5.1. Introduction to the Gauss Pseudospectral Method

After the sub-problem of task assignment is solved in the first layer, it is necessary to plan the optimal trajectories which meet all the dynamic constraints. As stated in Section 2, the sub-problem of trajectory planning is modeled as the optimal control problem. The optimal control algorithms can be divided into two categories. The first is the indirect method, which is based on the Pontryagin extremum principle. The control and state constraints are transformed into a two-point boundary problem. However, the numerical methods can hardly solve the two-point boundary problems of non-linear systems. The transversal conditions are difficult to determine by the analytic method; moreover, costate variables have no physical meaning, and it is also difficult to estimate an initial solution that can decide the convergence of the algorithm. The second is the direct method, which transforms the continuous system into a discrete system. In addition, then the nonlinear programming algorithms are applied to solving the problem. Compared with the indirect method, the direct method has wider applications in trajectory optimization, because there is no need to solve the cross-section conditions and the co-state with the direct method [27]. The shortcoming is that the solution is not as accurate as the indirect method. Thus, when the direct method is applied, the optimality should be verified generally. The Gaussian pseudospectral method (GPM) is one of the successful direct methods. Benson proves that the KKT condition of the GPM is equal to the discrete form of the optimal first-order necessary condition; thus, the solution of the GPM is concordant with the indirect method. The main idea of the GPM is presented below. First, it discretizes the unknown time series of state and control variables to a series of Gaussian points. The real state and control variables are estimated by the Lagrange interpolation polynomials, which are constructed by the Guassian points. Second, the dynamic differential equation is converted into a series of algebraic constraints. Finally, the non-linear optimization problem is reduced to a group of parameters optimization problems with algebraic constraints [28].

Specifically, the time interval [$t_0,t_f$] of the optimal control problem should be converted into the interval $\tau \in$(−1, 1) through mapping:

$$\tau =2t/(t_f-t_0)-(t_f+t_0)/(t_f-t_0)$$

(16)

Then, the GPM performs global interpolation polynomial approximation for state variables and control variables at a series of discrete points. Assume that there are $N+2$ discrete points $\left\{{\tau }_0,{\tau }_1,\cdots ,{\tau }_N,{\tau }_f\right\}$. Here, $\left\{{\tau }_1,{\tau }_2,\cdots ,{\tau }_N\right\}$ are N order Legendre-Gauss (LG) points. The values of the N-order Legendre polynomials would be 0 at these points. The LG points are distributed in the time interval $t\in$(−1,1). There are two edge points ${\tau }_0=-1,{\tau }_f=1$. Figure 6 illuminates the distributions of LG points when different values of N are taken. The distribution of LG points within $[-1,1]$ is sparse in the middle and dense at both ends.

Lagrange interpolation polynomial is used as the base function to approximate state variables and control variables:

$$x(\tau )\approx \overline{x}(\tau )={\displaystyle \sum _{i=0}^N{L}_i(\tau )}x({\tau }_i)$$

(17)

$$u(\tau )\approx \overline{u}(\tau )={\displaystyle \sum _{k=1}^N{\tilde{L}}_k(\tau )}u({\tau }_i)$$

(18)

where $L_i(\tau )$ and ${\tilde{L}}_k(\tau )$ are Lagrange interpolation base functions.

$$L_i(\tau )={\displaystyle \prod _{j=0,j\ne i}^N\frac{\tau -{\tau }_j}{{\tau }_i-{\tau }_j}},i=0,\dots ,N$$

(19)

$L_k(\tau )$ is similar to $L_i(\tau )$, except $k=1,\dots ,N$. The terminal state is not defined in the function (19).

The continuous optimal control problem is transformed into the discrete parameter optimization problem with the GPM, which is a nonlinear programming problem, and the standard form is:

$$\begin{array}{l}\underset{y\in {ℝ}^M}{min}J=J(y)\\ \mathrm{s}.\mathrm{t}. \{\begin{array}{l}{g}_j(y)\le 0,j=1,2,\dots ,p\\ h_i(y)=0,i=1,2,\dots ,q\end{array}\end{array}$$

(20)

where decision vector $y$ includes state variable $x({\tau }_i)$ $(i=0,\dots ,N)$ and control variable $u({\tau }_i)$ $(i=0,\dots ,N)$, and terminal moment $t_f$. $J(y)$ is the objective function, which minimizes the flight time of $U_i$ in our trajectory planning.

5.2. Discussion of the Initial Solution of Trajectory Optimization

The computational efficiency of the GPM relies heavily on the initial solution. To date, there have been few discussions regarding the performance of the initial solution of the GPM, because even if the initial solution is not the optimal, it is usually a feasible solution, for example, when it is applied in satellite mission planning [29,30]. A better initial solution can greatly improve the efficiency of the subsequent optimization process [31]. Many non-convex constraints are considered in the trajectory planning problem, such as the attitude constraints, platform maneuver constraints, obstacle avoidance constraints, and so on. Improper initial solutions may lead to computational inefficiency or even failure to generate feasible solutions.

The flight paths generated by the “CBC” curve consider state constraints, platform dynamic constraints and the obstacle avoidance requirements. To improve the efficiency of the GPM, this paper proposes generating the optimal trajectories by virtue of the “CBC” curves. The “CBC” curves are expressed by discrete flight waypoints.

According to the requirements of the GPM, the initial solution should be a continuous function of time $t$. Therefore, continuous initial trajectories should be generated in virtue of the discrete flight waypoints.

This paper proposes fitting the “CBC” curves into polynomial functions. According to the principle of Occam’s razor principle, lower-order functions are more extensible. The local environments for each pair of waypoints are different. For example, UAVs may have to take sharp turns to avoid obstacles in some flight segments. Considering the obvious differences among different flight segments, we divide the initial paths into three types:

1. The angles of the starting arc and ending arc are all acute angles.

In this case, the UAV does not need to take drastic heading maneuvers in two “C” parts. The angle difference between the start point and the end point is small. Thus, the order of the polynomial function is set to be low.

2. There exists an obtuse angle in the starting arc or ending arc.

In such a condition, the UAV needs to make a large-scale maneuver to satisfy the constraints of the starting and ending states. The order of polynomial fitting is set at an appropriate higher value.

3. There are obstacles to be avoided during the flight

To avoid obstacles, the trajectory of the UAV will be significantly bent. The order of the polynomial is accordingly increased.

There are multiple state parameters of $U_i$. The effects of different situations on particular parameters are different. Therefore, the parameters should be fitted separately. The function fitting process is shown in Figure 7, the constraints of the starting and ending states of $U_i$ and obstacles in the environment are considered.

• First, a set of default parameters are set for the trajectory with slight heading maneuvers in the start arc and the end arc, and no obstacle between two waypoints.
• Second, check whether there are any obtuse angles in the start arc and the end arc. Adjust the order value of the polynomial function when an obtuse angle is found.
• Third, the obstacle avoidance adjustments of the B-spline curve are checked, and the parameters are calculated giving consideration to the constraints of obstacles in practical environments.
• After the time-based continuous initial solutions are planned with the “CBC” curves, the initial solutions are brought into the GPM-based trajectory planning layer in our hierarchical mechanism.

6. Simulation Experiment

Our approach is implemented in Matlab R2014a with a 3.2 GHz Intel i7 eight-core processor with 16 GB memory and Windows 10 operating system. Tomlab optimization package integrated with the GPM optimization algorithm is used as the trajectory planner. LKH is applied to the experiment to solve the MBTSP [32]. The UAV model adopts the simulation model of the “Storm Shadow” UAV [33]. The maneuver constraints of the UAV are defined in

Table 1. The maneuver constraints of the UAV.

Parameters	Range	Parameters	Range
altitude	(2000 m, 10,000 m)	Pitch angle	(-pi/3, pi/3)
velocity	(180 ms−1, 300 ms−1)	Course angle	(0, 2pi)
Roll angle	(-pi/3, pi/3)	Attack angle	(-pi/30, pi/15)
Tuning rate of the roll angle	(-pi/20, pi/20)	Tuning rate of the attack angle	(-pi/30, pi/30)

:

According to the dynamics constraints of UAVs and Equation (5), $R_{min,i}^{horizontal}$ is set to be 3000 m and $R_{min,i}^{vertical}$ be 4000 m.

6.1. Simulation Scenario

A scenario where multiple UAVs cooperatively process multiple tasks is designed. In this scenario, there are three homogeneous UAVs and five targets to be detected. The entry and exit positions of the three UAVs are shown in

Table 2. The constraints of UAVs with respect to the entry and exit positions.

UAV ID	Entry Positions (m, m, m, ms−1, Degree, Degree, Degree)	Exit Positions (m, m, m, ms−1, Degree, Degree, Degree)
1	10,000, 16,000, 4000, 240, 30, 0, 0	73,000, 82,000, 4000, 240, 30, 0, 0
2	8000, 12,000, 4500, 240, 90, 0, 0	80,000, 75,000, 4000, 240, 30, 0, 0
3	6000, 5000, 4800, 240, 90, 0, 0	75,000, 80,000, 4000, 240, 30, 0, 0

. The course angle increases clockwise and the course angle 0 is defined as the point toward the positive direction of axis Y.

To obtain satisfactory views of ground targets and guarantee the stability of UAV platforms, proper detection positions and specific attitudes of UAVs should be set. Moreover, there are hemisphere obstacles in the environment. The target detection constraints and obstacle information are shown as

Table 3. Target detection constraints and obstacle information.

Index Number	Target Detection Constraints (m, m, m, ms−1, Degree, Degree, Degree)	Obstacle Information (m, m, m, m)
1	43,999, 13,311, 4019, 250, 119, 0, 0	37,000, 33,000, 0, 5100
2	72,188, 22,320, 3983, 250, 6, 0, 0	55,000, 79,000, 0, 4600
3	61,032, 47,372, 4000, 250, 27.5, 0, 0	27,000, 12,000, 0, 4800
4	37,077, 75,353, 4013, 250, 32.2, 0, 0	77,000, 50,000, 0, 4800
5	12,556, 50,657, 4008, 250, 18.4, 0, 0	-

. The information regarding obstacles includes their positions (longitude, latitude, altitude) and the radiuses of hemisphere ranges.

6.2. Simulation Experiment for Three UAVs to Detect Five Targets

Figure 8 illuminates the maneuvering constrained flight paths of the UAVs, which are generated by using the “CBC” planning method. The red and green rectangular points identify the entry and exit points of UAVs respectively, and the pink hollow points are the sampling task detection positions. The obstacles are expressed by the red hemisphere.

As shown in Figure 8, the flight paths are quite smooth. These flight paths satisfy the minimum turn radius constraint and keep a safe distance from obstacles in the environment.

A weighted directed graph is constructed based on the estimated cost of flight paths. The MBMTSP problem is solved by using the LKH model. The results of task assignment are shown in

Table 4. Task assignment.

UAV ID	Tasks Assignment
1	5,4
2	1,2
3	3

:

After the task assignment is accomplished, the precise optimal trajectories should be acquired to guide UAVs. As stated above, the efficiency of trajectory planning is affected by the initial solutions. The flight paths that are obtained by the CBC planning method are utilized to design the initial flight trajectories. The “CBC” paths are expressed by sets of way points. However, the GPM method needs the initial trajectories of UAVs to be continues functions of time $t$. The polynomial fitting method is adopted to fit CBC paths. Since UAVs are simplified as point mass models when the “CBC” paths are planned, the changes of the roll angles could not be determined by the CBC paths. Therefore, we define that ${\mu }_i(t)=0,i\in N_U$.

The GPM method is used to compute the optimal trajectories of UAVs from initial trajectories. Figure 9 shows the optimal trajectories of three UAVs in processing their assigned tasks.

The green curves are the initial “CBC” trajectories and the blue curves are the optimal trajectories. As shown in Figure 9, most optimal flight trajectories are similar to the green initial trajectories while some others are not. In the middle parts of flight trajectories, UAVs fly at a lower attitude than the CBC-based initial flight trajectories. In the trajectory planning phase, the influence of gravity on UAVs are considered. During the flights, UAVs fly from a high altitude to a low altitude to convert the potential energy into kinetic energy, thus UAVs can fly faster.

Figure 10a illuminates the variations of course angles of the three UAVs during the flight. UAVs would change their course angles before and after they execute the assigned tasks, as shown in the black box. During the flight, the variations of the course angles are relatively smooth. Similarly, the pitch angles of UAVs are set below 0 to reduce flight altitude when they leave the entry points and target detection positions. They would climb up rapidly to approach to the planned detection positions when they are close to the targets, as shown in Figure 10b. The speeds of UAVs at the entry and exit positions are 240 m/s, and their speeds should be 250 m/s when they are detecting the assigned targets. As the objective is to shorten the flight time period, UAVs would accelerate to the maximum feasible speed to reduce the task processing time. To satisfy the target detection requirements, UAVs reduce their speeds rapidly when they are close to the task regions, as shown in Figure 10c.

The variations of the attack angles of UAVs are shown in Figure 10d. Their attack angles do not exceed the limit, and the adjustment rate does not exceed the attack angle variation constraint. It shows that UAVs would keep their attack angles constant in most time. The variations of roll angles are shown in Figure 10e. UAVs make horizontal turns by banking. Therefore, their roll angle would change rapidly when they get close to the task regions. Figure 10e illuminates that the variations of roll angles can satisfy the maneuver constraints defined in Table 1. Figure 10f shows the variations of throttle control during the flight. To gain the maximum thrust, the throttle control is set to be 1 when UAVs are flying to the task regions. To satisfy the task processing constraints, the throttles are set to be 0 when they arrive in the task regions.

This shows that the proposed hierarchical optimization framework was able to solve the MUMTMP effectively, on the basis of the results of the simulation experiment. The mission planning method was able to generate the optimal trajectories, maintain the safety of UAVs and reduce the computation cost.

6.3. Computational Experience for Multiple UAVs in Random Instances

In this section, we demonstrate the stability and computational efficiency of the proposed approach by making a comparison with existing methods. The following elements are taken into account in the simulation:

(a)

The number of UAVs ranges from 3 to 7. The distances between the entry positions of these UAVs are in the range [3 km, 6 km].

(b)

The number of tasks of the UAVs ranges within [10,34]. The distances between these tasks are in the range [25 km, 45 km].

(c)

There are obstacles in the environment. To demonstrate the capability of the “CBC” curves, obstacles are placed between task points.

The proposed hierarchical mission planning method is used to deal with the mission planning problem. The main innovation of our approach is the “CBC”-based path planning method. To demonstrate the efficiency of the proposed method, we compare this method with other existing methods. There are several methods to estimate the flight paths of UAVs, such as the straight line from the initial positions to the goal positions (straight line as the initial solution, SI) [35], the B-spline curves-based method (B-spline curves as the initial solution, BI) [36], and the vertical section-based method (VI) [7]. Our proposed method is defined as CI. We demonstrate our proposed method from three perspectives: the computational efficiency, the proportions of the distances of estimated flight paths and the planned trajectories, and the probabilities of generating the optimal trajectories by searching from different initial estimations.

The performances of these methods are depicted in six different scenarios as below: (a) The angles of the starting arc and the ending arc are both acute angles and there are no obstacles in the environment (both acute angles no obstacles, BANO); (b) There exists one obtuse angle in the starting arc or the ending arc and there are no obstacles in the environment (one obtuse angle no obstacles, OONO); (c) There exist two obtuse angles in the starting arc and the ending arc and there are no obstacles in the environment (two obtuse angles no obstacles, TONO); (d) The angles of the starting arc and the ending arc are both acute angles and there are obstacles in the environment (both acute angles with obstacles, BAWO); (e) There exists one obtuse angle in the starting arc or the ending arc and there are obstacles in the environment (one obtuse angle with obstacles, OOWO); (f) There exits two obtuse angles in the starting arc and the ending arc and there are obstacles in the environment (two obtuse angles with obstacles, TOWO).

We have collected 200 flight path estimation samples derived by each method on the six different scenarios, respectively. The average computation time for each method of planning one segment of the flight path is recorded in

Table 5. Average computational consumption.

Methods	SI	BI	VI	CI	GPM (with a “CBC”-Based Initial Trajectory)
Average computation consumption	<1 ms	954 ms	200 ms	1023 ms	300,000 ms

.

The results show that our method would take more time to find the “CBC”-based flight paths. However, the computation time is obviously less than the GMP method. This means that our method satisfies the online planning requirement.

The estimations of the flight distances in different scenarios are compared with the trajectory planning results. The proportions of the distances of the estimated flight paths and the distances of the real flight trajectories are shown in Figure 11. This shows that the flight distances of the flight paths estimated by using the CI method are almost similar to those of real trajectories. The other three methods were not able to provide reliable estimation results in any of these six scenarios.

We demonstrate the performances of initial solution search methods with consideration given to trajectory planning by using different initial flight trajectory estimations. In the simulation, the GPM-based trajectory planning method searches for distinct initial solutions, and the probabilities of acquiring the optimal solutions are recorded. As shown in Figure 12, our method outperforms other methods. The initial solutions generated by the vertical section-based method is not smooth. In the simulation, the data smoothing method is employed on the vertical section-based initial solutions. However, this method is still inferior to the B-spline-based method.

The simulation results show that our method outperforms the existing methods in acquiring more precise flight paths and supporting the trajectory planning.

7. Conclusions

In this paper, the problem of multi-task mission planning for multiple cooperative unmanned aerial vehicles (UAVs) is studied. A hierarchical optimization framework is designed to solve the problem. In the first stage, the flight paths are generated by virtue of the Dubins curve and B-spline mixed path planning method. In the second stage, the task assignment problem is solved as a multi-base traveling salesman problem. In the third stage, the Gaussian pseudospectral method (GPM) is used to plan the precise trajectory and calculate control variables of UAVs. The initial feasible trajectories planned in the first stage can enhance the computation efficiency. The proposed algorithm is implemented and tested in simulation experiments in MATLAB. Simulation results demonstrate the satisfactory performance of the proposed algorithm in dealing with the online mission.