Improved APF-RRT* for 3D Urban Path Planning of UAV Drones

The rapid evolution of UAV drones has positioned low-altitude economies as one of the most dynamic strategic emerging industries worldwide. Applications span urban logistics, emergency rescue, smart agriculture, and many other domains that demand reliable three-dimensional navigation. Path planning constitutes one of the core technologies enabling autonomous flight of UAV drones, directly governing flight efficiency, mission success rate, and operational safety. Consequently, computing optimal trajectories for UAV drones in cluttered three-dimensional urban environments has become a central research problem in the field.

I organize the remainder of this article as follows. First, I formulate the path planning problem for UAV drones and review the baseline algorithms. Second, I analyze the sampling deficiencies of conventional random tree methods. Third, I propose a direction-optimized sampling strategy coupled with an artificial potential field to accelerate convergence for UAV drones. Fourth, I introduce redundant node removal and cubic B-spline smoothing. Finally, I present comparative simulations that quantify the gains achieved by the proposed algorithm for UAV drones operating in complex urban environments.

1. Problem Formulation and Preliminaries

I consider a bounded three-dimensional workspace $\mathcal{W} \subset \mathbb{R}^3$ populated by $N_{obs}$ axis-aligned rectangular obstacles representing buildings and static structures. The state of a UAV drone is encoded by its position $\mathbf{p} = (x, y, z)$. The planning task consists of finding a collision-free trajectory from a start configuration $\mathbf{X}_{start}$ to a goal configuration $\mathbf{X}_{goal}$ that minimizes path length while satisfying dynamic constraints of the UAV drones.

Let the workspace be discretized conceptually as $\mathcal{W} = [0, L_x] \times [0, L_y] \times [0, L_z]$. The obstacle region is denoted by $\mathcal{O} \subset \mathcal{W}$, and the free space is $\mathcal{F} = \mathcal{W} \setminus \mathcal{O}$. A feasible path for UAV drones is a continuous mapping

$$\sigma : [0, 1] \rightarrow \mathcal{F}, \quad \sigma(0) = \mathbf{X}_{start}, \quad \sigma(1) = \mathbf{X}_{goal}$$

The optimal path planning problem for UAV drones can then be expressed as

$$\sigma^{*} = \arg\min_{\sigma \in \Sigma_{feasible}} \int_{0}^{1} \left\| \frac{d\sigma}{dt} \right\| dt$$

where $\Sigma_{feasible}$ denotes the set of all collision-free trajectories satisfying kinematic limits. Table 1 summarizes the principal notation used throughout this article.

Symbol Meaning
$\mathbf{X}_{start}$ Initial position of the UAV drone
$\mathbf{X}_{goal}$ Target position of the UAV drone
$\mathbf{X}_{rand}$ Randomly sampled point in the workspace
$\mathbf{X}_{near}$ Nearest tree node to the sample
$\mathbf{X}_{new}$ Newly generated tree node
$Step$ Fixed extension step length
$MaxIter$ Maximum number of iterations
$d_0$ Influence radius of the repulsive field
$k_{gra}, k_{rep}$ Attractive and repulsive gain coefficients
$J(\cdot, \cdot)$ Path cost function
$V_1, V_2, V_3, V_4$ Partitioned sampling subspaces
$f_1, f_2, f_3$ Sampling probabilities assigned to subspaces

2. Baseline Algorithms for UAV Drones

2.1 Rapidly-exploring Random Tree

The Rapidly-exploring Random Tree (RRT) algorithm is a sampling-based planner well-suited to high-dimensional problems. For UAV drones, RRT offers structural simplicity and strong exploration capability. The classic procedure proceeds as follows. The algorithm initializes the start node, the step length, and the maximum iteration count. At each iteration, a random sample $\mathbf{X}_{rand}$ is drawn uniformly from the workspace. The nearest node $\mathbf{X}_{near}$ on the current tree is located, and a new node is generated by extending from $\mathbf{X}_{near}$ toward $\mathbf{X}_{rand}$ by at most $Step$. A collision check is then performed along the connecting segment. If the segment is collision-free, the node is added; otherwise the sample is discarded. The procedure terminates when the goal region is reached or the iteration budget is exhausted.

