A 3D UAV Path Planning Algorithm Based on Bidirectional RRT* with Adaptive Directional Sampling and Cooperative Dual-Tree Expansion

Abstract

UAV path planning in complex three-dimensional obstacle environments requires a balance between search efficiency and flight feasibility. However, existing RRT*-based methods often fail to satisfy this requirement, as their random sampling lacks directional guidance and makes limited use of environmental information. To this end, this paper proposes an environment-aware cooperative bidirectional RRT* algorithm (EAC-Bi-RRT*). In the sampling stage, the sampling probability of each direction is adaptively adjusted according to the obstacle distribution across 26 directional sectors and the relative goal orientation, so that the search receives stronger directional guidance. During bidirectional expansion, the two trees are assigned leader and follower roles according to the local expandability on the start and goal sides, and their cooperative search is combined with an environment-adaptive step size and a climbing-angle constraint to balance search efficiency and flight reachability. When an expanding node approaches an obstacle, a repulsive-only local directional correction suppresses oscillation, and the initial path is then smoothed by a curvature-constrained B-spline to form a continuous flight trajectory. Across all test scenarios, EAC-Bi-RRT* achieves a 100% planning success rate. Compared with the baseline algorithms, it reduces planning time by approximately 54–90% and path length by approximately 5–18% while maintaining low average turning angles, which demonstrates competitive overall performance.

1. Introduction

Unmanned Aerial Vehicles (UAVs) are widely used in urban logistics [1,2], disaster search and rescue [3,4], infrastructure inspection [5,6], and confined-space operations [7,8]. Unlike ground robots that mainly operate in a 2D plane, UAVs performing these tasks must plan paths in three-dimensional space [9]. The planner needs to consider collision risks from different directions, vertical occlusion caused by obstacle heights, and the kinematic constraints imposed by climbing capability [10,11]. When the search space extends from two dimensions to three, the geometric and topological relationships among obstacles become more complex, and feasible trajectory generation is subject to stricter constraints [12].

In complex 3D environments, sampling-based methods remain widely used for path planning because they adapt well to high-dimensional spaces [13]. RRT [14] and RRT* [15] are two of the most commonly used algorithms in this category. RRT is simple to implement and does not require environment preprocessing, but its random expansion process provides little control over path quality and often produces redundant paths [16,17]. RRT* improves path quality through parent selection and rewiring, yet in cluttered 3D environments its uniform random sampling and fixed-step expansion still limit search efficiency [18]. The piecewise linear paths generated in this way also make it difficult to satisfy kinematic constraints such as the climbing-angle requirement of UAVs. Although many improved variants enhance RRT* from different aspects, they still do not jointly balance search efficiency, path quality, and environmental adaptability in complex 3D workspaces.

In the sampling stage, Gammell et al. [19] proposed Informed-RRT*, which restricts sampling to the heuristic region defined by the current best solution and reduces invalid samples. HBAI-RRT* [20] and G-RRT* [21] further compress the sampling domain by incorporating segmented informed regions or heuristic cost information, thereby improving sampling efficiency. These methods mainly improve the sampling range, but their direct guidance on tree expansion direction remains limited. Jiang et al. [22] proposed FS-RRT, which constrains subsequent sampling angles according to collision feedback generated during expansion. The directional deflection sampling strategy [23] further adjusts the sampling deflection angle according to obstacle distribution along the goal direction to improve the success rate of expansion. Overall, existing methods improve sampling guidance from different perspectives, but how sampling resources should be organized and allocated across different directions in 3D space is still not sufficiently discussed.

In bidirectional search, B-RRT* [24] preserves the asymptotic optimality of RRT* within a dual-tree expansion framework while reducing computational cost through heuristic strategies. IB-RRT* [25] mainly improves the connection process between the two trees, using an intelligent sample insertion mechanism to speed up their connection. These methods shorten the time required to obtain an initial feasible path, but the connection process is still sensitive to randomness and may introduce additional computational overhead. CBQ-RRT* [26] further improves path smoothness by introducing kinematic constraints into bidirectional expansion and optimizing connection quality during tree merging. Cao et al. [27] combined an artificial potential field with a variable-step expansion strategy and adaptively adjusted the expansion direction and step size according to feedback on the distance between nodes and obstacles, thereby improving search efficiency and obstacle avoidance capability in cluttered environments. These methods improve the performance of bidirectional search, but most of them still apply similar sampling and expansion rules to both trees, making it difficult to fully exploit the different local obstacle distributions on the start side and the goal side.

For expansion guidance and obstacle avoidance, combining the artificial potential field with RRT* is a common improvement strategy. Qureshi and Ayaz [28] incorporated the artificial potential field into the random sampling process and used the potential-function gradient to steer samples toward the goal, thereby reducing invalid sampling and accelerating convergence. PF-RRT* proposed by Fan et al. [29] considers the combined effect of random points, the goal, and obstacles during new node generation to further improve convergence speed and path quality. HMA-RRT* [30] further integrates dynamic-region sampling, a hierarchical escape mechanism, heading-angle constraints, and adaptive step-size adjustment to improve search efficiency and planning feasibility in complex environments. The artificial potential field itself is still susceptible to local minima and oscillation near the goal region. Kilic et al. [31] introduced tangential forces, inertial heuristic forces, local minimum detection, and dynamic coefficient adjustment to enhance stable obstacle avoidance in complex environments. Even so, most existing APF-RRT* hybrid methods still retain the dominant role of the attractive term in global expansion; when this mechanism is combined with goal-biased guidance, the two can overlap functionally and may further intensify oscillatory behavior near the goal region.

To address the aforementioned issues, this paper proposes an environment-aware cooperative bidirectional RRT* (EAC-Bi-RRT*) algorithm for UAV path planning in complex 3D environments. The main contributions of this work are summarized as follows:

(1)

An adaptive directional sampling strategy is proposed. The strategy partitions the 3D sampling space into 26 spherical sectors, assigns differentiated sampling probabilities by integrating obstacle density and goal-direction information, and further introduces a distance-adaptive goal-bias probability, which raises the effective sampling rate while preserving global exploration capability.

(2)

A cooperative dual-tree expansion strategy is developed. It incorporates an adaptive variable step-size mechanism and realizes a one-time automatic assignment of the leader and follower roles based on a local expandability assessment at the start and goal states. In addition, the maximum climbing angle of the UAV is embedded into the expansion process as a hard constraint to ensure the kinematic feasibility of each expansion segment.

(3)

A repulsive-only local potential field correction mechanism is introduced. In the vicinity of obstacles, the mechanism uses the repulsive term for directional correction and incorporates a bounded goal influence factor so that the repulsive effect gradually weakens as the node approaches the goal. This design mitigates path oscillation near the goal region.

The paper is organized as follows. Section 2 sets up the 3D path planning problem together with the notation used throughout. RRT* and its main extensions are reviewed in Section 3. Section 4 describes the proposed EAC-Bi-RRT* algorithm, focusing on its three core mechanisms. Simulation studies and their interpretation are given in Section 5, and Section 6 closes the paper with conclusions and directions for further work.

2. Problem Definition

Let the 3D bounded workspace be $\mathcal{X}\subset {\mathbb{R}}^3$, the obstacle region be ${\mathcal{X}}_{obs}\subset \mathcal{X}$, and the free space be ${\mathcal{X}}_{free}=\mathcal{X}\setminus {\mathcal{X}}_{obs}$. Given a start point ${\mathbf{x}}_{start}\in {\mathcal{X}}_{free}$ and a goal point ${\mathbf{x}}_{goal}\in {\mathcal{X}}_{free}$, the goal region is defined as

$${\mathcal{X}}_{goal}=\left\{\mathbf{x}\in {\mathcal{X}}_{free}|\parallel \mathbf{x}-{\mathbf{x}}_{goal}\parallel \le r_{goal}\right\},$$

(1)

where $r_{goal}>0$ is the arrival tolerance radius.

A path is defined as an ordered sequence of $K+1$ waypoints $\sigma =\{{\mathbf{x}}_0,{\mathbf{x}}_1,\dots ,{\mathbf{x}}_K\}$, where ${\mathbf{x}}_k={(x_k,y_k,z_k)}^T$, ${\mathbf{x}}_0={\mathbf{x}}_{start}$, and ${\mathbf{x}}_K\in {\mathcal{X}}_{goal}$. Consecutive waypoints are connected by straight-line segments to form a piecewise-linear path. For any two consecutive waypoints ${\mathbf{x}}_k$ and ${\mathbf{x}}_{k+1}$, the climb angle is defined as

$${\gamma }_k=arctan\left({\displaystyle \frac{z_{k+1}-z_k}{\sqrt{{(x_{k+1}-x_k)}^2+{(y_{k+1}-y_k)}^2}}}\right).$$

(2)

Definition 1.

A path σ is feasible if every waypoint and connecting segment lies within the free space, i.e., ${\mathbf{x}}_k\in {\mathcal{X}}_{free}$ and $\overline{{\mathbf{x}}_k{\mathbf{x}}_{k+1}}\subset {\mathcal{X}}_{free}$ for $k=0,1,\dots ,K-1$, and the climb angle of every segment satisfies $|{\gamma }_k|\le {\gamma }_{max}$, where ${\gamma }_{max}$ is the maximum allowable climb angle. Let Σ denote the set of all feasible paths.

