An Innovative Path Planning Algorithm for Complex Obstacle Environments with Adaptive Obstacle Density Adjustment: AODA-PF-RRT*

Abstract

To address the limitations of low node utilization and inadequate adaptability in complex environments encountered by Rapidly-exploring Random Tree (RRT) algorithms during the expansion phase, this study presents an enhanced path planning algorithm—AODA-PF-RRT* (Adaptive Obstacle Density Adjustment-PF-RRT*). The proposed algorithm implements a random extension strategy for nodes that fail collision detection, thereby improving node efficiency. Furthermore, it dynamically partitions the area surrounding sampling points and calculates local obstacle density in real time. By leveraging this density information, the algorithm flexibly adjusts both the number of expansion points and the dichotomy threshold, significantly enhancing its responsiveness to environmental changes. We rigorously demonstrate the algorithm’s probabilistic completeness and asymptotic optimality. Simulation and benchmarking results demonstrate that the AODA-PF-RRT* algorithm not only generates smooth and high-quality paths compared to existing algorithms but also maintains low computational costs in complex environments, showcasing exceptional stability and robustness.

1. Introduction

The evolution of mobile robots spans various phases, from fundamental research to extensive real-world applications, encompassing critical areas such as sensor technology [1], path planning [2], and control theory [3]. With their growing integration into sectors like logistics, agriculture, and military operations [4,5], these advancing technologies are revolutionizing the way we live and work. In the future, mobile robots are expected to become even more intelligent, assuming pivotal roles across a broader range of fields.

Path planning is a core technology in autonomous navigation and task execution for mobile robots. It involves designing a collision-free and optimal path from a starting point to a destination in either known or unknown environments [6]. Essentially, path planning serves as the link between a mobile robot’s environmental perception and its motion control system, ensuring safe and efficient navigation through dynamic and complex settings. Classical path planning algorithms include graph search algorithms such as A* and Dijkstra’s algorithm [7], the artificial potential field method (APF) [8], the dynamic window approach [9], and sampling-based algorithms such as RRT [10] and Probabilistic Roadmap (PRM) [11].

While each of these algorithms offers distinct advantages and is suited for specific scenarios, they also present notable limitations. Sampling-based path planning methods, for instance, are highly effective in high-dimensional and complex environments, but they often suffer from issues like low path quality, an over-reliance on randomness, and inconsistent computational efficiency. The PRM algorithm, introduced by Kavraki in 1996, builds a roadmap by randomly sampling the configuration space, checking for collisions, and evaluating the connectivity of neighboring samples. However, PRM’s efficiency declines significantly when navigating through dense obstacle fields or narrow passages [12]. In 1998, LaValle introduced the RRT algorithm, which can be regarded as a single-query version of PRM. Unlike PRM, which uses a pre-generated graph for multiple path queries, RRT constructs a new search tree from scratch for each path planning task. Due to its speed and adaptability, RRT has been widely studied, leading to the development of various improved versions. To improve the efficiency of the RRT algorithm, LaValle et al. [13] proposed the RRT-Connect algorithm. This method builds two search trees—one at the start point and another at the goal point—alternately expanding them using a greedy heuristic until the two trees connect successfully. RRT-Connect has been shown to possess probabilistic completeness. Inspired by the concept of bidirectional expansion, the Quad-RRT algorithm [14] constructs four RRT trees in the target map to enhance response speed, making it more suitable for large-scale maps with dense obstacles.

However, neither the RRT nor the RRT-Connect algorithm focus on optimizing path length. To address this limitation, Karaman et al. [15] introduced the RRT* algorithm, which incorporates a path rewiring mechanism to gradually improve path quality through local optimization operations, namely ChooseParent and Rewire. As the number of iterations increases, the path generated via RRT* increasingly approaches the optimal path. However, multiple iterations lead to higher computational costs and slower convergence for the RRT* algorithm. Therefore, some improvements focus on enhancing sampling efficiency to reduce path cost. Wu et al. [16] introduced a rejection sampling strategy, which samples only in the unexplored space of the random tree to avoid ineffective growth. The Informed-RRT* algorithm [17] represents the sampling subset with an ellipsoid, focusing the search by directly sampling within the ellipsoid subset. However, when the associated hypersphere is larger than the planning space itself, this method cannot effectively focus the search. Ding et al. [18] extended the initial path obtained using the RRT algorithm and then performed heuristic iterative sampling within the extended area to improve algorithm efficiency in complex environments such as narrow corridors and mazes. However, this algorithm relies heavily on the quality of the initial solution.

To ensure rapid convergence, some studies have optimized the rewiring structure of the RRT* algorithm. Kang et al. [19] combined the RRT-Connect algorithm with the triangle inequality principle to search for collision-free paths connecting new nodes with their ancestor nodes, effectively reducing path length. Li et al. [20] proposed the Fast-RRT* algorithm based on a backtracking approach, which eliminates the consideration of search radius and directly searches for potential parent nodes of new nodes after the start point. The Quick-RRT* algorithm [21] shares a similar structural optimization with Fast-RRT*, allowing for customization of the backtracking depth of ancestor nodes and reducing path costs from ancestor nodes to the surrounding nodes of the new node during the rewiring process. These algorithms primarily search for target nodes among existing nodes and are significantly affected by obstacles and existing paths. To address this issue, F-RRT* [22] and FF-RRT* [23] optimize path cost by creating parent nodes for random points using a dichotomy method rather than filtering among existing ancestor vertices. This creation process is divided into two steps, FindReachest and CreateNode, which are computationally efficient and generate paths with lengths closer to obstacles than Q-RRT*.

One reason for the slow convergence speed of RRT* is its purely exploratory nature, which leads to sampling that lacks heuristics, resulting in the generation of redundant nodes. Heuristic RRT* algorithms have emerged to address this issue by adjusting the method of sampling point generation to reduce path unpredictability and cost. Based on this, Liu et al. [24], Wu et al. [25], and Feng et al. [26] used the attractive force concept of APF to guide the sampling point expansion direction in RRT* and IRRT* algorithms, enhancing the obstacle avoidance capability and environmental adaptability of the algorithms. Fan [27] proposed the Bidirectional Artificial Potential Fields-Rapidly-exploring Random Trees Star (Bi-APF-RRT*) algorithm, which simultaneously expands two search trees to further improve the convergence speed of the algorithm. Another approach to enhancing heuristic RRT* algorithms is optimizing their rewiring structure. Li et al. [28] introduced a new algorithm: PQ-RRT*, which combines the advantages of Artificial Potential Fields-Rapidly-exploring Random Trees Star (APF-RRT*) and Quick-RRT* to ensure rapid convergence to the optimal solution and generate better initial solutions, with theoretical proofs of the algorithm’s completeness, asymptotic optimality, and faster convergence. Similarly, Fan et al. [29] proposed the PF-RRT* algorithm, which, after generating new nodes guided via APF, creates a parent node closest to the obstacle for the new node using a dichotomy method, significantly reducing computational cost while inheriting the smooth path advantages of APF-RRT*.