Despite its simplicity, RRT suffers from two fundamental issues when applied to UAV drones in urban canyons. First, isotropic uniform sampling lacks directional bias toward the goal, which drastically reduces the proportion of useful extensions. Second, the resulting paths contain a large number of redundant waypoints, which impose frequent heading adjustments on UAV drones and increase energy expenditure.

2.2 The RRT* Algorithm

RRT* augments the baseline RRT with a two-stage optimization mechanism that yields asymptotic optimality. The first stage, parent reselection, evaluates candidate neighbors within a radius $R$ around $\mathbf{X}_{new}$ and selects the parent that minimizes cumulative path cost from the start. The second stage, rewiring, checks whether any neighbor can be connected more cheaply through $\mathbf{X}_{new}$ and updates the tree accordingly. Formally, the optimal parent selection is

$$\mathbf{x}_{newbest}^{(star)} = \arg\min_{\mathbf{x} \in N_{\mathbf{x}_{new}}} \left[ J(\mathbf{x}_{star}, \mathbf{x}) + \|\mathbf{x} – \mathbf{x}_{new}\| \right]$$

where $N_{\mathbf{x}_{new}}$ denotes the neighbor set of the newly generated node, $J(\mathbf{x}_{star}, \mathbf{x})$ is the accumulated cost from the start node to the neighbor $\mathbf{x}$, and $\|\mathbf{x} – \mathbf{x}_{new}\|$ is the Euclidean distance between the neighbor and the new node. Although RRT* improves path quality relative to RRT, the per-iteration computational burden grows because neighborhood queries must be performed at every extension. Moreover, uniform sampling continues to dominate, so the fraction of redundant nodes in the tree increases approximately linearly with the measure of the obstacle region.

2.3 Artificial Potential Field

The Artificial Potential Field (APF) method constructs a virtual field that drives a UAV drone toward the goal while repelling it from obstacles. The attractive potential is defined as

$$U_{gra}(X) = \frac{1}{2} k_{gra} \| X – X_{goal} \|^2$$

where $X$ denotes the current position of the UAV drone, $X_{goal}$ is the goal position, and $k_{gra}$ is the attractive gain. The corresponding attractive force is

$$F_{gra}(X) = -\nabla U_{gra}(X) = -k_{gra} (X – X_{goal})$$

The repulsive potential generated by the nearest obstacle is

$$U_{rep}(X) = \begin{cases} \frac{1}{2} k_{rep} \left( \frac{1}{d(X)} – \frac{1}{d_0} \right)^2, & d(X) \leq d_0 \\ 0, & d(X) > d_0 \end{cases}$$

where $k_{rep}$ is the repulsive gain, $d(X)$ is the distance from the UAV drone to the nearest obstacle, and $d_0$ is the influence radius. The total potential is the superposition

$$U(X) = U_{gra}(X) + U_{rep}(X)$$

While APF accelerates convergence for UAV drones when combined with RRT*, the combined field can produce local minima in narrow corridors or concave obstacle configurations. In such regions the attractive and repulsive gradients cancel, and the UAV drone becomes trapped. The proposed algorithm addresses this by layering a directional sampling strategy on top of the potential field.

3. The Improved APF-RRT* Algorithm

3.1 Motivation

I observed three recurring deficiencies when applying APF-RRT* to UAV drones in dense urban environments. First, convergence is slow because a large fraction of samples are drawn in directions that do not contribute to progress toward the goal. Second, path curvature is discontinuous, which violates the kinematic constraints of UAV drones and forces aggressive attitude corrections. Third, the generated paths contain many redundant waypoints, which inflate the effective path length and increase energy consumption. Table 2 organizes these deficiencies and the countermeasures I adopt.

Deficiency Consequence for UAV Drones Proposed Countermeasure
Isotropic uniform sampling Slow convergence, wasted extensions Direction-optimized sampling over partitioned subspaces
Local minima in APF UAV drones stall in concave regions Goal-biased sampling probabilities
Redundant waypoints Excess energy, longer flight Redundant node removal via backtracking
Sharp heading changes Dynamic infeasibility, instability Cubic uniform B-spline smoothing

3.2 Direction-Optimized Sampling