Definition 2.

The optimal path ${\sigma }^{*}$ minimizes the cost over all feasible paths:

$${\sigma }^{*}=arg\underset{\sigma \in \Sigma }{min}{\displaystyle \sum _{k=0}^{K-1}}\parallel {\mathbf{x}}_{k+1}-{\mathbf{x}}_k\parallel .$$

(3)

3. Related Works

3.1. RRT

RRT [14] is one of the foundational algorithms in sampling-based path planning and serves as the basis of the proposed method. Given the start configuration ${\mathbf{x}}_{start}$, the algorithm initializes a search tree T rooted at ${\mathbf{x}}_{start}$. At each iteration, a sample ${\mathbf{x}}_{rand}$ is drawn uniformly from the free space, and the node ${\mathbf{x}}_{nearest}$ in T that is closest to ${\mathbf{x}}_{rand}$ in Euclidean distance is identified. A new node ${\mathbf{x}}_{new}$ is then generated by extending from ${\mathbf{x}}_{nearest}$ toward ${\mathbf{x}}_{rand}$ with a fixed step size $\eta$. The algorithm first checks whether the segment between ${\mathbf{x}}_{nearest}$ and ${\mathbf{x}}_{new}$ is collision-free. If no collision is detected, ${\mathbf{x}}_{new}$ is added to T; otherwise, the current expansion is not executed. The algorithm terminates when the newly generated ${\mathbf{x}}_{new}$ enters the prescribed radius of the goal ${\mathbf{x}}_{goal}$ and the connecting segment is collision-free. The feasible path is then obtained by tracing the parent pointers from this node back to the root.

3.2. RRT*

RRT* is an asymptotically optimal extension of RRT, and its overall procedure is summarized in Algorithm 1. After generating ${\mathbf{x}}_{new}$ through the same sampling and expansion procedure as RRT, RRT* no longer connects it directly to ${\mathbf{x}}_{nearest}$, but instead performs two additional optimization operations within the overall framework, as illustrated in Figure 1. The first is parent selection (ChooseParent, Algorithm 2), which identifies all neighboring nodes within a given radius of ${\mathbf{x}}_{new}$, evaluates the cumulative cost from the start node to ${\mathbf{x}}_{new}$ through each neighbor, and selects the collision-free node with the minimum cost as the parent of ${\mathbf{x}}_{new}$. The second is rewiring (Rewire, Algorithm 3), which checks whether routing through ${\mathbf{x}}_{new}$ yields a lower-cost path to any neighboring node; if so, and if the connection is collision-free, that node is rewired through ${\mathbf{x}}_{new}$. By continuously performing parent selection and rewiring, RRT* gradually reduces the cost of the best path found so far and asymptotically converges to the global optimum as the number of samples becomes sufficiently large.

Algorithm 1 RRT*

Input: ${\mathbf{x}}_{start}$, ${\mathbf{x}}_{goal}$, ${\mathcal{X}}_{obs}$, $\eta$, N

Output: Path $\sigma$ or ∅

1: $T\leftarrow \{V=\left\{{\mathbf{x}}_{start}\right\},E=\varnothing \}$

2: for  $i=1$  N  do

3:    ${\mathbf{x}}_{rand}\leftarrow \mathrm{SAMPLEFREE}\left({\mathcal{X}}_{free}\right)$

4:    ${\mathbf{x}}_{nearest}\leftarrow \mathrm{NEAREST}(T,{\mathbf{x}}_{rand})$

5:    ${\mathbf{x}}_{new}\leftarrow \mathrm{STEER}({\mathbf{x}}_{nearest},{\mathbf{x}}_{rand},\eta )$

6:    if $\mathrm{COLLISIONFREE}({\mathbf{x}}_{nearest},{\mathbf{x}}_{new},{\mathcal{X}}_{obs})$ then

7:         $Q_{near}\leftarrow \mathrm{NEAR}(T,{\mathbf{x}}_{new},r_n)$

8:         ${\mathbf{x}}_{parent}\leftarrow \mathrm{CHOOSEPARENT}(Q_{near},{\mathbf{x}}_{nearest},{\mathbf{x}}_{new})$

9:         $V\leftarrow V\cup \left\{{\mathbf{x}}_{new}\right\}$; $E\leftarrow E\cup \left\{({\mathbf{x}}_{parent},{\mathbf{x}}_{new})\right\}$

10:         $\mathrm{REWIRE}(T,Q_{near},{\mathbf{x}}_{new})$

11:    end if

12: end for

13: return  $\mathrm{EXTRACTPATH}\left(T\right)$

Algorithm 2 ChooseParent

Input:  $Q_{near}$, ${\mathbf{x}}_{nearest}$, ${\mathbf{x}}_{new}$

Output:  ${\mathbf{x}}_{parent}$

1: ${\mathbf{x}}_{parent}\leftarrow {\mathbf{x}}_{nearest}$; $c_{min}\leftarrow \mathrm{COST}\left({\mathbf{x}}_{nearest}\right)+\parallel {\mathbf{x}}_{nearest}-{\mathbf{x}}_{new}\parallel$

2: for each  ${\mathbf{x}}_{near}\in Q_{near}$  do

3:    $c\leftarrow \mathrm{COST}\left({\mathbf{x}}_{near}\right)+\parallel {\mathbf{x}}_{near}-{\mathbf{x}}_{new}\parallel$

4:    if $c<c_{min}$ and $\mathrm{COLLISIONFREE}({\mathbf{x}}_{near},{\mathbf{x}}_{new},{\mathcal{X}}_{obs})$ then

5:         ${\mathbf{x}}_{parent}\leftarrow {\mathbf{x}}_{near}$; $c_{min}\leftarrow c$

6:    end if

7: end for

8: return ${\mathbf{x}}_{parent}$

Algorithm 3 Rewire

Input:  $T=(V,E)$, $Q_{near}$, ${\mathbf{x}}_{new}$

Output: Updated T

1: for each  ${\mathbf{x}}_{near}\in Q_{near}$  do

2:    $c\leftarrow \mathrm{COST}\left({\mathbf{x}}_{new}\right)+\parallel {\mathbf{x}}_{new}-{\mathbf{x}}_{near}\parallel$

3:    if $c<\mathrm{COST}\left({\mathbf{x}}_{near}\right)$ and $\mathrm{COLLISIONFREE}({\mathbf{x}}_{new},{\mathbf{x}}_{near},{\mathcal{X}}_{obs})$ then

4:        $E\leftarrow E\setminus \left\{(\mathrm{PARENT}\left({\mathbf{x}}_{near}\right),{\mathbf{x}}_{near})\right\}\cup \left\{({\mathbf{x}}_{new},{\mathbf{x}}_{near})\right\}$

5:    end if

6: end for

7: return  T

3.3. APF-RRT*

To alleviate the uninformed expansion behavior of RRT*, APF-RRT* incorporates an artificial potential field (APF) into the tree expansion process. The basic idea is derived from a physical field analogy: the goal is modeled as an attractive source that pulls each node toward the target, while obstacles are modeled as repulsive sources that push the node away once it falls within a preset influence radius. The repulsive effect acts only in the vicinity of obstacles and becomes stronger as the node moves closer to an obstacle. The expansion direction is determined by the combined action of the attractive and repulsive fields, allowing the tree to move toward the goal while bypassing regions with dense obstacles. The pseudocode is given in Algorithm 4.

Algorithm 4 APF-RRT*

Input:  ${\mathbf{x}}_{start}$, ${\mathbf{x}}_{goal}$, ${\mathcal{X}}_{obs}$, $\eta$, N, $k_{att}$, $k_{rep}$, ${\rho }_0$

Output: Path $\sigma$ or ∅

1: $T\leftarrow \{V=\left\{{\mathbf{x}}_{start}\right\},E=\varnothing \}$

2: for  $i=1$  N  do

3:    ${\mathbf{x}}_{rand}\leftarrow \mathrm{SAMPLEFREE}\left({\mathcal{X}}_{free}\right)$

4:    ${\mathbf{x}}_{nearest}\leftarrow \mathrm{NEAREST}(T,{\mathbf{x}}_{rand})$

5:    {APF-guided expansion}

6:    ${\mathbf{F}}_{att}\leftarrow \mathrm{ATTRACTION}({\mathbf{x}}_{nearest},{\mathbf{x}}_{goal},k_{att})$

7:    ${\mathbf{F}}_{rep}\leftarrow \mathrm{REPULSION}({\mathbf{x}}_{nearest},{\mathcal{X}}_{obs},k_{rep},{\rho }_0)$

8:    ${\mathbf{F}}_{total}\leftarrow {\mathbf{F}}_{att}+{\mathbf{F}}_{rep}$

9:    ${\mathbf{x}}_{new}\leftarrow {\mathbf{x}}_{nearest}+\eta ·{\mathbf{F}}_{total}/\parallel {\mathbf{F}}_{total}\parallel$

10:    if $\mathrm{COLLISIONFREE}({\mathbf{x}}_{nearest},{\mathbf{x}}_{new},{\mathcal{X}}_{obs})$ then

11:        $Q_{near}\leftarrow \mathrm{NEAR}(T,{\mathbf{x}}_{new},r_n)$

