A Four-Dimensional Space-Time Automatic Obstacle Avoidance Trajectory Planning Method for Multi-UAV Cooperative Formation Flight

Abstract

Trajectory planning of multiple unmanned aerial vehicles (UAVs) is the basis for them to form the formation flight. By considering trajectory planning of multiple UAVs in formation flight in three-dimensional space, a trajectory planning method in four-dimensional space-time is proposed which, firstly, according to the formation configuration, adopts the Hungarian algorithm to optimize the formation task allocation. Based on that, by considering the flight safety of UAVs in formation, a hierarchical decomposition algorithm in four-dimensional space-time is innovatively put forward with spatial positions and time constraints both considered. It is applied to trajectory planning and automatic obstacle avoidance under the condition of no communication available between UAVs in the formation. The simulation results illustrated that the proposed method is effective in cooperative trajectory planning and automatic obstacle avoidance in advance for multiple UAVs. Meanwhile, it has been tested in a Swarm Unmanned Aerial System project and boasts quite significant value in engineering applications.

1. Introduction

In recent years, as the technology of unmanned aerial vehicles (UAVs) advances and artificial intelligence (AI) develops, UAVs have been widely adopted in military fields, such as intelligence reconnaissance and military exercises [1]. Combat by a single UAV has been unable to meet the demand due to the increasing complexity of military combat tasks. Therefore, combat by multi-UAV cooperative formations has gradually become the trend in development [2].

Multi-UAV cooperative trajectory planning [3] is an indispensable part of multi-UAV mission execution, which is essentially an optimization problem. First, the task is assigned, then combined with flight safety UAV performance and other constraints [4], the optimal trajectory [5] is planned for the UAV formation to meet the flight performance and environmental constraints. Compared with single UAVs, trajectory planning for multiple UAVs to form the formation needs to further consider position constraints and cooperative obstacle avoidance among UAVs.

At present, the commonly-used UAV trajectory planning algorithms can be divided into two categories: one is traditional algorithms for classical optimization [6,7], mainly consisting of dynamic planning, rapidly exploring random tree (RRT), Voronoi diagram, A * algorithm [8,9,10,11,12,13] and Dijkstra algorithm; and the other is modern intelligent optimization algorithms [14], including differential evolution (DE) [15], ant colony optimization (ACO), particle swarm optimization, and whale optimization algorithm (WOA) [16,17,18]. As plenty of researchers have studied and applied these two types of algorithms, they have suggested a cooperative coevolutionary genetic algorithm in the literature [19], which solves the problem of trajectory planning of multiple UAVs in two-dimensional space. In the literature [20], the artificial bee colony (ABC) algorithm and simulated annealing (SA) algorithm have been combined to solve issues of cooperative trajectory planning in two-dimensional space. A hierarchical potential field algorithm was designed in the literature [21], introducing rotational force for the trajectory planning of multiple UAVs. However, this has the issue that planning becomes slower as hierarchies increase. The Particle Swarm Optimization–Hook Jeeves (PSO-HJ) algorithm was adopted in the literature [22]. As an integrated way of multi-UAV trajectory planning, it improves planning effectiveness to some extent. However, with the increase of the number of UAVs, the calculation efficiency decreases. In the literature [23], UAV formation was configured based on an improved consensus algorithm. It introduced particle swarm optimization (PSO) and a model predictive control (MPC) algorithm to deal with obstacle avoidance, which proved effective by numerical simulation. In the literature [24], a time-varying formation was formed on the basis of a consistency protocol. It then guided formation movement by setting virtual leaders, planned obstacle avoidance by artificial potential field algorithm, and proved the stability of the closed-loop system by using Lyapunov stability theory. In the end, a numerical simulation experiment was performed and verified the effectiveness of obstacle avoidance during formation flight.

It can be seen from the above studies that: (1) some algorithms cover only two-dimensional situations; (2) there is still room for further improvement in trajectory planning and algorithm efficiency; (3) in some literature, only numerical simulations instead of actual experiments are performed to verify algorithm feasibility.

