## 3 Four-Dimensional Dynamic Cooperative Path Planning Algorithm
### Dynamic Priority RRT* Algorithm
### 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. #### 3.3.1 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:

\[U_{rep}(\mathbf{X})= \begin{cases}\frac{1}{2}k_{rep}(\frac{1}{\rho(\mathbf{X} \mathbf{x}_{o})}-\frac{1}{\rho_{o}})^{2}\rho^{n}(\mathbf{X},\mathbf{X}_{g})& \rho(\mathbf{X},\mathbf{X}_{o})\leq\rho_{o}\\ 0&\rho(\mathbf{X},\mathbf{X}_{o})>\rho_{o}\end{cases}  \tag{11}\]

where \(n\in\mathcal{R}^{+}\). Then the repulsion function is:

\[\mathbf{F}_{rep}(\mathbf{X})=-\
abla U_{rep}(\mathbf{X})= \begin{cases} \mathbf{F}_{rep1}+\mathbf{F}_{rep2}&\rho(\mathbf{X},\mathbf{X}_{o})\leq\rho_ {o}\\ 0&\rho(\mathbf{X},\mathbf{X}_{o})>\rho_{o}\end{cases}  \tag{12}\]where:

\[\left\{ \begin{array}{l}\mathbf{F}_{rep1}(\mathbf{X})=k_{rep}(\frac{1}{\rho( \mathbf{X}\mathbf{x}_{c})}-\frac{1}{\rho_{o}})\frac{\rho^{n}(\mathbf{X}\mathbf{ X}_{s})}{\rho^{n}(\mathbf{X}\mathbf{X}_{s})}\frac{\partial\rho(\mathbf{X}\mathbf{X}_{s})}{ \partial\mathbf{X}}\\ \mathbf{F}_{rep2}(\mathbf{X})=-\frac{n}{2}k_{rep}(\frac{1}{\rho(\mathbf{X} \mathbf{x}_{c})}-\frac{1}{\rho_{o}})^{2}\rho^{n-1}(\mathbf{X},\mathbf{X}_{g}) \frac{\partial\rho(\mathbf{X}\mathbf{X}_{g})}{\partial\mathbf{X}}\end{array} \right. \tag{13}\]

In the above formula, it can be seen that \(\mathbf{F}_{rep1}\) is the repulsive force of the obstacle against the UAV and \(\mathbf{F}_{rep2}\) is the attractive force of the UAV to the goal.