## 3 Four-Dimensional Dynamic Cooperative Path Planning Algorithm
### Dynamic Priority RRT* Algorithm
The heuristic sub-target strategy adopts the idea of transforming multi-objective optimization problems into single-objective optimization problems to handle sub-targetpoint 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:

\[U_{all}(t+\Delta t)-U_{all}(t)>\varepsilon \tag{14}\]

where \(U_{all}\) is the combined potential energy at the current initial step and \(U_{all}(t+\Delta 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 \(\varepsilon\geq 0\). If the decision is correct, escape processing will be carried out through the algorithm. Considering the flight constraints of UAVs, \(\varepsilon\) 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 \(\textbf{X}_{j}\) as the center and \(r_{\text{min}}\) as the radius, as shown in Figure 1, and the calculation of \(r_{\text{min}}\) is as follows:

\[r_{\text{min}}=\min\rho\big{(}\textbf{X}_{i},\textbf{X}_{j})j=1,\ldots K \tag{15}\]

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 \(\phi_{\text{max}}\) and the climbing/diving angle \(\gamma_{\text{max}}\) of the UAV, respectively. In the horizontal plane, the range angle \(\phi_{t}\) is selected as \(\phi-\phi_{\text{max}}\leq\phi_{t}\leq\phi+\phi_{\text{max}}\). Then, in the vertical plane, the range angle \(\mu_{t}\) is selected as \(-\gamma_{\text{max}}\leq\gamma_{t}\leq\gamma_{\text{max}}\). If the resultant force of UAV at path node \(\textbf{X}_{j}\) is \(\textbf{F}=(f_{x},f_{y},f_{z})\), then:

\[\phi=\arctan(f_{y}/f_{x}) \tag{16}\]

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 \(\{\textbf{G}_{1},\textbf{G}_{2},\ldots\textbf{G}_{n}\}\). \(\omega_{1}\) is the angle between \(\textbf{X}_{j}\textbf{G}_{i}\) and \(\textbf{X}_{j}\textbf{G}_{0}\), and \(\omega_{2}\) is the angle between \(\textbf{X}_{j}\textbf{G}_{i}\) and \(\textbf{G}_{i}\textbf{G}_{0}\). \(\Delta\omega=\mid\)\(\omega_{2}-\omega_{1}\) is set as the angle cost of selecting \(\textbf{G}_{i}\) as the sub-target node. The end node is \(\textbf{G}_{0}\). Then, the heuristic function can be defined as:

\[f=\xi_{1}D+\xi_{2}A \tag{17}\]

where \(\xi_{1}\) and \(\xi_{2}\) are the cost function weights. If \(\xi_{1}>\xi_{2}\), heuristic sub-target nodes with lower length costs can be obtained, but there may be significant angle changes. If \(\xi_{1}<\xi_{2}\), heuristic sub-target nodes with a lower cost of angle change can be obtained, but the

Figure 1: Heuristic sub-target strategy. planned path length to the sub-target will increase. In this paper, we choose \(\xi_{1}=\xi_{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. \(\mathbf{G}_{i}\) is any node in the node group.