Collision-Free 4D Dynamic Path Planning for Multiple UAVs Based on Dynamic Priority RRT* and Artiﬁcial Potential Field

: In this paper, a four-dimensional (4D) dynamic cooperative path planning algorithm for multiple unmanned aerial vehicles (UAVs) is proposed, in which the cooperative time variables of UAVs, as well as conﬂict and threat avoidance, are considered. The algorithm proposed in this paper uses a hierarchical framework that is divided into a 4D cooperative planning layer and a local threat avoidance planning layer. In the cooperative planning layer, the proposed algorithm, named dynamic priority rapidly exploring random trees (DPRRT*), would be used for the 4D cooperative path planning of all UAVs involved in a given task. We ﬁrst designed a heuristic prioritization strategy in the DPRRT* algorithm to rank all UAVs to improve the efﬁciency of cooperative planning. Then, the improved RRT* algorithm with the 4D coordination cost function was used to plan the 4D coordination path for each UAV. Whenever the environment changes dynamically (i


Introduction
In recent years, UAVs have been widely used in both military and civilian fields, such as urban penetration, battlefield reconnaissance, aerial photography, disaster area search and rescue, etc. [1,2].However, with the increasing diversification of missions and the complexity of flight environments, more and more users are reaching the limitations of single UAVs when performing missions [3].Therefore, multiple UAV systems have gradually grown into a popular and challenging research topic [4].As the most basic problem in the research of multi-UAV systems, the cooperative path planning technology of multiple UAVs directly affects the ability of UAVs to complete tasks smoothly, safely, and autonomously [5,6].
Multi-UAV systems are usually deployed for more complex tasks, such as coordinated multi-UAV attack and detection missions, which require all UAVs to have the same and shortest arrival time [7].In addition, since there are usually a variety of threats in flight environments where multiple UAV systems perform tasks, the planning of a safe path that must simultaneously avoid environmental threats and collisions between individual UAVs is a difficult problem [8].Furthermore, ground-based navigation systems are often unable to detect the precise location and configuration information of all threats in complex environments [9].Therefore, it is particularly difficult to coordinate the paths of all UAVs involved and adjust their existing paths in a timely and efficient manner according to real-time environmental information [10], especially for some tasks that require accurate arrival time and dynamic conflict avoidance simultaneously.The existing methods of solving the cooperative path planning problem of multiple UAVs include several collision avoidance methods and cooperative path generation methods [11].Collision avoidance methods are often used to deal with local spatial conflicts.First, each UAV independently plans the shortest path while avoiding threats in the environment.During the path execution, each UAV regularly detects the position and speed of other UAVs around it.If there is a risk of collision, the UAVs adjust their paths in real time to avoid conflicts.Many local collision avoidance path planning algorithms based on collision avoidance methods have been proposed, such as the artificial potential field algorithm (APF) [12,13], the centralized control barrier function algorithm (CCBF) [14], the distributed control barrier function algorithm (DCBF) [15], and the optimal reciprocal collision avoidance algorithm (ORCA) [16].Since collision avoidance is only performed in a local range, these algorithms usually have high computational efficiency and the ability to dynamically and rapidly replan paths.However, as a local collision avoidance method, with the increase in the number of UAVs and threats, these algorithms may fall into local minima and lead to the failure of cooperative path planning.Additionally, since such methods focus on space conflict avoidance, the consistency of flight time is not considered.
Cooperative path generation methods can be divided into two categories: coupling methods and decoupling methods [17].The coupling method usually regards all UAVs as comprising an agent system and directly searches for the cooperative path of the agent system.Some popular coupling methods, such as increasing cost tree search (ICTS) [18] and conflict-based search (CBS) [19], have been extensively studied and many variant algorithms have been derived [20][21][22][23].The coupling method directly deals with the path planning problem of the agent system and can usually obtain the optimal solution for the system.However, it is well-known that even a simple variant of the path planning problem for multiple UAVs, such as a planned cooperative path with the sudden appearance of a threat, interferes with the implementation of a cooperative path that encounters both conflict-free space and path execution time consistency.In addition, in computing, the coupling method is very difficult to solve [24].
Decoupling methods usually divide the problem of multi-UAV cooperative path planning into two steps, i.e., the coordination of the path planning sequence and the single UAV cooperative path planning [25].As the most representative algorithm of the decoupling method, the prioritized planning algorithm (PP) [26] has become a very popular algorithm in cooperative path planning [27], along with other methods such as the revised prioritized planning algorithm (RPP) [28], the decentralized prioritized planning (DPP) [29], and the asynchronous decentralized prioritized planning algorithm (ADPP) [30].In the prioritized planning algorithm, each UAV has its own priority and calculates its path according to the relative order of its priority.Therefore, the final path of each UAV is able to avoid static threats in the environment and UAVs with a higher priority than it.The common single UAV cooperative path planning algorithms in prioritized planning algorithms are the grid-based A* algorithm [31] and the sample-based RRT* algorithm [32,33].The calculation time of the A* algorithm usually increases exponentially with the increase in spatial dimension and grid resolution, and it does not perform well in large planning spaces of UAVs [34].The RRT* algorithm is able to reach the optimal solution with fewer samples than the fixed high-resolution grid method by sampling the extended tree structure.Therefore, RRT* is widely used in path planning in high-dimensional space [35].Furthermore, if the priority can be assigned reasonably, the prioritized planning algorithm can usually plan the cooperative path for multiple UAVs more efficiently.The existing decoupling methods also focus on conflict avoidance and do not conduct in-depth research on the time consistency of all UAV planning paths.In addition, in complex environments with unknown threats, the ability of the existing decoupling methods to avoid dynamic sudden threats is unknown.
Therefore, combining the practical applications of the environment and the complex task categories of multiple UAV systems, as well as the possible improvement of existing Drones 2023, 7, 180 3 of 32 path planning algorithms for multiple UAVs, the main motivations of this paper are as follows.

1.
The actual flight environment of UAVs is mostly wide open space.Due to the complexity of the actual flight environment, certain detected static threats can suddenly start to move and previously unknown static or moving threats can suddenly appear.Only relying on the known environment broadcast by the navigation system for cooperative path planning often carries the risk of unfeasible paths being followed.

2.
The tasks performed by multi-UAV systems have gradually become more complex and diverse.Three-dimensional cooperative planning based on conflict-free space no longer meets the needs of most tasks.

3.
Existing algorithms based on the local collision avoidance method, the coupling method, and the decoupling method are rarely studied in the field of 4D cooperative path planning [36,37].In addition, the local collision avoidance method is able to adapt to the frequently changing dynamic environment but cannot ensure the success of global cooperative path planning.The decoupling method can quickly plan cooperative paths for multiple UAVs but it cannot guarantee the efficiency of cooperative replanning brought about by environmental changes.
Therefore, this paper focuses on the research of a 4D dynamic cooperative path planning algorithm for multiple UAVs.The algorithm needs to be consistent in terms of the conflict-free space and flight time of multiple UAVs simultaneously and must be able to adapt to frequent changes in the environment so that the algorithm can quickly avoid threats and suddenly encountered threats and ensure a lack of conflict between UAVs.More specifically, the algorithm proposed in this paper adopts a hierarchical strategy for the 4D cooperative path planning of multiple UAVs and the local cooperative collision avoidance planning for dynamic environments.The 4D cooperative planning algorithm based on DPRRT* consists of a heuristic dynamic prioritization strategy, priority planning order coordination, and an improved RRT* algorithm based on 4D cooperative constraints.The local threat avoidance planner based on HAPF effectively overcomes the problems of unreachable targets, the risks of falling into local minima, and the path oscillation which exists in traditional APF by improving potential field functions, heuristic optimal subtarget node selection strategies and memory resultant force strategies, respectively.In fact, the proposed algorithm based on a hierarchical strategy combines the advantages of the decoupling method and the collision avoidance method and overcomes their shortcomings.In addition, the algorithms based on the two methods are improved and upgraded to be used for more extensive practical applications in the future.
Overall, the main contributions of this paper are as follows: 1.
A 4D dynamic cooperative path planning algorithm based on a hierarchical strategy is proposed.The proposed upper layer based on DPRRT* is able to plan a 4D cooperative path for UAVs that simultaneously meets the requirements of conflict-free space and consistent flight time.The proposed lower layer based on HAPF is able to deal with online dynamic local collision avoidance planning in frequently changing environments.

2.
In the proposed DPRRT* algorithm, a heuristic dynamic prioritization strategy was designed.Within a planned environment, the heuristic prioritization algorithm was used to reasonably and dynamically adjust the priorities of UAVs to efficiently and robustly plan their path generation sequences.

3.
In the single-UAV planning algorithm of DPRRT*, a 4D cost function based on time cooperative constraints was proposed, which guides the expansion of new nodes and the optimization process of neighborhood nodes to quickly plan a path that satisfies the 4D cooperative constraints of UAVs.

4.
In HAPF, a heuristic sub-target node selection strategy was designed to select the optimal sub-target node for the UAVs in local obstacle avoidance planning in order to get rid of the local minimum node.In addition, a memory resultant force strategy was designed to improve path oscillation, so that the obstacle avoidance path is smoother and is able to meet the performance constraints of the UAVs.
This paper is organized as follows.Section 2 describes the 4D dynamic cooperative path planning problem.In Section 3, the 4D dynamic cooperative path planning algorithm proposed in this paper is explained in detail.Section 4 outlines the simulation and comparison experiments of the proposed algorithm and discusses the simulation results.Section 5 describes some of the conclusions reached, as well as future works.

Problem Formulation
The 4D multi-UAV dynamic cooperative path planning problem discussed in this paper refers to a problem in dynamic 4D space X (x, y, z, t) ⊆ R 4 comprising sudden threats for each UAV in the group, UAV N : (UAV 1 , . . . ,UAV n ), composed of n UAVs, resulting in the planning of a path that meets 4D cooperative constraints and involves no collisions with threats in the dynamic environment.
Considering the diversity of tasks performed by UAV groups, the starting node or end node of multiple UAVs may overlap (such as in rendezvous tasks), so this paper assumes that if multiple UAVs have the same start node or goal, there will be no collision.In addition, each UAV has communication equipment to relay information.This paper assumes that communication is completely reliable and there are no delays.

Dynamic Cooperative Path Planning Constraints of UAVs
For the constraints of the general UAV's 4D dynamic cooperative path planning, the UAV performance constraints, dynamic environment limitations, and 4D cooperative constraints should be considered.Each UAV i in UAV N needs to take off from its own starting node at the same time.The planned path needs to avoid conflicts with the other UAV N/i : UAV N \UAV i while avoiding all threats (including radar, anti-aircraft artillery, anti-aircraft missiles, and terrain) and finally reaching the intended goal at the same time.The planned path needs to take into consideration the performance constraints of the UAV.In addition, sudden, undetected threats may appear during flight, i.e., the planning space is dynamic.Therefore, the UAVs affected by the change of environment need to carry out local path replanning to avoid these threats.Note that the local path replanning destroys the 4D cooperative constraints, making it also necessary to carry out cooperative replanning for each UAV N to allow the path to meet the cooperative path planning requirements of the UAV.

UAV Performance Constraints
The cooperative path, σ i , of any UAV i in UAV N is composed of a group of path nodes needs to meet the performance constraints of the UAVs, including the maximum flight distance L max , the maximum steering angle φ max , the maximum climbing/diving angle γ max , and the shortest path segment distance l min .UAV performance constraints are shown in Definition 1.

Definition 1 (UAV performance constraints).
For any UAV i in-planning space X , the maximum flight distance constraint is: and the maximum steering angle constraint is: where Drones 2023, 7, 180

of 32
The maximum climbing/diving angle constraint is: and the shortest path segment distance constraint is:

Environmental Limitations
In the dynamic planning environment, there are five different types of threats, i.e., terrain threats, radar, anti-aircraft artillery, missiles, and sudden threats.Of these, the sudden threats refer to threats that suddenly appear (i.e., radar, anti-aircraft artillery, and missiles).Sudden threats can be static or moving.The modeling process of the first four threats has been described in detail in our previous work [38].This paper mainly describes sudden threats and environmental limitations.
Considering the time uncertainty and state uncertainty of sudden threats, we describe the sudden threats in Definition 2.

Definition 2 (sudden threats).
For planning space X , all UAVs in UAV N fly according to the currently planned cooperative path, and the flight time interval is t s , t g .At t ∈ t s , t g , m new threats X threat new : X threat 1 , . . ., X threat m are sudden threats.The position change rule X threat new is expressed as: where ∆X threat m = ∅ means that the sudden threat is static, ∆X threat m = f (x, t i ) means that the dynamic sudden threat is moving, and ∆X threat m = unpredictable means that the threat movement is unpredictable.
The definition of environmental limitations is given in Definition 3.

Definition 3 (environmental limitations).
For any UAV i in-planning space X , the limit of the planning environment is: where z (x i ,y i ) DMG represents the terrain threat altitude when the UAV is located at p i and X threat = X threat known ∪ X threat new represents the known radar, anti-aircraft artillery, and missile threats, as well as sudden threats.

Four-Dimensional Coordination Constraints
The four-dimensional coordination constraints consist of spatial coordination constraints and time coordination constraints.In this paper, it is assumed that any UAV i in UAV N flies at a uniform speed in any speed interval [V min , V max ]; that is, the estimated time of arrival (ETA) interval of UAV i is t i min = σ i /V max , t i max = σ i /V max .

Definition 4 (four-dimensional coordination constraints).
The cooperative path of all UAVs in UAV N must meet the following requirements: 1.At moment t, the distance D i,j (t) between any two UAVs is: where D min is the minimum safe distance.

2.
The set T of ETA of all UAVs is:

Dynamic 4D Cooperative Path Planning Formulation
A set of 4D cooperative paths for UAV N can be obtained only when Definitions 1, 3, and 4 are met.
Problem 1 describes the 4D dynamic cooperative path planning problem of the multiple UAVs studied in this paper.
Problem 1 (dynamic 4D cooperative path planning).In the current dynamic planning space X , the set starting nodes and goal nodes [s 1 , g 1 ], . . . ,[s n , g n ] of all UAVs in UAV N are given.A group of paths σ N = {σ 1 , . . . ,σ n } must be found so that all paths in σ N meet the necessary constraints of Definitions 1, 3, and 4.

Four-Dimensional Dynamic Cooperative Path Planning Algorithm
The overall frameworks of the 4D dynamic cooperative path planning algorithm, the DPRRT* algorithm, and the HAPF algorithm will be introduced below.

Overall Framework
Algorithm 1 shows the executive process of the 4D dynamic cooperative path planning algorithm for multiple UAVs.
In the pre-cooperative planning based on the initial environment (lines 02-07), the algorithm first initializes the search tree T i and sets the start and goal [s i , g i ] of any UAV i , as well as the initial planning space X .Then, the classical RRT* shown in Algorithm 1 is used to plan the optimal path for all UAVs without considering the 4D cooperative constraints.After the UAV planning is completed, the shortest coordination time t co of the UAVs is determined through the process ETA_Len.Then, the DPRRT* algorithm is used to plan the paths of all UAVs that meet the 4D cooperative constraints in the initial planning space X .Finally, all UAVs execute their respective cooperative paths.
The purpose of the process ETA_Len is to obtain the minimum coordination time t co of all the UAVs by calculating the minimum flight time set for all UAVs and calculating the maximum value in the set.It should be noted that all UAVs can achieve the shortest path (Line 03) through classical RRT*.In other words, all UAVs are able to achieve the shortest flight time without considering 4D cooperative constraints.We assume that the minimum flight time UAV i is t i , the minimum flight time UAV j is t j , and that t i > t j .Since t i and t j are already the shortest flight times, the only way to ensure that UAV i and UAV j satisfy the time coordination constraints and ensure the shortest coordination time is to increase t i so that it is equal to t j .
Len_cost(t) ← [Len_cost 1 (t), . . . ,Len_cost n (t)] 03 : (Len_cost i , σ i (t)) ← RRT*(X , T i , s i , g i ) / * Synchronous execution of all UAVs * / 04 : wait for all UAVs to finish planning their own σ i and Len_cost i 05 : [t co , l co ] ← ETA_Len(σ(t), Len_cost(t)) 06 : σ(t) ← DPRRT*(X , t co , l co , [s 1 , g 1 ], . . . ,[s n , g n ], σ) 07 : all UAVs move along σ(t) 08 : while goal not reach and ∆X threat new detected 09 : X ← X ∪ ∆X threat new 10 : all UAVs move along σ(t 0 , t co ) 17 : end if 21 : all UAVs move along σ(t 0 , t co ) 22 : end while If the UAV detects a sudden threat ∆X threat new in planning space X during flight, the cooperative path based on the previous planning space may be affected, resulting in the failure of the path to cooperate.Therefore, cooperative replanning needs to be executed in the new planning space containing ∆X threat new .We use the Conflicts_occurred process to find the UAV set UAV con f lict (and its conflict path σ con f lict ) affected by ∆X threat new from among all UAVs.Then, HAPF is applied to all UAVs in the set UAV con f lict for the efficient online onboard replanning of paths.As HAPF is a local conflict avoidance algorithm, it can only ensure that the UAV meets the spatial coordination constraints in the 4D coordination constraints.Therefore, we choose the path node σ con f lict which, as the generation node of the attractive potential field of UAV con f lict , is not reached and not affected by the threat, thus enabling UAV con f lict to return to the previous 4D cooperative path after avoiding the sudden threat.At the same time, the UAVs not affected by ∆X threat new continue to fly along the previously planned coordinated path.When the UAV con f lict avoids sudden threats, the DPRRT* is used to replan the path from the current position of the UAVs to their destination, while meeting the 4D cooperative constraints for all UAVs.If the planning space is not changed, all UAVs fly along the current initial path until they reach the goal and the algorithm is terminated.Otherwise, when new sudden threats are detected, lines 08-22 of Algorithm 1 are executed until the goal is reached.

Dynamic Priority RRT* Algorithm
In the cooperative path planning layer, we used the proposed DPRRT* to complete the 4D cooperative path planning of multiple UAVs based on the current initial planning space.The pseudo-code for DPRRT* is shown in Algorithm 2. The core idea of DPRRT* is to enable UAVs with lower priority to avoid conflict with the planned paths of the UAVs with higher priority when planning the cooperative paths in order to meet the requirements of 4D cooperative constraints.
Considering that the prioritization of UAVs usually has a great impact on the efficiency and robustness of cooperative path planning, we designed a dynamic heuristic prioritization strategy (i.e., the HeuDyn_Proritization process) to assign the priorities of all UAVs, where UAV j i represents the priority of UAV i as j and σ(t) represents the planned cooperative path of the UAVs based on the previously planned space.Thereafter, the cooperative paths of all UAVs are planned in order of priority from high to low.In addition, in order to simultaneously meet the time and space cooperative constraints, this paper improves the classical RRT* to an RRT* algorithm based on 4D cooperative constraints, named 4DRRT* (i.e., the 4DRRT* process), so that each UAV is able to quickly plan a path that meets the cooperative 4D constraints.Note that before planning the lower-priority UAV paths, the planned paths of the high-priority UAVs need to be added to the space-time occupied area H j to ensure that the planned paths of the low-priority UAVs do not conflict with those of high-priority UAVs.The dynamic heuristic prioritization strategy and 4DRRT* algorithm will be introduced in detail below.

Dynamic Heuristic Prioritization Strategy
A key factor affecting the efficiency and robustness of DPRRT* is the prioritization strategy.If the prioritization is not carefully defined based on a specific problem scenario, it may lead to low-priority UAVs spending more computing time in planning to avoid the occupied area in space and time or even making the planning path infeasible.To solve this problem, a dynamic heuristic prioritization strategy is proposed.Whenever the planning space changes, this strategy can reasonably and dynamically adjust the priority of all UAVs so that the DPRRT* algorithm is able to find a 4D cooperative path planning solution with higher efficiency and robustness.
Note that the generation of cooperative paths needs to meet the 4D cooperative constraints based on time consistency and no conflicts of space.Since the HAPF described below can effectively avoid sudden threats in a local range, as well as avoid spatial conflicts between UAVs and allow the UAV to return to the cooperative path based on the previous spatial planning cycle, when DPRRT* is used to replan the cooperative path, not only does the original cooperative path pass through conflict-free space but the time coordination constraint is broken.If the flight time of the UAV, UAV i , along the original planned path is significantly different from the shortest cooperation time t co based on the current planning space, the difference between the replanned cooperation path for UAV i and the original path will be even greater.Furthermore, when the cooperative path is replanned for UAV i , the probability of conflict with other UAVs' occupied space and time area is greater.Therefore, it is necessary to ensure that the priority of UAV i is assigned to a higher level by planning its path first to improve planning efficiency.The prioritization evaluation function f UAV i HDP of UAV i is calculated as follows: where σ i (t 0 , t co ) represents the original planned path of the UAV not flying, t co represents the shortest cooperation time, and v i represents the speed of UAV i .It is not difficult to see that the prioritization evaluation function f HDP of UAV i is positively related to the amount of time cooperation violations.Therefore, the priority ranking definition of UAVs based on prioritization evaluation functions can be given.

Definition 5 (prioritization strategy based on prioritization evaluation function). A group of UAVs UAV
of UAV N is mapped from large to small in order to generate the set in turn corresponds to the {1, . . . ,N} priority of UAV N .
Considering that there may be a situation where some UAVs are not affected by sudden threats, they will continue to fly along their originally planned path.The prioritization evaluation function of these UAVs may be the same.For such UAVs, their priority order remains unchanged.The steps of the dynamic heuristic prioritization strategy are shown as the HeuDyn_Proritization function of Algorithm 2.

4DRRT* Algorithm
As a decoupled cooperative path planning algorithm, the DPRRT* algorithm needs to carry out path planning for each UAV to meet the 4D cooperative constraints in order of priority after ranking the priorities of all UAVs in question.Because there is a space-time occupation area composed of high-priority UAV paths and the conflict-free space and time coordination constraints of paths need to be met simultaneously during planning, the classical RRT* cannot meet these planning requirements.
Therefore, this paper proposes that the 4DRRRT* in the single UAV cooperative path planning process of DPRRT* be used in order to ensure that all UAVs are able to plan paths that meet the 4D cooperative constraints.Firstly, a cost function based on the 4D cooperation constraints is proposed to evaluate the expansion of the tree.In addition, we introduce the time variable into the RRT* algorithm in order to detect whether there is a spatial conflict between the growth tree and the space-time occupied area.On this basis, a tree node expansion method based on the 4D cost function is designed to guide tree growth.Similar to the tree node expansion method, we also used the 4D cost function to guide the updating and rewiring of nodes in the neighborhood.The 4DRRT* algorithm can ensure that the planned cooperative path meets the performance constraints, environmental constraints, and 4D cooperative constraints of UAVs at the same time to provide a 4D cooperative path based on the current planning space for multiple UAVs.Next, we introduce 4DRRT* in terms of two aspects: the 4D cost function and the algorithm process.

The 4D Cost Function Design Based on the 4D Cooperation Constraint
Through the process of ETA_Len, we can obtain the shortest cooperation time t co based on the current planning space and then calculate the 4D cooperation path length l i co of any UAV, UAV i .If the path length planned by UAV i is essentially the same as that planned by l i co and does not collide with threats and other UAVs, then it can be said that the planned path of UAV i meets the 4D cooperation constraints.Therefore, we designed a cost function that is able to meet the time cooperation constraints, namely the 4D cost function, to evaluate and optimize the tree structure constructed by the algorithm to ensure that the planning path can meet the 4D cooperation constraints.
Unlike the classical RRT* algorithm, the 4D cost function no longer aims to optimize the shortest path but to optimize the length of the cooperative path.The 4D cost function of any UAV is defined as follows: (10) where f T i (x) = g T i (x) + λ h h(x).g T i (x) represents the actual distance from the root node of tree T i to node x, h(x) represents the estimated distance from node x to the goal of the UAV i , and λ ∈ R + represents the coefficient.

2.
The 4DRRT* Algorithm Process Functions 4DRRT*, 4DRRT *_rewire, and 4D _Extand in Algorithm 2 show the overall flow of the 4DRRT* algorithm, taking UAV i as an example.
After obtaining the sampling nodes in the current planning space through to the end node-guided offset sampling strategy, the algorithm enters the process of tree node expansion based on the 4D cost function guidance (i.e., 4D _Extand).Similar to classical RRT*, 4DRRT* first calculates the expansion distance between the tree node x T and the sample node x rand .It then uses the constraints defined in Definition 1 (corresponding to UAV_free), Definition 3 (corresponding to obstacle_free), and Definition 4 (corresponding to Conflict_free) to determine the tree node x T .If the constraints are met, the 4D cost function is used to calculate the time cooperation cost.After all tree nodes are calculated, the tree node with the lowest cost for the expansion is selected.
After obtaining the extended new node x new guided by the 4D cost function, a neighborhood centered on x new is built and the node in the tree structure within its range is found in order to form the set X near .Then, the optimization process of the neighborhood parent node is begun (i.e., 4DRRT* _rewire is called for the first time).First, the constraint judgment is performed on the edge (x new , x near ) composed of the new node x new and the node x near in the neighborhood.If the UAV performance constraints, environmental constraints, and spatial cooperation constraints in the 4D cooperation constraints are met, the cost of taking x near as the potential parent node of x new is calculated.After all the neighborhood nodes are calculated, the region node x near with the lowest cost is selected as the optimal parent node of x new .Similarly, the 4DRRT *_rewire function is called for the second time in order to complete the reconnection of the tree structure.The purpose of this call is to optimize the parent node of x near by taking x new as the potential parent node of all neighborhood nodes x near to reduce the cost.
When the new node x new is extended to the terminal range of UAV i , a potential 4D cooperative path σ i (s i , g i ) is obtained through the backtracking tree structure.If the deviation between the path length Cost T (s i , g i ) of σ i (s i , g i ) and the cooperative path length is within the tolerance ε, the algorithm is terminated and outputs the 4D cooperative path with σ i (s i , g i ) as UAV i .Otherwise, the tree structure continues to expand in order to reduce the 4D cost until the tolerance between σ i (s i , g i ) and the cooperative path length l co is less than ε.

Sudden Threat Avoidance Planning Based on the HAPF Algorithm
When sudden threats occur in the planning space and block the 4D cooperative path of the UAVs, local path adjustment is required for the affected UAVs to avoid these sudden threats and space conflicts with other UAVs.In addition, because the UAV is in flight, greater efficiency of local obstacle avoidance planning algorithms is usually required.As an online onboard local obstacle avoidance planning algorithm, the APF has the characteristics of simple calculation, high planning efficiency, and can deal with various state threats, which are able to meet the local cooperative obstacle avoidance path planning of multiple UAVs.However, the traditional APF still has some shortcomings, such as unreachable targets, the risk of falling into local minima, and path oscillation when approaching threats, which lead to the failure of local obstacle avoidance in cooperative path planning.
Therefore, in order to ensure the efficiency and robustness of local dynamic cooperative path planning, this paper proposes a heuristic artificial potential field algorithm (HAPF).The proposed algorithm can effectively solve the shortcomings of the traditional APF to ensure the success rate of local cooperative collision avoidance path planning.The HAPF algorithm will be introduced in detail regarding the following three aspects: the improved APF function, heuristic sub-target strategy, and memorial potential field force strategy.

Improved APF Function
It can be seen from the traditional APF method that the attraction force generated by the attraction potential field decreases with the continuous approach of UAVs, while the repulsion generated by the obstacle repulsion potential field increases when the UAV approaches an obstacle.If the threat appears near the end node of the local space, a situation will arise where the repulsion force is greater than the attraction force, resulting in the UAV not being able to continue to fly to the end node under the effect of the repulsion force.This situation is described as that of an unreachable target.In order to solve the problem, the potential field function needs to be improved to ensure that the attraction force can continue to pull the UAV to its goal even when the UAV is near the goal and threats are extant.The goal is guaranteed to be the global minimum node of the resultant potential field, thus enabling the UAV to stop stably when it reaches its goal.
Therefore, the design strategy of the repulsion potential field function in [39] is introduced in this paper, i.e., the distance between the end node and the UAV is added to the repulsion potential field function.The improved repulsion potential field function is as follows: where n ∈ R + .Then the repulsion function is: where: In the above formula, it can be seen that F rep1 is the repulsive force of the obstacle against the UAV and F rep2 is the attractive force of the UAV to the goal.As the UAV approaches the goal, F rep1 gradually decreases to zero.Therefore, under the effect of the improved potential field resultant force, the UAV will still approach the end node even if there is a threat nearby and finally reach the goal.
Furthermore, considering the constraint problem of UAVs, it is necessary to add the constraints of the maximum steering angle φ max and the maximum climbing/diving angle γ max of UAVs when calculating the resultant force direction.The steering angle constraint condition is −φ max ≤ ∆φ ≤ φ max and the constraint of climb/dive angle is −γ max ≤ ∆γ ≤ γ max .∆φ and ∆µ are, respectively, the angular changes of the angle between the resultant forces of the current initial step and the next step in the horizontal plane and the vertical plane.

Heuristic Sub-Target Strategy
When using the APF for local conflict avoidance planning, the goal is designed as the global minimum of the potential field, so that the UAV moves in the descending direction of the potential field.However, in a complex environment involving other UAVs and various threats, there may be local potential field minima other than the goal which causes the UAVs to enter the local potential field minima by mistake, leading to the failure of path planning.Note that the reason for the local minimum is that the resultant force is zero.Therefore, the ideas to solve this problem are generally divided into three categories: optimization approaches based on control theory [40], changing repulsive potential field [41], and changing attractive potential field [42].The optimization method based on control theory usually uses the optimal control method to get the UAV out of the trap when encountering the local minimum.However, this paper mainly studies the path planning algorithm of general cooperative UAVs and does not involve the description of some control variables.In addition, changing the repulsion potential field means placing a virtual obstacle between the UAV and the target point to make the UAV quickly disengage from the local minimum.This method has certain advantages in the path planning of a single UAV.However, for local collision avoidance planning of multiple UAVs, adding virtual obstacles may affect the obstacle avoidance planning of multiple UAVs.In other words, this method will force multiple UAVs to re-plan the path to increasing the calculation cost.Similarly, changing the attractive potential field refers to setting virtual sub-target points for the UAV trapped in the local minimum trap to quickly break away from the local minimum by changing the attractive potential field.After the UAV reaches the sub-target point, change the attractive potential field to the final target point to make the UAV continue to fly.This method has the same characteristics as adding virtual obstacles; that is, it is simple to calculate without additional calculation process.Therefore, real-time performance can be guaranteed.Meanwhile, the virtual sub-target point is only effective for the affected UAV, thus eliminating the disadvantage that the virtual obstacle affects other UAVs.However, random setting of sub-target points may lead to high cost of path length or large angle deflection of UAV when escaping from the local minimum.Therefore, in order to further improve the sub-target guidance strategy, this paper proposes a heuristic sub-target strategy to optimize the selection of sub-target points.This strategy designs a cost function that considers both path length and angle constraints, and further optimizes the path on the basis of ensuring that the UAV can escape the local minimum, so as to improve the path quality on the basis of ensuring real-time and reliability.
The heuristic sub-target strategy adopts the idea of transforming multi-objective optimization problems into single-objective optimization problems to handle sub-target point selection [43] and introduces it into the HAPF algorithm.The strategy includes four steps:

•
Determining whether the UAV is trapped in a local minimum trap; • If it is trapped, set the sub-target node selection range according to the UAV constraints;

•
Set the heuristic function and selecting the sub-target node with the minimum cost as the generating node of the gravitational potential field;

•
After reaching the sub-target node, change the target node to the final target node.
The main process and algorithm are as follows: Before the UAV falls into the local minimum, the UAV always flies in the direction of low potential energy.If the UAV falls into the local minimum node, it will vibrate near this node; that is, the resultant energy will vibrate.Therefore, we can set the condition of falling into the local minimum as: where U all is the combined potential energy at the current initial step and U all (t + ∆t) is the combined potential energy at the next step.When falling into the local minimum trap, the potential energy at the next time will be greater than that at the current initial time, so ε ≥ 0. If the decision is correct, escape processing will be carried out through the algorithm.
Considering the flight constraints of UAVs, ε is set to zero in this paper.
After detecting that the UAV is trapped in a local minimum, a partial virtual sphere R t is established with the current path node X j as the center and r min as the radius, as shown in Figure 1, and the calculation of r min is as follows: point selection [43] and introduces it into the HAPF algorithm.The strategy includ steps: • Determining whether the UAV is trapped in a local minimum trap; If it is trapped, set the sub-target node selection range according to the UA straints;

•
Set the heuristic function and selecting the sub-target node with the minimu as the generating node of the gravitational potential field; • After reaching the sub-target node, change the target node to the final target The main process and algorithm are as follows: Before the UAV falls into the local minimum, the UAV always flies in the d of low potential energy.If the UAV falls into the local minimum node, it will vibr this node; that is, the resultant energy will vibrate.Therefore, we can set the cond falling into the local minimum as: where all U is the combined potential energy at the current initial step and ( all U t the combined potential energy at the next step.When falling into the local minimu the potential energy at the next time will be greater than that at the current initial 0 ≥ ε .If the decision is correct, escape processing will be carried out through th rithm.Considering the flight constraints of UAVs, ε is set to zero in this paper.
After detecting that the UAV is trapped in a local minimum, a partial virtua The scope of the virtual sphere is determined by the constraints of the UAV scope of the sphere is determined by the maximum steering angle max φ and the ing/diving angle max γ of the UAV, respectively.In the horizontal plane, the rang t φ is selected as Then, in the vertical plane, the range angl selected as If the resultant force of UAV at path node j X is ( , , ) , then: The scope of the virtual sphere is determined by the constraints of the UAV and the scope of the sphere is determined by the maximum steering angle φ max and the climbing/diving angle γ max of the UAV, respectively.In the horizontal plane, the range angle φ t is selected as φ − φ max ≤ φ t ≤ φ + φ max .Then, in the vertical plane, the range angle µ t is selected as −γ max ≤ γ t ≤ γ max .
If the resultant force of UAV at path node X j is F = ( f x , f y , f z ), then: The partial sphere R t , which determines the range, is shown in Figure 1.R t is placed around uniformly distributed n 0 nodes to construct a sub-target node set {G 1 , G 2 , . . .G n }. ω 1 is the angle between X j G i and X j G 0 , and ω 2 is the angle between X j G i and G i G 0 .∆ω =|ω 2 − ω 1 | is set as the angle cost of selecting G i as the sub-target node.The end node is G 0 .Then, the heuristic function can be defined as: where ξ 1 and ξ 2 are the cost function weights.If ξ 1 > ξ 2 , heuristic sub-target nodes with lower length costs can be obtained, but there may be significant angle changes.If ξ 1 < ξ 2 , heuristic sub-target nodes with a lower cost of angle change can be obtained, but the planned path length to the sub-target will increase.In this paper, we choose ξ 1 = ξ 2 = 0.5.D is the cost function of the relative distance between the sub-target and the final target to reduce the path loss.A is the cost function of angle conversion to smooth the path and avoid an overly large turning angle of the UAV.G i is any node in the node group.Of these parameters: Then, the optimal sub-target node G i can be selected through the heuristic function: After the UAV reaches the sub-target, the attractive potential field is constructed at the goal, so that the UAV will fly toward the goal.

Memorial Potential Field Force Strategy
When using the potential field method to plan local collision avoidance near threats, UAVs will dominate the attraction of the current path node, and the planned path direction will tend toward the target node.However, at the next path node, due to the proximity to the threat, the repulsive force may dominate, and the planned path will tend to deviate from the direction of the target node.The cycle of the two situations may cause the planned path to oscillate.
UAV flight constraint optimization or some path smoothing methods are usually used to improve the local path oscillation problem existing in APF.Flight constraint optimization is to improve the oscillation problem by restricting the maximum turning angle of the UAV.However, when the vibration amplitude between adjacent nodes is large, the UAV will repeatedly turn back and forth at the maximum turning angle during flight and the vibration will still exist, which can also cause certain potential safety hazards.Therefore, the constraining of the flight angle is only applicable to some situations with small vibration amplitudes.In addition, the approach of trajectory optimization can be used to improve the vibration.Usually, the approach of trajectory optimization needs to obtain the vibration path and then perform a secondary calculation to smooth the trajectory, which does not meet the real-time requirements of online local obstacle avoidance planning in this paper.Furthermore, inspired by sliding mode control in the early work, we try to use the sigmoid function to smooth the oscillation path.This approach can obviously improve the vibration, but there is a certain lag.Therefore, we adopted the memory resultant strategy proposed in this paper to improve local oscillation.This approach has the following two advantages.

•
Simple calculation, high efficiency, and good real-time.
The memory resultant force strategy only needs to record the resultant force of the previous step and fit it with the resultant force of the current step, without additional complex optimization or calculation processes, so it can ensure the real-time performance of the local obstacle avoidance planning level.

•
Capable of adaptive adjustment.
The memory resultant force is calculated by the adjacent two-step resultant force.When the resultant force deviation of the two steps is large, i.e., when the potential planned path has a large oscillation, the direction of the memory resultant force can obtain a large deflection compared with the current step resultant force, so it can effectively improve the oscillation.When the resultant force deviation of the two steps is small; that is, when the potential planned path has a small oscillation, the resultant force deviation of the memory resultant force and the current step are also relatively small, so as to smooth the collision avoidance path.Therefore, this paper proposes a memory resultant strategy to dynamically improve local path oscillations.This strategy can still suppress the vibration when the resultant force direction of adjacent path nodes changes significantly.After UAV i calculates the resultant force direction to the next path node X j+1 , it still retains the resultant force direction to the path node X j .The resultant force on the two path nodes is fused to reduce the variation of the resultant force angle and improve local vibration.The calculation method is as follows: where F j and F j+1 are the resultant forces of the j and j + 1 path nodes, respectively.λ 1 and λ 2 are weights, and λ 1 + λ 2 = 1.The value of the weight depends on the change of the resultant force angle of path node j and path node j + 1, i.e., the amplitude of the oscillation.In the simulation, we found that if the resultant angle change ∆α ≤ π 4 is small, then λ 1 ≤ λ 2 should be adjusted.If the resultant angle change is π 4 < ∆α ≤ π 2 , λ 1 > λ 2 should be adjusted.

Time Complexity Analysis
This section mainly analyzes the time complexity of the proposed algorithm (i.e, Algorithm 2).The proposed algorithm is mainly composed of three levels; namely, the pre-cooperative planning level, the cooperative path planning algorithm for a single UAV level based on DPRRT* (i.e., Algorithm 2), and the local dynamic threat avoidance planning level based on HAPF.
The purpose of the pre-cooperative planning level is to initialize the path of UAVs, i.e., to plan the optimal path for all UAVs without considering the 4D cooperative constraints.Therefore, in this level, we adopt the synchronous decentralized architecture and use RRT* algorithm to plan the optimal path for all UAVs.The pseudocode of the RRT* algorithm is shown in Algorithm A1 of "Appendix A".As described in [44,45], the computational complexity of the RRT* algorithm is similar to that of the RRT algorithm and RRG algorithm, which run in time O(n log n).The other steps of the pre-planning level are only executed once, and the time complexity is very low.Therefore, it can be proved that the computational complexity of the pre-planning level of the proposed algorithm is O(n log n).
Then, we derive the time complexity of the DPRRT* algorithm (i.e., Algorithm 2).As can be seen from the main program (i.e., MAIN ALGORITHM) of Algorithm 2, DPRRT* is mainly composed of two parts; namely, the dynamic hydraulic prioritization strategy (FunctionHeuDyn_Proritization) and the 4DRRT* algorithm (Function 4 DRRT*).The time complexity of the dynamic hydraulic prioritization strategy is O(n).We derive the time complexity of 4DRRT* by comparing it with RRT* [44].The main difference between the two algorithms is the cost function.The purpose of the cost function of RRT* is to calculate the shortest path, while the purpose of the cost function of 4DRRT* is to calculate the path with the shortest ETA error.The different cost functions lead to the difference between the expansion process of the two algorithms and the update process of the neighborhood optimal node.However, the highest order of the two cost functions is the same, so the time complexity of the above two processes is basically the same.In addition, the collision checking process (Conflict_free) and the sampling process (Algorithm 2 Line 5-9 and Algorithm A1 Line 3-7) of the two algorithms are the same, and they can be executed in a certain number of steps.Therefore, for the 4DRRT* and RRT*, since each iteration has only one sampling, they have the same number of nodes in the iteration i, i.e., V RRT * i = V 4DRRT * i .Therefore, the two algorithms run in time O(log n) in iteration i.Therefore, when the number of iterations i tends to infinity, similar to [40], we can prove that the time complexity of the two algorithms is basically the same, which runs in time O(n log n).Therefore, when the number of UAVs is δ, the DPRRT* algorithm (Algorithm 2) runs in time O(δn log n).
The proposed HAPF algorithm is improved from the APF algorithm.Assume that the worst iteration number of HAPF is n, where n → ∞ .In other words, after n steps expansion, HAPF reaches the target point.Then, HAPF can run in time O(n).
Corollary 1. 4D Dynamic Cooperative path planning algorithm runs in time O(δn log n), where n is the number of samples and δ is the number of UAVs.

Simulation Results and Discussion
In this section, we carried out a simulation on the 4D dynamic cooperative path planning algorithm of UAVs and compared the proposed algorithm's cooperative path planning layer and local anti-collision planning layer to relevant algorithms, respectively, thus proving the effectiveness, robustness, and high efficiency of the algorithm.

Environments and Parameters
In the simulation of the proposed algorithm, we designed two types of simulated scenarios: the normal scenario (Scenario 1) and the complicated scenario (Scenario 2).For each UAV, we assume that they all have their own independent CPU computing units, which are able to achieve synchronous preplanning and local anti-collision planning.At the same time, each UAV has a completely reliable zero-delay communication unit, thus enabling the transmission and reception of path information.In addition, all UAVs in the two scenarios take off at the same time.

1.
Normal Scenario (Scenario 1).In this scenario, we set up three UAVs with the same performance.All UAVs take off from different starting nodes at the same time and arrive at the same destination.Figure 2 shows the planning environment of the Normal Scenario, including radar, missiles, artilleries, no-fly towers, moving threats, and terrain threats based on elevation maps.Of these, moving threats are sudden unknown threats, and UAVs need to conduct dynamic cooperative path planning to avoid them during flight.See Table 1 for relevant parameters of general scenarios.
Drones 2023, 7, x FOR PEER REVIEW 20 of 34 algorithm in the cooperative planning layer to replan the 4D cooperative path of all UAVs.Due to the constraints of spatial cooperation, the cooperative paths of the three UAVs do not collide, thus meeting the requirements of spatial cooperation.At the same time, combining the deviation between the expected cooperation path and the actual planned cooperation path in Table 4, it can be seen that the deviation percentage is within the allowable range, meeting the time cooperation requirements.Based on the above simulation results and analysis, the proposed algorithm is able to realize the planning of a 4D dynamic cooperative path of all UAVs in the Normal Scenario, thus proving the effectiveness of the proposed algorithm.

2.
Complicated Scenario (Scenario 2).In this scenario, we set up five UAVs with the same performance.All UAVs take off from different starting nodes at the same time and arrive at the same goal.Figure 8 shows the planning environment of the Complicated Scenario, including radar, missiles, artilleries, no-fly towers, moving threats, and terrain threats based on elevation maps.Of these, sudden threats appear many times at different times, and their status is static or moving.In addition, a sudden threat group (simulating the possible local minimum traps in the local collision avoidance planning) is set in Scenario 2. See Table 2 for the relevant parameters of the Complicated Scenario.

Scenario 1: Normal Scenario
In Scenario 1, the relevant parameters of the HAPF are set as follows: the attractive potential field coefficient is k att = 8 and the repulsive potential field coefficient is k rep = 30.The influence distance of obstacle threat is ρ o = 20 m, the unit time step is t = 0.01 s, and the adjustment factor is n = 0.7.The maximum steering angle is φ max = 60 • and the maximum climbing/diving angle is γ max = 35 • .The generating node of the attractive potential field is the path node [157,296,50].Figure 2 shows the 4D cooperative path (i.e., the generation path of the initial cooperative planning layer) when the UAV does not detect the moving threat.Figures 3-6 show the collision avoidance path of the UAV at different times after detecting the moving threat (i.e., the generation path of local collision avoidance planning layer).The 4D dynamic cooperative replanning path of the UAV is shown in Figure 7 (i.e., the generation path of the cooperative planning layer).Table 3 shows the shortest cooperation time for 4D cooperative replanning.See Table 4 for the deviation ratio between the actual path length obtained by the 4D cooperative path algorithm and l co .(20,200,40) (20, 250, 40) (20,300,40) It can be seen in Figures 3-6 that, under the guidance of the HPAF algorithm of the local anti-collision planning layer, UAV3 is able to evade sudden moving threats.It should be noted that the path of UAV2 in Figure 6 shows that the moving threat could involve a collision.However, HAPF is an online planning algorithm.Each time a collision avoidance path in a unit time step is planned, the UAV flies along the planned path.When UAV3 has flown for 6 s, it reaches the path node with coordinates [147.6170,326.0181, 49.2858].When t = 6 s, the path node avoids the moving threat, so there is no collision.Finally, after 7.59 s of flight, UAV3 reaches the sub-target node; that is, the preplanned path node [157, 296, 50].When t = 7.59 s, UAV3 has completed the avoidance of the moving threats.While UAV 2 evades the sudden moving threat, other UAVs fly along the initial cooperative path.
At this point, the 4D cooperative planning layer comes into play.It can be seen in Table 3 that the shortest cooperation time is 12.22 s.Based on this, the l co of each UAV can be obtained to ensure time coordination.It can be seen in Figure 7 that after HAPF is used to circumvent the sudden moving threat, the proposed algorithm uses the DPRRT* algorithm in the cooperative planning layer to replan the 4D cooperative path of all UAVs.Due to the constraints of spatial cooperation, the cooperative paths of the three UAVs do not collide, thus meeting the requirements of spatial cooperation.At the same time, combining the deviation between the expected cooperation path and the actual planned cooperation path in Table 4, it can be seen that the deviation percentage is within the allowable range, meeting the time cooperation requirements.

Scenario 2: Complicated Scenario
In Scenario 2, the relevant parameters of the HAPF are set as follows: the attractive potential field coefficient is =8 .It can be seen in Table 2 that we have set up two kinds of sudden threats at different times.The first one is a mobile threat and the second one is a static threat group simulating a local minimum trap.Figures 8-10 show the pre-planned 4D coordination path and the satisfaction of 4D coordination constraints of the path when t = 0 s. Figure 11 shows the local collision avoidance planning path of some UAVs and the 4D cooperative replanning path of all UAVs after the first sudden moving threat.Figures 12 and 13 show the satisfaction of 4D cooperative constraints of the pre-planned path based on the current planning space.Figure 14 shows the dynamic planning path of the proposed algorithm when affected by the sudden threat group for the second time, including local collision avoidance planning and 4D cooperative path replanning.Figures 15 and 16 show the satisfaction of 4D cooperative constraints of the planned path.Based on the above simulation results and analysis, the proposed algorithm is able to realize the planning of a 4D dynamic cooperative path of all UAVs in the Normal Scenario, thus proving the effectiveness of the proposed algorithm.

Scenario 2: Complicated Scenario
In Scenario 2, the relevant parameters of the HAPF are set as follows: the attractive potential field coefficient is k att = 8 and the repulsive potential field coefficient is k rep = 30.The influence distance of obstacle threat is ρ o = 20 m, the unit time step is t = 0.01 s, and the adjustment factor is n = 0.7.The maximum steering angle is φ max = 60 • and the maximum climbing/diving angle is γ max = 35 • .It can be seen in Table 2 that we have set up two kinds of sudden at different times.The first one is a mobile threat and the second one is a static threat group simulating a local minimum trap.Figures 8-10 show the pre-planned 4D coordination path and the satisfaction of 4D coordination constraints of the path when t = 0 s. Figure 11 shows the local collision avoidance planning path of some UAVs and the 4D cooperative replanning path of all UAVs after the first sudden moving threat.Figures 12 and 13 show the satisfaction of 4D cooperative constraints of the pre-planned path based on the current planning space.Figure 14 shows the dynamic planning path of the proposed algorithm when affected by the sudden threat group for the second time, including local collision avoidance planning and 4D cooperative path replanning.Figures 15 and 16 show the satisfaction of 4D cooperative constraints of the planned path.It can be seen in Figure 9 that the shortest distance between each pair of UAVs at the same time is larger than the minimum safety distance (2 m) we set, so it can be shown that the pre-planned path meets the space constraints in 4D cooperative constraints.At the same time, it can be seen in Figure 10 that the ETA tolerance of the five UAVs executing the pre-planned path is within 0.2 s, i.e., they meet the time cooperative constraints.When t = 4.7 s, a sudden moving threat appears in the planning space for the first time.Compared with Scenario 1, the sudden moving threat in Scenario 2 will affect two UAVs at the same time, which is more complex.The collision avoidance planning of two UAVs based on the HAPF algorithm will be executed online at the same time, and the planning needs to avoid sudden threats while preventing conflicts with other UAVs.It can be seen in Figure 9 that the shortest distance between each pair of UAVs at the same time is larger than the minimum safety distance (2 m) we set, so it can be shown that the pre-planned path meets the space constraints in 4D cooperative constraints.At the same time, it can be seen in Figure 10 that the ETA tolerance of the five UAVs executing the pre-planned path is within 0.2 s, i.e., they meet the time cooperative constraints.
When t = 4.7 s, a sudden moving threat appears in the planning space for the first time.Compared with Scenario 1, the sudden moving threat in Scenario 2 will affect two UAVs at the same time, which is more complex.The collision avoidance planning of two UAVs based on the HAPF algorithm will be executed online at the same time, and the planning needs to avoid sudden threats while preventing conflicts with other UAVs.Figure 11 shows the path of local collision-checking planning and 4D cooperative replanning.It can be seen in Figures 11 and 12a that UAV1 and UAV2 can avoid threats in the environment during local collision-checking planning, and two UAVs can maintain a safe distance to avoid conflicts.After completing local collision-checking planning, all UAVs need to conduct cooperative path replanning to meet 4D coordination constraints.It can be seen in Figure 12b that the cooperative replanning path can meet the spatial constraints.In addition, Figure 13 proves that the collaborative replanning path can meet the time coordination constraints.
Similarly, when t = 16.5 s, the planning space changes for the second time.We set up a group of sudden threats to simulate the planning of escaping from the local minimum trap in local collision avoidance.It can be seen in Figure 14 that after being affected by threats, UAV3 can plan the path to escape from the local minimum.After that, all UAVs continue to replan the 4D cooperative path, so that all UAVs can reach the goal without conflict.It can be seen in Figures 15 and 16 that the second cooperative path replanning can plan the path satisfying 4D cooperative constraints for all UAVs.
Based on the above analysis, we can prove that the proposed algorithm can still complete the dynamic, 4D cooperative path planning in complex scenarios.Therefore, we prove the effectiveness of the proposed algorithm in different scenarios.

Comparison and Algorithm Performance Analysis
In this section, we compare the 4D cooperative path planning layer (DPRRT* algorithm) and local collision avoidance planning layer (HAPF algorithm) of the 4D dynamic cooperative path planning algorithm with some related algorithms.On the basis of comparison, the performance of the 4D dynamic cooperative path planning algorithm is analyzed.

Comparative Simulation and Performance Analysis of Cooperative Path Planning Layer
The DPRRT* algorithm is a decoupled cooperative planning algorithm.The generation of the cooperative path can be divided into two stages; namely, the planning sequence sequencing and the single machine cooperative path planning.At the same time, it should be noted that the existing priority planning algorithms and their derivative algorithms have not involved in-depth research on the 4D cooperative path planning problem.Therefore, in order to eliminate the impact of considering both time and space cooperation constraints on the performance of the algorithm, we uniformly set the cooperation constraints that the path needs to meet in terms of 4D cooperation constraints in comparison with related algorithms to more accurately compare and analyze the performance of the DPRRT* algorithm.In the context of the scenario of cooperative path planning based on the decoupling method, we introduced the prioritized A* algorithm [31], the traditional priority planning algorithm (RPP) [26], and the coupling degree-based heuristic prioritized planning algorithm (CDH-PP) [46] for comparison.
This simulation compares the two types of mission scenarios mentioned in Sections 4.2 and 4.3.To fully analyze the performance of the DPRRT* algorithm in both the Normal Scenario and the Complicated Scenario, four algorithms are used for 4D cooperative path planning of three groups of UAVs with different numbers, where the number of UAVs is set as {2,4,6}.In the two scenarios, the starting and goal nodes of all UAVs are randomly generated in the planning space and 30 random problem situations are generated for each scenario.
The characteristics of the four comparisons are as follows: 1.
The longest running time, the shortest running time, and the median running time of the algorithm (analyzing the computational efficiency and robustness of the algorithm).

2.
Compared with the RPP algorithm (the single UAV planning layer uses the RRT* algorithm based on 4D constraints), the acceleration ratio is used to evaluate the impact of different heuristic priority ranking strategies or different underlying algorithms on the cooperative planning algorithm.The formula for calculating the acceleration ratio is . Where t RPP i represents the running time of the RPP algorithm and t ALG i represents the running time of the comparison algorithm.

3.
The ETA time difference is used to evaluate the quality of the path planned by the algorithm.The calculation formula is t ALG maxETA − t ALG minETA , where t ALG maxETA represents the maximum flight time of the planned path of the four algorithms and t ALG minETA represents the minimum flight time of the planned path of the four algorithms.

4.
The success rate can be used to verify the effectiveness of the algorithm.
It can be seen from the comparison of the running time in Figure 17 that the median running time of the DPRRT* algorithm is the shortest and the difference between the maximum and minimum running times is relatively small.In addition, DPRRT* has the highest acceleration ratio.The comparison results of DPRRT* with RPP and CDH-PP prove that the heuristic prioritization strategy based on time cooperation constraints is more reasonable, efficient, and robust.In addition, the running time comparison between DPRRT* and Prioritized A* proves that the single machine cooperative path planning algorithm based on 4DRRT* in DPRRT* has higher efficiency and stability than the A* algorithm.Since the four algorithms have the ability to find the optimal solution, there is little difference in the ETA.In addition, the four algorithms are decoupled algorithms, so they all have a high success rate.
The comparison results of the four algorithms are shown in Figure 17.

Comparative Simulation and Performance Analysis of the Local Anti-Collision Planning Layer
We simulated the three improvements of the HAPF algorithm and compared them to the classic APF algorithm.
In the simulation, an improved repulsive force function was added to solve the problem of the unreachability of the target.The traditional artificial potential field was compared with HAPF in the path collision avoidance planning of obstacles near the target.
In order to verify the effectiveness of the algorithm and ensure the optimal performance of the traditional algorithm, the traditional algorithm was simulated using the best parameters.At the same time, in order to intuitively show the superiority of the improved potential field function, this simulation used the radar threat as the environment model for verification.The potential field parameters of the HAPF algorithm are k att = 8, k rep = 30, ρ o = 2 km, l = 0.2 km, and n = 0.7.According to the simulation data, the generation node of the repulsion potential field corresponding to the last path node is [7.1654, 9.9636, 1.9927] km.The relative distance between this node and the target node is 1.0541 km, which is less than the obstacle influence distance, so obstacle threat near the target node is extant.
3. The ETA time difference is used to evaluate the quality of the path planned by the algorithm.The calculation formula is max  It can be seen from the comparison of the running time in Figure 17 that the median running time of the DPRRT* algorithm is the shortest and the difference between the maximum and minimum running times is relatively small.In addition, DPRRT* has the highest acceleration ratio.The comparison results of DPRRT* with RPP and CDH-PP prove that the heuristic prioritization strategy based on time cooperation constraints is more reasonable, efficient, and robust.In addition, the running time comparison between DPRRT* and Prioritized A* proves that the single machine cooperative path planning algorithm based on 4DRRT* in DPRRT* has higher efficiency and stability than the A* algorithm.Since the four algorithms have the ability to find the optimal solution, there is little Figure 18 shows the comparison between the two algorithms.It can be seen in Figure 18 that HAPF reaches the target node, while the traditional potential field method has the problem of target unreachability.The relative distances between different path nodes and target nodes of UAVs in the two algorithms are recorded separately.It can be seen that when there is an obstacle threat near the target, the traditional artificial potential field method oscillates near the target node and cannot reach the target node.The improved potential field method is able to reach the target node, which shows that the improved algorithm can solve the problem of the unreachable target.
problem of target unreachability.The relative distances between different path nodes and target nodes of UAVs in the two algorithms are recorded separately.It can be seen that when there is an obstacle threat near the target, the traditional artificial potential field method oscillates near the target node and cannot reach the target node.The improved potential field method is able to reach the target node, which shows that the improved algorithm can solve the problem of the unreachable target.Regarding the problem of the traditional artificial potential field method easily falling into the local minimum, this paper proposes a strategy to set heuristic target nodes in order to escape the local minimum.In the simulation, HAPF is compared with the traditional artificial potential field method.The potential field parameters are set as , and 0.2km l = in the simulation.The starting node of the UAV is [0,0,0] km, and the goal is [22,22,5] km.
In order to fully verify the effectiveness of the algorithm, five combined obstacles with different obstacle threats are established in the simulation, and the five obstacles constitute the local minimum area.Figure 19a shows the traditional artificial potential Regarding the problem of the traditional artificial potential field method easily falling into the local minimum, this paper proposes a strategy to set heuristic target nodes in order to escape the local minimum.In the simulation, HAPF is compared with the traditional artificial potential field method.The potential field parameters are set as k att = 8, k rep = 20, n = 0.7, ρ o = 2 km, and l = 0.2 km in the simulation.The starting node of the UAV is [0,0,0] km, and the goal is [22,22,5] km.
In order to fully verify the effectiveness of the algorithm, five combined obstacles with different obstacle threats are established in the simulation, and the five obstacles constitute the local minimum area.Figure 19a shows the traditional artificial potential field method and Figure 19b shows the HAPF algorithm.It can be seen in Figure 19a that when the UAV reaches the path node [13.9930, 14.5990, 4.7578] km, it falls into the local minimum value and oscillates back and forth near it, unable to escape it.In Figure 19b after the UAV has been trapped in the local minimum area, it successfully escapes and finally flies to the target node due to the guidance of heuristic sub-target nodes.The virtual spherical radius of the selected sub-target node is 3.1370 km, and the heuristic sub-target node coordinates are [13.1161, 17.5908, 4. In this paper, regarding the problem of the improved potential field easily generating local path vibrations when avoiding obstacles and planning trajectory collision avoidance, a memory resultant force strategy was proposed to improve this problem.In the simulation, by comparing the traditional artificial method with the potential field method, the effect of resultant force memory on improving the local vibration is verified.In the simulation, the potential field simulation parameters are set as =8  In this paper, regarding the problem of the improved potential field easily generating local path vibrations when avoiding obstacles and planning trajectory collision avoidance, a memory resultant force strategy was proposed to improve this problem.In the simulation, by comparing the traditional artificial method with the potential field method, the effect of resultant force memory on improving the local vibration is verified.In the simulation, the potential field simulation parameters are set as k att = 8, k rep = 20, n = 0.7, ρ o = 2 km, and l = 0.2 km.In the HAPF algorithm with a memory factor, the value of the memory factor is set to λ 1 = 0.7 and λ 2 = 0.3.Figure 20 simulates the flight path of the two algorithms.

Conclusions and Future Work
In this paper, a 4D dynamic cooperative path planning algorithm is proposed to solve the cooperative path planning problem under the 4D cooperative constraints of multiple UAVs.Each UAV shares the same arrival time.There is no conflict between them and they are able to successfully implement threat avoidance.The algorithm proposed in this paper adopts a hierarchical architecture; namely, a 4D cooperative path planning layer and a local anti-collision planning layer.First, the DPRRT* algorithm was used to complete the 4D cooperative preplanning based on the initially known environment before UAV flights.Later, if a UAV encounters sudden threats while in flight, the HAPF algorithm of the local anti-collision planning layer is used to avoid these sudden threats.Simultaneously, UAVs not affected by sudden threats continue to fly along their planned paths.It should be noted that the 4D cooperation constraint cannot be satisfied after the sudden threat avoidance.Therefore, after the sudden threat avoidance has been completed, the DPRRT* algorithm in the 4D cooperative path planning layer is used to replan the 4D cooperative path.Whenever a new threat appears, local obstacle avoidance planning and 4D coordinated path replanning of the threat are required until all UAVs reach the target at the same time.The simulation and comparison experiments show that the proposed algorithm is able to plan a cooperative path, thus satisfying the 4D cooperative constraints for UAVs, and the algorithm has high efficiency, stability, and robustness.
In future work, we plan to extend the proposed algorithm to other application levels; that is, we aim to use multiple UAVs for the actual experimental verification of the proposed algorithm.In addition, considering that the swarm intelligence cooperation composed of a large number of UAVs may be a hot topic in the near future, we plan to improve the algorithm proposed in this paper and explore a 4D cooperative planning algorithm that can be synchronized, so that the algorithm is able to achieve efficient and robust cooperative path planning for 50 or even 100 UAVs.It can be seen in Figure 20 that the UAV flight path was significantly improved after adding the memory resultant force without the local path oscillation.It can also be seen in Figure 20 that after adding the memory resultant force, the variations of resultant force angle between adjacent path nodes of the UAV become significantly smaller and there is no significant angle oscillation.This shows that the introduction of this strategy satisfies the flight constraints of UAVs, does not cause large angle changes between adjacent path nodes, makes the flight path smoother, and is suitable for the collision avoidance trajectory planning of UAVs.

Conclusions and Future Work
In this paper, a 4D dynamic cooperative path planning algorithm is proposed to solve the cooperative path planning problem under the 4D cooperative constraints of multiple UAVs.Each UAV shares the same arrival time.There is no conflict between them and they are able to successfully implement threat avoidance.The algorithm proposed in this paper adopts a hierarchical architecture; namely, a 4D cooperative path planning layer and a local anti-collision planning layer.First, the DPRRT* algorithm was used to complete the 4D cooperative preplanning based on the initially known environment before UAV flights.Later, if a UAV encounters sudden threats while in flight, the HAPF algorithm of the local anti-collision planning layer is used to avoid these sudden threats.Simultaneously, UAVs not affected by sudden threats continue to fly along their planned paths.It should be noted that the 4D cooperation constraint cannot be satisfied after the sudden threat avoidance.Therefore, after the sudden threat avoidance has been completed, the DPRRT* algorithm in the 4D cooperative path planning layer is used to replan the 4D cooperative path.Whenever a new threat appears, local obstacle avoidance planning and 4D coordinated path replanning of the threat are required until all UAVs reach the target at the same time.The simulation and comparison experiments show that the proposed algorithm is able to plan a cooperative path, thus satisfying the 4D cooperative constraints for UAVs, and the algorithm has high efficiency, stability, and robustness.
In future work, we plan to extend the proposed algorithm to other application levels; that is, we aim to use multiple UAVs for the actual experimental verification of the proposed algorithm.In addition, considering that the swarm intelligence cooperation composed of a large number of UAVs may be a hot topic in the near future, we plan to improve the algorithm proposed in this paper and explore a 4D cooperative planning algorithm that can be synchronized, so that the algorithm is able to achieve efficient and robust cooperative path planning for 50 or even 100 UAVs.

Figure 2 .
Figure 2. t = 0 s cooperative paths of UAVs, where (a) is the 3D view and (b) is the 2D view.Figure 2. t = 0 s cooperative paths of UAVs, where (a) is the 3D view and (b) is the 2D view.

Figure 2 .
Figure 2. t = 0 s cooperative paths of UAVs, where (a) is the 3D view and (b) is the 2D view.Figure 2. t = 0 s cooperative paths of UAVs, where (a) is the 3D view and (b) is the 2D view.

Figure 2 .Figure 3 .
Figure 2. t = 0 s cooperative paths of UAVs, where (a) is the 3D view and (b) is the 2D view.

Figure 3 .Figure 4 .Figure 5 .
Figure 3. t = 3 s cooperative paths of UAVs, where (a) is the 3D view and (b) is the 2D view.Figure 3. t = 3 s cooperative paths of UAVs, where (a) is the 3D view and (b) is the 2D view.

Figure 4 .
Figure 4. t = 4.5 s cooperative paths of UAVs, where (a) is the 3D view and (b) is the 2D view.

Figure 4 .Figure 5 .
Figure 4. t = 4.5 s cooperative paths of UAVs, where (a) is the 3D view and (b) is the 2D view.
n = .The maximum steering angle is max 60 φ °= and the maximum climbing/diving angle is max 35 γ °=

Figure 7 .
Figure 7. 4D cooperative paths of UAVs, where (a) is the 3D view and (b) is the 2D view.

Figure 8 .
Figure 8. 4D cooperative pre-planning when t = 0 s in the complicated scenario.(a) is the 3D view and (b) is the 2D view.

Figure 9 .
Figure 9.The shortest distance between all UAVs at any time.

Figure 8 .Figure 8 .
Figure 8. 4D cooperative pre-planning when t = 0 s in the complicated scenario.(a) is the 3D view and (b) is the 2D view.

Figure 9 .
Figure 9.The shortest distance between all UAVs at any time.

Figure 9 .
Figure 9.The shortest distance between all UAVs at any time.

Figure 9 .
Figure 9.The shortest distance between all UAVs at any time.

Figure 10 .
Figure 10.Time tolerance for all UAVs to reach the goal.

Figure 10 .Figure 11 .Figure 12 .
Figure 10.Time tolerance for all UAVs to reach the goal.

Figure 11 .Figure 11 .Figure 12 .
Figure 11.Local collision-checking planning and 4D coordinated path replanning when affected by the sudden moving threat for the first time.(a) is the 3D view and (b) is the 2D view.

Figure 12 .
Figure 12.(a) The shortest distance between UAV1 and UAV2 during local collision-checking planning.(b) The shortest distance between all UAVs in 4D coordinated path replanning.

Figure 12 .
Figure 12.(a) The shortest distance between UAV1 and UAV2 during local collision-checking planning.(b) The shortest distance between all UAVs in 4D coordinated path replanning.

Figure 13 .
Figure 13.Time tolerance for all UAVs to reach the goal in the first replanning.
Figure 11 shows the path of local collision-checking planning and 4D cooperative replanning.

Figure 13 .Figure 14 .
Figure 13.Time tolerance for all UAVs to reach the goal in the first replanning.

Figure 15 .
Figure 15.The shortest distance between all UAVs in 4D coordinated path replanning.

Figure 14 .Figure 14 .
Figure 14.Local collision-checking planning and 4D coordinated path replanning when affected by sudden threats for the second time.(a) is the 3D view and (b) is the 2D view.

Figure 15 .
Figure 15.The shortest distance between all UAVs in 4D coordinated path replanning.

Figure 15 .
Figure 15.The shortest distance between all UAVs in 4D coordinated path replanning.

Figure 15 .
Figure 15.The shortest distance between all UAVs in 4D coordinated path replanning.

Figure 16 .
Figure 16.Time tolerance for all UAVs to reach the goal in the second replanning.Figure 16.Time tolerance for all UAVs to reach the goal in the second replanning.

Figure 16 .
Figure 16.Time tolerance for all UAVs to reach the goal in the second replanning.Figure 16.Time tolerance for all UAVs to reach the goal in the second replanning.
flight time of the planned path of the four algorithms and min ALG ETA t represents the minimum flight time of the planned path of the four algorithms.4. The success rate can be used to verify the effectiveness of the algorithm.The comparison results of the four algorithms are shown in Figure 17.

Figure 17 .
Figure 17.Comparison results of the cooperative path planning layers under different scenarios.

Figure 17 .
Figure 17.Comparison results of the cooperative path planning layers under different scenarios.

Figure 18 .
Figure 18.A comparison of solving the problem of target unreachability.(a) Is the complete view.(b) Is the enlarged view of the goal region.

Figure 18 .
Figure 18.A comparison of solving the problem of target unreachability.(a) Is the complete view.(b) Is the enlarged view of the goal region.

.
HAPF algorithm with a memory factor, the value of the memory factor is set to 1 Figure20simulates the flight path of the

Figure 20 .
Figure 20.A comparison of local shocks.(a) Is the complete view.(b) Is the enlarged view of the goal region.

Figure 20 .
Figure 20.A comparison of local shocks.(a) Is the complete view.(b) Is the enlarged view of the goal region.

Table 4 .
The deviation ratio between the actual path length obtained by the 4D cooperative path algorithm and co l .

Table 3 .
Relevant parameters of 4D cooperative replanning after avoiding sudden threats.

Table 4 .
The deviation ratio between the actual path length obtained by the 4D cooperative path algorithm and l co .