The core innovation of my approach is to partition the neighborhood of the newly generated node $\mathbf{X}_{new}$ into directional subspaces and to assign non-uniform sampling probabilities that favor the goal direction. Let $\mathbf{X}_{new} = (x_{new}, y_{new}, z_{new})$ and $\mathbf{X}_{goal} = (x_{goal}, y_{goal}, z_{goal})$. I define

$$z_{max} = \begin{cases} z_{new}, & z_{new} < z_{goal} \\ z_{goal}, & z_{goal} < z_{new} \end{cases}$$

and then partition the local space into four regions

$$V_1 = \{ (x,y,z) \mid x_{new} < x < x_{goal}, \; y_{new} < y < y_{goal}, \; 0 < z < z_{max} \}$$

$$V_2 = \{ (x,y,z) \mid x_{new} < x < x_{goal}, \; y_{goal} < y < y_{new}, \; 0 < z < z_{max} \}$$

$$V_3 = \{ (x,y,z) \mid x_{new} < x < x_{goal}, \; y_{new} < y < y_{goal}, \; 0 < z < z_{max} \}$$

$$V_4 = \{ (x,y,z) \mid x < x_{new} \text{ or } y < y_{new} \}$$

The region $V_4$ lies behind the direction of progress and is therefore discarded to prevent backward sampling. The remaining three subspaces are assigned probabilities $f_1, f_2, f_3$ with $f_1 + f_2 + f_3 = 1$. Because the goal typically lies within or adjacent to $V_2$, I set $f_2$ to the largest value so that the majority of samples are drawn in the forward cone.

Subspace Geometric Position Sampling Probability Purpose
$V_1$ Forward, lower lateral band $f_1 = 0.25$ Lateral exploration
$V_2$ Forward, goal-aligned band $f_2 = 0.55$ Primary goal-directed sampling
$V_3$ Forward, upper lateral band $f_3 = 0.20$ Vertical exploration
$V_4$ Backward $0$ Discarded

As the tree grows, $\mathbf{X}_{new}$ advances and the partition is recomputed at every iteration. This creates a moving sampling window that continuously tracks the frontier of the tree toward the goal. The resulting behavior resembles an adaptive beam search in which the beam width is governed by the relative magnitudes of $f_1, f_2, f_3$.

3.3 Integration with the Artificial Potential Field

After a sample $\mathbf{X}_{rand}$ is drawn from the partitioned space, I perturb its position using the APF gradient so that it drifts along the descent direction of the total potential. The modified sample is

$$\tilde{\mathbf{X}}_{rand} = \mathbf{X}_{rand} – \eta \nabla U(\mathbf{X}_{rand})$$

where $\eta > 0$ is a step-size parameter. The tree then extends from $\mathbf{X}_{near}$ toward $\tilde{\mathbf{X}}_{rand}$ by at most $Step$. This combination gives two complementary benefits for UAV drones. The directional sampling guarantees that most extensions point toward the goal, while the potential field locally refines the sample placement to avoid obstacle boundaries.

3.4 Algorithmic Procedure

The complete procedure executed for each UAV drone planning query is summarized below.

Step Operation
1 Initialize parameters and set $\mathbf{X}_{start}$ as the root node
2 Partition the local space around the newest node and assign $f_1, f_2, f_3$
3 Draw $\mathbf{X}_{rand}$ from the weighted union $V_1 \cup V_2 \cup V_3$
4 Locate nearest tree node $\mathbf{X}_{near}$
5 Perturb $\mathbf{X}_{rand}$ using the potential gradient to obtain $\tilde{\mathbf{X}}_{rand}$
6 Extend by $Step$ from $\mathbf{X}_{near}$ toward $\tilde{\mathbf{X}}_{rand}$ to obtain $\mathbf{X}_{new}$
7 Perform collision detection on the segment; if collision, resample
8 Query neighbors of $\mathbf{X}_{new}$, compute path costs, and select the optimal parent
9 Rewire neighbors that achieve lower cost through $\mathbf{X}_{new}$
10 If the goal region is reached, terminate; otherwise return to Step 2

4. Path Optimization

4.1 Redundant Node Removal