Recent advancements in the RRT algorithm primarily focus on improvements in sampling strategies, heuristic guidance, and structural optimization, often neglecting the algorithm’s adaptability to different environments and the effective reuse of failed sample points. To address these issues, we propose an optimal RRT algorithm based on adaptive obstacle density adjustment (AODA-PF-RRT*). The key contributions of this work are as follows:

• New node generation phase: the algorithm introduces a dual repulsive field APF model to guide the direction of new nodes in the random tree. Combined with a variable step-size strategy, this approach significantly enhances the success rate of path planning in complex maze environments;
• Random tree expansion phase: a novel random expansion strategy for sampling points is proposed, which attempts to expand nodes that fail collision detection into their surroundings. Simultaneously, the surrounding map is continuously rasterized to evaluate obstacle coverage, with real-time updates to the obstacle density distribution. The number of random expansion points is dynamically adjusted based on obstacle density, improving both the connection success rate and node utilization;
• Random tree reconnection phase: the algorithm dynamically adjusts the termination threshold of the dichotomy based on obstacle density. This adjustment allows the algorithm to allocate computational resources more effectively between open and congested areas, further enhancing its adaptability to diverse environments.

The structure of this paper is as follows: Section 2 defines the problems and provides an overview of the RRT*, APF-RRT*, Q-RRT*, and F-RRT* algorithms. In Section 3, the AODA-PF-RRT* algorithm is described in detail. Section 4 includes the algorithm flowchart and demonstrates its probabilistic completeness and asymptotic optimality. Comparative simulation results of the initial solution and convergence rate comparison between the proposed method and existing algorithms are discussed in Section 5. Finally, Section 6 highlights the key advantages of AODA-PF-RRT* and suggests potential directions for future research.

2. Background

In this section, the problems to be addressed in path planning will be defined, followed by an introduction to the RRT*, APF-RRT*, Q-RRT*, and F-RRT* algorithms. These algorithms serve as the foundation for the optimizations and improvements presented in this paper.

2.1. Theorem Definition

In the context of path planning, this paper addresses three core challenges. Consider a configuration space $\mathcal{P}\subseteq {ℝ}^d$, where $d\in N$ and $d\ge 2$. Let ${\mathcal{P}}_{obs} \subset \mathcal{P}$ represent the obstacle region, while the obstacle-free space is defined as ${\mathcal{P}}_{free} =\mathcal{P}/{\mathcal{P}}_{obs}$. The starting and goal points, located in the obstacle-free space, are denoted as ${\mathcal{P}}_{start}$ and ${\mathcal{P}}_{goal}$, respectively. A path $\sigma :[0,1]\to \mathcal{P}$ is defined as a continuous function, and if $\sigma (\tau )\in {\mathcal{P}}_{free}$ for all $\tau \in [0,1]$, the path is deemed collision-free.

Theorem 1.

(Feasible Path Planning). Feasible path planning involves determining a collision-free path $\sigma$ that leads from the initial position $\sigma (0)={\mathcal{P}}_{init}$ to the goal region $\sigma (1)\in {\mathcal{P}}_{goal}$, while remaining collision-free throughout its path.

Theorem 2.

(Optimal Path Planning). Let $\mathcal{L}$ represent the set of all paths and ${\mathcal{L}}_{feas}$ denote the subset of feasible paths. The path length cost function $c(\sigma )$ is defined as the cumulative Euclidean distance between consecutive points on the path:

$$c(\sigma )={\displaystyle \sum _{i=1}^{n-1}\parallel \sigma ({\tau }_{i+1})-\sigma ({\tau }_i )\parallel }$$

(1)

where  $\left\{\sigma ({\tau }_1),\sigma ({\tau }_2),\cdot \cdot \cdot ,\sigma ({\tau }_n)\right\}$   are discrete points along the path, and  $n$   represents the number of nodes. The objective of the optimal path planning is to find a feasible path  ${\sigma }^{\ast }$   given a path planning problem  $\left\{{\mathcal{P}}_{init},{\mathcal{P}}_{obs},{\mathcal{P}}_{goal}\right\}$   and a cost function  $c:\mathcal{L}\to ℝ\ge 0$, such that 

$${\sigma }^{\ast }=\left\{min c(\sigma )\left|\sigma \in {\mathcal{L}}_{feas}\right.\right\}$$

(2)

Theorem 3.

(Rapid Path Planning). Let $T^{ALG}$ denote the total time required for planning. Rapid path planning aims to identify an optimal path ${\sigma }^{\ast }$ within the shortest possible time, ensuring that the path cost is minimized while adhering to the time constraint  $T^{ALG}$.

2.2. RRT*

The RRT algorithm, introduced by LaValle in 1998, is an efficient method for solving path planning problems in high-dimensional spaces (Figure 1). The core idea of RRT is to explore the space through random sampling, expanding a tree structure to efficiently cover the search space and generate feasible paths. While the RRT algorithm can eventually find a feasible path, its paths are often suboptimal. To overcome this, RRT* was developed, keeping RRT’s fast exploration while adding path optimization to approach the optimal solution. The pseudocode for RRT* is shown in Algorithm 1.

Compared to the RRT algorithm, the RRT* algorithm further improves path quality by introducing two key optimization processes: ChooseParent (Algorithm 2) and Rewire (Algorithm 3). As illustrated in Figure 2, the ChooseParent process is responsible for reselecting the optimal parent node for the newly generated node $x_{new}$. During the Rewire process, if the path cost of any neighboring node $x_{near}$ as a child of $x_{new}$ is lower, the parent of $x_{near}$ is updated to $x_{new}$. The combined use of Algorithms 2 and 3 allows RRT* to eventually converge to the optimal path in a complex search space.

The following are explanations of key functions used in the pseudocode:

RandomSample: generates a random sample point $x_{rand}$ within the obstacle-free $X_{free}$;

NearestNeighbor: identifies the node $x_{nearest}$ in the tree $T$ that is closest to the random sample node;

Steer: guides the generation of a new node $x_{new}$ from the given node $x_{nearest}$ toward the random sample point $x_{rand}$;

CollisionFree: verifies that the path $\sigma$ between two nodes lies entirely within the obstacle-free space;

Near: returns the set of nodes $X_{near}$ within a specified radius $R_{near}$ around a given point in the tree T;

Cost: computes the cumulative path length (Euclidean distance) from the start node $x_{start}$ to the given node;

Distance: calculates the Euclidean distance between two specified points;