12:        ${\mathbf{x}}_{parent}\leftarrow \mathrm{CHOOSEPARENT}(Q_{near},{\mathbf{x}}_{nearest},{\mathbf{x}}_{new})$

13:        $V\leftarrow V\cup \left\{{\mathbf{x}}_{new}\right\}$; $E\leftarrow E\cup \left\{({\mathbf{x}}_{parent},{\mathbf{x}}_{new})\right\}$

14:        $\mathrm{REWIRE}(T,Q_{near},{\mathbf{x}}_{new})$

15:    end if

16: end for

17: return  $\mathrm{EXTRACTPATH}\left(T\right)$

3.4. Bi-RRT*

Bidirectional search is a common way to improve search efficiency. Bi-RRT* builds two search trees, $T_1$ and $T_2$, from the start point ${\mathbf{x}}_{start}$ and the goal point ${\mathbf{x}}_{goal}$, respectively, and brings them gradually closer through alternating expansion. Taking $T_1$ as an example, each iteration consists of sampling, nearest-neighbor search, and node expansion. If the expanded segment passes collision checking, the new node ${\mathbf{x}}_{new}$ is inserted into tree $T_1$. ChooseParent and Rewire are then applied to optimize the local tree structure. The algorithm then searches in $T_2$ for the node ${\mathbf{x}}_{close}$ that is nearest to ${\mathbf{x}}_{new}$. If the distance between them is smaller than a predefined connection threshold and the connecting segment is collision-free, the two partial paths are concatenated to form a complete path from the start to the goal. Otherwise, the roles of $T_1$ and $T_2$ are exchanged in the next iteration, and the process continues until the two trees are connected or the iteration budget is exhausted. Algorithm 5 outlines the full procedure of Bi-RRT*. By growing from both ends, bidirectional search approximately halves the distance that a single tree needs to traverse and can significantly accelerate the discovery of an initial feasible path. However, the two trees use identical sampling strategies and step-size parameters, which prevents them from adapting to local environmental differences. In addition, their connection still relies entirely on random sampling, without any active mechanism for tracking the frontier of the other tree, leaving considerable room for improvement in obstacle-rich environments.

Algorithm 5 Bi-RRT*

Input:  ${\mathbf{x}}_{start}$, ${\mathbf{x}}_{goal}$, ${\mathcal{X}}_{obs}$, $\eta$, ${\eta }_{connect}$, N

Output: Path $\sigma$ or ∅

1: $T_1\leftarrow \{V_1=\left\{{\mathbf{x}}_{start}\right\},E_1=\varnothing \}$; $T_2\leftarrow \{V_2=\left\{{\mathbf{x}}_{goal}\right\},E_2=\varnothing \}$

2: for  $i=1$  N  do

3:    {Expand $T_1$}

4:    ${\mathbf{x}}_{rand}\leftarrow \mathrm{SAMPLEFREE}\left({\mathcal{X}}_{free}\right)$

5:    ${\mathbf{x}}_{nearest}\leftarrow \mathrm{NEAREST}(T_1,{\mathbf{x}}_{rand})$

6:    ${\mathbf{x}}_{new}\leftarrow \mathrm{STEER}({\mathbf{x}}_{nearest},{\mathbf{x}}_{rand},\eta )$

7:    if $\mathrm{COLLISIONFREE}({\mathbf{x}}_{nearest},{\mathbf{x}}_{new},{\mathcal{X}}_{obs})$ then

8:        $Q_{near}\leftarrow \mathrm{NEAR}(T_1,{\mathbf{x}}_{new},r_n)$

9:        ${\mathbf{x}}_{parent}\leftarrow \mathrm{CHOOSEPARENT}(Q_{near},{\mathbf{x}}_{nearest},{\mathbf{x}}_{new})$

10:        $V_1\leftarrow V_1\cup \left\{{\mathbf{x}}_{new}\right\}$; $E_1\leftarrow E_1\cup \left\{({\mathbf{x}}_{parent},{\mathbf{x}}_{new})\right\}$

11:        $\mathrm{REWIRE}(T_1,Q_{near},{\mathbf{x}}_{new})$

12:        {Attempt connection}

13:        ${\mathbf{x}}_{close}\leftarrow \mathrm{NEAREST}(T_2,{\mathbf{x}}_{new})$

14:        if $\parallel {\mathbf{x}}_{new}-{\mathbf{x}}_{close}\parallel <{\eta }_{connect}$ and $\mathrm{COLLISIONFREE}({\mathbf{x}}_{new},{\mathbf{x}}_{close},{\mathcal{X}}_{obs})$ then

15:             return $\mathrm{MERGEPATH}(T_1,T_2,{\mathbf{x}}_{new},{\mathbf{x}}_{close})$

16:        end if

17:    end if

18:    $\mathrm{SWAP}(T_1,T_2)$ {Alternate trees}

19: end for

20: return  ∅

4. The Proposed EAC-Bi-RRT* Algorithm

4.1. Overall Algorithmic Framework

This section presents the EAC-Bi-RRT* algorithm. The proposed method adopts bidirectional RRT* as the basic framework and embeds environment-aware mechanisms into key stages including sampling, tree expansion, near-obstacle correction, and path postprocessing, thereby forming an integrated planning pipeline for complex 3D obstacle environments.

During the sampling stage, the algorithm partitions the local 3D directional space around the expansion node into spherical sectors and assigns differentiated sampling probabilities by jointly considering obstacle distribution and goal-direction information. A distance-adaptive goal bias is further introduced to suppress invalid expansions when the goal direction is obstructed. During the dual-tree expansion stage, the expansion step size is adaptively adjusted according to the local environmental complexity. The leader/follower roles are assigned once at initialization based on a local expandability assessment, so as to enhance the coordination between the two trees and promote rapid connection. In near-obstacle regions, a local potential field containing only a repulsive term corrects the expansion direction, while a climb-angle constraint ensures the physical executability of each expansion. After the two trees are successfully connected, the initial polyline path is further processed by curvature-constrained B-spline smoothing with collision-safe fallback, so as to improve path quality and kinematic feasibility. Figure 2 illustrates the overall workflow of EAC-Bi-RRT*.

On this basis, Algorithm 6 presents the main pseudocode of EAC-Bi-RRT* in order to further clarify the calling relationships and execution order of the constituent modules within the overall planning loop. The following subsections describe the design principles and implementation details of each key mechanism.

Algorithm 6 EAC-Bi-RRT*

Input:  ${\mathbf{x}}_{start}$, ${\mathbf{x}}_{goal}$, ${\mathcal{X}}_{obs}$ (obstacles), $\mathcal{X}$ (workspace), N (max iterations)

Output:  Optimized path ${\sigma }^{*}$

1: {Initialization}

2: $T_1\leftarrow \{V_1=\left\{{\mathbf{x}}_{start}\right\},E_1=\varnothing \}$; $T_2\leftarrow \{V_2=\left\{{\mathbf{x}}_{goal}\right\},E_2=\varnothing \}$

3: $d_{initial}\leftarrow \parallel {\mathbf{x}}_{start}-{\mathbf{x}}_{goal}\parallel$

4: ${\eta }_{max}^{global}\leftarrow {\eta }_0·(1-R_v)/e^{R_n}$ {Global complexity assessment, Equation (14)}

5: {Environment-aware role assignment}

6: ${\eta }_s\leftarrow \mathrm{ADAPTIVESTEP}({\mathbf{x}}_{start},{\mathcal{X}}_{obs})$; ${\eta }_g\leftarrow \mathrm{ADAPTIVESTEP}({\mathbf{x}}_{goal},{\mathcal{X}}_{obs})$ {Equation (18)}

7: if  ${\eta }_s\ge {\eta }_g$  then

8:       $T_{Leader}\leftarrow T_1$; $T_{Follower}\leftarrow T_2$

9: else

10:       $T_{Leader}\leftarrow T_2$; $T_{Follower}\leftarrow T_1$

11: end if

12: {Main loop}

13: for  $i=1$  N  do

14:       $(\sigma ,found)\leftarrow \mathrm{COOPERATIVEEXPAND}(T_{Leader},T_{Follower},{\mathcal{X}}_{obs})$

15:       if $found$ then

16:            ${\sigma }^{*}\leftarrow \mathrm{GREEDYSHORTCUT}(\sigma ,{\mathcal{X}}_{obs})$ {Greedy path pruning}

17:            ${\sigma }^{*}\leftarrow \mathrm{BSLINESMOOTH}({\sigma }^{*},{\mathcal{X}}_{obs},{\kappa }_{max})$ {Curvature-constrained smoothing, Equation (28)}

18:            return ${\sigma }^{*}$

19:      end if

20: end for

21: return ∅ {Failure}

4.2. Adaptive Directional Sampling Strategy

We define a local spherical coordinate system centered at the current expansion node ${\mathbf{x}}_{near}$. A spatial direction is described by an azimuth angle $\theta \in [0,2\pi )$ and an elevation angle $\varphi \in [-\pi /2,\pi /2]$. Here, $\theta$ measures the horizontal direction counterclockwise from the positive x-axis, and $\varphi$ gives the angular deviation from the horizontal plane, with upward taken as positive. This local frame is used only to parameterize the candidate expansion directions, while all node positions and collision-free checks remain in the global Cartesian frame.

