HDP-TSRRT*: A Time–Space Cooperative Path Planning Algorithm for Multiple UAVs

Abstract

This paper proposes a fast cooperative path planning algorithm for multiple UAVs that satisfies the time–space cooperative constraints, namely, the RRT* algorithm based on heuristic decentralized prioritized planning (HDP-TSRRT*), which takes into account the simultaneous arrival time variables of each UAV as well as the avoidance of conflicts and threats. HDP-TSRRT* is a hierarchical decoupling algorithm. First, all UAV pre-paths are planned simultaneously at the synchronous decentralized planning level. Second, at the coordination path level, the heuristic decentralized prioritized planning algorithm (HDP) is proposed to quickly complete the coordination process of the path planning sequence. This strategy assigns reasonable and robust priority to all UAVs based on the performance evaluation function composed of the number of potential collisions and the violation of collaboration time of the pre-planned path. Third, the time–space cooperative constraints-based RRT* algorithm (TSRRT*) is proposed at the single-machine cooperative path planning level. Based on this, the algorithm uses multiple sampling and cost evaluation strategies to guide the expansion of new nodes, and then optimizes neighborhood nodes based on the time coordination cost function so as to improve the efficiency of coordination path planning. Simulation and comparison show that HDP-TSRRT* has certain advantages in algorithm performance.

1. Introduction

Recently, with the complexity of unmanned aerial vehicle (UAV) flight environments and the diversification of mission load requirements, single UAVs with limitations of load and performance have been gradually replaced by multi-UAV cooperative systems with obvious advantages in scalability, applicability and robustness [1]. The application of multi-UAV cooperative systems in civil and military fields (e.g., geophysical survey, wide-area personnel search and rescue, high-risk target penetration and multi-target detection and strike) has received widespread attention. Multi-UAV cooperative path planning (MUCPP) is one of the most basic and important problems to realize the application of multi-UAV system [2].

This paper is devoted to the study of the MUCPP problem, which refers to planning collision-free paths from the start to the goal for multiple UAVs that satisfy the cooperative constraints [3]. As multi-UAV cooperative systems are increasingly used in more complex tasks, such as single- and multiple-target cooperative attacks or multi-source information acquisition at the same time, all UAVs need the same minimum estimated time of arrival (ETA) on the basis of meeting the requirements of path collision avoidance, so as to meet the mission requirements [4]. In other words, the path planning cooperative constraints that meet most of the mission requirements of UAVs need to be composed of both spatial and temporal constraints (i.e., time–space cooperative constraints), which makes it more difficult to deal with the MUCPP problem.

Since the configuration space is highly dimensional and the constraints are various (i.e., environmental, UAV performance and cooperative constraints), solving the MUCPP problem has become a challenging topic and has received widespread attention. In recent years, many different methods have been developed to solve this problem. These methods can be divided into two categories—reactive collision avoidance methods and active cooperative path generation methods. In reactive collision avoidance methods, each UAV only considers planning the optimal path that can avoid threats in the environment, without considering collision avoidance and time coordination with other UAVs. Then, when the risk of collision with other UAVs is detected, local re-planning is started to avoid collision [5]. Many algorithms based on reactive collision avoidance methods have been proposed, such as optimal reciprocal collision avoidance (ORCA) [6], distributed reactive collision avoidance (DRCA) [7] and some intelligent algorithms (e.g., PRIMAL₂ [8] and co-evolutionary genetic programming [9]). The reactive collision avoidance method has been widely used in practice because of its fast calculation speed [10]. However, it should be noted that such algorithms only plan the local collision avoidance path, so they may fall into the local optimal solution, meaning that the UAV cannot reach the goal. In addition, this local collision avoidance method struggles to meet the requirements of time–space constraints at the same time if the time coordination constraint is introduced.

Active cooperative path generation methods are divided into coupling methods and decoupling methods. Coupling methods usually design only one path planner for multiple UAVs. The planner includes the start and goal of all UAVs and can plan collision-free cooperative paths for all UAVs. Some coupling methods based on the reduction method obtain the cooperative path by simplifying the MUCPP problem to a problem that has been fully studied, such as linear programming (LP) [11], constraint satisfaction problem (CSP) [12], etc. In addition, some search-based coupling methods, such as the conflict-based search (CBS) [13] and incremental cost tree search (ICTS) [14], have also been proposed to transform the MUCPP problem into a global single-agent path search problem, and simultaneously generate the cooperative path of all UAVs. The coupling method can usually find the optimal path. However, with the increase in the number of collisions between UAVs and the increase in the dimensions and scope of the planning environment, the calculation time of the cooperative path obtained by the coupling method may increase exponentially. Furthermore, the coupling method has been proved to have a good effect when planning the conflict-free paths of multiple UAVs. However, if we want to deal with the MUCPP problem that meet the time–space cooperative constraints, the computational time of the algorithm will be further increased due to the introduction of time cooperative constraints. In addition, game theory-based optimal cooperative path planning has been deeply studied in recent years. The game-based particle swarm optimization (GPSO) algorithm [15] develops a hierarchical method based on the framework of game theory to extend the spherical vector-based particle swarm optimization (SPSO) algorithm [16]. Ref. [17] combines the Stag Hunt game approach with GSPO. These two algorithms can efficiently find the optimal cooperative path with the formation being maintained and have good performance. However, we are not sure whether these two algorithms can still perform well in a large number of UAV formations due to the formation constraints.

On the other hand, decoupling methods plan the path for each UAV and coordinate the planning order of all UAVs through path coordination methods to obtain the collision free path, such as hierarchical cooperative A* algorithm (HCA*) [18] and prioritized planning algorithm (PP) [19]. Among them, the PP algorithm has been widely studied recently because of its ability to transform the MUCPP problem into the single UAV path planning problem to quickly and efficiently obtain the cooperative path [20,21]. In the PP algorithm, the upper planning coordinator assigns different priorities to all UAVs and then uses the single UAV path planner to plan the path from high priority to low priority. Although the classical PP algorithm can obtain the cooperative path quickly, the centralized framework of PP can be further improved to raise the computational efficiency. The authors of [22,23] proposed an asynchronous decentralized prioritized planning algorithm (ADRPP), which can make reasonable use of distributed computing resources to achieve faster algorithm convergence. Compared with the centralized prioritized planning algorithm (i.e., classical PP algorithm), the asynchronous decentralized prioritized planning algorithm (ADRPP) proposed in the literature [22,23] can usually complete the generation of cooperative paths with fewer cycles. In addition, compared with the synchronous decentralized prioritized planning algorithm (SDRPP) [24], ADRPP can start the next round of planning without waiting for all UAVs to complete the planning, so it has higher planning efficiency. However, ADRPP algorithm does not provide a prioritization approach that can improve efficiency, so there is some room for improvement. However, the ADRPP and SDRPP algorithm does not provide a prioritization approach that can improve efficiency, so there is some room for improvement. In addition, none of the decoupling methods have been used in in-depth research on the cooperative problem of multiple unmanned aerial vehicles simultaneously. However, after transforming the cooperative path planning problem of multiple UAVs into a single path planning problem, the realization of time–space cooperative constraints is relatively easy and has little impact on the computational efficiency. In addition, decoupling methods have not been used in in-depth research on the MUCPP problem that meets the time–space cooperative constraints. However, it should be noted that after transforming the MUCPP problem into a single-UAV path planning problem, the realization of time–space cooperative constraints is relatively easy, and the computational efficiency is relatively high.

1.1. Motivation

It can be seen from the analysis that the above algorithms still have some room and possibility for improvement:

• In order to meet the needs of most cooperative tasks of multiple UAVs, it is necessary to introduce the time–space cooperative constraints to deal with the MUCPP. The above algorithms focus more on spatial conflict avoidance but do not consider spatial and temporal cooperative constraints at the same time.
• The reactive collision avoidance methods mainly focus on conflict avoidance in local areas and struggle to achieve time coordination. The coupling method adds the time coordination constraint to the path planner of multiple UAVs, which will further increase the calculation time and reduce the calculation efficiency. The decoupling method can simplify the MUCPP problem to the single-UAV path planning problem. Compared with the coupling method, the decoupling method has the possibility of introducing time-space coordination constraints by adding less computation.
• In the PP algorithm, if the random prioritization method is adopted, or only the collision number based on the space cooperative constraint is considered for prioritization, the calculation efficiency of the algorithm will be reduced, and the calculation time will be increased.
• The existing single-UAV path planning algorithms usually take the shortest path as the optimization standard. Additionally, the RRT* algorithm has been proved to be able to quickly obtain the optimal solution in high-dimensional planning space [25]. However, the existing RRT* algorithm or its variant algorithms cannot meet the time–space coordination constraints of UAVs.

Therefore, in combination with the above motivations, this paper introduces the time–space cooperative constraints into the MUCPP problem so as to meet the needs of most multi-UAV systems to perform tasks. Meanwhile, the decentralized priority planning algorithm based on a decoupling method is further improved by combining the prioritization strategy suitable for time–space cooperative constraints. Furthermore, the RRT* algorithm is improved in the underlying single UAV planner to meet the time–space cooperative constraint, so that the path can be quickly planned for each UAV.

1.2. Contributions

In this paper, we propose the HDP-TSRRT* algorithm to solve the MUCPP problem based on time–space cooperative constraints. HDP-TSRRT* is a decoupled active cooperative path generation algorithm and adopts a hierarchical architecture. First, HDP-TSRRT* uses a synchronous decentralized architecture based on the RRT* algorithm to quickly plan the optimal pre-planning path for each UAV without considering cooperative constraints, which is the foundation for cooperative planning. Second, in the path coordination level, the heuristic decentralized prioritized planning algorithm (HDP) is proposed to coordinate the path planning priorities of the multi-UAV. The algorithm ranks the priority of UAVs by combining the number of potential collisions and the violation of the cooperative time into a performance evaluation function, which makes the priority allocation more reasonable and can improve the efficiency of cooperative planning. The algorithm also uses the asynchronous decentralized prioritized planning framework to coordinate the path planning of multiple UAVs to further improve the computational efficiency. Third, the RRT* algorithm based on time–space cooperative constraints (TSRRT*) is proposed as the execution algorithm of the underlying single-UAV cooperative path planner. The TSRRT* algorithm designs a cost function based on time constraints to gradually optimize the path. In the TSRRT* algorithm, the strategy of multiple sampling and cost function evaluation of optimal sampling proposed in this paper is used to expand the new node, and the time-constrained cost function is used to update the neighborhood nodes to quickly plan the path that meets the time–space cooperative constraint for each UAV.

Compared with the existing cooperative path planning algorithm for multi-UAV, the HDP-TSRRT* proposed in this paper mainly includes the following contributions:

