Path Planning for Unmanned Aerial Vehicle in Complex 3D Environments

Unmanned aerial vehicles (UAVs) have become essential platforms for a wide range of applications, including military reconnaissance, disaster monitoring, logistics and delivery, agriculture, and urban inspection. The ability to autonomously plan safe and efficient paths in complex three-dimensional (3D) environments is a key enabling technology for these missions. Path planning for an unmanned aerial vehicle is not merely a shortest-path problem; it must account for terrain constraints, obstacle avoidance, radar threats, physical flight limitations, and smoothness requirements. In this article, we present our research on improving UAV path planning using an adaptive hybrid particle swarm optimization algorithm and a fusion method combining particle swarm optimization with an improved artificial potential field. We focus on complex 3D environments where conventional algorithms often struggle with local optima, poor convergence, low safety margins, and path feasibility issues.

The main contributions of our work are twofold. First, we propose an adaptive hybrid algorithm for UAV path planning over mountainous terrain. This algorithm integrates adaptive inertia weight, a mutation mechanism, an artificial bee colony search strategy, and a logistic chaotic mapping to enhance global search capability, avoid premature convergence, and improve convergence accuracy. Second, we develop a fusion path planning approach for environments with dense obstacles. This method uses a Gaussian repulsion function to smooth the potential field and employs particle swarm optimization to refine key waypoints, thereby mitigating the local minimum and target unreachable problems of the classical artificial potential field. We also build a multi-index evaluation system and a path executability verification framework based on controller tracking simulation.

Problem Formulation and Environment Modeling

To solve the path planning problem for an unmanned aerial vehicle, we first construct realistic 3D environments. Two types of environments are used: mountainous terrain and obstacle clusters. The mountainous terrain model is generated by summing exponential functions representing peaks:

$$z(x,y) = \sum_{i=1}^{n} h_i \exp\left(-\left(\frac{x-x_i}{x_{si}}\right)^2 – \left(\frac{y-y_i}{y_{si}}\right)^2\right)$$

where \(n\) is the number of peaks, \((x_i,y_i)\) is the center coordinate of the \(i\)-th peak, \(h_i\) is the height, and \(x_{si}\), \(y_{si}\) control the slope in the \(x\) and \(y\) directions. The continuous surface is discretized into a uniform grid to support numerical calculation.

For obstacle clusters, we model vertical cylindrical obstacles. The occupied region of the \(i\)-th obstacle is expressed as:

$$O_i = \left\{ (x,y,z) \,\middle|\, (x-x_i)^2 + (y-y_i)^2 \le R_i^2 , \; 0 \le z \le H_i \right\}$$

where \((x_i,y_i)\) is the horizontal center, \(R_i\) is the radius, and \(H_i\) is the height. By arranging multiple cylinders with different parameters, we can create dense and irregular obstacle fields. These two models provide the basis for evaluating path planning algorithms under different environmental complexities.

Cost Function for UAV Path Planning

The path is represented as a sequence of waypoints \(p_i = (X_i,Y_i,Z_i)\), \(i=1,\dots,n\). The overall fitness function combines five cost components: path length, collision cost, altitude cost, smoothness cost, and radar threat cost.

Path Length Cost

To minimize the total flight distance, we compute the sum of Euclidean distances between consecutive waypoints:

$$f_1 = \sum_{i=1}^{n-1} \sqrt{(X_{i+1}-X_i)^2 + (Y_{i+1}-Y_i)^2 + (Z_{i+1}-Z_i)^2}$$

Collision Cost

The collision cost penalizes paths that intersect terrain or fly too close to the ground. Let \(\Delta d_i = Z_i – z_c(X_i,Y_i)\), where \(z_c\) is the terrain height at the horizontal position of waypoint \(i\). Then:

$$f_2 = \begin{cases} +\infty, & \exists i, \Delta d_i \le 0 \\ \sum_{i=1}^{n} \frac{c_{\text{punk}}}{\Delta d_i}, & \Delta d_i > 0 \end{cases}$$

where \(c_{\text{punk}}\) is a penalty coefficient. When the waypoint is below terrain, the cost becomes infinite, making the path infeasible.