InitialPathFound: determines whether a feasible path from $x_{start}$ to $x_{goal}$ has been found in T If a path is found, the algorithm terminates early and returns the current tree.

Algorithm 1: RRT*

Algorithm 2: ChooseParent

Algorithm 3: Rewire

2.3. APF-RRT*

The artificial potential field (APF) algorithm models the goal as an “attractive source” that pulls the robot towards it, while obstacles are “repulsive sources” that push the robot away. The robot’s movement is based on the combined forces from these sources. Although APF creates smooth paths, it often gets stuck in local minima. In contrast, RRT* is purely exploratory and generates more winding paths. The APF-RRT* algorithm combines both methods, using RRT*‘s random sampling to avoid APF’s local minima while the potential field guides the tree’s growth for smoother paths. Algorithm 4 shows an improved version of APF-RRT* that enhances pathfinding in complex environments.

Algorithm 5 outlines the detailed steps of RGD. It traverses the obstacle set $X_{obs}$ to find the one closest to the current node $x_{nearest}$, with the corresponding Euclidean distance denoted as $d_{min}$. If $d_{min}$ is less than the obstacle’s effective influence threshold $d_{obs\ast }$, the node $x_{nearest}$ will experience a repulsive force $F_{attg}$ from this obstacle. Combining the attractive forces $F_{attg}$ from the goal point $x_{goal}$ and $F_{attr}$ from the random sample point $x_{rand}$, the resultant force $F_{total}$ is computed using the parallelogram rule. This resultant force determines the growth direction of the random tree, and the step size $\lambda$ dictates the growth distance.

The following subroutines are associated with the RGD process:

MinDistance: calculates the Euclidean distance from the current node $x_{nearest}$ to the nearest obstacle;

APG: computes the potential field gradient at a given point, indicating the magnitude and direction of the force, i.e., the rate and direction of change in the potential field in space.

Algorithm 4: APF-RRT*

Algorithm 5: RGD

2.4. Q-RRT*

Q-RRT* builds on the RRT* algorithm by introducing the concepts of backtracking depth and ancestor nodes. By expanding the search range of potential ancestor nodes and optimizing node reconnections, Q-RRT* can obtain a better initial solution with the same number of sampling points. As shown in lines 7–9 of Algorithm 6, during the re-selecting of the parent node for a new node $x_{new}$, Q-RRT* evaluates not only the neighboring nodes $X_{near}$ but also a set of ancestor nodes at depth $n_{ancestor}$, denoted as $X_{parent}$. By broadening the search range, the number of potential parent nodes for $x_{new}$ is increased, the algorithm increases the likelihood of identifying the optimal parent node. The expansion process is illustrated in Figure 3a. Additionally, due to the structure of the random tree, neighboring nodes typically share common ancestor nodes, so backtracking in the random tree does not significantly increase the computational load.

Algorithm 6: Q-RRT*

In the process of rewiring, Q-RRT* not only performs the simple reconnection process of the RRT* algorithm but also further considers the influence of ancestor nodes. Specifically, for each neighboring node $x_{near}$, Q-RRT* extends its reconnection targets from just the single node $x_{new}$ to the set of ancestor nodes $X_{Ancestry}$ of $x_{new}$, as shown in Figure 3b, to achieve a lower path cost. Algorithm 7 provides a detailed explanation of the reconnection process in Q-RRT*.

Algorithm 7: Rewire-RRT*

The functions in Algorithms 6 and 7 are defined as follows:

AncestryGroup: given a random tree $T$, a node $x_p$, and a search depth $n_{ancestor}$, this function returns the set of all ancestor nodes of $x_p$ from level 1 to $n_{ancestor}$ in the random tree $T$. If a set of nodes $S$ is given, it returns the ancestors for each node within that set $S$, denoted as ${\displaystyle \sum _{x_p\in S}{X}_{Ancestry}}$;

Ancestry: given a random tree $T$, a node $x_p$, and a search depth $n_{ancestor}$, this function retrieves the ancestor node at specified depth $n_{ancestor}$ for the given node $x_p$ in $T$.

2.5. F-RRT*

The Q-RRT* algorithm has two inherent drawbacks during convergence. First, increasing the backtracking depth and the reconnect search radius $R_{near}$ expands the search range but also increases the time cost. Second, although the Rewire-RRT* process reduces the path length, there remains a gap between the path cost and the optimal solution with a limited number of nodes. To address these issues, the F-RRT* algorithm improves upon Q-RRT* by introducing the FindReachest and CreateNode processes. Detailed steps of F-RRT* are outlined in Algorithm 8.

The purpose of the FindReachest process is to identify the farthest ancestor node $x_{reachest}$ among the ancestors of $x_{nearest}$ that can connect to $x_{rand}$ without collisions. Algorithm 9 provides a detailed description of its pseudocode. As shown in Figure 4a, unlike Q-RRT*, which uses the radius parameter $R_{near}$ to search around $x_{new}$, F-RRT* backtracks within the ancestor nodes to find the farthest reachable node $x_{reachest}$ that can connect to $x_{new}$ collision-free. This approach reduces the number of nodes that need to be searched and enhances the overall efficiency of path expansion.

Algorithm 8: F-RRT*

Algorithm 9: FindReachest

In the CreateNode process (Algorithm 10), F-RRT* creates a new node $x_{create}$ by using a binary search method to establish a connection between $x_{new}$ and $x_{reachest}$ without collisions. A key parameter in this process is the threshold $D_{dichotomy}$, which controls how close the generated node is to obstacles. By creating nodes $x_{create}$ closer to obstacles, F-RRT* effectively reduces the initial path cost. The algorithm also adopts the reconnection strategy from Q-RRT*, with a notable difference shown in Figure 4b. If the node $x_{create}$ is successfully created during the CreateNode process, the potential parent nodes in the neighboring set will include this newly generated node, which allows for further path optimization.

Algorithm 10: CreateNode

3. AODA-PF-RRT*

The AODA-PF-RRT* algorithm builds upon the APF-RRT* and F-RRT* algorithms, introducing a novel path planning approach that incorporates an adaptive obstacle density strategy. The key improvement is the enhancement of the APF algorithm and dynamically adjusting both the number of sampling nodes and the dichotomy threshold in response to real-time environmental conditions. This dynamic adjustment enhances the algorithm’s flexibility and adaptability, enabling the efficient generation of smooth and high-quality paths across a wide range of environments. Algorithm 11 outlines the pseudocode for AODA-PF-RRT*, and this section provides a detailed explanation of the algorithm’s underlying principles.

Algorithm 11: AODA-PF-RRT*

3.1. Improve APF