• A decoupled UAV cooperative path planning hierarchical framework is designed, which is composed of a synchronous decentralized pre-planner, an asynchronous decentralized prioritized planning path coordinator based on heuristic prioritization, and a single-UAV cooperative path planner based on time–space coordination constraint RRT*, to quickly plan a UAV’s time–space cooperative path.
• In the path coordinator, a heuristic prioritization approach based on time–space coordination constraints is proposed to sort the UAVs, so as to ensure the rationality of the prioritization and reduce the calculation time of path coordination.
• A cost function based on time coordination constraints is established to optimize the planned path of UAVs so as to ensure that the single-UAV cooperative path planner can find the path satisfying the time coordination constraint.
• In the process of sampling and new node expansion of the single-UAV cooperative path planner, a multi-sampling and cost function evaluation strategy is proposed to expand the new node to improve the efficiency of cooperative path planning.
• In the tree structure generation of the single-UAV cooperative path planner, a neighborhood node update method based on the time coordination cost function is proposed to guide the path generation meeting the time–space coordination constraints.

The rest of this paper is organized as follows: In Section 2, we introduce the related works; Section 3 describes the MUCPP problems based on the time–space coordination constraints; Section 4 introduces the HDP-TSRRT* algorithm in three aspects; Section 5 provides us with the simulation of the proposed algorithm and discusses the results; and Section 6 describes some conclusions and future work.

2. Related Works

This section provides a brief introduction to the classical PP algorithm and the classical RRT* algorithm.

2.1. Prioritized Planning Algorithm

PP is an algorithm that can quickly solve the cooperative path planning problems, which was first proposed in [26]. Algorithm 1 gives the framework of classical PP. The classical PP algorithm assigns different priorities to each robot. Then, the path planning of each robot is carried out from the highest priority 1 to the lowest priority $n$. During $ith$ iteration, PP algorithm computes the path ${\sigma }_i$ of the robot $i$ that not only avoids the threats in the environment, but also the time–space occupied area set $H_i$ occupied by robots $1,\dots ,i-1$. The path of robot $i$ is computed in function ${A}\ast \left(\mathcal{X},H_i\right)$, which returns a path for robot $i$ such that the robot avoids not only the threats in the environment, but also the paths that high-priority robots take. If the required path cannot be planned, the algorithm stops. Otherwise, the algorithm will continue to be executed until the robot with the lowest priority completes the planning.

Algorithm 1: Classical Prioritized Planning Algorithm

$\begin{array}{l}01: \mathcal{X}:\mathrm{Environment}; H_i\leftarrow \varnothing \\ 02: for i=1\dots n\\ 03: \hspace{1em}\hspace{1em}{\sigma }_i\leftarrow {A}\ast \left(\mathcal{X},H_i\right)\\ 04: \hspace{1em}\hspace{1em}if {\sigma }_i=\varnothing \\ 05: \hspace{1em}\hspace{1em}\hspace{1em}\mathrm{report} \mathrm{failure} \mathrm{and} \mathrm{terminate}\\ 06: \hspace{1em}\hspace{1em}end if\\ 07: \hspace{1em}\hspace{1em}{H}_i\leftarrow H_i\cup {\sigma }_i\left(t\right)\\ 08: endfor\end{array}$

In the classic PP, the A* algorithm [27] is used to complete the cooperative path planning of a single UAV. However, as a grid-based search algorithm, the planning time of A* usually increases exponentially with the increase in spatial dimensions and is rarely used in UAV path planning in large three-dimensional environments. Meanwhile, some local obstacle avoidance planning algorithms, such as artificial potential field (APF) [28] and interfered fluid dynamic system (IFDS [29]), may encounter the problem of local minima. Intelligent algorithms, such as genetic algorithm (GA) [30] and ant colony optimization (ACO) [31] algorithm, may only find sub-optimal solutions when there are complex or many constraints. In addition, the classic PP algorithm does not specify the approach of prioritization. However, the prioritization approach is one of the important factors affecting the quality and efficiency of the coordination path. Therefore, the design of prioritization is particularly important while improving the PP algorithm framework.

2.2. RRT* Algorithm

On the basis of the RRT algorithm [32], which can only guarantee the probability completeness, Karaman and Frazzoli proposed the RRT* algorithm [33], which introduced the neighborhood node selection mechanism, thus ensuring both the probability completeness and the asymptotic optimality. The framework of the goal-guided RRT* algorithm is shown in Algorithm 2. During $ith$ iteration of tree growth, RRT * algorithm generates sampling node $x_{rand}$ through the goal node bias strategy (i.e., the probability number $\mathrm{P}$ is randomly generated between 0 and 1. If $\mathrm{P}>0.1$, the $x_{rand}$ is generated by random sampling; otherwise, the $x_{rand}$ is set as the goal $g_i$). Then, the algorithm selects the node $x_{nearest}$ nearest to $x_{rand}$ in the tree through function $\mathrm{Nearest}$. After that, the new node $x_{new}$ is obtained by extending a certain distance along the $\overrightarrow{x_{nearest}{x}_{rand}}$ through the function $\mathrm{Steer}$. If edge $\left(x_{new},x_{nearest}\right)$ meets the collision checking constraints (i.e., $\mathrm{obstacle}\_free$), the node $x_{new}$ will be added to the tree $\mathcal{T}$. Then, the function ${Near}$ $x_{new}$ as the center and $\gamma$ as the radius construct the neighborhood $x_{new}$, which selects the neighborhood node set $X_{near}$. Then, the parent node $x_{new}.parent$ with the lowest cost of $x_{new}$ is re-selected among all neighbor nodes $x_{near}$. Then, compare the cost between $x_{new}$ as the potential parent node of all neighborhood nodes and the current parent node of neighborhood nodes. If the cost becomes low, the parent node of $x_{near}$ is updated to $x_{new}$. The algorithm continues to iterate to expand the tree structure until a tree node falls into the goal region, and the path can be found retroactively.

Algorithm 2: RRT* Algorithm

$\begin{array}{l}01:initialization \mathrm{Tree} \mathrm{node}:\mathcal{T}.\mathrm{V}\leftarrow \left\{s_i\right\}; \mathrm{Tree} \mathrm{edge}: \mathcal{T}.\mathrm{E}\leftarrow \varnothing \\ 02:for i=1 to n\\ 03: \hspace{1em}\hspace{1em}if \mathrm{P}:\mathrm{random}\left(\left[0,1\right]\right)<0.1\\ 04: \hspace{1em}\hspace{1em}\hspace{1em}{x}_{rand}\leftarrow g_i\\ 05: \hspace{1em}\hspace{1em}else\\ 06: \hspace{1em}\hspace{1em}\hspace{1em}{x}_{rand}\leftarrow {Sample}\left(\mathcal{X}\right)\\ 07: \hspace{1em}\hspace{1em}end if\\ 08: \hspace{1em}\hspace{1em}{x}_{nearest}\leftarrow \mathrm{Nearest}\left({\mathrm{x}}_{rand}\right)\\ 09: \hspace{1em}\hspace{1em}{x}_{new}\leftarrow \mathrm{Steer}\left(x_{nearest}\right)\\ 10: \hspace{1em}\hspace{1em}if \mathrm{obstacle}\_\mathrm{free}\left(x_{new},x_{nearest}\right)\\ 11: \hspace{1em}\hspace{1em}\hspace{1em}\mathcal{T}.\mathrm{V}\leftarrow \mathcal{T}.\mathrm{V}\cup \left\{x_{new}\right\}\\ 12: \hspace{1em}\hspace{1em}end if\\ 13: \hspace{1em}\hspace{1em}{X}_{near}\leftarrow \mathrm{Near}\left(x_{new},\mathcal{T}.\mathrm{V},\gamma \right)\\ 14: \hspace{1em}\hspace{1em}for \mathrm{all} x_{near}\in X_{near}\\ 15: \hspace{1em}\hspace{1em}\hspace{1em}\hspace{1em}if \mathrm{obstacle}\_\mathrm{free}\left(x_{near},x_{new}\right)\wedge \\ \hspace{1em}\hspace{1em}\hspace{1em}{\mathrm{Cost}}_{\mathcal{T}}\left(s_i,x_{near}\right)+{\mathrm{Cost}}_{\mathcal{T}}\left(x_{near},x_{new}\right)<{\mathrm{Cost}}_{\mathcal{T}}\left(s_i,x_{new}\right)\\ 16: \hspace{1em}\hspace{1em}\hspace{1em}\hspace{1em}{x}_{new}.parent\leftarrow x_{near}\\ 17: \hspace{1em}\hspace{1em}\hspace{1em}\hspace{1em}end if\\ 18: \hspace{1em}\hspace{1em}end for\\ 19: \hspace{1em}\hspace{1em}for \mathrm{all} x_{near}\in X_{near}\\ 20: \hspace{1em}\hspace{1em}\hspace{1em}\hspace{1em}if \mathrm{obstacle}\_\mathrm{free}\left(x_{new},x_{near}\right)\wedge \\ \hspace{1em}\hspace{1em}\hspace{1em}{\mathrm{Cost}}_{\mathcal{T}}\left(s_i,x_{new}\right)+{\mathrm{Cost}}_{\mathcal{T}}\left(x_{new},x_{near}\right)<{\mathrm{Cost}}_{\mathcal{T}}\left(s_i,x_{near}\right)\\ 21: \hspace{1em}\hspace{1em}\hspace{1em}\hspace{1em}{x}_{near}.parent\leftarrow x_{new}\\ 22: \hspace{1em}\hspace{1em}\hspace{1em}\hspace{1em}end if\\ 23: \hspace{1em}\hspace{1em}end for\\ 24: end for\end{array}$

The RRT* algorithm is widely used in the path planning of single UAVs due to its advantages of fast solutions in highly dimensional spaces. However, for the MUCPP problem, it is insufficient to rely only on RRT*, and the relevant research is relatively small. It is noted that the RRT* can introduce space cooperative constraints into the collision checking process, thus highly combining with the PP algorithm. Therefore, there are many possibilities and advantages to improve the RRT* to find the single-UAV cooperative path, which can meet the time–space cooperative path planning needs and effectively improve the efficiency of planning.

Therefore, this paper mainly studies the decoupling cooperative path planning approach based on PP and RRT*.

3. Problem Formulation

Consider the UAVs group ${\mathrm{UAV}}_N:\left({\mathrm{UAV}}_1,\dots ,{\mathrm{UAV}}_n\right)$ composed of $n$ UAVs flying in the planned space $\mathcal{X}\subseteq {\mathbb{R}}^3$ composed of a three-dimensional battlefield environment. In a cooperative flight, all UAVs in ${\mathrm{UAV}}_N$ take off at the same time and reach the goal at the same time. Any ${\mathrm{UAV}}_i$ needs to start flying from its start node $s_i$ and avoid all threats, including radar, anti-aircraft gun, missile and terrain in flight (the model of the threats is described in our previous work [34]), as well as the remaining ${\mathrm{UAV}}_{N/i}:{\mathrm{UAV}}_N\backslash {\mathrm{UAV}}_i$, and finally reach its goal node $g_i$.

Since the start node or goal node of multiple UAVs may overlap, we assume that when different UAVs are at the same start node or goal node, their positions do not coincide, and there will be no conflict. In addition, we assume that each UAV in the group has an independent path planner and wireless communication equipment. Meanwhile, we assume that the communication equipment is completely reliable and has no delay.