Flight Altitude Cost

To maintain a safe and efficient altitude band between \(z_{\min}\) and \(z_{\max}\), we define:

$$f_3 = \sum_{i=1}^{n} h_{p,i}, \quad h_{p,i} = \begin{cases} h_{\text{punk}} \left(\frac{Z_i – z_{\text{mean}}}{z_{\max} – z_{\min}}\right)^2, & z_{\min} < Z_i < z_{\max} \\ 100 h_{\text{punk}}, & \text{otherwise} \end{cases}$$

with \(z_{\text{mean}} = (z_{\max}+z_{\min})/2\). This term encourages the unmanned aerial vehicle to fly near a comfortable mean altitude, penalizing both too-low and too-high flights.

Path Smoothness Cost

Smoothness is quantified by the turning angle and climb angle changes along the path. For each intermediate waypoint \(i\), we calculate:

$$\theta_{\text{turn},i} = \arctan\left(\frac{\| \mathbf{p}_1 \times \mathbf{p}_2 \|}{\mathbf{p}_1 \cdot \mathbf{p}_2}\right), \quad \phi_i = \arctan\left(\frac{Z_{i+1}-Z_i}{\sqrt{(X_{i+1}-X_i)^2+(Y_{i+1}-Y_i)^2}}\right)$$

where \(\mathbf{p}_1\) and \(\mathbf{p}_2\) are direction vectors of the two adjacent segments. The smoothness cost is:

$$f_4 = \sum_{i=2}^{n-1} \left[ \lambda_{\text{turn}} \left(\max(0, |\theta_{\text{turn},i}| – \theta_{\max})\right)^2 + \lambda_{\text{climb}} \left(\max(0, |\phi_i – \phi_{i-1}| – \phi_{\max})\right)^2 \right]$$

Radar Threat Cost

In military or security scenarios, the path must avoid radar detection zones. Suppose there are \(m\) radars, where radar \(j\) is located at \(R_j = (x_j,y_j,z_j)\) with detection radius \(r_j\) and safety buffer \(\Delta s\). The radar threat cost is:

$$f_5 = \sum_{i=1}^{n} \sum_{j=1}^{m} \begin{cases} +\infty, & d_{i,j} \le r_j \\ \frac{\omega_{\text{radar}}}{d_{i,j} – r_j}, & r_j < d_{i,j} \le r_j + \Delta s \\ 0, & d_{i,j} > r_j + \Delta s \end{cases}$$

where \(d_{i,j}\) is the Euclidean distance between waypoint \(i\) and radar \(j\).

The complete fitness function is a weighted sum:

$$Fit = \sum_{i=1}^{5} b_i f_i, \quad \sum_{i=1}^{5} b_i = 1$$

In our experiments, the weight vector is set to \(\{0.1, 0.3, 0.3, 0.2, 0.1\}\) for length, collision, altitude, smoothness, and radar threat, respectively.

Adaptive Hybrid Particle Swarm Optimization for UAV Path Planning

Particle swarm optimization is well known for its simple structure and fast convergence, but it often suffers from premature convergence and insufficient local search capability when applied to complex 3D path planning. To overcome these drawbacks, we propose the Adaptive Hybrid Artificial Bee Particle Swarm Optimization (AHAPSO) algorithm. AHAPSO combines several enhancement mechanisms:

Adaptive Inertia Weight

Instead of using a fixed or linearly decreasing inertia weight, we adopt a nonlinear adaptive weighting scheme:

$$\omega(t) = \omega_1 \exp\left(-\frac{t}{T_{\max}}\right)$$

where \(\omega_1\) is the initial maximum inertia weight, \(t\) is the current iteration, and \(T_{\max}\) is the maximum number of iterations. This formulation maintains high exploration ability in early iterations and gradually shifts to fine exploitation near the end.

Artificial Bee Colony Search Strategy

To enhance local search, we incorporate the employed bee operator from the artificial bee colony algorithm. For each particle \(i\), a new candidate is generated by:

$$x_{ij}^{\text{new}} = x_{ij} + \phi_{ij} (x_{ij} – x_{kj})$$