The improved APF algorithm is designed to address the issue of local inaccessibility inherent in the traditional APF model. In the conventional APF model, the mobile robot is influenced by both the attractive force from the goal point and the repulsive forces from obstacles. As the robot nears the goal, the attractive force gradually decreases to zero, while the repulsive forces remain, causing potential oscillation or stalling near the goal. Additionally, if the combined repulsive forces from obstacles counterbalance the attractive force of the goal point, the robot may fail to reach its destination, hindering the path planning process.

To overcome the issue of local inaccessibility, a dual repulsive field is introduced, as detailed in Algorithm 12 and illustrated in Figure 5. This enhanced repulsive field consists of two components: the first part is the traditional repulsive force exerted from the obstacle towards the robot, determined by the distance between the robot, the obstacle, and the target point (represented by the blue arrow $F_{req1}$). The second component is a repulsive force directed from the robot towards the target point, based on the same factors (represented by the purple arrow $F_{req2}$). These two repulsive forces combine to form the total repulsive force $F_{req}$ (yellow arrow), which is then superimposed with the total attractive force $F_{att}$ (green arrow) to calculate the resultant force $F_{total}$ (red arrow) guiding the robot’s movement.

Algorithm 12: Improved-RGD

The expressions for the improved attractive and repulsive potential fields are as follows:

$$U_{attg}(x_i)={\displaystyle \frac{1}{2}}{\zeta }_a{d}^2\left(x_i,x_{goal}\right)$$

(3)

$$U_{attr}(x_i)={\displaystyle \frac{1}{2}}{\zeta }_a{d}^2\left(x_i,x_{rand}\right)$$

(4)

