## 2 UAV Three-Dimensional Path Planning
### Safety and Feasibility Constraints  
In path planning, ensuring the safe operation of the UAV is crucial. This involves avoiding obstacles in the environment while maintaining flight within a specified altitude range. Furthermore, the flight path should be as smooth as possible to prevent abrupt ascents and descents. Consequently, the remaining cost functions include threat cost, flight altitude cost, and path smoothness cost.  
Threat Cost:  
\[\text{F}_{2}(X_{i})=\sum_{j=1}^{n-1}\sum_{K=1}^{K}T_{k}(\overrightarrow{P_{ij} P_{i,j+1}}) \tag{2}\]  
where  
\[T_{k}(\overrightarrow{P_{ij}P_{i,j+1}})\begin{cases}0&if\ d_{k}>S+D+R_{k}\\ (S+D+R_{k})-d_{k}&if\ D+R_{k}<d_{k}\leq S+D+R_{k}\\ \infty&if\ d_{k}\leq D+R_{k}\end{cases} \tag{3}\]  
In the problem, it is assumed that the set \(K\) contains several cylindrical obstacles, each defined by a center coordinate \(C_{k}\) and a radius \(R_{k}\). The diameter of the UAV is represented by \(D\). The vertical distance from adjacent path nodes to the origin is denoted as \(d_{k}\), and \(S\) represents the danger zone of the obstacles. The experimental parameters were set as k = 6, \(R_{k}\) = 80, \(P_{\text{start}}\) = [200,100,150], and \(P_{\text{end}}\) = [800;800;150]. The obstacle layout is illustrated in Figure 1.  
In practice, there are usually specific altitude requirements for UAVs, which stipulate that the UAV must operate within a defined range between a minimum and maximum height. The cost associated with flight altitude can be calculated using the following formula:  
\[H_{ij}=\begin{cases}\left|h_{ij}-\frac{h_{max}+h_{min}}{2}\right|&if\ h_{min} \leq h_{ij}\leq h_{max}\\ \infty&otherwise\end{cases} \tag{4}\]  
The total altitude cost \(F_{3}\) can be calculated using the following formula:  
\[F_{3}(X_{i})=\sum_{j=1}^{n}H_{ij} \tag{5}\]  
where \(h_{ij}\) represents the current altitude of the UAV, and \(h_{max}\) and \(h_{min}\) denote the maximum and minimum allowable flight altitudes, respectively. The experimental parameters were configured as \(h_{\text{max}}\) = 200; \(h_{\text{min}}\) = 100.  
Figure 1: Schematic diagram of obstacles.  
Smoothness Cost:  
The smoothness cost evaluates the turning and ascent angles of the path. The turning angle \(\phi_{ij}\) and the ascent angle \(\psi_{ij}\) are calculated using the following formulas:  
Turning Angle:  
\[\phi_{ij}=\arctan\left(\frac{\left\|\frac{\overline{p}_{ij}^{\prime}p_{j+1}^{ \prime}\times\overline{p}_{j+1}^{\prime}\overline{p}_{j+1}^{\prime}\overline{ p}_{j+2}^{\prime}}{\left\|\overline{p}_{ij}^{\prime}p_{j+1}^{\prime} \right\|}\cdot\left\|\frac{\overline{p}_{ij+1}^{\prime}\overline{p}_{j+2}^{ \prime}}{\left\|\overline{p}_{ij}^{\prime}p_{j+1}^{\prime}\right\|}\right)}\right) \tag{6}\]  
Ascent Angle:  
\[\psi_{ij}=\arctan\left(\frac{z_{i,j+1}-z_{ij}}{\left\|\overline{p}_{ij}^{\prime }\overline{p}_{j,j+1}^{\prime}\right\|}\right) \tag{7}\]  
The total smoothness cost can be calculated using the following formula:  
\[F_{4}(X_{i})=a_{1}\sum_{j=1}^{n-2}\phi_{ij}+a_{2}\sum_{j=1}^{n-1}\left|\psi_{ ij}-\psi_{i,j-1}\right| \tag{8}\]  
\(a_{1}\) and \(a_{2}\) are the penalty coefficients for the horizontal turning angle and the vertical pitch angle, respectively. The experimental parameters were set as \(a_{1}=1\); \(a_{2}=1\).