where \(k \neq i\) is a randomly chosen particle and \(\phi_{ij}\) is a uniform random number in \([-1,1]\). This operator explores the neighborhood of the current solution and helps refine the path near a promising region.

Mutation Mechanism

To prevent population stagnation and increase diversity, we apply a small random mutation to the particle velocity or position with a predefined probability. The mutation is implemented as:

$$v_{\text{mut}} = \text{rand}(0,1) \cdot (v_{\max} – v_{\min}) + v_{\min}$$

This allows particles to escape from a crowded search region and explore new areas of the solution space.

Logistic Chaotic Mapping

In the later phase of evolution (after 75% of total iterations), we introduce a logistic chaotic map to further perturb particle positions:

$$c_v = m \cdot \text{rand}(0,1) \cdot (1 – \text{rand}(0,1))$$
$$p_n = p_s + c_v \cdot (b_{\max} – b_{\min})$$

where \(m\) is the logistic map control parameter, \(p_s\) is the original position, and \(p_n\) is the new position after chaotic perturbation. The chaotic sequence has good ergodicity and can help the unmanned aerial vehicle jump out of local optima.

Numerical Evaluation on Benchmark Functions

We first validated AHAPSO using the CEC2017 benchmark suite, which contains 29 test functions including unimodal, multimodal, hybrid, and composition functions. The maximum iteration was set to 1000 and population size to 30. Each experiment was repeated 30 times independently. We compared AHAPSO against PSO, Grey Wolf Optimizer (GWO), Dung Beetle Optimizer (DBO), and Sparrow Search Algorithm (SSA). The parameter settings are summarized in the following table.

Algorithm Parameters
DBO \(P_{\text{percent}} = 0.2; R(t)=1-t/M; \theta=\pi/180 k\)
GWO \(a=2-2t/T; A=2ar_1-a; C=2r_2\)
PSO \(w=0.8; c_1=c_2=1.5\)
SSA \(P_{\%}=0.2; \rho=0.8; N_{SD}=20\)
AHAPSO \(w=0.9\exp(-t/T); c_1=c_2=1.5; p_m=0.1; \mu=4\)

The average results over 30 runs are summarized in the following table. For brevity, we show the mean value of each algorithm on a subset of representative functions, while the full numerical results were reported in our experiments.

Function PSO GWO DBO SSA AHAPSO
F1 3.88E+07 1.23E+09 2.26E+09 3.00E+03 6.31E+03
F5 6.72E+02 6.16E+02 6.76E+02 7.39E+02 5.95E+02
F9 3.58E+03 2.61E+03 2.33E+03 5.30E+03 1.62E+03
F16 3.20E+03 2.60E+03 2.53E+03 2.92E+03 2.44E+03
F22 5.12E+03 4.10E+03 4.73E+03 6.73E+03 2.92E+03
F26 6.46E+03 5.05E+03 4.85E+03 6.28E+03 3.62E+03

The Friedman average rank by AHAPSO was 1.17, which is the lowest among all compared algorithms. Meanwhile, the algorithm won on 26 out of 29 functions, lost only on two functions and tied on one. These results demonstrate that AHAPSO offers superior robustness and convergence accuracy for complex optimization problems.

3D Path Planning Experiments with AHAPSO

We applied AHAPSO to path planning in two mountainous environments with different complexity. The map size was \(1000 \times 1000 \times 1000\), with the start at (1,1,1) and goal at (1000,900,600). The first map had 9 peaks and 3 radar threats, while the second had 12 peaks and 4 radar threats. We compared AHAPSO with PSO, GWO, DBO, CLSPSO, and SAPSO over 300 iterations with a population of 50.

The statistical results over multiple independent runs are summarized below.