$$U_{req}(x_i)=\left\{\begin{array}{ll}{\displaystyle \frac{1}{2}}{\zeta }_r{\left({\displaystyle \frac{1}{d\left(x_i,x_{obs}\right)}}-{\displaystyle \frac{1}{d_{obs*}}}\right)}^2{d}^n\left(x_i,x_{goal}\right),\hfill & d\left(x_i,x_{obs}\right)\le d_{obs*} \hfill \\ 0,\hfill & d\left(x_i,x_{obs}\right)\ge d_{obs*}\hfill \end{array}\right.$$

(5)

where $x_i$, $x_{goal}$, $x_{rand}$, $x_{obs}$ represent the current position of the mobile robot, the goal point, the random sampling point, and the center of the nearest obstacle, respectively. The term $d\left(x_i,x_j\right)$ is a vector representing the Euclidean distance $\left|x_i-x_j\right|$ between the robot’s position $x_i$ and a given point $x_j$, with the vector direction pointing from $x_i$ to $x_j$. $d_{obs*}$ denotes the maximum safe distance from the obstacle; ${\zeta }_a$ and ${\zeta }_r$ are the gain coefficients for the attractive and repulsive potential fields, respectively, while the parameter $n$ adjust the intensity of the repulsive field, ensuring smoothness in obstacle avoidance and maintaining the path’s flow.

The magnitude and direction of the forces exerted acting on the mobile robot are determined by the negative gradient of the potential field functions. The attractive force $F_{att}$ experienced by the mobile robot is described by the following equations:

$$F_{att}(x_i)=F_{attg}+F_{attr}$$

(6)

$$F_{attg}(x_i)=-\nabla U_{attg} (x_i )={\zeta }_{a}d\left(x_i,x_{goal}\right)$$

(7)

$$F_{attr}(x_i)=-\nabla U_{attr} (x_i )={\zeta }_{a}d\left(x_i,x_{rand}\right)$$

(8)

The repulsive force $F_{rep}$ is given as follows:

$$F_{rep}(x_i)=-\nabla U_{rep} (x_i )=F_{rep1}+F_{rep2}$$

(9)

$$F_{rep1}(x_i)={\zeta }_r\left({\displaystyle \frac{1}{d(x_i,x_{obs})}}-{\displaystyle \frac{1}{d_{obs*}}}\right){\displaystyle \frac{d^n(x_i,x_{goal})}{d^2(x_i,x_{obs})}}\nabla d(x_i,x_{obs})$$

(10)

$$F_{rep2}(x_i)={\displaystyle \frac{n}{2}}{\zeta }_r\left({\displaystyle \frac{1}{d(x_i,x_{obs})}}-{\displaystyle \frac{1}{d_{obs*}}}\right)d^{n-1}(x_i,x_{goal})\nabla d(x_{goal},x_i)$$

(11)

where $\nabla d\left(x_i,x_{obs}\right)$ and $\nabla d(x_{goal},x_i)$ represent the unit vectors from $x_{obs}$ to $x_i$ and from $x_i$ to $x_{goal}$, respectively. $F_{req}$ denotes the total repulsive force acting on the robot. When the robot reaches the goal, $F_{rep2}$ will decrease to zero to ensure that the robot arrives safely. The total force $F_{total}$ is determined by combining the total attractive force $F_{att}$ and the total repulsive force $F_{req}$ using the parallelogram rule. The direction of $F_{total}$ determines the growth direction of $x_{new}$, with the step size $\lambda$ being determined by the magnitude of the vector $F_{total}$ and $k_s$ being a scaling factor.

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

(12)

$$\lambda = {\displaystyle \frac{\left|F_{total}\right|}{k_s}}$$

(13)

The improved APF algorithm introduces a dual repulsive field model, which not only retains the attractive and repulsive forces from the traditional algorithm but also adds a repulsive force from obstacles towards the goal point. This addition helps avoid oscillations or stagnation of the robot near the goal point. Furthermore, the algorithm incorporates guidance from random sampling points $x_{rand}$ and employs a variable step size strategy (as shown in Equation (13)), enhancing the success rate of path planning in complex environments. These improvements provide the robot with greater flexibility and adaptability in multi-obstacle environments, significantly increasing the stability and robustness of the algorithm.

3.2. Dynamic Adjustment Strategy

In complex environments, the uneven distribution of obstacles requires the algorithm to adapt to varying obstacle densities. In regions with a high density of obstacles, fine path planning is needed, while in open areas, computational resources should be conserved without compromising the collision-free nature of the path. To achieve this, an evaluation method is introduced to assess the obstacle density around the current node, enabling dynamic adjustments to algorithm parameters. This allows the robot to navigate through narrow spaces with precision and traverse open areas more quickly. This section details the dynamic adjustment strategy, which is a key innovation of the AODA-PF-RRT* algorithm.

AODA-PF-RRT* algorithm’s dynamic adjustment strategy is based on two key considerations:

• Node connection success rate: in traditional RRT*, connecting sample points in crowded areas leads to a high probability of collision, resulting in multiple attempts and low node utilization. AODA-PF-RRT* addresses this by introducing a dynamic random expansion strategy. When a new node collides with an obstacle, instead of discarding the node, the algorithm attempts to expand it in the surrounding area. The number of expansion nodes is dynamically adjusted based on the local obstacle density. This enhances the flexibility and effectiveness of path exploration, improving the success rate of node connections;
• Path optimization efficiency: in F-RRT*, the CreateNode process splits edges using a dichotomy method, with a termination threshold for each split. In open areas, a larger threshold reduces the number of splits, while in densely cluttered areas, a smaller threshold ensures precise splitting to avoid obstacles. AODA-PF-RRT* dynamically adjusts this threshold based on the obstacle density, ensuring optimal path segmentation and improving algorithm efficiency across different environments.

3.2.1. Map Discretization

To facilitate more accurate path planning and obstacle avoidance, the continuous 2D plane is discretized into a grid-based map. This approach simplifies computation and analysis, particularly when dealing with irregular obstacle shapes. The specific steps for discretization are shown in Algorithm 13: before discretization, define $N_{mesh}(M, N)$ as the number of grids and $S_{searchsize}\left(L,W\right)$ as the grid size. The center of the search area is the node $x_{new}(x,y)$. The entire search area is divided into $M\times N$ discrete grids starting from $(x_{\min },y_{\min })$, with each grid having an area $L\times W$, specifically represented as follows:

$$\left\{\begin{array}{l}{x}_{\min }=x-{\displaystyle \frac{M\times L}{2}}\\ y_{\min }=y-{\displaystyle \frac{N\times W}{2}}\end{array}\right.$$

(14)

In Algorithm 13, the function GetMeshCenter returns the center coordinates of a grid cell located at column $i$ and row $j$ on the map $M$. This can be described by the following expression:

$$\mathcal{M}=\{(x_i ,y_j) \mid x_i=x_{\min }+(i+{\displaystyle \frac{1}{2}})\cdot L,y_j=y_{min}+(j+{\displaystyle \frac{1}{2}})\cdot W ,i\in [1,N],j\in [1,M]\}$$

(15)

Algorithm 13: DiscreteMap

3.2.2. Determining Grid Coverage

To assess the obstacle density in the discretized map, each grid cell is checked to determine if it is covered by an obstacle. Let $\mathcal{O}[i,j]$ be a binary indicator showing whether the grid cell $(x_i,y_j)$ is covered. The criterion for determining is as follows:

$$\mathcal{O}[i,j]=\left\{\begin{array}{ll}1,\hfill & if (x_i,y_j) is covered by obstacle\hfill \\ 0,& otherwise\hfill \end{array}\right.$$

(16)

This determination process involves checking each grid cell’s center coordinates and verifying if these coordinates intersect with or are contained within any obstacle boundary. The total obstacle-covered area is obtained by summing the area of all covered grid cells:

$$S_{obs}={\displaystyle \sum _{i=1}^M{\displaystyle \sum _{j=1}^N\mathcal{O}[i,j]\cdot L\times W}}$$

(17)

The obstacle density ${\rho }_{obs}$ is then calculated by dividing the total obstacle-covered area $S_{obs}$ by the area of the search region $S_{\mathcal{M}}$:

$${\rho }_{obs}={\displaystyle \frac{S_{obs}}{S_{\mathcal{M}}}}={\displaystyle \frac{{\displaystyle \sum _{i=1}^M{\displaystyle \sum _{j=1}^N\mathcal{O}[i,j]}}}{M\times N}}$$

(18)

3.2.3. Random Sampling Expansion Strategy

During the path planning process, the node $x_{nearest}$ in the tree generates a new node $x_{new}$ under the guidance of the Improved-APF method. Since the expansion direction and step size of $x_{new}$ are determined by the resultant potential force on $x_{nearest}$, which varies with the node’s position, there is a possibility of a collision between the edge $\sigma (x_{nearest},x_{new})$ and the obstacle $X_{obs}$. In traditional RRT*, the iteration terminates when a collision occurs, with no nodes or edges added to the tree. However, in AODA-PF-RRT*, if an edge collides with an obstacle, the algorithm uses a random sampling expansion strategy to explore new directions. Instead of discarding the node, random expansion points are generated around the current node, increasing the chances of finding a collision-free path.

Assume $x_{current}$’s coordinates are $(x_c,y_c)$, then coordinates of the $i$-th random expansion point $x_{sample,i}$ can be obtained by random sampling near the $x_{current}$. The calculation for the random expansion point is as follows:

$$x_{sample,i}=x_{current}+\Delta \tau$$

(19)

where $N_0$ is the number of random expansion points, and $\Delta \tau (\Delta x,\Delta y)$ represents the random increments of the expansion point coordinates within a specified range. Let $rand$ denote a uniformly distributed random number between $\left[0,1\right]$, and $\Delta \tau$ can be expressed as follows:

$$\left\{\begin{array}{l}\Delta x=\lambda \cdot \cos \left(2\pi \cdot rand\right)\\ \Delta y=\lambda \cdot \sin \left(2\pi \cdot rand\right)\end{array}\right.$$

(20)

The coordinates $(x_s,y_s)$ of the $i$-th random expansion point $x_{sample,i}$ are as follows:

$$\left\{\begin{array}{l}{x}_s=x_c+\Delta x\\ y_s=y_c+\Delta y\end{array}\right.$$

(21)

As illustrated in Figure 6, after obtaining the random expansion point coordinates, node $x_{nearest}$ attempts to connect to the expansion points, and the first expansion point that passes collision detection is updated as the new node $x_{new}$. This random expansion strategy allows further exploration of the surrounding space after detecting a collision, thereby increasing the probability of finding a collision-free path and improving the efficiency and robustness of the path planning.

3.2.4. Dynamic Adjustment of Sample Point Quantity

In the random expansion strategy, the number of expansion sample points $N_0$ is fixed. Considering that increasing the number of sample points in narrow environments can significantly improve the connection success rate, a dynamic adjustment strategy is introduced to improve the random expansion of sample points. This strategy dynamically increases the number of sample points in narrow environments and reduces it in open environments based on real-time obstacle density. The implementation method is shown in Algorithm 14, and the relationship between the number of sample points and obstacle density ${\rho }_{obs}$ is defined as follows:

$$n_{samples}=round(N_0\cdot exp(k_n\cdot {\rho }_{obs}))$$

(22)

where $k_s$ is the adjustment parameter, indicating the number of sample points added per unit of obstacle density, and the $round$ function is used to round the calculated value to determine the final number of sample points.

3.3. Dynamic Adjustment of Dichotomy Threshold

In the F-RRT* algorithm, as shown in Algorithm 10, after creating the node $x_{create}$ using the dichotomy method, $x_{create}$ is used to connect the parent nodes of $x_{new}$ with $x_{reachest}$. Therefore, $D_{dichotomy}$, as the stopping threshold of the dichotomy method, affects how closely $x_{create}$ approaches obstacles. The AODA-PF-RRT* algorithm introduces a dynamic adjustment strategy that adjusts the value of $D_{dichotomy}$ based on the analysis of obstacle density ${\rho }_{obs}$. Define the initial value of $D_{dichotomy}$ as $D_0$, and the relationship between $D_{dichotomy}$ and obstacle density ${\rho }_{obs}$ is as follows:

$$D_{dichotomy}=D_0\cdot exp(-k_d\cdot {\rho }_{obs})$$

(23)

where $k_d$ is an adjustment parameter that controls the sensitivity of $D_{dichotomy}$ to changes in obstacle density. This ensures that the node placement is optimal in both dense and open environments, improving the overall path planning efficiency.

Algorithm 14: RandomExtend

4. Analysis

The algorithm flow of this paper is illustrated in Figure 7. Initially, the random tree and environmental data (starting point and obstacles) are initialized. $Iter$ represents the maximum number of iterations, and the algorithm will terminate either when $Iter$ is reached or an initial path is found. If the current iteration number $i<Iter$, a random sample $x_{rand}$ is obtained, and the nearest neighbor $x_{nearest}$ is identified within the random tree. A new node $x_{new}$ is generated with guidance from the goal point $x_{goal}$, $x_{rand}$, and the resultant force from obstacles acting on $x_{nearest}$. The direction and step size depend on this resultant force. Subsequently, the map around $x_{new}$ is discretized, and the local obstacle density ${\rho }_{obs}$ is calculated. If $x_{new}$ passes collision detection, the neighboring node set $X_{near}$ is searched; otherwise, randomly expand $x_{new}$ taking the first point that passes collision detection as the new $x_{new}$. If all expansion points fail, terminate the current iteration. Then, trace back from $x_{new}$ to the starting point to find the farthest reachable point $x_{reachest}$, adjust the dichotomy threshold $D_{dichotomy}$ based on ${\rho }_{obs}$, create a new node $x_{create}$, and add the corresponding edges based on collision detection results. Finally, reconnect the existing random tree edges and complete the iteration. If InitialPathFound $InitialPathFound$ indicates that an initial path has been found, output the path and terminate; otherwise, continue to the next iteration.

4.1. Probability Completeness

A path planning algorithm is considered probabilistically complete if, given a feasible path exists in the space, the probability that the algorithm will find such a path within a finite amount of time approaches 1. Both RRT and its variant RRT* have been proven to be probabilistically complete [16,30,31]. Theorem 4 demonstrates the probability completeness of AODA-PF-RRT*.

Theorem 4.

If there exists a collision-free path $\sigma$ from $x_{start}$ to $x_{goal}$, then as the number of iterations  $n\to \infty$, the probability that the sample points in AODA-PF-RRT* will be in  $\sigma$ approaches 1. Specifically,

$$\underset{n\to \infty }{\lim }P\left(\exists x_i\in {\mathcal{T}}_n,x_i\in \sigma \ast \right)=1$$

(24)

where  ${\mathcal{T}}_n$  represents the set of nodes generated AODA-PF-RRT* after   $n$  iterations.

Proof of Theorem 4.

In AODA-PF-RRT*, the random sampling expansion strategy is designed to enhance sampling quality without restricting any region of $X_{free}$ due to the presence of obstacles. Therefore, every area in $X_{free}$ has the potential to be sampled. As the number of iterations $n$ increases, the sample set ${\mathcal{T}}_n$ progressively covers the entire $X_{free}$, ensuring that all feasible paths are eventually explored. For each new sample point $x_{new}$, AODA-PF-RRT* attempts to connect it to the existing nodes in the tree, maintaining path connectivity. Therefore, the AODA-PF-RRT* algorithm is probabilistically complete. □

4.2. Asymptotic Optimality

The RRT algorithm does not possess asymptotic optimality, whereas RRT*, a variant of RRT, is asymptotically optimal [16,32]. Theorem 5 shows that AODA-PF-RRT* inherits this asymptotic optimality.

Theorem 5.

The AODA-PF-RRT* algorithm is asymptotically optimal, meaning that as the number of iterations approaches infinity, the path length found by the algorithm converges to that of the optimal path. by the algorithm converges to the length of the optimal path. Mathematically:

$$\underset{n\to \infty }{\lim }P(c_{n }\le (1+\upsilon )\cdot c^{\ast })=1\hspace{1em}\hspace{1em}\forall \upsilon >0$$

(25)

where $c\left(\sigma \right)$ denotes the cost of path $\sigma$, $c^{\ast }$ is the cost of the optimal path ${\sigma }^{\ast }$, and $c_n$ represents the cost of the best path found via AODA-PF-RRT* at iteration $n$.

Proof of Theorem 5.

The primary distinction between AODA-PF-RRT* and RRT* lies in the sampling and connection strategies. Theorem 1 has already demonstrated that the sample set ${\mathcal{T}}_n$ of AODA-PF-RRT* is uniformly distributed over $X_{free}$. The CreateNode and Rewire strategies in AODA-PF-RRT* ensure that each iteration has the potential to discover a feasible path with a reduced cost. As the number of iterations $n\to \infty$, the path $\sigma$ in the random tree increasingly approximates the optimal path ${\sigma }^{\ast }$. Thus, with infinite iterations, the path length found via AODA-PF-RRT* almost surely converges to the length of the optimal path. □

5. Simulation Results and Analysis

In this paper, the AODA-PF-RRT* algorithm is compared to the RRT*, APF-RRT*, Q-RRT*, and F-RRT* algorithms through simulations in different environments, each with a size of 50 × 50. The simulations are conducted in two stages: the first evaluates the quality of the initial paths generated by each algorithm, while the second assesses the convergence speed toward the asymptotically optimal solution. During the first stage, three performance metrics are used for comparison: (1) Time (time taken to find the initial solution), (2) Pathlength (cost of the initial solution, i.e., path length), and (3) Iteration (the number of iterations required to find the initial solution). The algorithm terminates either when the initial solution is found or when the maximum iteration limit is reached. In the second stage, the metric $T_{5\%}^{ALG}$ is used to compare the speed of convergence to a suboptimal solution, defined as a solution that is 1.05 times the optimal cost $C_{optimal}$. All simulations were performed on an Intel i5-7300 processor with 16 GB of RAM. The algorithms were executed on Python 3.12 and the equipment was sourced from Shenzhou, located in Shenzhen, China.

5.1. Parameter Selection

The search radius (Radius) during algorithm reconnection is a key parameter. A larger radius increases the number of potential nodes, reducing the path cost. However, an excessive number of potential nodes increases the computational burden of collision detection, resulting in higher time costs. Similarly, the backtracking depth $n_{ancestor}$ in Q-RRT* and $D_{dichotomy}$ in F-RRT* also impacts the path length and time costs. Before conducting the formal comparisons, parameter tuning experiments were carried out for each algorithm.

The tuning experiments reveal that the performance of each algorithm varies as the Radius changes between 3 and 10, as illustrated in Figure 8. The curves represent the average values from 50 simulations. As shown in Figure 8a, Iteration is not significantly affected by changes in Radius. Figure 8b,c indicate that as Radius increases, Time increases while Pathlength decreases, which aligns with predictions. Specifically, when $9>Radius>7$, the Pathlength for all algorithms stabilizes, while $Time$ tends to increase. Furthermore, at $Radius=7$, the $Time$ differences between algorithms are minimal. Therefore, $Radius=7$ was selected for further comparative analysis.

At $Radius=7$, when the backtracking depth $n_{ancestor}=2$, the $Time$ for the Q-RRT* is 18% lower compared to $n_{ancestor}=3$ and is almost the same as when $n_{ancestor}=1$. Additionally, the $PathLength$ for $n_{ancestor}=2$ is 0.31 and 0.07 less than for $n_{ancestor}=1$ and $n_{ancestor}=3$, respectively. Thus, the backtracking depth $n_{ancestor}$ for Q-RRT* was set to 2. For F-RRT*, at $Radius=7$, regardless of whether $D_{dichotomy}=1,2 or 3$, the $Time$ is around 1.1 seconds, but the Pathlength is lowest at 61.69 when $n_{ancestor}=2$. Therefore, F-RRT* with $n_{ancestor}=2$ was selected for further analysis. In the AODA-PF-RRT* algorithm, to maintain consistent experimental conditions, the initial $n_{ancestor}$ was also set to 2.

5.2. Environment A

Compared to other environments, Environment A is relatively simple. The performance of each algorithm is shown in Figure 9. In the figure, the start and goal points are represented by a yellow circle and a red star, respectively. The nodes and edges of the random tree are displayed as cyan diamonds and lines, with the initial solution marked by red lines. In Figure 9a, RRT* demonstrates its classical exploration characteristics, generating sample points that cover the entire map. However, the excessive number of sample points results in a curved and lengthy path. As shown in Figure 9b, APF-RRT* directs new nodes towards the goal using repulsive forces from obstacles, resulting in a smoother path. Figure 9c,d show the paths generated via Q-RRT* and F-RRT*, respectively. Both paths closely follow the obstacles, but F-RRT* achieves the shortest path among the five algorithms at the cost of higher iterations and time. Figure 9e shows that AODA-PF-RRT* generates the smoothest path with the fewest sample points.

Table 1. Comparison of algorithm performance metrics in Environment A.

Algorithm	Average Iteration	Average PathLength	Average Time (s)
RRT*	454.06	64.36	1.14
APF-RRT*	567.18	63.63	2.00
Q-RRT*	434.18	64.18	1.27
F-RRT*	399.52	62.38	1.26
AODA-PF-RRT*	144.32	62.71	0.32

shows that AODA-PF-RRT* produces a path length close to F-RRT*, with an Average Iteration count of only 32% and an Average Time cost of only 25% of F-RRT*. From Figure 10, it can be seen that while the Pathlength of AODA-PF-RRT* is similar to F-RRT*, its Iteration and Time are significantly lower than other algorithms, and it has the smallest interquartile range. The data distribution is symmetrical, with symmetrical data distribution and no obvious outliers.

5.3. Environment B

In Environment B, two rectangular obstacles were added to the base layout of Environment A, increasing the complexity of obstacle crossings. Although the paths generated via the algorithms in Environment B are more winding than in Environment A, the path generated via AODA-PF-RRT* in Figure 11 still shows a clear feasibility advantage. The performance and metrics for each algorithm are shown in Figure 11 and

Table 2. Comparison of algorithm performance metrics in Environment B.

Algorithm	Average Iteration	Average PathLength	Average Time (s)
RRT*	527.46	65.15	1.33
APF-RRT*	725.42	64.33	2.15
Q-RRT*	476.96	65.38	1.57
F-RRT*	435.62	63.48	1.44
AODA-PF-RRT*	174.90	63.64	0.50

. AODA-PF-RRT* achieves an Average Iteration of 174.90, Average Pathlength of 63.64, and Average Time of 0.50 seconds. Compared to F-RRT*, its Average Iteration and Average Time are reduced by 40% and 35%, respectively, indicating that the proposed algorithm maintains a significant advantage even in complex environments.

Figure 12b shows more scattered path length distributions for each algorithm due to the obstacle crossings, but in Figure 12a,c, the median values of AODA-PF-RRT* in terms of Iterations and Time were the lowest, at 174.5 and 0.47 s, respectively, with the most concentrated data distribution. Compared to other algorithms, AODA-PF-RRT* demonstrated higher efficiency in iterations and computational speed while maintaining high-quality path lengths.

5.4. Environment C

Environment C is a simple maze, significantly increasing path planning complexity due to the dense distribution of obstacles, requiring more collision checks and path corrections. Figure 13 shows the paths generated via each algorithm in Environment C, where the initial path generated via AODA-PF-RRT* is shorter, smoother, and less redundant compared to the other algorithms. As shown in Figure 14 and

Table 3. Comparison of algorithm performance metrics in Environment C.

Algorithm	Average Iteration	Average PathLength	Average Time (s)
RRT*	1376.92	92.00	15.00
APF-RRT*	1301.60	92.36	13.27
Q-RRT*	1014.44	88.93	7.64
F-RRT*	1071.38	84.17	7.08
AODA-PF-RRT*	516.84	85.37	3.53

, the proposed algorithm achieves an Average Pathlength of 85.37, Average Time of 3.51 s, and Average Iteration count of 516.84. Compared to the other four algorithms, AODA-PF-RRT* is 33% faster than the fastest RRT*, with 50% fewer Iteration than the algorithm with the fewest Iteration (Q-RRT*), and Pathlength comparable to F-RRT*. These comparisons demonstrate that AODA-PF-RRT* can generate higher-quality paths in simple maze environments.

5.5. Environment D

Environment D features the most complex obstacle distribution, with many obstacles spread across the area, forming isolated regions. Figure 15 shows the comparison of the initial paths generated via each algorithm, where AODA-PF-RRT* clearly outperforms the other algorithms in terms of the number of nodes and the smoothness of the path. The simulation data in

Table 4. Comparison of algorithm performance metrics in Environment D.

Algorithm	Average Iteration	Average PathLength	Average Time(s)
RRT*	2022.12	76.94	10.96
APF-RRT*	2209.56	78.83	31.69
Q-RRT*	2026.70	77.31	22.17
F-RRT*	1830.76	74.62	14.00
AODA-PF-RRT*	1031.42	75.60	8.60

show that in the complex maze environment, AODA-PF-RRT* even outperforms F-RRT* in terms of time. This is because in such complex environments, the pure exploration nature of RRT* is less affected by the maze, whereas F-RRT* requires multiple node creations and collision checks, significantly increasing time and iteration costs. The Average Pathlength of AODA-PF-RRT* is close to F-RRT*, but the Average Iteration count is 51% of F-RRT*‘s, and the Average Time is 78% of RRT*’s. The data distribution for the three evaluation metrics in Figure 16 shows that AODA-PF-RRT* has a better interquartile range and whisker range compared to the other algorithms, with no outliers. This demonstrates that AODA-PF-RRT* performs well in complex maze environments.

When comparing the performance of algorithms, response time under computational load, memory utilization, and peak load conditions serve as additional benchmark metrics. These metrics help evaluate the resource efficiency and response speed of different algorithms in various environments. To verify the superior performance of AODA-PF-RRT* relative to existing algorithms, benchmark tests were conducted on each algorithm in the representative complex Environment D, with results shown in

Table 5. Benchmark performance testing of algorithms in Environment D.

Algorithm	Average CPU Load (%)	Max CPU Load (%)	Average Memory Usage (MB)	Max Memory Usage (MB)	Average Response Time (s)
RRT*	9.17	11.25	62.40	62.50	12.07
APF-RRT*	12.26	21.80	82.43	83.49	30.69
Q-RRT*	3.00	10.05	93.10	96.08	22.33
F-RRT*	2.98	8.35	81.78	84.00	14.43
AODA-PF-RRT*	2.02	5.80	82.64	82.68	9.05

. Table 5 presents a comparative analysis of the benchmark performance of each algorithm, where the AODA-PF-RRT* algorithm demonstrates superior efficiency in key performance indicators, exhibiting the lowest average response time (9.05 s) and minimal resource consumption, with an average CPU load of only 2.02% and average memory usage of 82.64 MB. In contrast, the APF-RRT* algorithm has the highest resource utilization, with an average CPU load of 12.26% and an average response time of 30.69 s, indicating potential limitations in computational efficiency. Although the Q-RRT* algorithm has the highest memory usage (93.10 MB), its average response time (22.33 s) suggests a trade-off between resource demand and processing speed.

Under sustained operation, the AODA-RRT algorithm exhibits outstanding stability and efficiency, particularly in resource management, with a maximum CPU load of only 5.80%. This indicates a low resource occupancy during prolonged operation, allowing it to maintain effective operation in resource-constrained environments. Simultaneously, the average memory usage is 82.64 MB, and the maximum memory usage does not exceed 82.68 MB, demonstrating stability in memory demand and reducing the risk of memory leaks or overload.

Overall, the AODA-PF-RRT* algorithm serves as the most balanced option, providing a good balance between efficient performance and low resource consumption, making it suitable for applications in complex constrained environments.

5.6. Compare the Effect of the Algorithms on the Convergence Rate

Convergence refers to the rate at which an algorithm’s path cost approaches the optimal path. To study the convergence behavior of each algorithm, the termination condition triggered after finding the initial path was removed. This modification allows the algorithms to continue running after finding the initial feasible path, iteratively optimizing it further. In each simulation, once the initial path is found, the algorithm records the current shortest path length every second until the optimal path is reached or the designated time window is exceeded. To further quantify convergence, the changes in path length during each run were recorded and analyzed. In the experiments, AODA-PF-RRT* was considered to have found the optimal path if it completed within 50 s, denoted as $C_{optimal}$. To visually demonstrate the convergence speed of each algorithm, the variations in path cost over time were fitted into curves, comparing convergence speeds across different environments.

As shown in Figure 17a,b, all algorithms converge to 1.05 times $C_{optimal}$ relatively quickly, regardless of whether the environment is simple or complex. However, AODA-PF-RRT* demonstrates a clear advantage, generating an initial solution much faster than the other algorithms, allowing it to find a near-optimal path in a very short time. This demonstrates that AODA-PF-RRT* has a high convergence efficiency in obstacle-rich environments. While F-RRT* also demonstrates strong optimization capabilities, it lags slightly in generating the initial path, requiring more time to reach a path cost comparable to that of AODA-PF-RRT*. In contrast, RRT*, APF-RRT*, and Q-RRT* have slower convergence rates and take longer to approach near-optimal paths.

In the maze environment, as shown in Figure 17c,d, both AODA-PF-RRT* and F-RRT* show slower initial solution generation due to the maze’s complexity. However, over time, AODA-PF-RRT* maintains a faster path optimization speed, approaching the optimal path within 10 seconds. Although F-RRT* eventually reaches a path cost comparable to AODA-PF-RRT*, its optimization process takes longer. RRT*, APF-RRT*, and Q-RRT* show noticeably slower convergence speeds, and their final path quality does not match that of AODA-PF-RRT*.

5.7. Performance Evaluation and Comparative Analysis of the AODA-PF-RRT*

The AODA-PF-RRT* algorithm demonstrates significant advantages across various environments. In the simple Environment A, the algorithm generates smooth and efficient paths using the fewest sampling points, with substantially lower iteration counts and computation times compared to other algorithms. As the complexity increases in Environments B and C, AODA-PF-RRT* maintains high path quality, with path lengths close to those of F-RRT* but with notable reduction in both iteration count and computation time. Particularly in the complex maze Environment D, AODA-PF-RRT* exhibits higher node efficiency and path smoothness, outperforming other algorithms in both time and iteration metrics. Furthermore, during prolonged operation, the AODA-PF-RRT algorithm maintains relatively stable CPU and memory usage, indicating its processing capability under high load conditions.

In Experiment 2, the convergence speed comparison shows that AODA-PF-RRT, with its efficient initial path generation capability, quickly converges to a near-optimal path in less time. Although F-RRT* has excellent path optimization abilities, its overall convergence speed is slower than AODA-PF-RRT* due to the longer time required to generate the initial path. In contrast, RRT*, APF-RRT*, and Q-RRT* show slower convergence speeds and lower optimization efficiency.

6. Conclusions

This paper introduces the AODA-PF-RRT*, an enhanced path planning algorithm that demonstrates notable efficiency and flexibility in complex environments. By incorporating improvements to the APF algorithm and leveraging adaptive adjustment strategies—such as dynamically adjusting the number of sampling points and modifying the dichotomy termination threshold—AODA-PF-RRT* can generate smooth, high-quality paths while optimizing path planning accuracy across different environments. These improvements have greatly enhanced the algorithm’s efficiency, giving it greater potential for real-world applications. Future research will focus on optimizing the algorithm to further enhance its adaptability in dynamic environments, particularly in long-duration tasks.