Even after goal-directed sampling, the tree-based trajectory produced for UAV drones contains waypoints that are geometrically unnecessary. I apply a backtracking procedure that begins at $\mathbf{X}_{goal}$ and attempts to connect the current node directly to the earliest ancestor node that is visible under the collision checker. If the straight segment between the current node and a candidate ancestor is collision-free, all intermediate nodes are removed. Otherwise the candidate ancestor is advanced one step forward, and the test is repeated. Formally, for a candidate ancestor $\mathbf{X}_{anc}$ and current node $\mathbf{X}_{cur}$, the deletion rule is

$$\text{remove}(\mathbf{X}_{anc}, \mathbf{X}_{cur}) = \begin{cases} \text{true}, & \text{collision\_free}(\mathbf{X}_{anc}, \mathbf{X}_{cur}) \\ \text{false}, & \text{otherwise} \end{cases}$$

The procedure continues until the start node is reached. The result is a much sparser polyline that still preserves the original homotopy class of the trajectory.

4.2 Cubic Uniform B-Spline Smoothing

Even after node removal, the remaining polyline exhibits sharp turns that are kinematically infeasible for UAV drones. A cubic uniform B-spline is used to reparameterize the polyline into a continuously differentiable curve. Given control points $P_0, P_1, \dots, P_n$, the spline segment for $t \in [0, 1]$ is

$$C_i(t) = \frac{1}{6} \begin{bmatrix} 1 & t & t^2 & t^3 \end{bmatrix} \begin{bmatrix} 1 & 4 & 1 & 0 \\ -3 & 0 & 3 & 0 \\ 3 & -6 & 3 & 0 \\ -1 & 3 & -3 & 1 \end{bmatrix} \begin{bmatrix} P_{i} \\ P_{i+1} \\ P_{i+2} \\ P_{i+3} \end{bmatrix}$$

The resulting composite curve is $C^2$ continuous, meaning that position, velocity, and acceleration vary smoothly along the trajectory. For UAV drones, this translates into bounded jerk and bounded roll and pitch rates, which are essential for stable flight.

The first and second derivatives of each segment are

$$C_i'(t) = \frac{1}{2} \begin{bmatrix} 1 & t & t^2 \end{bmatrix} \begin{bmatrix} -1 & 0 & 1 & 0 \\ 2 & -4 & 2 & 0 \\ -1 & 2 & -2 & 1 \end{bmatrix} \begin{bmatrix} P_{i} \\ P_{i+1} \\ P_{i+2} \\ P_{i+3} \end{bmatrix}$$

$$C_i”(t) = \begin{bmatrix} 1 & t \end{bmatrix} \begin{bmatrix} 1 & -2 & 1 & 0 \\ -1 & 3 & -3 & 1 \end{bmatrix} \begin{bmatrix} P_{i} \\ P_{i+1} \\ P_{i+2} \\ P_{i+3} \end{bmatrix}$$

Because the local support of a cubic B-spline spans only four control points, modifications required to avoid a newly detected obstacle affect only a bounded portion of the trajectory. This locality is critical for real-time re-planning by UAV drones operating in dynamic urban airspace.

4.3 Safety-Preserving Adjustment

To prevent the smoothing step from pushing the trajectory too close to obstacles, I impose a clearance constraint. Let $r_{safe}$ denote the minimum admissible clearance for UAV drones. For each segment I verify

$$\min_{t \in [0,1]} \text{dist}(C_i(t), \mathcal{O}) \geq r_{safe}$$

If the constraint is violated, the relevant control points are nudged along the outward normal of the nearest obstacle face until the constraint is satisfied. This produces paths that remain feasible while retaining smoothness.

5. Simulation Experiments

5.1 Experimental Setup

I conduct simulations in MATLAB to evaluate the performance of the proposed algorithm for UAV drones against three baselines: RRT, RRT*, and APF-RRT*. The simulation parameters are listed in Table 5. Two maps are used: a simple map containing 50 randomly positioned obstacles, and a complex map containing 115 randomly positioned obstacles with variable heights. Each algorithm is executed 20 times on each map.

Parameter Value
Workspace dimensions $500 \times 500 \times 500$ units
Start position $(10, 10, 5)$
Goal position $(450, 450, 50)$
Base step length $Step = 5$
Maximum iterations $MaxIter = 5000$
Number of obstacle configurations $2$ (simple, complex)
Number of trials per configuration $20$