This partition follows the classical 26-neighborhood connectivity in discrete 3D space. The spherical space is divided into five elevation layers. The upper, middle, and lower layers each contain eight azimuthal sectors spaced at $45$, while the two polar caps are not further subdivided.

Table 1. Partitioning of the 26 spherical sectors.

Layer	Sectors	Elevation Range	Count	Description
Top cap	$S_0$	$67.5°\le \varphi \le 90°$	1	Steep-climb cone
Upper	$S_1$–$S_8$	$22.5°\le \varphi <67.5°$	8	Primary climbing directions
Middle	$S_9$–$S_{16}$	$-22.5°<\varphi <22.5°$	8	Near-horizontal cruise zone
Lower	$S_{17}$–$S_{24}$	$-67.5<\varphi \le -22.5$	8	Descending directions
Bottom cap	$S_{25}$	$-90°\le \varphi <-67.5$	1	Steep-descent cone

gives the partitioning scheme, and Figure 3 shows the overall structure.

The center direction vector ${\mathbf{d}}_i$ specifies sector $S_i$. The center directions of the top and bottom polar caps are ${\mathbf{d}}_0={[0,0,1]}^T$ and ${\mathbf{d}}_{25}={[0,0,-1]}^T$, respectively. For the upper, middle, and lower layers, the sector center direction depends on the layer elevation parameter ${\varphi }_{layer}$ and the azimuth index $j=0,1,\dots ,7$:

$${\mathbf{d}}_{layer,j}=\left(\begin{array}{c}cos{\varphi }_{layer}cos(j·45°+22.5°)\\ cos{\varphi }_{layer}sin(j·{45}^{\circ }+22.5°)\\ sin{\varphi }_{layer}\end{array}\right),$$

(4)

where the upper layer takes ${\varphi }_{upper}={45}^{\circ }$, the middle layer takes ${\varphi }_{middle}=0°$, and the lower layer takes ${\varphi }_{lower}=-45°$.

The sampling probability assigned to each sector depends on the obstacle distribution within that sector. To this end, obstacles are first counted within the spherical region centered at ${\mathbf{x}}_{near}$ with radius $R_{sense}$ and denoted as

$${\mathcal{O}}_{sense}=\left\{O_j|\parallel {\mathbf{c}}_j-{\mathbf{x}}_{near}\parallel \le R_{sense}\right\},$$

(5)

where ${\mathbf{c}}_j$ is the center coordinate of the j-th obstacle. For each $O_j\in {\mathcal{O}}_{sense}$, the vector from the expansion node to the obstacle center is defined as ${\mathbf{v}}_j={(v_x,v_y,v_z)}^T={\mathbf{c}}_j-{\mathbf{x}}_{near}$. Its azimuth and elevation angles are computed as