Map Indicator PSO GWO DBO CLSPSO SAPSO AHAPSO
Map 1 Worst 1.395E+03 1.190E+03 1.290E+03 1.147E+03 1.256E+03 1.095E+03
Best 5.687E+02 7.605E+02 6.046E+02 9.245E+02 6.059E+02 5.427E+02
Mean 7.011E+02 8.160E+02 6.955E+02 9.267E+02 6.200E+02 5.896E+02
STD 238.298 90.494 157.211 15.576 69.078 81.888
Map 2 Worst 1.997E+03 2.459E+03 2.265E+03 2.364E+03 1.774E+03 1.569E+03
Best 6.270E+02 9.766E+02 6.762E+02 9.139E+02 6.222E+02 5.700E+02
Mean 8.456E+02 1.005E+03 8.802E+02 9.520E+02 6.464E+02 6.255E+02
STD 372.253 116.971 233.695 155.213 125.261 123.435

In both maps, AHAPSO achieved the smallest best, mean, and worst fitness values among all algorithms, and the standard deviation remained competitive, indicating not only better optimal solutions but also strong consistency across independent runs. The convergence curves show that AHAPSO descends rapidly in the early iterations and maintains its advantage later, resulting in smoother and shorter paths that can effectively avoid mountains and radar threats.

Fusion of Particle Swarm Optimization and Artificial Potential Field for Dense Obstacle Environments

While AHAPSO works well for terrain-based path planning, environments with dense, clustered obstacles require a different strategy. The classical artificial potential field is efficient but prone to local minima, target unreachable, and oscillation. To address these issues, we propose the FPAPF (Fusion of PSO and APF) method, which combines an improved potential field with particle swarm optimization.

Improved Potential Field with Gaussian Repulsion

We replace the standard repulsive function with a Gaussian-based repulsion model:

$$F_{\text{rep}}(q) = \begin{cases} K_{\text{rep}} \exp\left(-\frac{(\rho(q)-\zeta)^2}{2\sigma^2}\right) \left(\frac{1}{\rho(q)} – \frac{1}{\zeta}\right) \frac{1}{\rho(q)^2} \vec{d}, & \rho(q) \le \zeta \text{ and } z \le h \\ 0, & \text{otherwise} \end{cases}$$

where \(\rho(q)\) is the distance from the current position to the obstacle center, \(\zeta\) is the influence range, \(\sigma\) is the Gaussian width, \(h\) is the obstacle height, and \(\vec{d}\) is the unit vector pointing from the obstacle toward the current position. The Gaussian term provides a smooth repulsion profile that prevents abrupt force discontinuities at the boundary.

The attractive force remains linear:

$$F_{\text{att}} = K_{\text{att}} (q_{\text{goal}} – q_t)$$

The total force is:

$$F_{\text{total}} = F_{\text{att}} + F_{\text{rep}}$$

and the potential-field-guided position is:

$$q_{\text{apf}} = q_t + F_{\text{total}}$$

Local Optimization by PSO

Around \(q_{\text{apf}}\), a small particle swarm is initialized to refine the candidate waypoint. Each particle \(x_i\) is sampled from a Gaussian distribution centered at \(q_{\text{apf}}\) with covariance \(\sigma^2 I\). The fitness of each particle is:

$$f(x_i) = \|x_i – q_{\text{goal}}\| + \sum_{k=1}^{N_{\text{obs}}} P_k(x_i)$$

where \(P_k(x_i)\) is a penalty based on the distance to obstacle \(k\). The global best \(g_{\text{best}}\) is selected after a few iterations. The final next position is obtained by linear fusion:

$$q_{t+1} = \alpha \cdot q_{\text{apf}} + (1-\alpha) \cdot g_{\text{best}}$$

where \(\alpha\) controls the balance between the potential field guidance and the PSO refinement. In our implementation, we set \(K_{\text{att}} = 0.04\), \(K_{\text{rep}} = 0.10\), \(\sigma = 5\), \(\alpha = 0.7\), and population size 40.

Multi-Index Evaluation System Based on AHP

To assess path planning quality, we designed a composite score using three metrics: path length \(L\), average safety distance \(D_{\text{avg}}\), and minimum safety distance \(D_{\min}\). The normalized weighted score is:

$$\text{score} = \omega_1 \left(1 – \frac{L}{L_{\max}}\right) + \omega_2 \frac{D_{\text{avg}}}{D_{\text{avg,max}}} + \omega_3 \frac{D_{\min}}{D_{\min,\max}}$$

We used the analytic hierarchy process to determine the weights based on pairwise comparisons. The judgment matrix was:

$$A = \begin{bmatrix} 1 & 4 & 7 \\ 1/4 & 1 & 3 \\ 1/7 & 1/3 & 1 \end{bmatrix}$$

The largest eigenvalue is \(\lambda_{\max} = 3.032\), and after normalizing the eigenvector, the weights are \(\omega_1 = 0.705\), \(\omega_2 = 0.211\), \(\omega_3 = 0.084\). The consistency ratio is \(CR = 0.028 < 0.10\), indicating acceptable consistency.

Experimental Results in Static and Dynamic Environments

We compared FPAPF with five existing algorithms: TAPF, RRT, IAPF, MAPF, and GWOAPF. For static environments, we created three scenarios with increasing obstacle density (8, 10, and 12 obstacles). The environment information is summarized in the following tables.

Environment 1

Obstacle Center Height Radius
1 (70,50) 60 4
2 (75,80) 60 9
3 (130,60) 50 7
4 (25,25) 60 5
5 (100,48) 33 6
6 (150,110) 35 9
7 (110,80) 20 5
8 (90,40) 20 4

The path planning results in Environment 1 are shown below.

Algorithm Success Path Length Avg Safety Distance Min Safety Distance
TAPF Yes 363 14.06 2.61
RRT Yes 293 25.93 0.10
IAPF Yes 320 19.33 3.99
MAPF Yes 377 7.96 0.02
GWOAPF Yes 351 12.81 1.56
FPAPF Yes 236 12.42 2.38

FPAPF produced the shortest path and maintained a reasonable safety margin. The composite score, computed by the AHP-based formula, was the highest among all algorithms.

Environment 2

Obstacle Center Height Radius
1 (70,50) 45 6
2 (80,40) 70 4
3 (130,60) 70 7
4 (25,25) 60 5
5 (150,50) 50 4
6 (100,48) 33 6
7 (185,110) 35 9
8 (110,80) 40 5
9 (90,40) 30 4
10 (180,50) 30 5
Algorithm Success Path Length Avg Safety Distance Min Safety Distance
TAPF Yes 323 9.32 3.31
RRT Yes 281 22.72 0.12
IAPF Yes 289 19.03 4.05
MAPF Yes 298 7.16 3.65
GWOAPF Yes 225 13.50 0.36
FPAPF Yes 210 10.35 2.03

FPAPF again achieved the smallest path length and the highest weighted score in Environment 2. Its minimum safety distance is lower than IAPF but still above 2 units, which indicates a safe separation from obstacles.

Environment 3

Obstacle Center Height Radius
1 (70,50) 60 6
2 (140,100) 50 6
3 (45,35) 60 7
4 (110,140) 53 6
5 (185,110) 35 6
6 (100,95) 20 6
7 (80,110) 28 6
8 (50,90) 40 6
9 (100,115) 90 6
10 (90,84) 55 8
11 (160,160) 30 5
12 (150,180) 69 7
Algorithm Success Path Length Avg Safety Distance Min Safety Distance
TAPF No
RRT Yes 327 17.48 0.15
IAPF No
MAPF No
GWOAPF Yes 411 8.04 0.48
FPAPF Yes 276 9.40 1.90

In the most cluttered environment, TAPF, IAPF, and MAPF failed to find a feasible path due to local minima and the unreachable target problem. RRT and GWOAPF succeeded but produced long and potentially risky routes. Only FPAPF generated a reasonably short path with a minimum safety distance of 1.90, demonstrating its robustness in dense obstacle fields.

Dynamic Environment

We also generated a dynamic environment (Environment 4) with three moving obstacles whose trajectories followed elliptical horizontal paths with sinusoidal vertical variation. The start and goal points were (10,10,0) and (100,150,50), respectively. The results are shown below.

Algorithm Success Path Length Avg Safety Distance Min Safety Distance
TAPF Yes 202 6.70 0.10
RRT Yes 269 9.42 0.99
IAPF Yes 221 17.65 0.20
MAPF Yes 204 18.28 0.01
GWOAPF Yes 282 17.87 0.05
FPAPF Yes 224 16.54 4.45