3.1. Cooperative Path Planning Constraints of UAVs

The related constraints of UAV cooperative path planning are mainly divided into three categories, namely UAV performance constraints, planning space constraints and multi-UAV cooperative constraints. Firstly, the performance constraints of UAV are defined as follows.

• UAV performance constraints

Assume that the planned path ${\sigma }_i$ of ${\mathrm{UAV}}_i$ consists of a set of path node $P:\left\{\langle p_1,\dots ,p_k\rangle |p_k=\left(x_k,y_k,z_k\right)\right\}$. When planning the flight path of the UAV, it is necessary to take into account the maximum flight distance $L_{\max }$, the maximum steering angle ${\varphi }_{\max }$ and the climbing/diving angle ${\gamma }_{\max }$, as well as the shortest flight distance before steering $l_{\min }$. Then, the performance constraints of ${\mathrm{UAV}}_i$ can be expressed as [35]:

$$\{\begin{array}{l}{L}_{UA{V}_i}={\displaystyle \sum _{i=0}^k\sqrt{{(x_{i+1}-x_i)}^2+{(y_{i+1}-y_i)}^2+{(z_{i+1}-z_i)}^2}}\le L_{\max }\\ \left|{\varphi }_{UA{V}_i}\right|=\left|\frac{{\omega }_i^T{\omega }_{i+1}}{{\Vert {\omega }_i\Vert }_2\cdot {\Vert {\omega }_{i+1}\Vert }_2}\right|\le {\varphi }_{\max }\\ \left|{\gamma }_{UA{V}_i}\right|=\left|\arcsin (\frac{z_i-z_{i-1}}{{\Vert {\omega }_i\Vert }_2})\right|\le {\gamma }_{\max }\\ l_{{}_{UA{V}_i}}={\Vert p_{i+1}-p_i\Vert }_2\ge l_{\min }\end{array}$$

(1)

where ${\omega }_i={[x_i-x_{i-1},y_i-y_{i-1}]}^T$.

2.

Planning space constraints

The threat types set in the planning space include terrain threats, which is built by digital elevation model map (DEM, i.e., the digital expression of terrain surface morphology), radar, anti-aircraft guns and missiles. Then, the constraints of the planning space on UAVs can be expressed as:

$$\{\begin{array}{l}{z}_i-z_{{\left(x_i,y_i\right)}_{DMG}}\ge h_{\min }\\ \left(p_i,p_{i+1}\right)\cap {\mathcal{X}}_{obs}=\varnothing \end{array}$$

(2)

where $z_{{\left(x_i,y_i\right)}_{DMG}}$ represents the terrain threat level when the UAV is at $p_i$, and ${\mathcal{X}}_{obs}$ represents other threats.

3.

Multi-UAV cooperative constraints

In this paper, we mainly study the MUCPP problem that simultaneously satisfies the time–space coordination constraints. We first describe the space coordination constraint.

If any two UAVs in ${\mathrm{UAV}}_N$ can maintain a certain safety distance $D_{\min }$ at any time in flight, then these two UAVs meet the space coordination constraint. Therefore, the space coordination constraint can be expressed as:

$${\Vert {\sigma }_i\left(t\right)-{\sigma }_j\left(t\right)\Vert }_2\ge D_{\min } \mathrm{i},\mathrm{j}=1,2,\dots ,k,i\ne j$$

(3)

where ${\sigma }_i\left(t\right)$ and ${\sigma }_j\left(t\right)$, respectively, represent the positions of ${\mathrm{UAV}}_{\mathrm{i}}$ and ${\mathrm{UAV}}_j$ at any time $t$. In this paper, we assume that all UAVs pass through the planning space at the same velocity. The path of ${\mathrm{UAV}}_{\mathrm{i}}$ is discretized according to the time information to obtain a group of discrete path nodes. In order to determine the safe distance between UAVs more accurately, this paper sets the time interval between two adjacent discrete path nodes of ${\mathrm{UAV}}_{\mathrm{i}}$ as 0.01s. The potential collision between ${\mathrm{UAV}}_{\mathrm{i}}$ and ${\mathrm{UAV}}_j$ is detected by comparing the distance between two path nodes (i.e., ${\Vert {\sigma }_i\left(t\right)-{\sigma }_j\left(t\right)\Vert }_2$) and the minimum safety distance $D_{\min }$ at any same time $t$.

Meanwhile, in order to meet the mission requirements, UAVs need to take off at the same time and reach their respective goal at the same time to achieve time coordination. In this paper, we set all UAVs to fly at a constant speed and have the same speed range $\left[{\mathcal{V}}_{\min },{\mathcal{V}}_{\max }\right]$—that is, the flight time of any UAV is $\left[t_{\min }^i={\sigma }_i/{\mathcal{V}}_{\max },t_{\max }^i={\sigma }_i/{\mathcal{V}}_{\max }\right]$. The time coordination constraint that the ${\mathrm{UAV}}_N$ needs to meet is expressed as:

$${\displaystyle \underset{i=1}{\overset{n}{\cap }}\left[t_{\min }^i,t_{\max }^i\right]}\ne \varnothing$$

(4)

3.2. Cooperative Path Planning Formulation

When dealing with the MUCPP problem, not only the performance constraints and planning space constraints of a single UAV should be met, but also the coordination constraints of all UAVs. Firstly, the description of satisfactory path problem of ${\mathrm{UAV}}_{\mathrm{i}}$ is given.

Problem1.

(Satisfying Path Planning of ${\mathrm{UAV}}_{\mathrm{i}}$). In the planning space $\mathcal{X}$, for any ${\mathrm{UAV}}_{\mathrm{i}}$ in the ${\mathrm{UAV}}_N$, a satisfactory path ${\sigma }_i$ is planned for the given start-goal set $\left[s_i,g_i\right]$ so that ${\sigma }_i$ meets the constraint Equations (1) and (2).

If the satisfactory path ${\sigma }_i$ and the satisfactory path ${\sigma }_j$ of any ${\mathrm{UAV}}_j$ in ${\mathrm{UAV}}_{N/i}$ meet the space coordination constraint (i.e., Equation (3)), ${\sigma }_i$ and ${\sigma }_j$ are said to be collision-free. If ${\sigma }_i$ and ${\sigma }_j$ meet the time coordination constraint (i.e., Equation (4)), C ${\sigma }_i$ and ${\sigma }_j$ are said to be time-consistent. Then, Problem 2 can be described as follows.

Problem2.

(MUCPP Problem based on time–space coordination constraints). Given the planning space $\mathcal{X}$, and the start-goal set $\left[s_1,g_1\right],\dots ,\left[s_n,g_n\right]$ of ${\mathrm{UAV}}_N$, find the satisfactory path ${\sigma }_1,\dots ,{\sigma }_n$ of all UAVs, and the ${\sigma }_i$ and ${\sigma }_j$ is collision-free and time-consistent.

4. HDP-TSRRT* Algorithm

In this paper, the HDP-TSRRT* algorithm is proposed to quickly plan the path satisfying the time–space cooperative constraint of multi-UAV. As a decoupled path coordination algorithm, the algorithm realizes the path planning of a single UAV and the path coordination of multiple UAVs in a hierarchical manner. On the basis of finding the pre-planned path, the heuristic prioritization-based asynchronous decentralized prioritized planning algorithm (HDP) is used to allocate the cooperative path planning order of all UAVs. Then, the cooperative path planning of each UAV from high priority to low priority is completed by using the proposed TSRRT* algorithm so that the path can meet the time–space constraints at the same time. The HDP-TSRRT* algorithm will be introduced in detail through the overall framework of the HDP-TSRRT* algorithm, the HDP algorithm and the TSRRT* algorithm in the following subsections.

4.1. Overall Framework

The algorithm is mainly divided into three levels, i.e., the pre-planning level, the planning coordination level and the single-UAV cooperative path planning level. The pseudocode of HDP-TSRRT* is shown in Algorithm 3.

Algorithm 3: HDP-TSRRT* Algorithm

$\begin{array}{l}1: initialization {\mathcal{T}}_i,{\mathcal{T}}^i\leftarrow \varnothing , {\sigma }_i,{\sigma }^i\leftarrow \varnothing , \left[s_1,g_1\right],\dots ,\left[s_n,g_n\right],H_i\leftarrow \varnothing \\ /\ast \mathit{p}\mathit{r}\mathit{e}\mathit{-}\mathit{p}\mathit{l}\mathit{a}\mathit{n}\mathit{n}\mathit{i}\mathit{n}\mathit{g} \mathit{l}\mathit{e}\mathit{v}\mathit{e}\mathit{l}\ast /\\ 2: {\sigma }_i\leftarrow \mathrm{RRT}\ast \left(\mathcal{X},{\mathcal{T}}_i,s_i,g_i\right)/\ast \mathit{S}\mathit{y}\mathit{n}\mathit{c}\mathit{h}\mathit{r}\mathit{o}\mathit{n}\mathit{o}\mathit{u}\mathit{s} \mathit{e}\mathit{x}\mathit{e}\mathit{c}\mathit{u}\mathit{t}\mathit{i}\mathit{o}\mathit{n} \mathit{o}\mathit{f} \mathit{a}\mathit{l}\mathit{l} \mathit{U}\mathit{A}\mathit{V}\mathit{s}\ast /\\ 3: cos{t}_i\leftarrow \mathrm{Length}\left({\sigma }_i\right)\\ 4: \mathrm{wait} \mathrm{for} \mathrm{all} \mathrm{UAVs} \mathrm{to} \mathrm{finish} \mathrm{planning} \mathrm{their} \mathrm{own} {\mathsf{\sigma }}_{\mathrm{i}}\mathrm{and}{\mathrm{cost}}_{\mathrm{i}}\\ 5: \mathit{\sigma }\leftarrow \left({\sigma }_1,\dots ,{\sigma }_n\right)\\ 6: t\leftarrow \left(cos{t}_1/v_1,\dots ,cos{t}_n/v_{\mathrm{n}}\right)\\ 7: t_{co}\leftarrow \max t\\ 8: {\sigma }^1\leftarrow \arg \max \left(\mathrm{Length}\left(\mathit{\sigma }\right)/v\right)\\ /\ast \mathit{p}\mathit{l}\mathit{a}\mathit{n}\mathit{n}\mathit{i}\mathit{n}\mathit{g} \mathit{c}\mathit{o}\mathit{o}\mathit{r}\mathit{d}\mathit{i}\mathit{n}\mathit{a}\mathit{t}\mathit{i}\mathit{o}\mathit{n} \mathit{l}\mathit{e}\mathit{v}\mathit{e}\mathit{l} \& \mathit{s}\mathit{i}\mathit{n}\mathit{g}\mathit{l}\mathit{e}-\mathit{U}\mathit{A}\mathit{V} \mathit{c}\mathit{o}\mathit{o}\mathit{p}\mathit{e}\mathit{r}\mathit{a}\mathit{t}\mathit{i}\mathit{v}\mathit{e} \mathit{p}\mathit{a}\mathit{t}\mathit{h} \mathit{p}\mathit{l}\mathit{a}\mathit{n}\mathit{n}\mathit{i}\mathit{n}\mathit{g} \mathit{l}\mathit{e}\mathit{v}\mathit{e}\mathit{l}\ast /\\ 9: \langle UA{V}^1,\dots ,UA{V}^N\rangle \leftarrow \mathrm{Heu}\_Prioritization\left(\sigma \right)\\ 10: H_i\leftarrow \left(H_i\cup \left(UA{V}^1,{\sigma }^1\left(t\right)\right)\right)\\ 11: {\sigma }^i\leftarrow \mathrm{TSRRT}\ast \left(\mathcal{X}\backslash H_i,{\mathcal{T}}^i,s_i,g_i,t_{co}\right) /\ast \mathit{s}\mathit{i}\mathit{n}\mathit{g}\mathit{l}\mathit{e}-\mathit{U}\mathit{A}\mathit{V} \mathit{c}\mathit{o}\mathit{o}\mathit{p}\mathit{e}\mathit{r}\mathit{a}\mathit{t}\mathit{i}\mathit{v}\mathit{e}\\ \hspace{1em}\hspace{1em}\hspace{1em}\hspace{1em}\hspace{1em}\hspace{1em}\hspace{1em}\hspace{1em}\hspace{1em}\hspace{1em}\hspace{1em}\hspace{1em}\hspace{1em}\hspace{1em}\mathit{p}\mathit{a}\mathit{t}\mathit{h} \mathit{p}\mathit{l}\mathit{a}\mathit{n}\mathit{n}\mathit{i}\mathit{n}\mathit{g} \mathit{l}\mathit{e}\mathit{v}\mathit{e}\mathit{l}\ast /\\ 12: \mathrm{communicate} \mathrm{Coor}\_\mathrm{message}\left({\mathrm{UAV}}^i,{\sigma }^i\left(t\right)\right)\\ 13: \mathrm{wait} \mathrm{for}\mathrm{receiving} \mathrm{Coor}\_\mathrm{message} \mathrm{of} \mathrm{other} \mathrm{UAVs}\\ 14: while \mathrm{receiving} \mathrm{Coor}\_\mathrm{message}\left({\mathrm{UAV}}^k,{\sigma }^k\left(t\right)\right)\\ 15: \hspace{1em}\hspace{1em}if k<i\\ 16: \hspace{1em}\hspace{1em}\hspace{1em}{H}_i\leftarrow \left(\left\{H_i\backslash \left({\mathrm{UAV}}^k,{\sigma }^{k\prime }\left(t\right)\right)\right\}\cup \left({\mathrm{UAV}}^k,{\sigma }^k\left(t\right)\right)\right)\\ 17: \hspace{1em}\hspace{1em}\hspace{1em}{\sigma }^i{}^{\ast }\leftarrow \mathrm{TSRRT}\ast \left(\mathcal{X}\backslash H_i,{\mathcal{T}}^{i\ast },s_i,g_i,t_{co}\right) /\ast \mathit{s}\mathit{i}\mathit{n}\mathit{g}\mathit{l}\mathit{e}-\mathit{U}\mathit{A}\mathit{V} \mathit{c}\mathit{o}\mathit{o}\mathit{p}\mathit{e}\mathit{r}\mathit{a}\mathit{t}\mathit{i}\mathit{v}\mathit{e}\\ \hspace{1em}\hspace{1em}\hspace{1em}\hspace{1em}\hspace{1em}\hspace{1em}\hspace{1em}\hspace{1em}\hspace{1em}\hspace{1em}\hspace{1em}\hspace{1em}\hspace{1em}\hspace{1em}\mathit{p}\mathit{a}\mathit{t}\mathit{h} \mathit{p}\mathit{l}\mathit{a}\mathit{n}\mathit{n}\mathit{i}\mathit{n}\mathit{g} \mathit{l}\mathit{e}\mathit{v}\mathit{e}\mathit{l}\ast /\\ 18: \hspace{1em}\hspace{1em}if {\sigma }^i{}^{\ast }\ne {\sigma }^i\\ 19: \hspace{1em}\hspace{1em}\hspace{1em}\hspace{1em}{\sigma }^i\leftarrow {\sigma }^i{}^{\ast }\\ 20: \hspace{1em}\hspace{1em}\hspace{1em}\hspace{1em}communicate \mathrm{Coor}\_message\left({\mathrm{UAV}}^i,{\sigma }^i\left(t\right)\right)\\ 21: \hspace{1em}\hspace{1em}\hspace{1em}else\\ 22: \hspace{1em}\hspace{1em}\hspace{1em}\hspace{1em}{\sigma }^i\leftarrow {\sigma }^i\\ 23: \hspace{1em}\hspace{1em}\hspace{1em}\hspace{1em}\mathrm{keep} \mathrm{silent}\\ 24: \hspace{1em}\hspace{1em}\hspace{1em}end if\\ 25: \hspace{1em}\hspace{1em}end if\\ 26: \hspace{1em}\hspace{1em}if {\sigma }^1,\dots ,{\sigma }^n \mathrm{are} \mathit{c}\mathit{o}\mathit{l}\mathit{l}\mathit{i}\mathit{s}\mathit{i}\mathit{o}\mathit{n}-\mathit{f}\mathit{r}\mathit{e}\mathit{e} \mathrm{and} \mathit{t}\mathit{i}\mathit{m}\mathit{e}-\mathit{c}\mathit{o}\mathit{n}\mathit{s}\mathit{i}\mathit{t}\mathit{e}\mathit{n}\mathit{t}\\ 27: \hspace{1em}\hspace{1em}\hspace{1em}\mathrm{terminate} \mathrm{HDP}-TSRRT\ast \\ 28: \hspace{1em}\hspace{1em}else\\ 29: \hspace{1em}\hspace{1em}\hspace{1em}\mathrm{continue}\\ 30: \hspace{1em}\hspace{1em}end if\\ 31: end while\end{array}$

The definitions of some symbols appearing in Algorithm 3 are as follows. ${\mathcal{T}}_i$ is the tree structure of any $UA{V}_i$. ${\mathcal{T}}^i$ is the tree structure of $UA{V}^i$ with priority of $i$. ${\sigma }_i$ is the current path of any $UA{V}_i$. ${\sigma }^i$ is the current path of $UA{V}^i$ with priority of $i$. $H_i$ is the time–space occupied area set occupied by UAVs $1,\dots ,i-1$. $cos{t}_i$ is the length of the path ${\sigma }_i$, which is calculated by function $\mathrm{Length}$. $\mathit{\sigma }$ and $t$ are the path set and ETA set of UAVs, respectively. $t_{co}$ the minimum ETA of the UAVs group. $\langle UA{V}^1,\dots ,UA{V}^N\rangle$ represents the priority ranking set of all UAVs obtained through the function $\mathrm{Heu}\_Prioritization\left(\sigma \right)$. The function $\mathrm{Coor}\_message$ contains the path information of the $UA{V}^k$ with priority $k$. $\mathit{o}\mathit{b}\mathit{s}\mathit{t}\mathit{a}\mathit{c}\mathit{l}\mathit{e}-\mathit{f}\mathit{r}\mathit{e}\mathit{e}$ and $\mathit{t}\mathit{i}\mathit{m}\mathit{e}-\mathit{c}\mathit{o}\mathit{n}\mathit{s}\mathit{i}\mathit{t}\mathit{e}\mathit{n}\mathit{t}$ are defined in Equations (3) and (4).

Figure 1 shows the overall framework of HDP-TSRRT * algorithm. As shown in Figure 1, the HDP-TSRRT * algorithm can be divided into three levels.

• The pre-planning level

This level mainly includes the synchronous decentralized pre-planning process, which plans the initial optimal path for each UAV to obtain the shortest ETA and the initial parameters of the HDP algorithm. In this part, the UAVs do not need to consider the time–space coordination constraints when planning the path, and instead, only consider the performance constraints and planning space constraints of UAVs and use the RRT* algorithm (i.e., Algorithm 1) for optimal path planning. After each UAV obtains its optimal path, the path length is obtained through the $\mathrm{Length}\left({\sigma }_i\right)$ function, and then the flight time set $t:\left(t_1,\dots ,t_n\right)$ of all UAVs is obtained. It should be noted that all UAVs use the RRT* algorithm for optimal path planning, which means that all UAVs obtain the shortest flight time. However, for the MUCPP based on time–space coordination constraints, if the shortest flight time $t_{j,\min }$ of ${\mathrm{UAV}}_j$ is less than the shortest flight time $t_{i,\min }$ of the ${\mathrm{UAV}}_i$, only by increasing the $t_{j,\min }$ to make it the same as the $t_{i,\min }$ can the two UAVs achieve time coordination. Therefore, it is necessary to take the maximum time in the flight time set $t$ as the ETA, $t_{co}$, and set the UAV mapped by $t_{co}$ as the UAV with the highest priority to avoid repeated planning of the same path.

2.

The planning coordination level

This level mainly includes the coordination process of multi-UAV path planning priorities, which uses the proposed HDP algorithm to improve the efficiency of path coordination. First, the priority assignment process from 2 to $n$ of all UAVs based on the heuristic prioritization approach is completed through the $\mathrm{Heu}\_\mathrm{Prioritization}\left(\mathit{\sigma }\right)$ function. Since the cooperative constraints of UAVs are divided into two aspects, space and time, a priority performance evaluation function composed of potential collision times and cooperative time violations is designed in the algorithm. After $\mathrm{Heu}\_\mathrm{Prioritization}\left(\mathit{\sigma }\right)$ outputs the priority order of all UAVs, the asynchronous decentralized prioritized planning is used to coordinate the generation of time–space cooperative paths. In the asynchronous decentralized prioritized planning, each low-priority UAV needs to avoid the time–space occupied area set $H_i=\left({\mathrm{UAV}}^i,{\sigma }^i\left(t\right)\right)$ occupied by the high-priority UAV. In other words, the time–space area set of low-priority UAVs should contain high-priority paths to avoid collision with them.

3.

The single-UAV cooperative path planning level

This level mainly includes the process of cooperative path planning for a single UAV, which is realized by the TSRRT* algorithm. Under the framework of the HDP algorithm, UAVs regard the time–space occupied area occupied by other UAVs as threats and use the TSRRT* to plan the path satisfying the time–space coordination constraint for each UAV. The TSRRT* algorithm evaluates the generated path through the cost function based on the time coordination constraint so that the planned path can meet the time coordination constraint on the basis of no collisions. In addition, we effectively reduce the computational loss through the proposed multiple sampling and cost function evaluation strategy.

4.2. HDP Algorithm

This section describes the HDP algorithm from two aspects: the heuristic prioritization approach and the framework of the HDP algorithm.

4.2.1. Heuristic Prioritization Approach

The priority level of a UAV affects the robustness and efficiency of cooperative path generation. When the prioritized planning algorithm is used for the path coordination of multiple UAVs, the inappropriate priority order will not only reduce the efficiency of cooperative path planning but also cause the failure of cooperative path planning. To solve this problem, the heuristic prioritization approach is proposed in this paper.

Since the algorithm proposed in this paper is mainly used for path planning of multiple UAVs that satisfy the time-space coordination constraints, the design of heuristic prioritization approach should take into account the impact of both time and space coordination constraints. For the time coordination constraint, if the pre-planned path of ${\mathrm{UAV}}_i$ is significantly different from the minimum ETA time to be met, it is obvious that the cooperative re-planning path of the ${\mathrm{UAV}}_i$ will also change more. Furthermore, because the re-planning path of the ${\mathrm{UAV}}_i$ changes greatly, the change of its space and time occupied area set will be greater, and the impact on other UAVs will be greater. Therefore, we need to set the priority of the ${\mathrm{UAV}}_i$ higher, so that the ${\mathrm{UAV}}_i$ can carry out cooperative re-planning before other UAVs to reduce the impact on other UAVs. For the space coordination constraints, which are similar to the time coordination constraints, if the number of potential collisions between the pre-planned paths of ${\mathrm{UAV}}_i$ and other UAVs is more, the path of the ${\mathrm{UAV}}_i$ will change more during the collaborative re-planning, which means that more other UAVs will be affected. Therefore, if the priority of the ${\mathrm{UAV}}_i$ is assigned higher, the less other UAVs will be affected by its re-planning. It can be seen from the analysis that the heuristic prioritization approach based on time–space coordination constraints can improve the planning efficiency.

In the heuristic prioritization approach, we designed a performance evaluation function consisting of the number of potential collisions and the amount of cooperative time violations of the pre-planned path to assign reasonable priority to all UAVs. The evaluation function is defined as follows:

$$\begin{array}{l}{f}_{hp}^{{\mathrm{UAV}}_i}={\lambda }_{col}{f}_{col}\left({\mathrm{UAV}}_i\right)+{\lambda }_{time}{f}_{time}\left({\mathrm{UAV}}_i\right)\\ s.t.{\lambda }_{col}+{\lambda }_{time}=1\end{array}$$

(5)

where $f_{col}\left({\mathrm{UAV}}_i\right)$ is the space cost function, representing the normalized potential collision number cost between ${\mathrm{UAV}}_i$ and other UAVs. $f_{time}\left({\mathrm{UAV}}_i\right)$ is the time cost function, i.e., the normalized violation of ${\mathrm{UAV}}_i$ and cooperative ETA $t_{co}$. The final priority performance score of ${\mathrm{UAV}}_i$ is $f_{hp}^{{\mathrm{UAV}}_i}$. ${\lambda }_{col}$ and ${\lambda }_{time}$ are weights.

The purpose of the heuristic prioritization approach is to allocate the priority for each UAV reasonably, and the performance evaluation function designed will be used to evaluate the priority level. In addition, it is not difficult to find that the $f_{hp}^{{\mathrm{UAV}}_i}$ of any ${\mathrm{UAV}}_i$ is positively correlated with the number of collisions and the cost of cooperation time violation. Therefore, we give the prioritization definition of UAVs based on performance evaluation function as follows.

Definition1.

(Prioritization evaluation based on performance evaluation function) Given a group of ${\mathrm{UAV}}_N:\left({\mathrm{UAV}}_1,\dots ,{\mathrm{UAV}}_n\right)$ and any group of weights $\left[{\lambda }_l,{\lambda }_t\right]:{\lambda }_l+{\lambda }_t=1$, the performance evaluation function set $\left\{{\displaystyle \underset{i=1}{\overset{N-1}{\cup }}{f}_{hp}^{{\mathrm{UAV}}_i}}\right\}$ of ${\mathrm{UAV}}_N$ is ranked from largest to smallest to generate the set $\underset{\max \to \min }{\overset{\left\{{\displaystyle \underset{i=1}{\overset{N-1}{\cup }}{f}_{hp}^{{\mathrm{UAV}}_i}}\right\}}{\to }}$, whose elements map the priority of $\left\{1,\dots ,N\right\}$ of ${\mathrm{UAV}}_N$ in turn.

First, we analyze the calculation process of $f_{col}\left({\mathrm{UAV}}_i\right)$. Based on the space coordination constraint (Equation (3)), the potential collision number $Co{l}_{i,j}$ between the ${\mathrm{UAV}}_i$ and any other ${\mathrm{UAV}}_j$ based on the pre-planned path can be calculated as follows:

$$\begin{array}{l}\{\begin{array}{l}Co{l}_{i,j}\hspace{1em}{\Vert {\sigma }_i\left(t\right)-{\sigma }_j\left(t\right)\Vert }_2<D_{\min }\\ 0 \hspace{1em}\hspace{1em}{\Vert {\sigma }_i\left(t\right)-{\sigma }_j\left(t\right)\Vert }_2\ge D_{\min }\end{array}\\ s.t.j=1,\dots ,N \mathrm{and} i\ne j\end{array}$$

(6)

Obviously, the number of collisions of the same UAV is zero. In addition, if the number of collisions between ${\mathrm{UAV}}_i$ and ${\mathrm{UAV}}_j$ is $Co{l}_{i,j}$, the number of collisions between ${\mathrm{UAV}}_j$ and ${\mathrm{UAV}}_i$ is $Co{l}_{j,i}$, where $Co{l}_{i,j}=Co{l}_{j,i}$. From this, the total number of collisions of ${\mathrm{UAV}}_N$ can be determined as:

$$f_{col}^{all}=\frac{{\displaystyle \sum _{i=1,j=1}^{N}Co{l}_{i,j}}}{2}$$

(7)

Then, the $f_{col}\left({\mathrm{UAV}}_i\right)$ of ${\mathrm{UAV}}_i$ can be expressed as:

$$f_{col}\left({\mathrm{UAV}}_i\right)=\frac{{\displaystyle \sum _{j=1}^{N-1}Co{l}_{i,j}}}{f_{col}^{all}}$$

(8)

For the calculation of cooperative time violation cost $f_{time}\left({\mathrm{UAV}}_i\right)$ based on the time coordination constraint of ${\mathrm{UAV}}_i$, the path length $L_i^0$ needs to be obtained through the pre-planned path, so as to calculate the ETA time $t_i^0=L_i^0/v_i$. Then, the $f_{time}\left({\mathrm{UAV}}_i\right)$ is:

$$f_{time}\left({\mathrm{UAV}}_i\right)=\frac{\left|t_{co}-t_i^0\right|}{t_{co}}$$

(9)

It should be noted that there may be a specific situation, i.e., when the performance evaluation function of some UAVs may be equal, in which the priority of UAVs with equal $f_{hp}$ can be randomly assigned.

Then, the heuristic prioritization approach framework is shown in Algorithm 4.

Algorithm 4: Heuristic prioritization approach (i.e., $\mathrm{Heu}\_Prioritization\left(\sigma \right)$)

$\begin{array}{l}01:\mathrm{UA}{\mathrm{V}}_{\mathrm{N}-1}:\mathrm{UA}{\mathrm{V}}_{\mathrm{N}}\backslash \mathrm{UA}{\mathrm{V}}_{\mathrm{m}}\\ 02:f_{hp}^{\mathrm{UA}{\mathrm{V}}_{N-1}}\leftarrow \varnothing \\ 03:f_{col}^{all}\leftarrow \frac{{\displaystyle \sum _{i=1,j=1}^{N}Co{l}_{i,j}}}{2}\\ 04:for i=1 to N-1\\ 05: \hspace{1em}\hspace{1em}\hspace{1em}{f}_{hp}^{\mathrm{UA}{\mathrm{V}}_i}\leftarrow {\lambda }_{col}\frac{{\displaystyle \sum _{j=1}^{N-1}Co{l}_{i,j}}}{f_{col}^{all}}+{\lambda }_{time}\frac{\left|t_{co}-t_i^0\right|}{t_{co}}\\ 06: \hspace{1em}\hspace{1em}\hspace{1em}{f}_{hp}^{\mathrm{UA}{\mathrm{V}}_{N-1}}\leftarrow f_{hp}^{\mathrm{UA}{\mathrm{V}}_{N-1}}\cup \left\{f_{hp}^{\mathrm{UA}{\mathrm{V}}_i}\right\}\\ 07:end for\\ 08:for k=2 to N\\ 09: \hspace{1em}\hspace{1em}\hspace{1em}\mathrm{UA}{\mathrm{V}}^k\leftarrow \arg \max f_{hp}^{\mathrm{UA}{\mathrm{V}}_{N-1}}\\ 10: \hspace{1em}\hspace{1em}\hspace{1em}{f}_{hp}^{\mathrm{UA}{\mathrm{V}}_{N-1}}\leftarrow f_{hp}^{\mathrm{UA}{\mathrm{V}}_{N-1}}\backslash \max f_{hp}^{\mathrm{UA}{\mathrm{V}}_{N-1}}\\ 11:end for\end{array}$

4.2.2. The Framework of HDP Algorithm

This paper proposes the HDP algorithm, which improves the ADRPP algorithm. The HDP algorithm introduces a heuristic prioritization approach based on time–space coordination constraints, which can reasonably prioritize UAVs, thus improving the efficiency and robustness of path coordination. In addition, in order to deal with the MUCPP problem based on time–space cooperative constraints, we propose the TSRRT* algorithm in the single-UAV cooperative path planner of the HDP algorithm, which will be described in Section 4.3.

The framework of the HDP algorithm is shown in Algorithm 3. First, the priority of the UAV is ranked by the heuristic prioritization approach (i.e., function $\mathrm{Heu}\_Prioritization\left(\sigma \right)$). Then, any ${\mathrm{UAV}}_i^k$ with a priority of $k$ only needs to receive and process the path change message of the ${\mathrm{UAV}}_j^{m<k}$ with higher priority and response. If receiving the path change message from the ${\mathrm{UAV}}_j^{m<k}$, ${\mathrm{UAV}}_i^k$ will update the information of the time–space occupied area $H_i$ to the current path information of ${\mathrm{UAV}}_j^{m<k}$. After that, ${\mathrm{UAV}}_i^k$ checks whether the planned path based on the current $H_i$ can meet the space–time coordination constraints. If not, ${\mathrm{UAV}}_i^k$ uses the TSRRT* algorithm to re-plan the path that meets the time–space coordination constraints in combination with the current $H_i$. Otherwise, ${\mathrm{UAV}}_i^k$ does not need to re-plan the path and keeps the communication silent. The algorithm stops until all UAVs plan paths that meet the time–space cooperation constraints.

4.3. TSRRT* Algorithm

As a decoupled cooperative path planning algorithm, the HDP-TSRRT* algorithm needs to carry out path planning for each UAV that meets the time–space cooperative constraints after using the HDP algorithm to coordinate the planning of multi-UAV. In addition, it is noted that when the HDP algorithm is used for planning coordination, many rounds of path re-planning may be required for a single UAV to ensure that all UAVs can meet the coordination constraints, which is a great challenge to the efficiency of the path planning algorithm for a single UAV. If the efficiency of path planning is low, it will greatly increase the planning time of the cooperative path.

Therefore, this paper proposes the TSRRT* algorithm as the single-UAV cooperative path planning algorithm. The TSRRT* algorithm will be introduced in four parts, i.e., the cost function based on time–space coordination constraints, the tree expansion based on multi-sampling strategy, the neighborhood node optimization approach and the framework of TSRRT*.

4.3.1. Evaluation of the Cost Function Based on Time-Coordination Constraint

After obtaining the shortest cooperative ETA (i.e., $t_{co}$), we can obtain the path length $L_{co}^i$ of any ${\mathrm{UAV}}_i$ meeting the time cooperative constraint according to $t_{co}$, where $L_{co}^i=t_{co}{v}_i$. Therefore, in the TSRRT* algorithm, we introduce a new cost function that can constrain the length of the planned path, i.e., the cost function based on the time coordination constraint, so as to ensure that the planned path meets the time coordination constraint.

The cost function based on the time coordination constraint is no longer aimed at minimizing the path length but at minimizing the difference between the actual path length and the constrained path length. The cost function is defined as follows:

$$f_{\mathcal{T}}^{\ast }\left(x\right)=\left|f_{\mathcal{T}}\left(x\right)-L_{co}\right|$$

(10)

where $f_{\mathcal{T}}\left(x\right)=g_{\mathcal{T}}\left(x\right)+{\lambda }_{h_D}{h}_D\left(x\right)$. $g_{\mathcal{T}}\left(x\right)$ represents the actual distance from the start node $s_i$ to the current tree node $x$. $h_D\left(x\right)$ represents the estimated distance from the $x$ to the goal node $g_i$, and ${\lambda }_{h_D}$ is the estimated distance coefficient. This paper takes the Euclidean distance between two nodes as the estimated distance $h_D\left(x\right)$, where $h_D\left(x\right)={\Vert x-g_i\Vert }_2$.

4.3.2. Node Expansion Based on Multi-Sampling Strategy

In the tree structure of the classical RRT* algorithm, the expansion of a new node is only completed through one sampling. However, due to the large flight environment of UAVs, even though the generation of sampling points is guided by some offset sampling methods [36,37,38], there is still great randomness in the expansion, which affects the efficiency of path planning. For the decoupled cooperative path planning algorithm, the path planning efficiency of a single UAV is crucial.

Therefore, in order to ensure the efficiency of planning, this paper proposes a multi-sampling strategy, which directly selects the goal as the sampling point during sampling or selects a batch of sampling points for pre-expansion so as to reduce the probability of adding low-quality sampling nodes to the tree for expansions to improve efficiency. Algorithm 5 shows the process of the new node expansion approach based on a multiple sampling strategy.

Algorithm 5: Node expansion based on multi-sampling strategy ($\mathrm{MultSample}\_Extend$)

$\begin{array}{l}01:P=\mathrm{random}\left(\left[0,1\right]\right)\\ 02:if \mathrm{P}<0.1\\ 03: \hspace{1em}\hspace{1em}{x}_{rand}\leftarrow g_i\\ 04: \hspace{1em}\hspace{1em}{x}_{nearest}\leftarrow \mathrm{Nearest}\left({\mathrm{x}}_{rand}\right)\\ 05: \hspace{1em}\hspace{1em}{x}_{new}\leftarrow \mathrm{Steer}\left(x_{nearest}\right)\\ 06: \hspace{1em}\hspace{1em}if \mathrm{obstacle}\_free\left(x_{new},x_{nearest}\right)\wedge \mathrm{UAV}\_free\left(x_{new},x_{nearest}\right)\\ 07: \hspace{1em}\hspace{1em}\hspace{1em}\hspace{1em}\mathcal{T}.\mathrm{V}\leftarrow \mathcal{T}.\mathrm{V}\cup \left\{x_{new}\right\}\\ 08: \hspace{1em}\hspace{1em}end if\\ 09:else\\ 10: \hspace{1em}\hspace{1em}{X}_{rand}^K\leftarrow {\displaystyle \underset{m=1}{\overset{K}{\cup }}Sample\left(\mathcal{X}\right)}\\ 11: \hspace{1em}\hspace{1em}for m=1 to K\\ 12: \hspace{1em}\hspace{1em}\hspace{1em}{x}_{nearest}^m\leftarrow \mathrm{Nearest}\left({\mathrm{x}}_{rand}^m\right)\\ 13: \hspace{1em}\hspace{1em}\hspace{1em}{x}_{temp}^m\leftarrow \mathrm{Steer}\left(x_{nearest}^m\right)\\ 14: \hspace{1em}\hspace{1em}\hspace{1em}if \mathrm{obstacle}\_free\left(x_{temp}^m,x_{nearest}^m\right)\wedge \mathrm{UAV}\_free\left(x_{temp}^m,x_{nearest}^m\right)\\ 15: \hspace{1em}\hspace{1em}\hspace{1em}\hspace{1em}{f}_{\mathcal{T}}^{\ast }\left(x_{temp}^m\right)\leftarrow \left|f_{\mathcal{T}}\left(x_{temp}^m\right)-L_{co}\right|\\ 16: \hspace{1em}\hspace{1em}\hspace{1em}\hspace{1em}{f}_{\mathcal{T},all}^{\ast }\leftarrow f_{\mathcal{T},all}^{\ast }\cup f_{\mathcal{T}}^{\ast }\left(x_{temp}^m\right)\\ 17: \hspace{1em}\hspace{1em}end if\\ 18: \hspace{1em}\hspace{1em}end for\\ 19: \hspace{1em}\hspace{1em}\hspace{1em}{x}_{new}\leftarrow \mathrm{argmin} f_{\mathcal{T},all}^{\ast }\\ 20:end if\end{array}$

When $\mathrm{P}\ge 0.1$, the multi-sampling process is executed. First, we select a group of $K\in {ℝ}_{+}$ sampling nodes from the planning space, and any potential sampling point is $x_{rand}^m$, where $m\in \left\{1,2,\dots ,K\right\}$. Then, we calculate the corresponding potential expansion nodes $\left\{x_{temp}^1,\dots ,x_{temp}^K\right\}$ for all potential sampling points, and the calculation formula for any potential expansion node $x_{rand}^m$ is as follows:

$$\begin{array}{l}{x}_{temp}^m=x_{nearest}^m+l_{extend}\frac{\left(x_{rand}^m-x_{nearest}^m\right)}{{\Vert x_{rand}^m-x_{nearest}^m\Vert }_2}\\ s.t. \mathrm{Eq}.(1),(2),(3)\end{array}$$

(11)

where $x_{temp}^m$, $x_{rand}^m$, $x_{nearest}^m$ are UAV position coordinates. For potential expansion nodes that meet UAV performance constraints, planning space constraints and space coordination constraints, the expansion length of nodes is determined by the expansion step $l_{extend}$. Then, we use the sampling node $x_{rand}^m$ and the nearest node $x_{nearest}^m$ from $x_{rand}^m$ in the tree to determine the potential expansion node $x_{temp}^m$. If $x_{temp}^m$ does not meet the above constraints, it will be discarded.

After obtaining the potential expansion nodes set $X_{temp}$ that meet the constraints, the cost function based on time constraints is used to select the optimal potential expansion node as the new node $x_{new}$ for expansion, that is,

$$x_{new}=\arg \min f_{\mathcal{T}}^{\ast }\left(X_{temp}\right)$$

(12)

It can be seen that the new node expansion method based on multiple sampling and cost function evaluation strategy is divided into three steps. First, we select the expansion method of the new node—that is, we select the target point as the sampling point or conduct multi-sampling. If multi-sampling is selected, a batch of nodes are randomly sampled through multi-sampling strategy for pre-expansion to ensure the randomness of tree structure expansion. Then, all potential expansion nodes that meet the constraints are evaluated using the cost function based on the time coordination constraint to obtain the optimal expansion node as a new node. This method can reduce the randomness of the expansion of new nodes to a certain extent and ensure that the expansion of the tree structure can meet the solution of the time coordination constraint so as to improve the planning efficiency and make the planned path close to the optimal path under the current cost function. In addition, the evaluation process directly discards some sampling points with poor performance so that these nodes will not be subject to subsequent neighborhood node updates and other operations with high computational complexity, which can also reduce the planning time.

4.3.3. Neighborhood Nodes Update Approach Based on the Cost Function of Time Coordination Constraint

It should be noted that the key to ensuring the progressive optimality of the traditional RRT* algorithm is the introduction of neighborhood node optimization. Therefore, in the TSRRT* algorithm, in order to plan a path that meets the time–space coordination constraints, on the basis of obtaining the $x_{new}$ of the new node, the cost function based on the time coordination constraint should also be used in the neighborhood node update to guide the parent node’s updating and rewiring of the new node to ensure that the planned path gradually meets the time coordination constraint.

Because the neighborhood node update includes the parent node update process and the rewiring of the new node $x_{new}$, the two processes are basically the same. Therefore, Algorithm 6 gives a general framework for neighborhood node optimization of the TSRRT* algorithm.

Algorithm 6: Neighborhood node update (i.e.,$Rewire\_TSRRT\ast \left(x_1,x_2\right)$)

$\begin{array}{l}01:if \mathrm{obstacle}\_free\left(x_1,x_2\right)\wedge \mathrm{UAV}\_free\left(x_1,x_2\right)\\ 02: \hspace{1em}\hspace{1em}\cos \mathrm{t}\leftarrow {\Vert x_1-x_2\Vert }_2\\ 03: \hspace{1em}\hspace{1em}if \left|f_{\mathcal{T}}\left(x_1\right)+\cos \mathrm{t}+h_D\left(x_2\right)-L_{co}\right|<f_{\mathcal{T}}^{\ast }\left(x_2\right)\\ 04: \hspace{1em}\hspace{1em}\hspace{1em}{x}_2.parent\leftarrow x_1\\ 05: \hspace{1em}\hspace{1em}end if\\ 06:end if\end{array}$

First, we analyze the process of performing $\mathrm{Rewire}\_\mathrm{TSRRT}\ast$ for the first time—that is, the process of selecting the parent node with less cost based on the time cooperation constraint in the neighborhood for $x_{new}$. Firstly, it is determined whether the edge $\left(x_{near}^k,x_{new}\right)$ composed of any node $x_{near}^k$ in the neighborhood, and $x_{new}$ meets the UAV performance constraints, planning space constraints and space coordination constraints. If the constraints are met, the distance between two nodes is calculated. Note that all nodes in the neighborhood have been added to the tree, so we can directly obtain the actual path length from the start node $s_i$ to $x_{near}^k$ in the tree. Then, the estimated distance $h_D\left(x_2\right)$ from $x_{new}$ to the goal node $g_i$ is calculated. Then, the cost $f_{\mathcal{T}}^{\ast \prime }\left(x_{new}\right)$ based on time coordination constraint from $s_i$ through $x_{near}^k$ to $x_{new}$ is

$$f_{\mathcal{T}}^{\ast \prime }\left(x_{new}\right)=\left|f_{\mathcal{T}}\left(x_1\right)+\cos \mathrm{t}+h_D\left(x_2\right)-L_{co}\right|$$

(13)

After calculating the cost of all neighborhood nodes, the node with the lowest cost is selected as the parent node of $x_{new}$.

The process of the second $\mathrm{Rewire}\_\mathrm{TSRRT}\ast$ is basically similar to that of the first. The only difference is that this operation takes $x_{new}$ as the potential parent node of all nodes in the neighborhood to optimize the neighborhood nodes.

4.3.4. The Framework of TSRRT* Algorithm

This section mainly analyzes the overall framework of the TSRRT* algorithm, which is shown in Algorithm 7. After designing the cost function based on the time coordination constraint as the evaluation standard in TSRRT*, the multiple sampling and cost function evaluation strategy is designed in the tree node expansion process to improve the planning efficiency. Then, the cost function based on time coordination constraint is used to guide the updating and rewiring of nodes in the neighborhood.

Algorithm 7: TSRRT* algorithm

$\begin{array}{l}01: initialization {\mathcal{T}}_{\mathrm{i}}.\mathrm{V}\leftarrow \left\{s_i\right\};{\mathcal{T}}_{\mathrm{i}}.\mathrm{E}\leftarrow \varnothing \\ 02:L_{co}\leftarrow t_{co}{v}_i\\ 03:for i=1 to n\\ 04: \hspace{1em}\hspace{1em}{x}_{new}\leftarrow \mathrm{MultSample}\_Extend\left({\mathcal{T}}_{\mathrm{i}}\right)\\ 05: \hspace{1em}\hspace{1em}{\mathcal{T}}_{\mathrm{i}}.\mathrm{V}\leftarrow {\mathcal{T}}_{\mathrm{i}}.\mathrm{V}\cup \left\{x_{new}\right\}\\ 06: \hspace{1em}\hspace{1em}{X}_{near}\leftarrow \mathrm{Near}\left(x_{new},{\mathcal{T}}_{\mathrm{i}}.\mathrm{V},\gamma \right)\\ 07: \hspace{1em}\hspace{1em}for \mathrm{all} x_{near}\in X_{near}\\ 08: \hspace{1em}\hspace{1em}\hspace{1em}Rewire\_TSRRT\ast \left(x_{near},x_{new}\right)\\ 09: \hspace{1em}\hspace{1em}end for\\ 10: \hspace{1em}\hspace{1em}for \mathrm{all} x_{near}\in X_{near}\\ 11: \hspace{1em}\hspace{1em}\hspace{1em}Rewire\_TSRRT\ast \left(x_{new},x_{near}\right)\\ 12: \hspace{1em}\hspace{1em}end for\\ 13: \hspace{1em}\hspace{1em}if x_{new}\in G_i\\ 14: \hspace{1em}\hspace{1em}\hspace{1em}{g}_i\leftarrow x_{new}\\ 15: \hspace{1em}\hspace{1em}end if\\ 16:end for\\ 17:while \left|{\mathrm{Cost}}_{\mathcal{T}}\left(s_i,g_i\right)-L_{co}\right|\ge \epsilon \\ 18: \hspace{1em}\hspace{1em}\hspace{1em}\mathrm{Update} {\mathcal{T}}_{\mathrm{i}}.x_{new} \mathrm{with} \mathrm{Line} 03-12\\ 19:end while\\ 20:return {\sigma }_i\left(s_i,g_i\right)\end{array}$

When the new node $x_{new}$ extends into the goal region, a path ${\sigma }_i\left(s_i,g_i\right)$ can be found. If the deviation between the path length ${\mathrm{Cost}}_{\mathcal{T}}\left(s_i,g_i\right)$ of ${\sigma }_i\left(s_i,g_i\right)$ and the $L_{co}$ is greater than or equal to the given tolerance $\epsilon$, the tree structure is updated using the Algorithm 7, Line 3–12 to optimize the ${\sigma }_i\left(s_i,g_i\right)$${\sigma }_i\left(s_i,g_i\right)$. When ${\mathrm{Cost}}_{\mathcal{T}}\left(s_i,g_i\right)-L_{co}<\epsilon$, the path satisfying the time–space coordination constraints is output.

5. Simulation Results and Discussion

In this section, we show the simulation results of the HDP-TSRRT* algorithm and compare them with those of other algorithms to verify the feasibility, robustness and efficiency of the proposed algorithm in solving the MUCPP problem based on the time–space cooperative constraints.

5.1. Environments and Parameters

Due to the diversity of tasks performed by multi-UAV systems, the cooperative path planning problem of multiple UAVs has many forms. In order to comprehensively demonstrate the effectiveness and performance of the HDP-TSRRT* algorithm in dealing with MUCPP problems based on the time–space cooperative constraints, this paper designs a flight environment simulating real scenarios based on two different task categories to simulate and test the HDP-TSRRT* algorithm, i.e., rendezvous tasks [39] and allocation tasks [40].

For each planning example, we assume that the UAV can use its own independent CPU to calculate its path so as to achieve decentralized planning. In order to count and analyze the performance of HDP-TSRRT*, we use discrete events to simulate the parallel operation of the algorithm, including the simulation of cooperative message execution time and algorithm planning time between unmanned aerial vehicles to ensure that the process of executing the algorithm with the respective CPU of the ${\mathrm{UAV}}_N$ is consistent. Each UAV adopts a simulated idealized communication module, which can rely on zero-delay communication to achieve the sending and receiving of cooperative messages. The paths of synchronous decentralized pre-planning and the paths of single-UAV cooperative planning are the optimal paths. In addition, UAVs in all planning cases take off at the same time.

The following shows the simulation environment designed based on two types of tasks and the setting of some basic parameters.

• Rendezvous tasks

The goal of MUCPP in this task is to find the path from different start nodes to the goal for each UAV in the planning space. Additionally, all UAVs must reach the same target position at the same time to perform specific tasks. Figure 2a shows the planning space based on rendezvous tasks, including the real terrain (DEM map), radar, missile, anti-aircraft gun and conical no-flight tower. See

Table 1. Rendezvous task scenario.

Rendezvous tasks Scenario	Radar	Center 1:(100,350,5) m 2:(170,210,20) m	radius 35 m 35 m
	Missile	Center 1:(70,250,0) m 2:(300,100,20) m	Radius 30 m 30 m	Height 40 m 40 m
	Anti-aircraft gun	Center 1:(170,100,0) m 2:(280,280,20) m	Radius 30 m 25 m	Height 40 m 40 m
	No-fly Tower	Center 1:(170,100,0) m	Radius 15 m	Height 50 m

for the relevant parameters of threats.

2.

Allocation tasks

The goal of cooperative path planning of multiple UAVs in this mission is to plan the flight path from different starting positions to different target positions for each UAV in the flight environment. In addition, in order to meet the requirements of some tasks, such as a multi-target cooperative strike, all UAVs should be able to reach the target point at the same time. Figure 5a shows the planning space based on allocation tasks, including the real terrain (DEM map), radar, anti-aircraft missile, anti-aircraft gun and conical no-flight tower. See

Table 2. Allocation task scenario.

Allocation tasks Scenario	Radar	Center 1:(360,230,5) m 2:(190,210,20) m	radius 35 m 35 m
	Missile	Center 1:(90,290,0) m 2:(300,100,20) m	Radius 30 m 30 m	Height 40 m 40 m
	Anti-aircraft gun	Center 1:(90,110,0) m 2:(280,280,20) m	Radius 30 m 25 m	Height 40 m 40 m
	No-fly Tower	Center 1:(200,290,10) m	Radius 15 m	Height 50 m

for the relevant parameters of threats.

Some common basic parameters of the HDP-TSRRT* algorithm in two kinds of task scenario and comparative simulation are shown in

Table 3. Basic parameters of TSRRT*.

Parameter	Value
$\left[{\lambda }_{col},{\lambda }_{time}\right]$	[0.4, 0.6]
$D_{\min }$	2 m
$v \mathrm{for} \mathrm{all} \mathrm{UAVs}$	8 m/s
${\lambda }_{h_D}$	1
$\mathrm{K}$	16
$\epsilon$	0.35 s
Node expansion length	30 m

.

5.2. Simulation of HDP-TSRRT* Algorithm in Different Categories of Task Scenarios

This section conducts benchmark simulation experiments on HDP-TSRRT* in a rendezvous task scenario and allocation task scenario, respectively, and shows the process of using HDP-TSRRT* to achieve time–space collaborative constraint paths in both scenarios. The simulation results show that HDP-TSRRT* can plan the cooperative path of multi-UAV satisfying the time–space cooperative constraints in two scenarios.

5.2.1. Rendezvous Task Scenario

In the rendezvous task scenario, we show the cooperative planning process of ${\mathrm{UAV}}_N$, where $N=15$. The pre-path obtained through the synchronous decentralized pre-planning process of the HDP-TSRRT* algorithm is shown in Figure 2a,b. Note that the purpose of pre-planning is to obtain the number of potential collisions and the shortest cooperative ETA so as to lay the foundation for the subsequent priority allocation and coordination path planning. The pre-planned path lengths of the 15 UAVs are 510.1156, 483.2191, 466.5199, 440.6697, 418.8264, 404.5157, 387.4666, 382.2667, 504.2721, 480.8086, 454.6412, 431.5821, 413.4772, 403.0816 and 391.2380 m. The number of potential collisions are 3, 2, 1, 2, 2, 0, 1, 1, 0, 2, 2, 2, 0, 1, 1 and 1. The shortest cooperative ETA in this instance is $t_{co}=63.89\mathrm{s}$ through the pre-planned path length set.

Figure 2c,d show the time–space cooperative path map finally obtained by UAVs using HDP-TSRRT* in this scenario. It is difficult to determine whether the path of each UAV meets the time–space coordination constraint only through Figure 2c,d. Therefore, Figure 3 shows the shortest paired distance $l_{co}^{\min }$ of the cooperative path of 15 UAVs at any time. The $l_{co}^{\min }$ of any $t$ is:

$$l_{co}^{\min }=\min \left({\displaystyle \underset{\begin{array}{l}i,j=1\\ i\ne j\end{array}}{\overset{N}{\cup }}{\Vert {\sigma }_i\left(t\right)-{\sigma }_j\left(t\right)\Vert }_2}\right)$$

(14)

Since the scenario of this planning example is a rendezvous task scenario, we assume that all UAVs will not collide at the target location. It can be seen in Figure 3 that at every moment, the actual shortest distance between any two UAVs is greater than the minimum safe distance $D_{\min }$, so it can be proved that all cooperative paths meet the spatial cooperative constraint. Figure 4 shows the collaboration time tolerance $\Delta t_i$ of the coordination path. The coordination time tolerance of any ${\mathrm{UAV}}_i$ is,

$$\Delta t_i=\frac{L_i-L_{co}}{v_i}$$

(15)

As can be seen from Figure 4, the time tolerance of all cooperative paths is $\Delta t_i<\epsilon =0.35$, which means that all paths meet the time cooperative constraint. Therefore, the simulation shows that HDP-TSRRT* can plan the cooperative path that meets the time–space cooperative constraints in the rendezvous tasks scenario.

5.2.2. Allocation Tasks Scenario

In the allocation task scenario, we show the cooperative planning process of ${\mathrm{UAV}}_N$, where $N=15$. The pre-planning path obtained through the synchronous decentralized planning layer of the HDP-TSRRT* algorithm is shown in Figure 5a,b. In order to better demonstrate the performance of the HDP-TSRRT* algorithm, in this planning example, we will divide 15 UAVs into 3 groups, with 5 in each group. The three groups of UAVs cross fly. As with the rendezvous task scenario, the pre-planned optimal path is obtained by the RRT* algorithm. The shortest cooperative ETA in this instance is $t_{co}=56.21\mathrm{s}$ through the pre-planned path length set. Figure 5c,d show the time–space cooperative path finally obtained ${\mathrm{UAV}}_N$ using HDP-TSRRT*. Figure 6 and Figure 7, respectively, show the shortest paired distance of ${\mathrm{UAV}}_N$ at any time and the time tolerance in the allocation tasks scenario. It can be seen that the finally found path meets the time–space coordination constraints. Therefore, the simulation shows that HDP-TSRRT* can plan the cooperative path that meets the time–space coordination constraints in the allocation task scenario.

Figure 5. Allocation task scenario. The red dot represents the starting point of the UAV, and the black dot represents the ending point of the UAV. (a) The flight environment of allocation tasks, and the optimal path of 15 UAVs through RRT* synchronous decentralized pre-planning. (b) The top-down view of (a). (c) Fifteen UAVs use the HDP-TSRRT* algorithm to obtain the path that meets the time–space coordination constraints. (d) The top-down view of (c).

Figure 5. Allocation task scenario. The red dot represents the starting point of the UAV, and the black dot represents the ending point of the UAV. (a) The flight environment of allocation tasks, and the optimal path of 15 UAVs through RRT* synchronous decentralized pre-planning. (b) The top-down view of (a). (c) Fifteen UAVs use the HDP-TSRRT* algorithm to obtain the path that meets the time–space coordination constraints. (d) The top-down view of (c).

5.3. Comparison and Algorithm Performance Analysis

In this section, we compare the planning coordination level and the single-UAV cooperative path planning level of HDP-TSRRT* with some existing algorithms and analyze the performance of HDP-TSRRT*.

5.3.1. Comparison and Performance Analysis of Planning Coordination Level of HDP-TSRRT*

Note that the existing asynchronous prioritized planning algorithms and their derivative algorithms do not consider both time and space coordination constraints. Therefore, in order to eliminate the impact of the single-UAV cooperative path planning algorithm and accurately verify the performance of the HDP-TSRRT* algorithm, we uniformly take TSRRT* as the single-UAV cooperative path planning algorithm and compare HDP-TSRRT* with the asynchronous decentralized prioritized planning algorithm based on three different prioritization approaches. These three algorithms are the asynchronous decentralized prioritized planning algorithm based on random prioritization (ADRPP) [23], asynchronous decentralized prioritized planning algorithm based on distance heuristic prioritization (DH-ADRPP) [26] and asynchronous decentralized priority planning algorithm based on collision heuristic prioritization (CH-ADRPP) [41].

This simulation compares the two types of task scenarios mentioned in Section 5.1 at the planning coordination level. In order to fully analyze the performance of the HDP-TSRRT* algorithm, in the rendezvous tasks scenario, the HDP-TSRRT* algorithm and the other three algorithms are, respectively, used to perform cooperative path planning in ten groups of different numbers of UAVs, including $N_{RT}=3,6,9,12,15,18,21,24,27,30$. In the allocation tasks scenario, ten groups of UAVs with a given number are also compared, including $N_{AT}=4,8,12,16,20,24,28,32,36,40$. Each group of UAVs with different numbers in the 2 kinds of mission scenarios are simulated 30 times.

We compared the following performance of HDP-TSRRT* with other three algorithms.

• The average running time of the successful termination of the algorithm, which evaluates the efficiency and stability of the algorithm.
• The average acceleration ratio of the four algorithms relative to the RPP algorithm, which evaluates the asynchronous execution effect of the algorithm using different prioritization approaches. The calculation formula of the acceleration ratio is $\frac{t_i^{RPP}}{t_i^{AD\_ALG}}$, where $t_i^{RPP}$ represents the running time of the RPP algorithm, and $t_i^{AD\_ALG}$ represents the running time of the four comparison algorithms.
• The average number of $\mathrm{Coor}\_\mathrm{message}$ sent by the UAV. Whenever the UAV running the algorithm re-plans the path, it will send new path information to other UAVs. Therefore, the number of $\mathrm{Coor}\_\mathrm{message}$ transmissions directly corresponds to the number of times the UAV is re-planned.
• Success rate, which verifies the effectiveness of the algorithm.

We can see from Figure 8 that HDP-TSRRT* takes the shortest amount of time to plan the cooperative path in all different UAV groups in the two kinds of task scenarios. In addition, compared with the other three algorithms, HDP-TSRRT* has the highest acceleration ratio. This shows that the heuristic prioritization algorithm based on the time–space coordination constraint of HDP-TSRRT* is more reasonable, efficient and robust than the heuristic prioritization algorithm that only considers the length or the number of collisions and the random prioritization algorithm. Therefore, it can be proved that HDP-TSRRT* has the best planning efficiency and stability when dealing with the MUCPP problem based on the time–space coordination constraint.

Furthermore, it can be that seen the average number of $\mathrm{Coor}\_\mathrm{message}$ of each UAV in HDP-TSRRT* is also the lowest. That is to say, HDP-TSRRT* has the least path re-planning, so it can complete the cooperative path planning of all UAVs in the shortest time, and the planning efficiency is higher. Note that the four comparison algorithms are only different in the prioritization algorithm, so it also proves that the prioritization approach of HDP-TSRRT* is more reasonable and efficient than other algorithms. As the four algorithms are asynchronous decentralized improved algorithms based on RPP algorithm, the success rate is basically the same.

5.3.2. Comparison and Performance Analysis of Single-UAV Cooperative Path Planning Level

The single-UAV cooperative path planning algorithm TSRRT* uses multiple sampling strategies to complete a batch of $K$ pre-sampling nodes in sampling and new node expansion and optimizes the selection of sampling nodes in combination with the time–space coordination cost function. In this process, the selection of the pre-defined parameter $K$ is crucial. If $K$ is too small, the number of samples that can be evaluated by the time–space cost function is also very small, and there is a certain probability that the quality of this batch of samples is very low. On the other hand, if the $K$ is too large, the cost of evaluating sampling nodes and the cost of the computing time of collision checking will also increase correspondingly. Therefore, whether the value of $K$ is reasonable directly determines whether the TSRRT* algorithm can obtain better expansion nodes to complete the cooperative path planning faster, and then affects the efficiency of the HDP-TSRRT* algorithm to complete the cooperative path planning.

Therefore, we first simulated and evaluated the planning time of the TSRRT* algorithm with different values of $K$. In the experiment, 60 simulations were performed when $K=1,4,8,12,16,20,24,28$, including 30 runs for the rendezvous tasks scenario and 30 runs for the allocation tasks scenario. Figure 9 shows the relationship between the value of $K$ and the running time. It can be seen that when $K$ is between 16 and 20, the TSRRT* algorithm has the highest planning efficiency and the most stable robustness, and the planning time is basically the same. Therefore, we choose $K=16$ to execute HDP-TSRRT*.

When introducing the proposed algorithm, this paper theoretically demonstrates that the new node expansion strategy combined with the multi-sampling approach and time–space cost function optimization can improve the efficiency of cooperative path planning. In this section, we prove this view by comparing simulation results.

Because TSRRT* introduces a time–space-based cost function to evaluate the expansion of new nodes and the optimization of neighborhood nodes, in order to compare and discuss the performance of TSRRT* more accurately, when comparing TSRRT* with the classical RRT* algorithm (i.e., Algorithm 1) based on random sampling, this paper unifies the cost function of the two algorithms into the time–space-constrained cost function. The maximum planning time of the simulation is 10s, and the optimal path cost of TSRRT* is taken as the baseline.

Figure 10 shows the change in path cost based on multi-sampling and the time–space cost function of TSRRT* and the number of nodes in the tree structure, as well as the number of nodes in random sampling and the tree structure of the RRT* algorithm. As shown in Figure 10, using the multiple sampling and optimization strategy proposed in this paper, TSRRT* can find the first feasible path within a few milliseconds, and the cost will be reduced rapidly after that. In contrast, the RRT* algorithm takes nearly 1000 ms to find the first feasible path, and it takes longer to approach the optimal path. As we selected a reasonable number of multiple samples, the algorithm in this paper generates higher quality samples. Therefore, the algorithm in this paper is more likely to obtain the optimal solution with higher efficiency and faster convergence speed. In addition, from the distribution of the number of nodes in the tree structure, it can be seen that the quality of the tree structure generated by the algorithm in this paper is higher than that of RRT*, and the optimal path can be obtained faster in the generation of fewer tree nodes. This also proves that TSRRT* has advantages in planning efficiency.

Furthermore, we conducted 60 cooperative path planning simulations involving two types of task scenarios (every scenario runs 30 trials) using TSRRT*, RRT* (in the cooperative path planning) and PRRT* [42] (in the cooperative path planning) algorithms based on the time–space coordination. The simulation results are shown in Figure 11. It can be seen that TSRRT* has the shortest and most stable planning time to achieve the optimal time–space path.

Therefore, through comparative simulations, we have proved that TSRRT* can plan the cooperation path that meets the time–space cooperation constraints, the planning efficiency is high, and the stability is good.

6. Conclusions

In this paper, a decoupling cooperative path planning algorithm named HDP-TSRRT* is proposed to solve the MUCPP based on the time–space cooperative constraints. Each UAV shares the same arrival time, and there is no conflict or threat avoidance among them. The proposed algorithm adopts a hierarchical architecture. The first is the pre-planning level, which aims to obtain the relevant parameter information and lay the foundation of cooperative path planning. Then, at the coordination planning level, an HDP algorithm is proposed, which introduces the heuristic prioritization approach based on time–space coordination constraints so as to assign reasonable priority to each UAV to improve the planning efficiency and robustness. Lastly, at the single-UAV cooperative path planning level, we propose the TSRRT* algorithm, which can quickly and stably plan a path that meets the time–space cooperative constraints for a single UAV.

In future work, we plan to expand the HDP-TSRRT* algorithm to the multi-UAV system with local communication to complete the real outdoor experiment. In real experiments, the proposed algorithm requires each UAV to have a separate communication unit to achieve real-time information reception and broadcast. However, ensuring the reliability and real-time of communication between UAVs is also a difficult problem to be faced in the implementation of this algorithm. In addition, we plan to use the decoupling prioritized planning algorithm or its derivative algorithm to deal with the MUCPP problem in a dynamic environment.