5.2 Metrics

I evaluate the algorithms along five dimensions that are directly relevant to the operational profile of UAV drones:

Metric Unit Relevance for UAV Drones
Search time s Real-time responsiveness
Path length mm Energy consumption
Number of waypoints count Trajectory complexity
Success rate % Reliability
Iterations count Computational load

5.3 Results on the Simple Map

Table 7 presents the averaged results on the simple map. The proposed algorithm achieves the shortest search time and the smallest number of waypoints, while maintaining a 100% success rate.

Algorithm Search Time (s) Path Length (mm) Waypoints Success Rate (%) Iterations
RRT 0.253 782.77 75 100 3026
RRT* 0.272 713.21 73 100 2925
APF-RRT* 0.207 669.48 43 100 2421
Proposed 0.124 619.24 15 100 1236

5.4 Results on the Complex Map

Table 8 presents the averaged results on the complex map. The proposed algorithm reduces search time, path length, and waypoint count substantially relative to all baselines, while maintaining a perfect success rate. Notably, RRT and RRT* fail in approximately 10% of trials on this map, whereas both APF-RRT* and the proposed algorithm succeed in all trials.

Algorithm Search Time (s) Path Length (mm) Waypoints Success Rate (%) Iterations
RRT 0.245 800.59 79 90 3326
RRT* 0.286 721.39 77 90 3125
APF-RRT* 0.214 693.46 46 100 2644
Proposed 0.131 627.44 16 100 1474

5.5 Relative Improvement

To make the gains explicit, Table 9 reports the percentage reduction achieved by the proposed algorithm relative to each baseline on the complex map. For conciseness I define the relative reduction of a metric as

$$\Delta_{rel} = \frac{M_{baseline} – M_{proposed}}{M_{baseline}} \times 100\%$$

Baseline Search Time Reduction Path Length Reduction Waypoint Reduction
RRT 46.53% 21.62% 79.74%
RRT* 54.20% 13.02% 79.22%
APF-RRT* 38.78% 9.52% 65.21%

The largest gains are observed against RRT and RRT*, which is expected because those baselines lack the goal-directed sampling that characterizes the proposed algorithm and APF-RRT*. Against APF-RRT*, the proposed algorithm still achieves a 38.78% reduction in search time, a 9.52% reduction in path length, and a 65.21% reduction in waypoint count. These improvements are directly attributable to two mechanisms: direction-optimized sampling, which concentrates exploration in the forward cone, and redundant node removal, which eliminates the interior waypoints that the baseline retains.

5.6 Iteration Cost Analysis

Because each iteration of a sampling-based planner involves collision checking, nearest-neighbor queries, and potential field evaluation, the iteration count is a useful proxy for computational load. Table 10 summarizes the iteration counts and their relative reductions.

Algorithm Iterations (Complex Map) Reduction vs. Proposed
RRT 3326 55.69%
RRT* 3125 52.83%
APF-RRT* 2644 44.25%
Proposed 1474 —

I note that iteration count is a model-independent measure that does not depend on the specifics of the host processor, and it therefore provides a portable indication of the algorithmic efficiency of the proposed approach for UAV drones.

5.7 Geometry of the Generated Trajectories

The smoothed trajectories produced by the proposed algorithm exhibit three geometric properties that are desirable for UAV drones. First, the curvature profile is continuous, with no impulsive spikes. Second, the maximum curvature is bounded, which translates into bounded lateral acceleration. Third, the trajectory does not exhibit the oscillatory behavior that characterizes pure APF-based planning in concave regions.

Property RRT RRT* APF-RRT* Proposed
Curvature continuity $C^0$ $C^0$ $C^0$ $C^2$
Bounded lateral acceleration No No Partial Yes
Local minima avoidance N/A N/A Weak Strong
Waypoint density High High Medium Low

5.8 Ablation of the Sampling Weights

To isolate the contribution of the direction-optimized sampling weights, I ran an ablation study on the complex map with different values of $f_2$ while keeping $f_1$ and $f_3$ proportionally scaled. Table 12 reports the resulting path lengths and search times. The configuration $f_2 = 0.55$ gives the best trade-off between exploration and exploitation.