To this end, this paper proposes a trajectory planning approach with automatic obstacle avoidance for multiple UAVs to achieve formation flight, which is a way of prior trajectory planning according to flight tasks. The main innovations of this paper are listed as follows: (1) compared with the literature [19,20], the algorithm in this paper could be applied to trajectory planning in three-dimensional space; (2) this method combined the Hungarian algorithm with the hierarchical decomposition strategy, which is of significantly better efficiency and can achieve planning the optimal trajectory in only a few hundred milliseconds; (3) under the condition of no communication between UAVs in formation, the method proposed in this paper could realize automatic obstacle avoidance and be able to automatically generate an obstacle-free path through collision detection; (4) unlike previous studies [23,24] without actual tests, the method proposed in this paper is verified by actual tests and achieved good experimental results.

Problems related to automatic obstacle avoidance of multi-UAV cooperative formation flying are studied in this paper. Considering the solution of multi-dimensional trajectory cooperative planning and complex constrained optimization problems, the main content of this paper is organized through the following parts: in Part 1, the shortest path and collision judgments during the flight formation are converted into the corresponding mathematical models; in Part 2, the Hungarian algorithm and the four-dimensional spatiotemporal hierarchical decomposition algorithm are studied respectively according to descriptions of the mathematical models; in Part 3, the effectiveness of the method is verified through simulation software and a practical experiment. The results showed that the method is outstandingly adaptable and applicable to multi-UAV cooperative planning.

2. Establishment of the Mathematical Model

Trajectory planning with automatic obstacle avoidance for multi-UAV cooperative flight formation can be described in the form of a mathematical model:

(1)

Mission assignment to formations: missions will be assigned to all UAVs according to the initial waypoint information $O(x_1,y_1,z_1,\dots x_n,y_n,z_n)$ of the given n UAVs and the waypoint information $T({x^{\prime }}_1,{y^{\prime }}_1,{z^{\prime }}_1,\dots {x^{\prime }}_n,{y^{\prime }}_n,{z^{\prime }}_n)$ of the target point to plan the shortest trajectory, to minimize the total flight distance on the premise that the mission objective of a formation and flight constraints of the UAVs themselves are satisfied. The above problem can be translated into the following mathematical description: Take the set of the total distance of trajectories of different waypoints corresponding to different UAVs as $D\{d_1,d_2,\dots d_n\}$, For any $i\in \{1,2,\dots ,n\}$, $d_i<d_n$ is always true.

(2)

Trajectory planning for formations: when UAVs are performing corresponding tasks in formation, it is necessary to ensure that all the UAVs do not collide with each other when performing the tasks in formation, in addition to meeting the shortest trajectory planning. Therefore, judgments should be made on trajectories assigned by missions to make sure UAVs will not collide or the distance between UAVs is not too small. In this case, the determination of whether UAVs collide is converted into the problem of whether the minimum distance between two routes is greater than or equal to the safe spacing between UAVs. The above problem can be transformed into the following mathematical description:

Take the set of all routes of UAVs in formation as $L\{l_1,l_2,\dots l_n\}$, for any $i\in \{1,2,\dots ,n\}$, $j\in \{1,2,\dots ,n\}$ and $i\ne j$, the minimum distance between any two routes is set as $d_{\min l_i{l}_j}$, $d_{safe}$ as the safe distance for UAVs, distance between routes should satisfy the condition of $d_{\min }{}_{l_i,l_j}\ge d_{safe}$.

3. Method to Plan Multi-UAV Trajectories during Flight Formation

Essentially, the problem with trajectory planning for multi-UAV formation is how to distribute n UAVs to the next target waypoint from one waypoint and, in the meantime, satisfy time constraints and safety constraints of formation flying. To address this problem, a method to plan multi-UAV trajectories is proposed in this paper. Firstly, the Hungarian algorithm is applied to roughly plan the multi-UAV trajectory; secondly, the collision judgment algorithm is utilized to detect the collision situation; thirdly, precise planning combined with the four-dimensional spatiotemporal hierarchical decomposition strategy is contrived to generate the final collision-free flight trajectory. The schematic diagram of the planning method flow is presented in Figure 1.

3.1. Method to Assign Tasks to Multiple UAVs in Formation Based on the Hungarian Algorithm

To enable multiple UAVs to realize the formation flight, trajectory planning according to mission objectives is the first thing to do, which is usually difficult in calculation, time-consuming in planning, and not efficient to meet actual demands. Therefore, it is important to explore a path planning algorithm with simple calculation and high efficiency for multiple UAVs.