$$\begin{array}{cc}\hfill {\theta }_j& =\left\{\begin{array}{cc}arctan\left({\displaystyle \frac{v_y}{v_x}}\right),\hfill & v_x>0,v_y\ge 0,\hfill \\ arctan\left({\displaystyle \frac{v_y}{v_x}}\right)+2\pi ,\hfill & v_x>0,v_y<0,\hfill \\ arctan\left({\displaystyle \frac{v_y}{v_x}}\right)+\pi ,\hfill & v_x<0,\hfill \\ {\displaystyle \frac{\pi }{2}},\hfill & v_x=0,v_y>0,\hfill \\ {\displaystyle \frac{3\pi }{2}},\hfill & v_x=0,v_y<0,\hfill \end{array}\right.\hfill \\ \hfill {\varphi }_j& =arctan\left({\displaystyle \frac{v_z}{\sqrt{v_x^2+v_y^2}}}\right).\hfill \end{array}$$

(6)

The angle ${\varphi }_j$ determines the elevation layer of obstacle $O_j$. Within that layer, ${\theta }_j$ determines its azimuthal sector. In this way, $O_j$ is assigned to the corresponding sector $S_i$. After all obstacles are assigned to sectors, the obstacle density of each sector can be written as

$${\rho }_i={\displaystyle \frac{|{\mathcal{O}}_i|}{N_{total}}},$$

(7)

where $|{\mathcal{O}}_i|$ is the number of obstacles assigned to sector $S_i$, and $N_{total}=\left|{\mathcal{O}}_{sense}\right|$ is the total number of obstacles in the sensing region. A larger ${\rho }_i$ means that the direction of sector $S_i$ is more crowded with obstacles.

Obstacle distribution alone is not sufficient to determine the sampling probability, and the goal direction also needs to be taken into account. To measure the alignment between each sector and the goal direction, the cosine between the sector center direction ${\mathbf{d}}_i$ and the goal direction is computed as

$$cos{\Theta }_i={\mathbf{d}}_i·{\mathbf{d}}_{goal},$$

(8)

where ${\mathbf{d}}_{goal}=({\mathbf{x}}_{goal}-{\mathbf{x}}_{near})/\parallel {\mathbf{x}}_{goal}-{\mathbf{x}}_{near}\parallel$ is the unit vector pointing from the current node to the goal.

Combining the sector obstacle density ${\rho }_i$ and the goal-direction cosine $cos{\Theta }_i$, the sampling probability of each sector is defined as

$$P_{sector}\left(i\right)={\displaystyle \frac{e^{-\alpha ·{\rho }_i}·(1+\beta ·cos{\Theta }_i)}{{\sum }_{j=0}^{25}{e}^{-\alpha ·{\rho }_j}·(1+\beta ·cos{\Theta }_j)}},$$

(9)

where $\alpha >0$ is the obstacle sensitivity coefficient and $\beta \in (0,1)$ is the goal orientation coefficient.

The sampling process consists of two stages: sector selection and intra-sector sampling. First, a target sector $S_k$ is selected according to the probability distribution $P_{sector}$ via roulette wheel selection. Then, the azimuth angle ${\theta }_s$ and elevation angle ${\varphi }_s$ are uniformly sampled within the angular range of $S_k$, and the radial distance $r=R_{sample}·u^{1/3}$ ($u\sim U(0,1)$) is computed with a cube-root transformation to ensure volumetric uniformity inside the sphere. The resulting sample point is

$${\mathbf{x}}_{sample}={\mathbf{x}}_{near}+r·{[cos{\varphi }_{s}cos{\theta }_s,cos{\varphi }_{s}sin{\theta }_s,sin{\varphi }_s]}^T.$$

(10)

On top of the sector-based directional bias sampling, a distance-adaptive goal-bias probability is further introduced. Before each sampling step, the algorithm selects the goal point directly as the sample with probability $P_{goal}$ and enters the sector-based directional bias sampling procedure with probability $1-P_{goal}$. The value of $P_{goal}$ is dynamically adjusted according to the distance from the current search tree to the goal:

$$P_{goal}\left(t\right)=P_{min}+(P_{max}-P_{min})·{\displaystyle \frac{d_{current}}{d_{initial}}},$$

(11)

where $d_{initial}$ is the initial distance from the start to the goal, $d_{current}$ is the distance from the nearest node in the current search tree to the goal, and $P_{min}$ and $P_{max}$ are the lower and upper bounds of the goal-bias probability, respectively.

The sector-based directional bias sampling and the distance-adaptive goal-bias strategy together constitute the adaptive directional sampling strategy. The complete procedure is summarized in Algorithm 7.

Algorithm 7 Adaptive directional sampling

Input:  T (search tree), ${\mathbf{x}}_{target}$ (sampling target), ${\mathcal{X}}_{obs}$, $d_{initial}$

Output:  Sampled point ${\mathbf{x}}_{sample}$

1: {Stage 1: Distance-adaptive goal-bias probability}

2:  $d_{current}\leftarrow {min}_{\mathbf{v}\in V}\parallel \mathbf{v}-{\mathbf{x}}_{target}\parallel$

3: $P_{goal}\leftarrow P_{min}+(P_{max}-P_{min})·d_{current}/d_{initial}$ {Equation (11)}

4: if  $\mathrm{rand}\left(\right)<P_{goal}$  then

5:      return  ${\mathbf{x}}_{target}$

6: end if

7: {Stage 2: 26-sector direction-biased sampling}

8: ${\mathbf{x}}_{near}\leftarrow arg{min}_{\mathbf{v}\in V}\parallel \mathbf{v}-{\mathbf{x}}_{target}\parallel$

9: ${\mathbf{d}}_{goal}\leftarrow ({\mathbf{x}}_{target}-{\mathbf{x}}_{near})/\parallel {\mathbf{x}}_{target}-{\mathbf{x}}_{near}\parallel$

10: for $i=0$ to 25 do

11:       ${\rho }_i\leftarrow |{\mathcal{O}}_i|/|{\mathcal{O}}_{sense}|$ {Obstacle density of sector $S_i$}

12:       $cos{\Theta }_i\leftarrow {\mathbf{d}}_i·{\mathbf{d}}_{goal}$ {Goal alignment, ${\mathbf{d}}_i$ from Equation (4)}

13:       $w_i\leftarrow exp(-\alpha ·{\rho }_i)·(1+\beta ·cos{\Theta }_i)$ {Equation (9)}

14: end for

15: $P_i\leftarrow w_i/{\sum }_{j=0}^{25}{w}_j,\forall i\in \{0,\dots ,25\}$ {Normalize}

16: {Stage 3: Sector selection and intra-sector sampling}

17: $S^{*}\leftarrow \mathrm{ROULETTESELECT}\left(\{P_0,\dots ,P_{25}\}\right)$

18: Sample ${\theta }_s$, ${\varphi }_s$ uniformly within the angular range of $S^{*}$

19: $r\leftarrow R_{sample}·u^{1/3},u\sim U(0,1)$ {Cube-root for volumetric uniformity}

20: ${\mathbf{x}}_{sample}\leftarrow {\mathbf{x}}_{near}+r·{[cos{\varphi }_{s}cos{\theta }_s,cos{\varphi }_{s}sin{\theta }_s,sin{\varphi }_s]}^T$ {Equation (10)}

21: return ${\mathbf{x}}_{sample}$

4.3. Environment-Adaptive Variable-Step-Size Mechanism

A large step size increases the risk of collision when the search tree expands through narrow passages or obstacle-dense regions. A small step size improves local obstacle avoidance, but it also creates many redundant nodes in open areas and reduces overall search efficiency. The expansion step size therefore follows a three-level adjustment scheme and changes with the local environment.

The 3D volumetric occupancy ratio of the workspace is defined at initialization as

$$R_v={\displaystyle \frac{{\sum }_{i=1}^{N_{obs}}{V}_{obs,i}}{V_{total}}},$$

(12)

where $V_{obs,i}$ and $V_{total}$ denote the volume of the i-th obstacle and the total workspace volume, respectively.

An obstacle number density is further introduced as

$$R_n={\displaystyle \frac{N_{obs}·V_{unit}}{V_{total}}}.$$

(13)

where $N_{obs}$ is the total number of obstacles and $V_{unit}$ is the unit volume.

This gives the global upper bound of the step size as

$${\eta }_{max}^{global}={\displaystyle \frac{{\eta }_0·(1-R_v)}{e^{R_n}}},$$

(14)

where ${\eta }_0$ is a nominal step size related to the map scale.

At each expansion step, the 3D distance from the current node ${\mathbf{x}}_{near}$ to the nearest obstacle surface is computed as $d_{obs}^{3D}\left(\mathbf{x}\right)={min}_{i\in \mathcal{O}}(\parallel \mathbf{x}-{\mathbf{c}}_i\parallel -r_i)$. Let this distance be denoted by d. The locally adaptive step size is then written as

$${\eta }_{base}=\left\{\begin{array}{cc}{\eta }_{max}^{global},\hfill & d\ge D_{safe},\hfill \\ {\eta }_{min}+({\eta }_{max}^{global}-{\eta }_{min}){\left({\displaystyle \frac{d}{D_{safe}}}\right)}^{\kappa },\hfill & d<D_{safe},\hfill \end{array}\right.$$

(15)

where $D_{safe}$ sets the safety distance threshold, $\kappa$ controls the curvature of the mapping, and ${\eta }_{min}$ sets the minimum step size.

If multiple obstacles fall within the sensing range, a local density correction further reduces the step size:

$${\eta }_{adjusted}={\displaystyle \frac{{\eta }_{base}}{1+{\beta }_{local}·max(0,n_{local}-1)}},$$

(16)

where $n_{local}$ counts the obstacles within the safety distance range, and ${\beta }_{local}$ controls the local density decay.

When an expansion results in a collision, a bisection search along the expansion direction is performed to find the farthest collision-free point:

$${\mathbf{x}}_{fallback}^{\left(k\right)}={\mathbf{x}}_{near}+{\displaystyle \frac{{\eta }_{adjusted}}{2^k}}·\mathbf{d},k=1,2,\dots ,k_{max}.$$

(17)

The algorithm halves the step size progressively and accepts the first expansion point that is collision-free and satisfies $\parallel {\mathbf{x}}_{fallback}^{\left(k\right)}-{\mathbf{x}}_{near}\parallel \ge {\eta }_{min}$. If no valid point is found after $k>k_{max}$ iterations, the current expansion attempt is abandoned.

4.4. Cooperative Dual-Tree Expansion Strategy

In conventional bidirectional RRT*, the two search trees adopt symmetric expansion behavior, sharing the same step size, sampling strategy, and expansion rules without any division of labor. To address this limitation, a cooperative dual-tree expansion strategy is developed that enhances the collaboration efficiency of the two trees through a one-time asymmetric role assignment and a dynamic connection threshold.

During algorithm initialization, the adaptive step sizes at the start and goal states are computed as

$$\left\{\begin{array}{cc}\hfill {\eta }_{start}& =\mathrm{AdaptiveStep}\left({\mathbf{x}}_{start}\right)\hfill \\ \hfill {\eta }_{goal}& =\mathrm{AdaptiveStep}\left({\mathbf{x}}_{goal}\right)\hfill \end{array}\right.$$

(18)

The role assignment is then determined as follows:

$$(\mathrm{Leader},\mathrm{Follower})=\left\{\begin{array}{cc}(T_1,T_2),\hfill & {\eta }_{start}\ge {\eta }_{goal},\hfill \\ (T_2,T_1),\hfill & {\eta }_{start}<{\eta }_{goal},\hfill \end{array}\right.$$

(19)

where the tree with the larger step size is designated as the leader and the other as the follower.

During the planning process, the step sizes of both trees are independently computed by the variable step-size mechanism based on the local environment of their respective current nodes:

$$\left\{\begin{array}{cc}\hfill {\eta }_{Leader}& =\mathrm{AdaptiveStep}\left({\mathbf{x}}_{near,Leader}\right)\hfill \\ \hfill {\eta }_{Follower}& =\mathrm{AdaptiveStep}\left({\mathbf{x}}_{near,Follower}\right)\hfill \end{array}\right.$$

(20)

The leader tree uses the sampling strategy described in Section 4.2 to sample toward its natural target direction. The follower tree selects the most recently expanded node of the leader tree ${\mathbf{x}}_{Leader,new}$ as its sampling target with probability $P_{bias}$, and performs random sampling with probability $1-P_{bias}$. The connection test between the two trees uses a dynamic threshold that is coupled with the current step sizes:

$${\eta }_{connect}={\gamma }_c·min({\eta }_{Leader},{\eta }_{Follower}),$$

(21)

where ${\gamma }_c$ is the connection threshold coefficient.

The role assignment, sampling target selection, and dynamic connection test together constitute the cooperative dual-tree expansion strategy. The complete procedure is summarized in Algorithm 8.

4.5. Climb-Angle Constraint

The vertical maneuverability of a UAV is strictly limited by its thrust and power budget. To ensure the physical executability of the planned path, this paper embeds the maximum climb angle ${\gamma }_{max}$ as a hard constraint into the expansion process. Given the original expansion direction ${\mathbf{d}}_{raw}={[d_x,d_y,d_z]}^T$, its climb angle is defined as

$$\gamma =arctan\left({\displaystyle \frac{d_z}{\sqrt{d_x^2+d_y^2}}}\right).$$

(22)

The direction is kept unchanged when $\left|\gamma \right|\le {\gamma }_{max}$. When $\left|\gamma \right|>{\gamma }_{max}$, the direction is projected onto the boundary of the feasible cone:

$${\mathbf{d}}_{\mathrm{constrained}}={\displaystyle \frac{{\mathbf{d}}^{\prime }}{\parallel {\mathbf{d}}^{\prime }\parallel }},{\mathbf{d}}^{\prime }=\left[\begin{array}{c}{d}_x\\ d_y\\ \mathrm{sign}\left(d_z\right)h_{xy}tan{\gamma }_{max}\end{array}\right],$$

(23)

where $h_{xy}=\sqrt{d_x^2+d_y^2}$ is the magnitude of the horizontal component and ${\gamma }_{max}$ is the maximum allowable climb angle of the UAV.

Algorithm 8 Cooperative dual-tree expansion

Input:  $T_L$ (leader tree), $T_F$ (follower tree), ${\mathbf{x}}_{goal}$, ${\mathcal{X}}_{obs}$

Output:  Connection flag, path $\sigma$

1: {Phase 1: Leader tree expansion}

2: ${\mathbf{x}}_{sample}\leftarrow \mathrm{ADAPTIVEDIRECTIONALSAMPLING}(T_L,{\mathbf{x}}_{goal},{\mathcal{X}}_{obs})$ {Algorithm 7}

3: ${\mathbf{x}}_{near}^L\leftarrow \mathrm{Nearest}(T_L,{\mathbf{x}}_{sample})$

4: ${\eta }_L\leftarrow \mathrm{ADAPTIVESTEP}({\mathbf{x}}_{near}^L,{\mathcal{X}}_{obs})$ {Equations (15) and (16)}

5: $\widehat{\mathbf{d}}\leftarrow \mathrm{STEER}({\mathbf{x}}_{near}^L,{\mathbf{x}}_{sample})$

6: $\widehat{\mathbf{d}}\leftarrow \mathrm{REPULSIVECORRECTION}({\mathbf{x}}_{near}^L,\widehat{\mathbf{d}},{\mathbf{x}}_{goal},{\mathcal{X}}_{obs})$ {Equations (24)–(27)}

7: $\widehat{\mathbf{d}}\leftarrow \mathrm{CLIMBANGLECLAMP}(\widehat{\mathbf{d}},{\gamma }_{max})$ {Equation (23)}

8: $({\mathbf{x}}_{new}^L,o{k}_L)\leftarrow \mathrm{EXTEND}({\mathbf{x}}_{near}^L,\widehat{\mathbf{d}},{\eta }_L,{\mathcal{X}}_{obs})$ {with bisection fallback, Equation (17)}

9: if  $o{k}_L$  then

10:      $T_L\leftarrow \mathrm{INSERTANDREWIRE}(T_L,{\mathbf{x}}_{new}^L,{\mathcal{X}}_{obs})$

11: end if

12: {Phase 2: Follower tree expansion}

13: if  $o{k}_L$  then

14:      ${\mathbf{x}}_{target}\leftarrow {\mathbf{x}}_{new}^L$

15: else

16:      ${\mathbf{x}}_{target}\leftarrow \mathrm{Nearest}(T_L,T_F.\mathrm{latest})$

17: end if

18: if  $\mathrm{rand}\left(\right)<P_{bias}$  then

19:      ${\mathbf{x}}_{sample}^F\leftarrow {\mathbf{x}}_{target}$ {Biased toward leader}

20: else

21:      ${\mathbf{x}}_{sample}^F\leftarrow \mathrm{RANDOMSAMPLE}\left(\mathcal{X}\right)$

22: end if

23: ${\mathbf{x}}_{near}^F\leftarrow \mathrm{Nearest}(T_F,{\mathbf{x}}_{sample}^F)$

24: ${\eta }_F\leftarrow \mathrm{ADAPTIVESTEP}({\mathbf{x}}_{near}^F,{\mathcal{X}}_{obs})$

25: $({\mathbf{x}}_{new}^F,o{k}_F)\leftarrow \mathrm{STEERANDEXTEND}({\mathbf{x}}_{near}^F,{\mathbf{x}}_{sample}^F,{\eta }_F,{\mathbf{x}}_{goal},{\mathcal{X}}_{obs})$

26: if  $o{k}_F$  then

27:      $T_F\leftarrow \mathrm{INSERTANDREWIRE}(T_F,{\mathbf{x}}_{new}^F,{\mathcal{X}}_{obs})$

28: end if

29: {Phase 3: Dynamic connection detection}

30: ${\eta }_{conn}\leftarrow {\gamma }_c·min({\eta }_L,{\eta }_F)$ {Equation (21)}

31: for each $(ok,{\mathbf{x}}_{new},T_{own},T_{other})\in \{(o{k}_L,{\mathbf{x}}_{new}^L,T_L,T_F),(o{k}_F,{\mathbf{x}}_{new}^F,T_F,T_L)\}$ do

32:      if $ok$ then

33:         ${\mathbf{x}}_c\leftarrow \mathrm{Nearest}(T_{other},{\mathbf{x}}_{new})$

34:         if $\parallel {\mathbf{x}}_{new}-{\mathbf{x}}_c\parallel \le {\eta }_{conn}$ and $\mathrm{COLLISIONFREE}({\mathbf{x}}_{new},{\mathbf{x}}_c,{\mathcal{X}}_{obs})$ then

35:            return $(\mathbf{true},\mathrm{MERGEPATH}(T_L,T_F,{\mathbf{x}}_{new},{\mathbf{x}}_c))$

36:         end if

37:      end if

38: end for

39: return  $(\mathbf{false},\varnothing )$

4.6. Repulsive-Only Local Potential Field Correction

Since the sampling strategy in Section 4.2 already provides global directional guidance, this paper removes the attractive term from the conventional APF formulation and activates the repulsive correction only when a node enters the influence range of an obstacle ($\rho <{\rho }_0$). This design avoids the functional overlap between the attractive term and the sampling strategy, as well as the path oscillation that arises near the goal region.

The repulsive force is defined as

$${\mathbf{F}}_{rep}\left(\mathbf{x}\right)=\left\{\begin{array}{cc}{k}_{rep}{\left({\displaystyle \frac{1}{\rho }}-{\displaystyle \frac{1}{{\rho }_0}}\right)}^2·\psi \left(d\right)·{\mathbf{n}}_{obs},\hfill & \rho <{\rho }_0,\hfill \\ \mathbf{0},\hfill & \rho \ge {\rho }_0,\hfill \end{array}\right.$$

(24)

where $\rho$ denotes the distance from the current node to the nearest obstacle, and ${\rho }_0$ denotes the repulsive influence radius. $k_{rep}$ is the repulsive gain coefficient, and ${\mathbf{n}}_{obs}$ is the unit vector pointing from the obstacle toward the current node.

The goal influence factor $\psi \left(d\right)$ can be written as

$$\psi \left(d\right)={\displaystyle \frac{d^n}{1+d^n}},$$

(25)

where $d=\parallel \mathbf{x}-{\mathbf{x}}_{goal}\parallel$ is the distance from the current node to the goal and n is the shape parameter.

Within the near-obstacle range, the correction strength follows a distance-decaying weight

$$w={\left({\displaystyle \frac{{\rho }_0-d_{obs}}{{\rho }_0}}\right)}^2,$$

(26)

where $d_{obs}$ represents the distance from the current node to the nearest obstacle. The corrected expansion direction is obtained by

$${\mathbf{d}}_{out}={\displaystyle \frac{{\mathbf{d}}_{sample}+w·{\mathbf{F}}_{rep}}{\parallel {\mathbf{d}}_{sample}+w·{\mathbf{F}}_{rep}\parallel }},$$

(27)

where ${\mathbf{d}}_{sample}$ stands for the normalized sampling direction returned by the sampling strategy.

4.7. Path Smoothing and Safety Fallback

The piecewise-linear path generated by RRT* usually contains large turning-angle changes, which can affect the smoothness of trajectory tracking in UAV flight. A cubic B-spline is therefore used to smooth the planned path.

Using the waypoint sequence $\{{\mathbf{x}}_0,{\mathbf{x}}_1,\dots ,{\mathbf{x}}_K\}$ of the piecewise-linear path as control points, the cubic B-spline curve is expressed as

$$\mathbf{C}\left(t\right)={\displaystyle \sum _{i=0}^K}{N}_{i,3}\left(t\right){\mathbf{x}}_i,$$

(28)

where $N_{i,3}\left(t\right)$ is the cubic B-spline basis function, and t serves as the curve parameter.

The smoothed curve must satisfy the maximum curvature constraint $\kappa \left(t\right)\le {\kappa }_{max}=1/R_{min}$. The curve is then discretized at equal arc-length intervals, and the sampled points are checked against the free space. If a smoothed segment intersects an obstacle, that segment is discarded and replaced with the corresponding segment of the original piecewise-linear path so that the final output path remains collision-free.

4.8. Computational Complexity Analysis

The time cost of the proposed EAC-Bi-RRT* algorithm is primarily affected by the nearest-node search, the near-node search, obstacle density estimation, collision detection, and local rewiring. Let T denote the maximum number of iterations, N the number of nodes in the bidirectional trees, M the number of obstacles in the environment, and $S=26$ the fixed sector count of the adaptive directional sampling. In each iteration, the nearest-node search and the near-node search are performed via a linear traversal of the tree nodes, with a per-iteration cost of $O\left(N\right)$. The obstacle density estimation, collision detection, and local repulsive potential field correction scale linearly with the obstacle count, yielding a worst-case complexity of $O\left(M\right)$. The adaptive directional sampling only computes sampling probabilities over a fixed set of 26 sectors, giving a constant complexity of $O\left(S\right)=O\left(1\right)$. The cost of local rewiring is determined by the number of near nodes, and is already absorbed into the near-node search and collision detection steps above.

Consequently, the overall time complexity of EAC-Bi-RRT* can be expressed as $O\left(T·(N+M)\right)$, where T represents the maximum number of iterations. The proposed adaptive directional sampling, variable step-size, climb-angle constraint, and repulsive-only APF correction modules primarily introduce constant or local environment evaluation overhead, and therefore do not change the asymptotic order of complexity relative to standard RRT* and Bi-RRT*. If spatial indexing structures such as k-d trees or R-trees are introduced, the practical computational cost of the nearest-node search and obstacle queries can be further reduced.

5. Results and Discussion

To evaluate the planning performance of EAC-Bi-RRT* in different 3D obstacle scenarios, this section constructs four simulation environments. All experiments are conducted on the Windows 11 platform with an Intel Core i7-14650HX processor and 16 GB of memory, and MATLAB R2025a is used as the simulation software. To thoroughly validate the effectiveness of the proposed method, all simulations are implemented within a unified RRT* code framework, and EAC-Bi-RRT* is compared with five baseline algorithms, namely GB-RRT*, Bi-RRT*, Bi-APF-RRT*, AAE-RRT* [32], and DPF-Bi-RRT* [33].

Figure 4 illustrates the four types of 3D simulation environments used in this section. Environment 1 is a dense spherical obstacle environment, which is mainly used to evaluate the spatial search capability and local obstacle avoidance performance of the algorithm under densely distributed spherical obstacles. Environment 2 is a regular cylindrical array environment, which is mainly used to examine the corridor selection and path optimization capability of the algorithm in structured and constrained spaces. Environment 3 is a multi-layer building environment, which is mainly used to verify the constrained-space traversal capability and complex-space search performance of the algorithm under multi-level enclosed structures and narrow openings. Environment 4 is a large-scale random obstacle environment, which is mainly used to assess the global search capability and stability of the algorithm in large unstructured spaces.

The unified parameter settings used in the experiments are as follows. For all algorithms, the maximum number of iterations is set to 5000, and each algorithm is independently run 100 times in each environment. For GB-RRT*, Bi-RRT*, Bi-APF-RRT*, and DPF-Bi-RRT*, the fixed expansion step sizes in Environments 1–4 are set to 5 m, 10 m, 12 m, and 30 m, respectively. For AAE-RRT*, the basic step sizes are set in the same way, while its adaptive step-size ranges in Environments 1–4 are set to $[1,10]$ m, $[1,14]$ m, $[2,20]$ m, and $[5,60]$ m, respectively. EAC-Bi-RRT* adopts an environment-aware adaptive step-size strategy, in which the expansion step size is automatically adjusted according to the global environmental complexity and the local obstacle density. The evaluation metrics used in the experiments are path length, planning time, success rate, and average turning angle. Specifically, path length measures the total cost of the planned result, and planning time reflects the search efficiency. Success rate evaluates the ability to obtain a feasible path under given conditions, while the average turning angle quantifies the geometric smoothness of the path. The complete parameter settings used in our experiments, including the key parameters of the proposed EAC-Bi-RRT* and those of all baseline algorithms across the four environments, are summarized in Appendix A (

Table A1. Key parameters used in the proposed EAC-Bi-RRT* algorithm.

Group	Symbol	Meaning	Value	Notes
Adaptive step size	${\eta }_{min}$	Minimum step size	$0.25{\eta }_{max}$	Lower bound
	${\beta }_{\mathrm{local}}$	Local density decay	0.15	Step reduction coefficient
Spherical sector sampling	$P_{min}$	Min goal-bias probability	0.05	Lower bound
	$P_{max}$	Max goal-bias probability	0.40	Upper bound
APF local repulsion	$k_{\mathrm{rep}}$	Repulsion coefficient	0.5	Obstacle avoidance gain
Bi-tree cooperation	${\gamma }_c$	Connection threshold coeff.	1.5	Dynamic merge criterion
	$P_{\mathrm{bias}}$	Follower bias probability	0.6	Toward leader tree
Path optimization	${\kappa }_{max}$	Maximum curvature	$1/80$	Min turning radius = 80 m
	${\alpha }_{\mathrm{th}}$	Sharp turn threshold	$120$	Control point insertion
Global constraints	${\gamma }_{max}$	Maximum climb angle	$30$	3D flight envelope
	$N_{max}$	Maximum iterations	5000	Termination condition

,

Table A2. Shared parameters for all baseline algorithms across environments.

Parameter	Symbol	Environment 1	Environment 2	Environment 3	Environment 4
Step size	$\eta$	5	10	12	30
Rewire radius	$\gamma$	20	25	35	120
Connection tolerance	$d_{\mathrm{conn}}$	5	7	10	30
Collision resolution	${\Delta }_{\mathrm{col}}$	1	1	2	4
Maximum iterations	$N_{max}$	5000	5000	5000	5000

and

Table A3. Algorithm-specific parameters for baseline algorithms.

Method	Parameter	Symbol	Environment 1	Environment 2	Environment 3	Environment 4
GB-RRT*	Goal bias	$P_g$	0.20
Bi-RRT*	Swap probability	$P_s$	0.50
Bi-APF-RRT*	Goal bias	$P_g$	0.10
	Repulsion coefficient	$k_{\mathrm{rep}}$	0.30
	Repulsion range	$d_{\mathrm{rep}}$	10	14	20	60
AAE-RRT*	Attraction gain	${\lambda }_a$	200	1000	1000	1000
	Repulsion gain	${\lambda }_r$	100	500	500	500
	Safe distance	$R_{\mathrm{safe}}$	5	7	10	30
	Adaptive step range	$[{\eta }_{min},{\eta }_{max}]$	[1, 10]	[1, 14]	[2, 20]	[5, 60]
	Collision decay/growth	${\lambda }_{\mathrm{col}}/{\lambda }_{\mathrm{ncol}}$	0.6/2.0
DPF-Bi-RRT*	Attraction coefficient	$k_{\mathrm{att}}$	2.0
	Repulsion coefficient	$k_{\mathrm{rep}}$	0.8
	Repulsion range	$d_{\mathrm{rep}}$	150
	Greedy shortcut window	$n_g$	12

).

5.1. Dense Spherical Obstacle Environment

The workspace size in this scenario is $200\times 200\times 200$. Approximately 130 spherical obstacles of different scales are placed in the scene to construct a multi-scale and densely distributed 3D obstacle space. The start point is set to $(0,0,0)$, and the goal point is set to $(200,200,200)$. Statistics and paths are shown in

Table 2. Simulation data of six algorithms in different environments.

Environment	Algorithm	Success Rate	Path Length (m)	Time (s)	Avg. Turning Angle (°)
Environment 1	GB-RRT*	100%	367.20 ± 7.94	0.308 ± 0.136	7.33 ± 1.19
	Bi-RRT*	100%	403.07 ± 25.63	0.392 ± 0.148	12.50 ± 2.00
	Bi-APF-RRT*	100%	381.56 ± 13.74	0.215 ± 0.091	10.44 ± 1.61
	AAE-RRT*	100%	355.95 ± 6.91	0.214 ± 0.084	1.40 ± 1.16
	DPF-Bi-RRT*	100%	357.98 ± 7.74	0.122 ± 0.073	1.39 ± 0.88
	EAC-Bi-RRT*	100%	358.94 ± 7.75	0.039 ± 0.017	1.61 ± 0.79
Environment 2	GB-RRT*	73%	882.96 ± 51.13	0.950 ± 0.600	20.52 ± 1.96
	Bi-RRT*	100%	937.65 ± 64.21	0.431 ± 0.151	22.84 ± 2.28
	Bi-APF-RRT*	100%	888.12 ± 34.46	0.298 ± 0.110	20.39 ± 1.44
	AAE-RRT*	100%	813.78 ± 16.20	0.789 ± 0.121	11.75 ± 1.64
	DPF-Bi-RRT*	100%	866.05 ± 27.21	0.368 ± 0.171	8.96 ± 1.88
	EAC-Bi-RRT*	100%	768.46 ± 11.88	0.184 ± 0.064	8.56 ± 0.82
Environment 3	GB-RRT*	6%	1195.45 ± 114.80	1.681 ± 0.701	18.32 ± 1.91
	Bi-RRT*	100%	1196.76 ± 96.82	0.316 ± 0.222	22.10 ± 2.17
	Bi-APF-RRT*	100%	1141.89 ± 75.74	0.330 ± 0.228	19.46 ± 1.77
	AAE-RRT*	40%	989.03 ± 23.71	0.985 ± 0.400	5.55 ± 1.12
	DPF-Bi-RRT*	99%	1015.73 ± 50.86	0.371 ± 0.212	6.50 ± 1.21
	EAC-Bi-RRT*	100%	981.90 ± 26.21	0.805 ± 0.446	5.48 ± 0.81
Environment 4	GB-RRT*	84%	3581.47 ± 205.56	1.377 ± 0.561	25.81 ± 2.43
	Bi-RRT*	100%	3609.52 ± 245.02	1.158 ± 0.405	25.26 ± 2.67
	Bi-APF-RRT*	100%	3395.17 ± 133.10	1.030 ± 0.369	23.16 ± 2.17
	AAE-RRT*	100%	3076.69 ± 84.06	1.649 ± 0.391	9.46 ± 1.84
	DPF-Bi-RRT*	100%	3061.84 ± 101.89	0.562 ± 0.229	7.26 ± 1.47
	EAC-Bi-RRT*	100%	2960.57 ± 46.97	0.256 ± 0.049	5.98 ± 1.40

and Figure 5, respectively.

In the dense spherical obstacle environment, the obstacle sizes and distributions are highly random, and the random tree is prone to search oscillation and repeated expansion during local obstacle avoidance. Table 2 shows that EAC-Bi-RRT* achieves an average planning time of only 0.039 s. This represents reductions of 87.34%, 90.05%, and 81.86% relative to GB-RRT*, Bi-RRT*, and Bi-APF-RRT*, respectively, and is also significantly lower than those of AAE-RRT* and DPF-Bi-RRT*. In terms of path smoothness, EAC-Bi-RRT*, DPF-Bi-RRT*, and AAE-RRT* all achieve average turning angles below 2°, which are significantly better than those of GB-RRT*, Bi-RRT*, and Bi-APF-RRT*. This improvement comes from two aspects. The cooperative dual-tree expansion reduces ineffective search in densely obstructed regions. Meanwhile, the adaptive step size adjusts the expansion scale according to the local obstacle distribution. Together, these two mechanisms improve planning efficiency while preserving obstacle avoidance accuracy. Although the average path length and average turning angle of EAC-Bi-RRT* are comparable to those of AAE-RRT* and DPF-Bi-RRT*, its planning time is substantially shorter while maintaining comparable path quality and smoothness.

5.2. Regular Cylindrical Array Environment

The workspace size in this scenario is $500\times 500\times 200$. A total of 62 regularly arranged vertical column obstacles are placed in the scene, forming a continuous cylindrical array corridor structure. The start point is set to $(0,0,0)$, and the goal point is set to $(500,500,150)$. The corresponding statistical results are reported in Table 2.

The main challenge in the regular cylindrical array environment is that the algorithm needs not only to find a feasible path, but also to determine an appropriate traversal direction as quickly as possible within continuously constrained corridors. Figure 6 indicates that some baseline algorithms still produce clear backtracking and detours between the columns, while the path generated by EAC-Bi-RRT* stays closer to the main corridor direction. The quantitative results in Table 2 are consistent with this observation. EAC-Bi-RRT* records an average path length of 768.46 m and an average planning time of 0.184 s, both lower than those of the compared algorithms. The path length reductions are 12.97%, 18.04%, 13.47%, 5.57%, and 11.27% relative to GB-RRT*, Bi-RRT*, Bi-APF-RRT*, AAE-RRT*, and DPF-Bi-RRT*, respectively. This result suggests that EAC-Bi-RRT* can advance more efficiently along feasible passages in the regular cylindrical corridor environment while avoiding unnecessary detours between columns. The adaptive directional sampling strategy steers the random tree along corridor-compatible directions and reduces back-and-forth oscillation as well as repeated search between columns.

5.3. Multi-Layer Building Environment

The workspace size in this scenario is $550\times 550\times 400$. The scene contains multi-layer floor slabs, through-columns, guard columns, and block obstacles of different heights, which together form a complex 3D enclosed space with narrow openings. The start point is set to $(0,0,0)$, and the goal point is set to $(550,550,400)$. Table 2 reports the statistics, and Figure 7 shows the planned paths.

The multi-layer building environment involves the coupled constraints of floor-slab blockage, narrow openings, and 3D obstacles. When expansion toward the goal direction is repeatedly blocked, the random tree tends to remain trapped in locally enclosed regions, which leads to planning failure. Table 2 shows that the success rate of GB-RRT* in this environment is only 6%, while AAE-RRT* reaches only 40%, indicating that relying only on goal bias or adaptive step size is still insufficient for stable traversal in multi-layer constrained spaces. In contrast, EAC-Bi-RRT* achieves a success rate of 100%, demonstrating stronger traversal capability and search stability in complex enclosed environments. EAC-Bi-RRT* also attains the best average path length of 981.90 m. This is 17.95%, 14.01%, and 3.33% shorter than those of Bi-RRT*, Bi-APF-RRT*, and DPF-Bi-RRT*, respectively. Its average turning angle is only 5.48°, which is also markedly lower than the 6.50°–22.10° reported for the above algorithms. In terms of time cost, the average planning time of EAC-Bi-RRT* is 0.805 s. Although this value is not the lowest in this environment and is higher than those of Bi-RRT*, Bi-APF-RRT*, and DPF-Bi-RRT*, it remains lower than those of GB-RRT* and AAE-RRT*. This result indicates that the proposed algorithm does not simply pursue local search speed in strongly constrained 3D scenes, but instead trades a small amount of time for more stable search guidance, a higher success rate, and better path quality. The key reason is that the adaptive goal-bias mechanism actively reduces the bias probability based on local obstacle density when the goal direction is persistently blocked. This causes the random tree to fall back to sector-based directional exploration sampling, enabling it to escape locally constrained regions.

5.4. Large-Scale Random Obstacle Environment

The workspace size in this scenario is $2000\times 2000\times 400$. Various obstacles, including cuboids and cylinders, are randomly distributed in the scene to construct a large-scale unstructured environment with nonuniform distribution and local clustering characteristics. The start point is set to $(0,0,0)$, and the goal point is set to $(2000,2000,50)$. The corresponding statistical results are reported in Table 2.

The large-scale random obstacle environment is characterized by a vast search space, uneven obstacle distribution, and pronounced local clustering. As a result, the algorithm needs not only strong global exploration capability, but also the ability to avoid excessive invalid detours in locally cluttered regions. As shown in Figure 8, GB-RRT*, Bi-RRT*, Bi-APF-RRT*, AAE-RRT*, and DPF-Bi-RRT* all exhibit path deviations of varying degrees when traversing locally clustered obstacle regions, whereas the path generated by EAC-Bi-RRT* is more direct overall. Table 2 shows that EAC-Bi-RRT* maintains a success rate of 100% in this environment and achieves the shortest average path length of 2960.57 m and the fastest average planning time of 0.256 s. Compared with GB-RRT*, Bi-RRT*, and Bi-APF-RRT*, its average path length is reduced by 17.34%, 17.98%, and 12.80%, respectively. Compared with AAE-RRT* and DPF-Bi-RRT*, the average planning time is reduced by 84.48% and 54.44%, respectively. The path quality remains comparable, yet the search efficiency is markedly higher. Specifically, the sector-based sampling provides persistent directional guidance that reduces unnecessary detours in the vast search space. Meanwhile, the adaptive step-size mechanism enables faster expansion in open regions and finer obstacle avoidance in locally clustered areas.

5.5. Statistical Reliability Analysis

To further assess the run-to-run reliability of the experimental results, the coefficient of variation (CV) of path length across the 100 independent trials is summarized in

Table 3. Coefficient of variation (CV, %) of path length over 100 independent trials in the four environments.

Algorithm	Environment 1	Environment 2	Environment 3	Environment 4
GB-RRT*	2.16	5.79	9.60	5.74
Bi-RRT*	6.36	6.85	8.09	6.79
Bi-APF-RRT*	3.60	3.88	6.63	3.92
AAE-RRT*	1.94	1.99	2.40	2.73
DPF-Bi-RRT*	2.16	3.14	5.01	3.33
EAC-Bi-RRT*	2.16	1.55	2.67	1.59

. The CV is defined as the ratio of standard deviation to mean, with lower values indicating higher run-to-run consistency.

As shown in Table 3, EAC-Bi-RRT* maintains a path-length CV below 3% in all four environments while preserving a 100% success rate, indicating high run-to-run consistency across both densely cluttered and large-scale scenarios. It attains the lowest path-length CV in Environments 2 and 4. In contrast, GB-RRT* and Bi-RRT* exhibit path-length CV values exceeding 5% in the complex environments (Environments 3 and 4), reflecting larger run-to-run variability. The medians of the continuous metrics are close to the corresponding means reported in Table 2, suggesting approximately symmetric distributions without significant long-tail outliers.

5.6. Discussion

Figure 9 compares the average planning time, average path length, and average turning angle of all algorithms in the four environments. From the global statistics, the success rates of GB-RRT* in Environments 2–4 are only 73%, 6%, and 84%, respectively, while AAE-RRT* reaches only 40% in Environment 3. In contrast, EAC-Bi-RRT* maintains a success rate of 100% in all four environments. Compared with Bi-RRT* and Bi-APF-RRT*, the average path length of EAC-Bi-RRT* is reduced by 16.23% and 11.55%, respectively, across the four environments. In Environments 1, 2, and 4, the average planning time of EAC-Bi-RRT* is reduced by 80.98% and 57.49% relative to AAE-RRT* and DPF-Bi-RRT*, respectively. This difference reflects a clear efficiency advantage. In Environment 3, due to the special constraints imposed by the multi-layer enclosed structure, the proposed algorithm trades a higher time cost for a complete success rate and the best path quality, which indicates that its search strategy gives priority to reachability and path cost in strongly constrained scenes.

Figure 10 gives the boxplots of the key metrics of EAC-Bi-RRT* across the four environments. In Environment 1, all metrics show relatively compact boxplots. This suggests good repeatability across runs. In Environment 2, both planning time and path length show low median values with limited dispersion, suggesting that the search process remains stable in the regular corridor environment. In Environment 3, the coupled constraints of multi-layer floor slabs and narrow openings increase the uncertainty of the search path and lead to greater variation in planning time. Nevertheless, the distributions of path length and turning angle remain relatively concentrated, which indicates that the algorithm still maintains good robustness in strongly constrained enclosed spaces. In Environment 4, the boxes for path length and planning time remain compact, confirming stable global search performance in the large-scale unstructured environment.

6. Conclusions

UAV path planning in complex 3D obstacle environments requires not only fast generation of collision-free paths, but also satisfactory path smoothness and kinematic feasibility. To meet these requirements, this paper proposes the EAC-Bi-RRT* algorithm. Built on bidirectional RRT*, the proposed method introduces an environment-aware mechanism to regulate the search process. The result is improved adaptability in complex spaces. Experimental validation in four representative 3D obstacle environments shows that EAC-Bi-RRT* achieves strong stability and environmental adaptability in challenging scenes while maintaining both path quality and path smoothness.

Nevertheless, this study still leaves room for further improvement. The proposed method has several limitations that will be addressed in future research:

(1)

Embedded onboard deployment and computational profiling: The proposed method targets embedded onboard computing platforms rather than low-level flight-control microcontrollers. The current MATLAB R2025a prototype runs on a desktop CPU, and systematic profiling of onboard CPU, RAM, and real-time scheduling is left for future work.

(2)

Perception uncertainty and real-time replanning: The current simulations focus on static 3D environments and do not yet model sensing noise, localization errors, or dynamic obstacles. Future work will investigate uncertainty-aware environment representation and real-time replanning.

(3)

Real-world flight validation: All evaluations in this paper are conducted in MATLAB-based simulation, which cannot reproduce the perception, actuation, and disturbance characteristics of physical flight. Future work will integrate the planner with onboard perception, state estimation, and the flight-control loop, advancing from hardware-in-the-loop testing to outdoor physical-flight experiments.