FPAPF obtained the largest minimum safety distance (4.45) while maintaining a competitive path length. This indicates that the fusion algorithm is able to keep the unmanned aerial vehicle away from moving obstacles without excessive detours, thus striking a good balance between efficiency and safety.

Path Executability Verification Framework

Geometric metrics alone are insufficient to guarantee that a planned path is actually flyable. To verify the executability of the planned airway, we designed a simulation framework that combines the planned waypoints with a flight controller and a simplified drone motion model. We considered three controllers: PID, LQR, and an enhanced LQR (ELQR) with path preview and velocity feedforward. The discrete motion model is:

$$v_{k+1} = v_k + a_k \Delta t, \quad p_{k+1} = p_k + v_{k+1} \Delta t$$

where \(a_k = (u_k + d_k)/m_k\), \(u_k\) is the controller output, \(d_k\) is the external disturbance, \(m_k\) is the mass variation coefficient, and \(\Delta t\) is the sampling period.

The LQR controller minimizes a quadratic cost, while the PID controller uses proportional-integral-derivative feedback. The ELQR controller adds a preview term to the reference trajectory:

$$u_k = -K (X_k – X_k^{\text{ref}}) + \alpha (v_k^{\text{pred}} – v_k)$$

where \(v_k^{\text{pred}}\) is the predicted reference velocity computed from \(L\)-step lookahead:

$$v_k^{\text{pred}} = \frac{x_{k+L}^{\text{ref}} – x_k^{\text{ref}}}{L \Delta t}$$

The adaptive gain \(\alpha\) is:

$$\alpha = \alpha_0 + \frac{\alpha_1}{1 + \|X_k – X_k^{\text{ref}}\|}$$

We evaluated the tracking performance using three metrics: root mean square error (RMSE), maximum error, and mean jerk. The following tables show the results for the FPAPF path and the AHAPSO path under the three controllers.

Controller RMSE Max Error Final Error Mean Jerk Max Jerk
PID 24.658 37.987 27.807 19.520 32.876
LQR 3.9034 6.3940 5.1163 37.795 108.35
ELQR 3.5319 5.8692 4.8053 35.651 114.67
Controller RMSE Max Error Final Error Mean Jerk Max Jerk
PID 184.60 258.92 189.06 151.57 240.68
LQR 28.722 53.619 33.264 183.98 853.07
ELQR 27.078 51.023 31.447 177.50 822.51

For the FPAPF path, the ELQR controller reduced the RMSE by about 85% compared to PID. The ELQR also achieved the smallest maximum and final errors. For the AHAPSO path in the complex terrain environment, the absolute error values are larger due to the larger map scale and higher path variations, but the relative improvement of LQR/ELQR over PID is equally significant. These results confirm that the planned paths can be tracked with acceptable accuracy when an appropriate control strategy is used, demonstrating their practical executability.

Conclusion and Future Work

In this article, we have presented two path planning strategies for unmanned aerial vehicles in complex 3D environments. The first approach, AHAPSO, combines adaptive inertia weight, artificial bee colony search, mutation, and chaotic mapping to solve path planning over mountainous terrain. The algorithm was validated on CEC2017 benchmarks and further applied to two mountainous scenarios. The second approach, FPAPF, fuses an improved Gaussian artificial potential field with particle swarm optimization to handle dense obstacle fields. Comprehensive experiments in static, dynamic, and cluttered environments showed that FPAPF consistently produces shorter, safer, and more reliable paths than several existing methods. Finally, a path executability verification framework based on controller-in-the-loop simulation indicated that both planned paths can be effectively tracked by LQR-type controllers, with ELQR giving the best performance.

There are several avenues for future research. First, we plan to test the proposed algorithms on real unmanned aerial vehicle platforms with onboard sensors and processors. Second, the current study mainly focuses on static or slowly changing environments; dynamic obstacle avoidance and online replanning remain challenging and worthy of further investigation. Third, extending the planning methods to multi-UAV cooperative scenarios is an important next step. Finally, incorporating more detailed aerodynamic constraints and adaptive parameter tuning using machine learning could further improve the practical feasibility of the planned paths.

Scroll to Top