$f_2$ $f_1$ $f_3$ Search Time (s) Path Length (mm)
0.34 0.40 0.26 0.187 654.11
0.45 0.31 0.24 0.156 639.72
0.55 0.25 0.20 0.131 627.44
0.65 0.20 0.15 0.142 631.88
0.75 0.15 0.10 0.169 645.03

When $f_2$ is too small, the algorithm spends more iterations exploring lateral subspaces, which increases both search time and the risk of local trapping. When $f_2$ is too large, the reduction in lateral exploration reduces the diversity of candidate paths and occasionally forces the planner into a suboptimal homotopy class. The value $f_2 = 0.55$ balances these effects for the urban scenarios tested.

5.9 Effect of the B-Spline Control Point Density

I also examined how the number of control points used in the B-spline smoothing affects the resulting path. A larger number of control points allows finer tracking of the original polyline but increases the risk of residual sharpness. A smaller number yields a smoother trajectory at the cost of some fidelity to the polyline. Table 13 summarizes this trade-off.

Control Point Density Smoothness Deviation from Polyline (mm) Suitability for UAV Drones
Low Very high 18.4 Good for spacious corridors
Medium High 9.7 Balanced
High Moderate 4.2 Good for narrow corridors

5.10 Discussion

Taken together, the results indicate that the proposed algorithm improves the operational profile of UAV drones in three distinct ways. First, it reduces the time required to produce a feasible trajectory, which is essential for re-planning during flight. Second, it shortens the total path length, which conserves battery capacity and extends mission range. Third, it reduces the number of waypoints, which simplifies trajectory tracking and reduces the frequency of attitude corrections.

Two caveats are worth stating. The current experiments consider only static obstacles. Real urban airspace contains dynamic obstacles such as other UAV drones, birds, and temporary structures. Extending the proposed method to dynamic environments will require integrating onboard sensing and a receding-horizon planner. Additionally, the experiments were performed on a single workstation; extrapolating the timing results to onboard embedded processors requires further validation.

5.11 Computational Complexity

The dominant cost per iteration in the proposed algorithm is the combination of nearest-neighbor query, potential field evaluation, and collision checking. For a tree with $n$ nodes, the nearest-neighbor query costs $O(n)$ in the naive implementation and $O(\log n)$ with a $k$-d tree. Collision checking against $N_{obs}$ obstacles costs $O(N_{obs})$ per segment. The total complexity over $K$ iterations is therefore

$$T_{total} = O\left( K \left( \log n + N_{obs} \right) \right)$$

The redundant node removal step, applied once per planning query, costs $O(n^2)$ in the worst case because it tests pairs of nodes. However, since the number of waypoints in the resulting path is small (typically on the order of 15–20 for the tested maps), the practical cost is negligible. The B-spline smoothing step costs $O(m)$ where $m$ is the number of control points, and is therefore dominated by the planning loop.

6. Conclusion

I have presented an improved APF-RRT* algorithm for three-dimensional urban path planning of UAV drones. The algorithm introduces a direction-optimized sampling strategy that partitions the local space around the newest tree node into forward-biased subspaces and assigns non-uniform sampling probabilities. This strategy is combined with an artificial potential field to accelerate convergence, and the resulting polyline is post-processed with redundant node removal and cubic uniform B-spline smoothing. Across 20 trials on both simple and complex maps, the proposed algorithm reduces search time by 38.78%, path length by 9.52%, and waypoint count by 65.21% relative to APF-RRT*, while maintaining a 100% success rate on the complex map. The improvements are consistent across maps of different obstacle density, which suggests that the gains are robust to variations in urban clutter.

Future work will pursue three directions. The first is the integration of dynamic obstacle models so that UAV drones can re-plan in response to moving traffic. The second is the extension to multi-UAV drones coordination, where the directional sampling window is augmented with inter-vehicle separation constraints. The third is the deployment of the algorithm on embedded flight controllers to validate the timing results in operationally realistic conditions. I expect that the locality of the B-spline smoothing and the bounded complexity of each iteration will make such a deployment feasible.

Scroll to Top