Task assignment is of significance for multi-UAV trajectory planning, while for solving assignment problems, the Hungarian algorithm happens to be a very effective method with fast calculation and high efficiency. Therefore, this paper applies the Hungarian algorithm for the task allocation (assignment) of multiple UAVs to achieve the objective of obtaining the shortest total flight distance of all UAVs, as shown in Figure 2.

Trajectory planning for UAVs formation can be carried out based on the theorem of the optimal solution of the Hungarian algorithm. The specific steps are presented as follows:

Step 1:

Input the initial position matrix of $n$ UAVs $O(x_1,y_1,z_1,\dots x_n,y_n,z_n)$ and the target position matrix $T({x^{\prime }}_1,{y^{\prime }}_1,{z^{\prime }}_1,\dots {x^{\prime }}_n,{y^{\prime }}_n,{z^{\prime }}_n)$.

Step 2:

Calculate the distance between the initial position of $n$ UAVs and n target waypoints to form an $n*n$ distance matrix $A$. The element $A_{ij}$ represents the distance between the UAVs numbered $i$ and the $jth$ target waypoint.

Step 3:

Apply row transformation to the distance matrix $A$, where the smallest element of each row is subtracted from the elements of that row in matrix $A$.

Step 4:

Apply column transformation to the distance matrix $A$, where the smallest element of each column is subtracted from the elements of that column in matrix $A$.

Step 5:

The trial assignment:

(1)

Find the row and column with the least number of zero elements without lines, that is, traverse all the zero elements without lines to check how many zeros are in rows and columns where the elements are located, and finally select the zero elements in the row and column with the least number of zero elements;

(2)

Find the zero elements without a line in that row and column, which is an independent zero element. Draw a line for the row and column in which those zero elements are located;

(3)

Leave elements covered by lines alone for the moment and repeat steps (2) and (3) until there are no lines to draw;

(4)

Succeed when finding $n$ independent zero elements according to the number of zero elements found in (3) and execute the next step when the number of independent zero elements is less than $n$.

Step 6:

Draw a line to cover the zeros:

(1)

Tick rows without an independent zero element;

(2)

Tick the corresponding columns with one or more zero elements in the ticked rows;

(3)

Tick the corresponding rows with one or more independent zero elements in all ticked columns;

(4)

Repeat (3) and (4) until no more ticks can be made.

Step 7:

Update the matrix:

(1)

Find the smallest number among those without lines;

(2)

Subtract the smallest number from those without lines;

(3)

Add the smallest number to the numbers with two lines, ensuring that the position of zero elements remains unchanged.

Step 8:

Repeat steps 5–7 until success.

The resulting matrix obtained after the above steps is the solution matrix. Replace all the independent zero elements in this matrix with 1 and all the elements except the independent zero elements with 0. Such a 0–1 matrix is the optimal solution to the multi-UAV allocation problem.

Pseudo-code of the task allocation Algorithm 1 is:

Algorithm 1: UAV task assignment algorithm

3.2. The Multi-UAV Trajectory Planning Method Based on the Four-Dimensional Spatiotemporal Hierarchical Decomposition

According to the task assignment result, a trajectory can be designed for each UAV from the current point to the target point, as shown in Figure 3. A collision situation would occur if the mission is executed through the trajectory. To safely complete the formation flight mission, all UAVs need to take the same time to traverse the trajectory and not collide with each other, which satisfies the time coordination and safety of the formation. Therefore, the planning of the final flight trajectory also needs to meet the time constraints and safety constraints of the UAVs’ formation based on the task assignment.

(1)

Time constraints

Time constraints require all UAVs to arrive at the corresponding target point simultaneously. In the time constraint of this paper, all UAVs are required to reach the navigation point within the time constraint. In a formation mission, it is assumed in this paper that the speed of all UAVs is maintained at a certain level, and the approximate time of arrival at the target point is calculated based on the range of the UAVs to realize the cooperativeness in time for all UAVs during formation flying since each trajectory consists of a series of waypoints which do not contain accurate time information. The approximate time is employed as the benchmark to establish a time-constrained model for UAVs formation.

$$P^i=\{p^i{}_{{}_1}(x_1,y_1,z_1,t_1),p^i{}_{{}_2}(x_2,y_2,z_2,t_2),\dots ,p^i{}_n(x_n,y_n,z_n,t_n)\}$$

(1)

$P^i$ denotes waypoints of UAV $i$, $n$ the total number of all waypoints of UAV $i$’s trajectory, $p^i{}_n(x_n,y_n,z_n,t_n)$ the $nth$ waypoint of the trajectory $P^i$, $(x,y,z)$ the position coordinates of the trajectory, and $t_n$ the flight time to reach the waypoint.

Based on the above representation of a trajectory, cooperative time constraints on waypoints for a multi-UAV formation can be expressed as:

$$\max (t_n)$$

(2)

(2)

Safety constraints

Safety constraints require the distance between UAVs to be greater than or equal to the safety distance until they arrive at the target point to avoid collisions. Due to the large granularity of scattered waypoints, not only safety constraints at waypoints but also the safety of the entire trajectory within the same period should be determined. Specifically, $M1N1$ and $C1D1$ are set to represent the trajectories at the first and second waypoints of UAVs $i$ and $j$, respectively, and the coordinates of point $M1$ are set to be $(x1,y1,z1)$, point $N1$$(x2,y2,z2)$, point $C1$$(x3,y3,z3)$, and point $D1$$(x4,y4,z4)$. Safety constraints between the two UAVs are displayed in the following steps:

Step 1:

Set the safety distance between two UAVs to $d_{safe}$ meters according to UAVs’ control accuracy and relevant external equipment;

Step 2:

Determine whether the shortest distance between the line segments connecting the adjacent waypoints corresponding to UAVs is less than the safe distance $d_{safe}$ in a way shown as follows:

Set the line $H$ to be a point on the line $M1N1$. The coordinates $(X,Y,Z)$ of the point $H$ can be expressed as:

$$\{\begin{array}{c}X=x1+k(x2-x1)\\ Y=y1+k(y2-y1)\\ Z=z1+k(z2-z1)\end{array}$$

(3)

When the parameter k satisfies the condition where $0\le k\le 1$, $H$ is a point on the line $M1N1$; when the parameter $k<0$, $H$ is a point on the extended line of $N1M1$; when the parameter $k>1$, $H$ is a point on the extended line of $N1M1$.

Set the line $Q$ to be a point on the line $C1D1$ and the coordinates $(U,V,W)$ of the point $Q$ can be expressed as:

$$\{\begin{array}{c}U=x3+k(x4-x3)\\ V=y3+k(y4-y3)\\ W=z3+k(z4-z3)\end{array}$$

(4)

When the parameter y satisfies the condition where $0\le y\le 1$, $Q$ is a point on the line $C1D1$; when the parameter $y<0$, $Q$ is a point on the extended line of $D1C1$; when the parameter $y>1$, $Q$ is a point on the extended line of $C1D1$.

The distance between the point $H$ and the point $Q$ is:

$$HQ=\sqrt{{(X-U)}^2+{(Y-V)}^2+{(Z-W)}^2}$$

(5)

The squared distance is:

$$f\left(k,y\right)=H{Q}^2={\left(X-U\right)}^2+{\left(Y-V\right)}^2+{\left(Z-W\right)}^2$$

(6)

The shortest distance between the line $M1N1$ and the line $C1D1$ is the minimum value of $f(k,y)$.

Deduce the partial derivatives of $k,y$ for $f(k,y)$, respectively, and set the partial derivatives as 0. $H$ falls on line $M1N1$ and $Q$ falls on line $C1D1$, and $HQ$ is the shortest distance when the parameters obtained finally satisfy the condition where $0\le k\le 1$ and $0\le y\le 1$. Otherwise, it will be impossible to find a point $H$ on the line $M1N1$ and a point $Q$ on the line $C1D1$ to minimize the distance $HQ=\sqrt{{(X-U)}^2+{(Y-V)}^2+{(Z-W)}^2}$. At this point, deduce the shortest distance from $M1$ to the line $C1D1$, and from $N1$ to the line $C1D1$; the shortest distance from $C1$ to the line $M1N1$, and from $D1$ to the line $M1N1$.

Above is the method to determine whether two trajectories collide or not, and the process of determination is presented in Figure 4.

A four-dimensional spatiotemporal hierarchical decomposition algorithm combined with automatic obstacle avoidance trajectory planning is proposed in this paper to meet the time constraints and safety constraints for UAVs’ formation. When the distance between UAVs is too close, the procedure of automatic obstacle avoidance takes effect and plans a route for the UAVs to avoid obstacles. Thenceforth, the UAVs will continue to perform their original tasks. Targeting the specific tasks for formations, collision detection on trajectories of all UAVs is conducted in a combination of time constraints to automatically contrive routes for obstacle avoidance in the future. The specific implementation steps are as follows:

Step 1:

Set the safety distance as $d_{safe}$ and the time required for UAVs to fly the safety distance $d_{safe}$ as $t_{safe}$;

Step 2:

Step 2: Determine whether the shortest distances between the trajectory connecting the initial and target points of the first UAV and that of the remaining $(n-1)$ UAVs are greater than or equal to the safe distance s according to the initial point matrix $O$ and the target point matrix $T$ of a formation. Practice Step 3 if the shortest distance is smaller than the safe distance, and perform Step 4 if it is greater than the safe distance;

Step 3:

Select the smaller serial numbers of UAVs, put them into the array $E$, and conduct Step 4;

Step 4:

Determine the distances between the trajectory connecting the initial and target points of the $nth$ UAV and that of the remaining UAVs and repeat Step 3 until all the distances in a formation mission have been determined.

Step 5:

Sort the array $E$ in ascending order and copy the array $E$ to the array $F$;

Step 6:

Put all UAVs’ serial numbers in the formation mission other than those in the array $E$ into the array $G$ in ascending order;

Step 7:

Traverse the array $F$, copy the position information of all UAVs’ initial and target points in the array, and at the same time increase the two points’ heights by $d_{safe}$ meters, respectively. Insert the two points with increased heights into the middle of the original points (for example, the initial and target points are at $h1\left(x1,y1,z1,t1\right)$ and $h2\left(x2,y2,z2,t2\right)$, and there will four waypoints at $h1\left(x1,y1,z1,t1\right)$ $h{1}^{+}\left(x1,y1,z1+d_{safe}\cdot count,t1+t_{safe}\cdot count\right)$, $h{2}^{+}\left(x2,y2,z2+d_{safe}\cdot count,t2+t_{safe}\cdot count\right)$, $h2\left(x2,y2,z2,t2\right)$ after the two new points are inserted. Count denotes the number of times to carry out step 5, which will increase by 1 each time and whose initial value is 0. Practice Step 8;

Step 8:

Clear the array $E$ and traverse the array $F$ to redetermine whether the distances between trajectories of all UAVs are greater than the safe distance s. Perform Step 9 after repeating steps 2 to 5;

Step 9:

Traverse the array $F$ and modify the altitude information of newly added UAV waypoints in the array $F$ into $h1\left(x1,y1,z1,t1\right)$, $h{1}^{+}\left(x1,y1,z1+d_{safe}\cdot count,t1+t_{safe}\cdot count\right)$, $h{2}^{+}\left(x2,y2,z2+d_{safe}\cdot count,t2+t_{safe}\cdot count\right)$, $h2\left(x2,y2,z2,t2\right)$. Count means the number of times to practice Step 5, which will increase by 1 each time and whose initial value is 0. Repeat steps 8 to 9 until the distances between UAVs in the array $F$ are greater than the safe distance;

Step 10:

Traverse the array G and insert two new waypoints corresponding to the nth segment of trajectories of all UAVs in the array $G$. In this case, the original $h1\left(x1,y1,z1,t1\right)$ and $h2\left(x2,y2,z2,t2\right)$ become four waypoints of $h1\left(x1,y1,z1,t1\right)$, $h{1}^{+}\left(x1,y1,z1,t1+t_{safe}\cdot count\right)$, $h{2}^{+}\left(x2,y2,z2,t2+t_{safe}\cdot count\right)$, $h2\left(x2,y2,z2,t2\right)$;

Step 11:

Clear arrays $E$, $F$, and $G$, and move the positions of the initial and target points to the next section of the trajectory (for instance, set the first and the second waypoints of all UAVs as the initial and target points at the very beginning, and then take the second and the third waypoints as the initial and target points). Repeat steps 1 to 10 until the distances between trajectories of all UAVs have been checked. At this point, the algorithm ends, and multi-UAV trajectory obstacle avoidance is completed.

The schematic diagram of obstacle avoidance based on hierarchical decomposition is shown in Figure 5, where trajectories of UAV1 and UAV2 are $A1B1$ and $MN$, respectively. There is a crossover between $A1B1$ and $MN$, so automatic obstacle avoidance is required. According to the algorithm, UAV1 automatically inserts waypoints $A{1}^{\prime }$ and $B{1}^{\prime }$, and UAV2 automatically inserts waypoints $M^{\prime }$ and $N^{\prime }$, that is, the trajectory of UAV1 becomes $A1-A{1}^{\prime }-B{1}^{\prime }-B1$; the trajectory of UAV2 trajectory becomes $M-M^{\prime }-N^{\prime }-N$. UAV1 has performed a hierarchically decomposed flight and automatically avoided obstacles.

Pseudo-code of the hierarchical decomposition and automatic obstacle avoidance Algorithm 2:

Algorithm 2: High layered realization of UAV obstacle avoidance

4. Implementation of Algorithm, Comparison, and Simulation

4.1. Formation Task Allocation Algorithm Comparison

When compared to random matching and auction algorithms is shown in Figure 6, Figure 7 and Figure 8, to make the advantages of the method proposed in this paper much clearer, for task allocation from “linear” to “circular” formation, we considered only two indicators: total trajectory length and calculation time. In order to verify the effectiveness of the algorithm, 1000 times, 10,000 times, and 100,000 times of calculations were carried out, and the results are shown in

Table 1. Average total track length comparison.

Times	1000	10,000	100,000
Hungarian algorithm assignment(m)	163.5636	163.5636	163.5636
auction algorithm(m)	169.2375	168.6958	167.9658
randomly assigned(m)	177.6276	174.3265	174.9328

and

Table 2. Calculated time comparison.

Times	1000	10,000	100,000
Hungarian algorithm assignment(s)	0.509003	0.5888016	1.3911980
auction algorithm(s)	0.5229998	0.7292022	2.8083982
randomly assigned(s)	0.53226971	0.8156322	2.95687235

below. By comparing and analyzing the data in the table, we can tell from the results that the proposed method is able to achieve the formation flight planning with the shortest total trajectory length in a comparatively shorter time.

4.2. Flight Simulation of Multi-UAV Formation

A simulation test of the multi-UAV formation was completed by writing relevant programs on MATLAB R2019a developed by the Mathwork company of Natick, America in the computers with the main frequency of 3.20 GHz, a memory of 32GB, and the operating system of Windows 10. To verify the effectiveness of algorithms, a formation test was conducted for 10 UAVs, and the initial and target coordinates of the 10 UAVs are shown in Figure 9:

The following

Table 3. The “One-line” to “Circle” task assignment results.

Serial Number	1	2	3	4	5	6	7	8	9	10
1	0	0	0	0	0	0	0	1	0	0
2	0	0	0	0	0	0	0	0	1	0
3	0	0	0	0	0	0	1	0	0	0
4	0	0	0	0	0	0	0	0	0	1
5	0	0	0	0	0	1	0	0	0	0
6	1	0	0	0	0	0	0	0	0	0
7	0	0	0	0	1	0	0	0	0	0
8	0	1	0	0	0	0	0	0	0	0
9	0	0	0	1	0	0	0	0	0	0
10	0	0	1	0	0	0	0	0	0	0

and

Table 4. The “Circle” to “Triangle” task assignment results.

Serial Number	1	2	3	4	5	6	7	8	9	10
1	0	0	0	0	0	1	0	0	0	0
2	0	0	0	0	0	0	1	0	0	0
3	0	0	0	0	0	0	0	1	1	0
4	0	0	0	0	0	0	0	0	0	1
5	0	0	0	0	0	0	0	0	0	0
6	1	0	0	0	0	0	0	0	0	0
7	0	1	0	0	0	0	0	0	0	0
8	0	0	1	0	0	0	0	0	0	0
9	0	0	0	1	0	0	0	0	0	0
10	0	0	0	0	1	0	0	0	0	0

demonstrates the planned trajectory with the shortest distance for 10 UAVs based on the initial and target positions, from ”one-line” to “circle” and then to “triangle”, through the Hungarian algorithm. Graphical results of the task allocation are displayed below. Row numbers of the matrix represent the UAV numbers, and column numbers are markers of the target points. A 1 element in the matrix represents that the row and the column where the 1 element is located match.

Based on the results of allocation produced by the Hungarian algorithm, obstacles were avoided through the method of hierarchical decomposition after the target points were determined to ensure that all UAVs did not collide in the air. Results of obstacle avoidance are listed in the following table.

Table 5. “One-line” to “circle” obstacle avoidance results.

UAV	1	2	3	4	5	6	7	8	9	10
Rising number of UAV	2	1	0	1	2	1	0	0	1	0

and

Table 6. “Circle” to “triangle” obstacle avoidance results.

UAV	1	2	3	4	5	6	7	8	9	10
Rising number of UAV	2	1	1	1	1	0	0	0	0	0

shows the height hierarchy situation of the UAVs when the formation is transformed from “one-line” to “circle”, “circle” to “triangle”. Table 5 shows that the UAV 1, 5, the UAV 2, 4, 6, 9, and the UAV 3, 7, 8, 10 are at the same altitude respectively, and the times of UAV rising represents a different height. Table 5 has the same meaning as Table 6.

The above results suggest that the flight altitude of UAVs with different numbers should be planned based on their different results of obstacle avoidance to avoid obstacles in the air.

Trajectory planning is carried out according to the Hungarian algorithm and the four-dimensional spatiotemporal hierarchical decomposition algorithm to realize the formation flying from a “one-line” to the shape “circle”, and from the shape of a “circle” to “triangle”. The simulation diagram is shown in Figure 10, Figure 11 and Figure 12, where line segments with the same color indicate flying on the same plane to the target points, while those with different colors imply different flight altitudes.

4.3. Engineering Application and Flight Test

Quadrotor UAVs were employed to complete a formation flight test in this program to verify the effectiveness and feasibility of algorithms. A quadrotor UAV is demonstrated in the following figure, made from carbon fiber materials with a fixed foldable tripod of an inverted shape, and featuring firmness and stability in Figure 13.

In the flight test, 10 UAVs were selected to complete the UAVs formation, flying in three shapes, namely a “one-line”, the shape of a “circle”, and a “triangle”. Firstly, the initial coordinates of the pattern to be formed and the final coordinates of the UAVs were entered into the simulation software program. The waypoint information of all the UAVs information were output after the program was successfully operated. The trajectory of each UAV was saved in a text file, as shown in

Table 7. Generated information about the location of the drone 1 relative to the track point.

Waypoint Order	Waypoint Type	Relative Latitude (m)	Relative Longitude (m)	Waypoint Altitude (m)	Fly Time (s)
1	1	−11.4839229583740234	4.81873273849487305	20	10
2	2	−11.4839229583740234	4.81873273849487305	30	15
3	2	−23.2751808166503906	24.2204875946044922	30	15
4	2	−23.2751808166503906	24.2204875946044922	20	10
5	2	−23.2751808166503906	24.2204875946044922	30	15
6	2	−16.0270156860351563	37.4085922241210938	30	15
7	2	−16.0270156860351563	37.4085922241210938	20	10

. Then, the trajectory information of all UAVs was input into the ground station software and displayed on a map, as presented in Figure 14. Finally, the information was downloaded to the UAVs through the ground stations, and the UAVs performed the flight plan.

In Figure 15, each card in the above diagram represents a UAV, and the status related to each UAV (including latitude and longitude information, battery voltage, and flight mode) can be observed clearly.

The actual formation flying of UAVs is displayed in Figure 16. During the flight, each UAV was able to arrive at a place near the designated location, and all of them could form a predetermined pattern. However, the actual positions of the UAVs formation slightly deviated from the target points due to GPS and environmental issues such as wind direction. Nevertheless, the task assigned to each UAV was accurate, and no collision occurred during the flight. Therefore, this experiment proved the feasibility of the algorithms.

5. Conclusions

Targeting three-dimensional space trajectory planning for multi-UAV in cooperative formation, the Hungarian algorithm is adopted in this paper based on formation control under the virtual pilot to minimize costs of the total distance of trajectories during formation transformations, and a four-dimensional spatiotemporal hierarchical decomposition strategy on flight altitude is put forward to avoid collisions during formation flying. This method combines the Hungarian algorithm with the hierarchical decomposition strategy, which is of significantly better efficiency and can plan the optimal trajectory in only a few hundred milliseconds. Compared with the auction algorithm, the total track distance and planning time are reduced by about 3%. Meanwhile, time constraints of the UAVs’ formation flying are combined to preplan a route for automatic obstacle avoidance without other communication between UAVs. In the end, verification is conducted through MATLAB simulation and test, and 10 UAVs formation have realized shape transformation from a “one-line” to a “circle”, and then to a “triangle”. Tested in actual flight, each UAV can reach the corresponding destination quickly and without collision, thus verifying the effectiveness and reliability of this method stated in the paper.