Path Planning for Mobile Robots Based on the Improved DAPF-QRRT* Strategy

Abstract

The rapidly exploring random tree star (RRT*) algorithm is widely used to solve path planning problems. However, the RRT* algorithm and its variants fall short of achieving a balanced consideration of path quality and safety. To address this issue, an improved discretized artificial potential field-QRRT* (IDAPF-QRRT*) path planning strategy is introduced. Initially, the APF method is integrated into the Quick-RRT* (Q-RRT*) algorithm, utilizing the attraction of goal points and the repulsion of obstacles to optimize the tree expansion process, swiftly achieving superior initial solutions. Subsequently, a triangle inequality-based path reconnection mechanism is introduced to create and reconnect path points, optimize the path length, and accelerate the generation of sub-optimal paths. Finally, by refining the traditional APF method, a repulsive orthogonal vector field is obtained, achieving the orthogonalization between repulsive and attractive vector fields. This places key path points within the optimized vector field and adjusts their positions, thereby enhancing path safety. Compared to the Q-RRT* algorithm, the DPF-QRRT* algorithm achieves a 37.66% reduction in the time taken to achieve 1.05 times the optimal solution, and the IDAPF-QRRT* strategy nearly doubles generated path safety.

1. Introduction

Path planning plays a central role in the field of mobile robot kinematics, involving determining how mobile robots can locate themselves and navigate to target areas to accomplish their designated tasks [1]. This process encompasses not only the computation of collision-free paths from the starting point to the endpoint but also the fulfillment of additional optimization criteria, such as enhancing path efficiency and the shortest path [2]. Currently, path planning algorithms are widely applied across various domains, including unmanned aerial vehicles (UAVs), autonomous driving vehicles, automated guided vehicles (AGVs), and surgical robots [3,4,5].

Path planning algorithms primarily encompass grid-based algorithms, intelligent optimization-based algorithms, and sampling-based algorithms [6]. Grid search algorithms, such as A* [7] and Dijkstra [8], convert continuous spaces into finite grids to discover paths, ensuring the completeness and optimality of solutions [9]. These algorithms are particularly effective in environments with static obstacles because they can systematically explore all possible paths and select the most efficient ones. The A* algorithm optimizes the path search process by combining heuristic evaluations, while the Dijkstra algorithm ensures the shortest path by maintaining a priority queue of nodes to be visited. However, the performance of these algorithms highly depends on the resolution of the grid. Especially in complex high-dimensional spaces, the requirements for high-resolution and long-distance planning lead to a significant increase in computational and memory requirements, and the computational cost grows exponentially [10]. This makes grid-based algorithms impractical in systems with limited resources.

Intelligent optimization algorithms, including genetic algorithms (GAs) [11], particle swarm optimization (PSO) [12], ant colony optimization (ACO) [13], and the cuckoo search algorithm (CSA) [14], provide adaptable solutions to complex path planning problems, particularly in multi-constraint and high-dimensional scenarios where traditional grid-based methods may be limited. GA is known for its robust global search capabilities, which are effective in problems with complex constraints and high-dimensional spaces. PSO offers rapid initial convergence and short search times, while CSA is valued for its simplicity and effective exploration of the solution space. However, these algorithms face challenges such as high computational costs due to extensive search and evaluation processes and the risk of converging to local minima, leading to suboptimal paths. Specifically, PSO experiences reduced convergence speed in later stages and is prone to local optima, CSA lacks guarantees for global optimality and struggles in complex or large-scale problems, and GA suffers from slow convergence, weak local optimization, and low stability, especially in environments with dense obstacles or dynamic changes.

Sampling-based algorithms, owing to random sampling in continuous spaces, circumvent grid resolution limitations and minimize computational requirements, thereby excelling in path planning for complex, high-dimensional scenarios. For instance, the quintessential rapidly exploring random trees (RRT) algorithm [15] can rapidly search nonconvex high-dimensional spaces with a tree structure. It features few parameters and a simple structure but has drawbacks such as poor path quality, slow convergence, and numerous redundant points [16]. To address these issues, researchers have developed several RRT variants to meet the demands of various fields. The RRT* algorithm [17] was developed by Karaman et al. It incorporates the ChooseParent and Rewire processes to achieve asymptotic optimality, although it has slow convergence. To expedite convergence, the RRT*-Smart algorithm [18] was introduced by Islam et al. This algorithm combines intelligent sampling and path optimization strategies, giving priority to nodes in promising regions of the search space while reducing comprehensive exploration. However, its performance is contingent on the proper setting of bias radius and bias ratio; improper parameter settings may degrade performance and impede the identification of feasible paths. Gammell et al. proposed the Inform-RRT* algorithm [19], which dynamically adjusts the sampling space to accelerate convergence. Similar to RRT*-Smart, the algorithm performance depends on parameter settings within the ellipsoid region. Due to the large elliptic domain, the performance of the algorithm approaches that of RRT*. Conversely, a too-small elliptic field can hinder the rapid identification of viable paths, resulting in a long and complex search process. The P-RRT* algorithm proposed by Qureshi and Ayaz uses target point gravity to guide random sampling, thereby minimizing redundant search [20]. However, in scenarios with dynamic obstacles or narrow feasible areas, the sampling strategy may not adjust in time, leading to decreased search efficiency. It is noteworthy that the convergence of the sampling algorithm can be accelerated not only by improving the sampling process but also by optimizing the structure of the growing tree itself. Jeong et al. proposed the Q-RRT* algorithm [21], which, for the first time, used triangle inequalities to optimize the ChooseParent and Rewire processes. Liao et al. then developed the F-RRT* algorithm [22] to achieve a shorter path by adjusting the position of the node closer to the edge of the obstacle. However, similar to Q-RRT*, in complex environments with high obstacle density, the constraints of triangular inequalities limit the convergence speed and effectiveness of the path optimization of the F-RRT* algorithm [23]. The FC-RRT* algorithm [24] proposed by Wang et al. is a further optimization based on RRT*-connect [25], aiming to improve the speed and convergence of path search. However, when faced with highly complex obstacles or scenarios where the initial path is difficult to find, the FC-RRT* algorithm may experience increased initialization time and be sensitive to parameter selection. Liu et al. recognized the potential to enhance path search efficiency by reducing the exploration space of random trees and subsequently proposed the NT-ARS-RRT algorithm [26]. However, this approach encounters challenges in dynamic environments where constant changes in obstacles and maps require the rigorous and timely adjustment of preprocessing parameters. The delayed optimization of these parameters can lead to suboptimal paths or even an inability to plan a viable route. Ding et al. designed the EP-RRT* algorithm, which uses path expansion and a heuristic sampling strategy to accelerate convergence [27]. The essence of this algorithm is to reduce the search space and improve the probability of finding effective nodes. However, in complex scenarios with multiple feasible paths, the algorithm may require a larger expansion area to avoid local optimal solutions, which could potentially affect its performance. Despite extensive studies on path quality and convergence speed, these algorithms have yet to adequately address the issue of path safety. Given the inherent randomness in node distribution, sampling-based algorithms may generate paths that are close to obstacles, posing potential collision risks [28]. Path safety is crucial in certain urgent tasks, such as medical rescue and military reconnaissance [29,30].

The artificial potential field (APF) method, introduced by Khatib [31], employs virtual forces as a foundation for path planning. It produces paths that are smooth, continuous, and highly safe [32], yet it encounters limitations such as the inaccessibility of target points and entrapment in local minima [33]. Tong et al. proposed the A*-MTIAPF algorithm, which resolves the issue of local minima by setting multiple sub-target points [34]. He Z et al. introduced the model predictive artificial potential field (MPAPF) algorithm [35], which incorporates a model predictive strategy to prevent entrapment in local optima. Yang et al. recognized that enhancing obstacle avoidance ability and improving the safety of path planning could be achieved by refining the path planning algorithm. To this end, they proposed the AAPF* algorithm, which employs an obstacle expansion strategy to enhance path safety [36]. However, this strategy enlarges the actual influence range of obstacles, increases the complexity of the search space, and subsequently impacts the real-time performance of the algorithm.

While these improved algorithms have addressed the limitations of the traditional APF method to some extent, research on fine-tuning the relative relationship between repulsive and attractive potential fields to further optimize obstacle avoidance efficiency is still insufficient.

This paper presents an IDAPF-QRRT* path planning strategy and conducts simulations in three environments. Compared to RRT*, P-RRT, and Q-RRT, it has better initial solutions, faster convergence, and stronger path safety. In complex Environment B, increased obstacle density limits the advantage of Q-RRT* in reconnecting parent nodes, reducing its search performance and convergence speed. In maze Environment C, target point guidance causes random sampling points to deviate from the feasible region, weakening the performance of P-RRT*.

The main contributions of this paper are summarized as follows:

• The integration of potential field theory: We incorporated potential field theory into the Q-RRT* algorithm to optimize the growth tree structure, improve search ability, and new node generation efficiency compared to other sampling algorithms.
• Path reconnection mechanism based on triangular inequality: We propose a path reconnection mechanism based on triangular inequality to adjust original path points, generate new ones, accelerate convergence, and improve path planning efficiency.
• Improved artificial potential field method: We constructed an exclusion orthogonal vector field and introduced a gravitational adjustment process to improve obstacle avoidance efficiency, as well as discretize the reconnected path and improve the APF method to push key path points to enhance path safety.

The subsequent chapters are structured in the following manner: Section 2 defines and explains the key issues; Section 3 introduces the RRT*, P-RRT*, and Q-RRT* algorithms; Section 4 details the proposed IDAPF-QRRT* strategy; Section 5 details the conducted simulation experiments and their corresponding outcomes; Section 6 presents the conclusion of the paper.

2. Problem Definition

In path planning, the configuration space X is the set of all possible positions and orientations of a robot or vehicle in its environment. It is a mathematical construct that simplifies the complex environment into a set of points, where each point represents a possible configuration of the robot. Within this space, the obstacle-occupied region $X_{obs}$ denotes the subset of X that is occupied by obstacles, making it inaccessible for the robot to traverse. Consequently, the obstacle-free area is given by $X_{free}=X\setminus X_{obs}$, which represents the set of all points in the configuration space that are free from obstacles and can be potentially occupied by the robot [24].

The core of the path planning problem is to determine a viable trajectory from an initial point, $x_{start}$, to a desired destination, $x_{goal}$, within the obstacle-free area, denoted by the tuple (X, $x_{start}$, $x_{goal}$). Here, it is required that both the initial point $x_{start}$ and the goal point $x_{goal}$ lie within the obstacle-free area, i.e., $x_{start}\in X_{free}$ and $x_{goal}\in X_{free}$. A path can be represented as a continuous mapping, $\sigma :[0,1]\to X$, where $\sigma$ is a continuous function with bounded variation. If for all $\tau \in [0,1]$, $\sigma \left(\tau \right)\in X_{free}$, then $\sigma$ describes a collision-free path from $x_{start}$ to $x_{goal}$.

Definition 1. 

Feasible path planning refers to the set of solutions to the proposed path planning problem ($X_{free}$, $x_{start}$, $x_{goal}$) that ensures a collision-free path where $\sigma \left(0\right)=x_{start}$ and $\sigma \left(1\right)=x_{goal}$. If no feasible solution exists within the obstacle-free area, a failure is returned.

Definition 2. 

Optimal path planning aims to identify a path ${\sigma }^{*}$ within a given motion planning framework ($X_{free}$, $x_{start}$, $x_{goal}$) that is not only collision-free but also incurs the minimum cost. The optimal path ${\sigma }^{*}$ satisfies $c\left({\sigma }^{*}\right)=min\{c\left(\sigma \right)\mid \sigma isfeasible\}$, where $c\left(\sigma \right)$ is defined as the total Euclidean length of the path σ. If no solution is found within the obstacle-free area, a failure is reported.

3. Related Work

This section commences with a review of the RRT* algorithm, outlining its fundamental significance in path planning. Subsequently, a thorough analysis of the Q-RRT* algorithm is presented, setting the stage for the introduction of the IDAPF-QRRT* approach proposed herein. Additionally, the P-RRT* algorithm is incorporated for comparative evaluation, aiming to underscore the distinct advantages and limitations of diverse algorithmic approaches in path planning.

3.1. RRT*

As the precursor to the RRT* algorithm, the RRT algorithm has laid a solid foundation for the field of path planning. RRT first constructs a tree $G=(V,E)$ from the initial point $x_{start}$ and gradually extends to the target region $x_{goal}$. A brief description of the tree growth process is provided below: randomly select a sample, $x_{rand}$, from the free space $X_{free}$, and generate $x_{new}$ by linearly expanding from the nearest tree node, $x_{nearest}$, towards $x_{rand}$ with a step size. If the path between $x_{nearest}$ and $x_{new}$ remains obstacle-free, the new node and its corresponding path are incorporated into the tree. The probabilistic completeness of RRT ensures that a solution can be found in the search space with sufficient iterations. The RRT* algorithm is then detailedly introduced, and its core pseudocode is presented as Algorithm 1.

The grow-tree expansion process for RRT* is shown in Figure 1. RRT* introduces two core processes compared to RRT: ChooseParent and Rewire. The algorithmic implementation of the ChooseParent process is presented in Algorithm 2. When RRT* generates a new node $x_{new}$, it does not immediately connect to the nearest node $x_{nearest}$. Instead, it searches within a specific radius centered on $x_{new}$ for a node that optimizes the cost of the path leading to $x_{new}$ from the originating node, as shown in Figure 2a. The Rewire process constitutes a pivotal stage in guaranteeing the optimality of the resulting tree. Upon adding $x_{new}$ to the tree and selecting the cost-optimal parent node, if reassigning neighboring nodes to $x_{new}$ as their parent leads to a reduction in path cost, then such reassignment is performed, as illustrated in Figure 2c. The RRT* algorithm can perform path planning in high-dimensional complex environments and has asymptotic optimality and a simple structure that is easy to implement. However, due to the randomness of sampling, its convergence speed is slow. Especially in complex environments, the path quality is not good, and there are problems such as bends and redundant nodes.

Algorithm 1: RRT*($x_{start},x_{goal},X_{obs},Iter$)

Algorithm 2: ChooseParents$(X_{near},x_{new},x_{nearest},{\sigma }_{nearest})$

Some of the other basic functions covered in RRT* are described as follows:

• Sample: Conducts random sampling within the parameter space designated as X, returning an $x_{rand}$ that is independently and uniformly distributed.
• NearestNode: Retrieves and returns the node in the graph $G=(V,E)$ that is closest to V, considering a predefined distance threshold.
• Steer: Generates a new node, $x_{new}$, growing at a certain step length between $x_{near}$ and $x_{rand}$.
• CollisionFree: Verifies the absence of collisions along a given path. It returns True if the path is clear or False if any collisions are detected.
• NearNode: Given the graph $G=(V,E)$ and a specified state, x, this provides a subset of vertices from V that lie within a predefined radius centered around the point x.
• Cost: Computes and returns the length of the path traversing from the initial start point to the specified input node.

3.2. P-RRT*

The central tenet of the P-RRT* algorithm revolves around incorporating the concept of artificial potential fields during the growth of the tree, enabling a more targeted selection of the subsequent exploration point. The execution process for P-RRT* is detailed in Algorithm 3.

Algorithm 3: P-RRT*$(x_{start},x_{goal},X_{obs},Iter)$

The random sampling process has a critical impact on the performance and efficiency of sampling-based algorithms. The P-RRT* algorithm employs a potential energy function-driven P-Sample sampling strategy that effectively enhances the efficiency of the search space. Algorithm 4 elaborates on this process in detail. Firstly, the strategy calculates the directional vector extending from the current point to the target point (Line 3) while also assessing the shortest distance, denoted as $d_{obs}$, between the preceding random point and the obstacle space $x_{obs}$, utilizing the NearestObs function. Guided by the potential energy function, the random point progresses a certain distance, $\lambda$, toward the intended target. If at any point the condition $d_{obs}\le d_{obs}^{*}$ is met, the sampling process is interrupted and the directed extended sample point $x_{prand}$ is returned. Otherwise, this process is terminated after k repetitions. The constant $d_{obs}^{*}$ signifies the minimal distance between the randomly sampled point and the obstacle, and $\lambda$ denotes the small incremental step size directed towards the target. In spite of the fact that P-Sample is effective in most cases, it lacks local obstacle consideration and is sensitive to target point location. Its performance is limited in complex obstacle layouts. Thus, P-RRT* is more suitable for static environments with clear target points for path planning.

Algorithm 4: P-Sample$\left(x_{rand}\right)$

3.3. Q-RRT*

This section provides a concise overview of the Q-RRT* algorithm, with its pseudocode presented in Algorithm 5. The fundamental concept underlying the Q-RRT* algorithm revolves around two key aspects: (1) Within the Euclidean distance framework, the cost function adheres to the triangular inequality principle. (2) During the Rewire phase, nodes in a particular region tend to share a common parent node. Although the RRT* algorithm often enlarges the circular search area to expedite convergence, this approach concomitantly increases the density of neighboring nodes and, consequently, the computational overhead. Distinctively, the Q-RRT* algorithm accelerates convergence by improving the ChooseParent and Rewire processes to find more optimal paths. Compared to the circuitous paths of RRT*, Q-RRT* achieves more direct trajectories with lower costs, as illustrated in Figure 1.

Algorithm 5: Q-RRT*$(x_{start},x_{goal},X_{obs},Iter)$

The key novel programs utilized in Q-RRT* are outlined below:

• GetAncestor: Identify and retrieve the d-th ($d\in \mathbb{N}$) ancestor of a specified vertex p in a given graph $G=(V,E)$.
• AncestryNode: Utilizes the function $GetAncestor(G,p,i)$ to retrieve the ancestor vertex of p at the specified depth i. If $i\in \mathbb{N}$, it returns ${\displaystyle \bigcup _{i=1}^d}GetAncestor(G,p,i)$. Otherwise, it returns ∅.

In Figure 2b, the ChooseParent process in Q-RRT* is demonstrated. Initially, the algorithm traverses and identifies the set of ancestor nodes. Following this, it assesses the path cost associated with each ancestor serving as the parent of the new node $x_{new}$; ultimately, the lowest cost node is selected as the parent node of $x_{new}$.

Algorithm 6 presents the Rewire process of Q-RRT*. In the Rewire process, Q-RRT* also considers ancestor nodes, as shown in Figure 2d. During the Rewire process, the path cost through an ancestor node to $x_{near}$ is evaluated and compared with the original path cost to $x_{near}$. If lower, the parent node of $x_{near}$ is updated to that ancestor node. These improvement processes help optimize the tree structure and enhance path efficiency. Notwithstanding that the ChooseParent and Rewire processes improve path optimization efficiency and modify the RRT* structure, in high-obstacle-density environments, triangle inequality may limit convergence speed and efficiency. Thus, Q-RRT* is more suitable for scenarios with high path optimization requirements and relatively regular obstacle distribution, and its advantages are hard to show in highly complex and irregular environments.

Algorithm 6: Rewire-QRRT*$(G,x_{new},x_{near})$

4. IDAPF-QRRT*

In order to elevate the quality of initial trajectories and rapidly obtain suboptimal paths, this paper introduces the DPF-QRRT* algorithm, which integrates QRRT* with the APF approach. The DPF-QRRT* algorithm mainly improves QRRT* in two aspects: (1) it applies the APF method to optimize the random sampling and new node generation process of QRRT*, enhancing obstacle avoidance efficiency and accelerating the acquisition of initial solutions; (2) it utilizes a triangle inequality-based path reconnection mechanism to reconnect the discrete paths, which significantly reduces the trajectory’s length. The DPF-QRRT* algorithm is improved by introducing two key processes, APF-Steer and Path-Reconnection. During each iteration, the APF-Steer process is used to efficiently generate new nodes in the configuration space. In addition, when a newly generated node approaches the target region, the path reconnection process is activated to optimize the existing path in a timely manner. The specific implementation process and optimization methodologies are thoroughly outlined in Algorithm 7.

Algorithm 7: DPF-QRRT*$(x_{start},x_{goal},X_{obs},Iter)$

Figure 3 depicts the potential collision scenarios that P-RRT* may encounter during the generation of new nodes (Figure 3a). The core of DPF-QRRT* lies in the optimization of the direction of new nodes by the APF-Steer process. It not only introduces random nodes distributed by the attractive potential field but also precisely adapts the development direction of nodes by combining the repulsive potential field, effectively avoiding obstacle conflicts during the generation of new nodes, as shown in Figure 3b. Algorithm 8 outlines how the APF-Steer process works. Initially, it sets up an attractive force ${\overrightarrow{F}}_{att}$ towards the goal and checks for nearby obstacles. If an obstacle is too close (within a critical distance $d_{obs}^{*}$), a repulsive force ${\overrightarrow{F}}_{rep}$ is activated to push the point away from the obstacle. The algorithm then computes a new position, $x_{new}$, by extending from $x_{prand}$ along the combined force vector of attraction and repulsion, and returns this new position $x_{new}$.

Algorithm 8: APF-Steer$(x_{prand},x_{goal},x_{nearest},X_{obs})$

Another core process of the DPF-RRT* algorithm is the Path-Reconnection process, as shown in Algorithm 9. The Path-Reconnection process performs local optimization on $n^{*}$ points in the Path and reconstructs the path points to shorten the path length. Here, $n^{*}$ represents the number of path points after path discretization. This process conducts Screening—Obs operations based on the relative positions of adjacent path points and the layout of obstacles to screen out the obstacles that only affect the optimization of the current path segment. Then, the GetNewPoint process adjusts the initial waypoint and creates a new waypoint to reduce the path cost. Eventually, the updated path NewPath is returned.

The new functions used in DPF-QRRT* are as follows:

• ExtendNewNode: Generates a new node, $x_{new}$, by extending $x_{nearest}$ along the direction of an input vector at a specified step length;
• Dist: Returns the Euclidean distance between two input points;
• NearGoal: Determines if $x_{new}$ reaches a neighborhood of $x_{goal}$.

Algorithm 9: Path-Reconnection$(Path,X_{obs})$

Although the DPF-QRRT* algorithm can enhance the obstacle avoidance efficiency of new nodes, the issue of paths being too close to obstacles still affects safety. Therefore, this paper improves the APF method and integrates it with the DPF-QRRT* algorithm to obtain the IDAPF-QRRT* strategy. Compared to DPF-QRRT*, the IDAPF-QRRT* strategy employs a repulsive orthogonal vector field to adjust the position of critical path points, achieving the dual optimization of path quality and safety without sacrificing sampling efficiency. Compared with other sampling-based algorithms, the IDAPF-QRRT* strategy generates safe, high-quality paths more efficiently in various types of environments.

4.1. Triangle Inequality Path Reconnection Mechanism

4.1.1. Screening Obstacles

Before the reconnection operation is implemented, obstacles are first screened to obtain a set of obstacles that only affect the path reconnection mechanism, as shown in Figure 4.

In the path, the i-th path point $p_i$ and its direct adjacent path points $p_{i-1}$ and $p_{i+1}$ together form a local path segment. In this local path segment, we consider only the collection of obstacles that affect the path segment, $X_{filtered}$. The set of obstacles, $X_{filtered}$, as defined by Equation (1), must be situated within a specific elliptic region and a sector bounded by rays $l_1$ and $l_2$, with $p_i$ as the apex. The ellipse takes $p_{i-1}$ and $p_{i+1}$ as $p_{i-1}$ and $p_{i+1}$. The major half-axis $c_{maj,i}$ and minor half-axis $c_{min,i}$ of the ellipse are computed according to Equation (4). $r_j$ indicates the radius of the obstacle. ${\gamma }_{1,j}$ and ${\gamma }_{2,j}$, respectively, represent the angular relationships between obstacles and two path points. ${\gamma }_{pn}$ is the angle between the path segments. The specific calculation process is shown in Equations (2) and (3).

$$\begin{array}{cc}\hfill X_{filtered}& =\left\{(x_j,y_j,r_j)\in X_{obs,j}\mid \begin{array}{c}{\displaystyle \frac{{(x_j-x_0)}^2}{c_{maj,j}^2}}+{\displaystyle \frac{{(y_j-y_0)}^2}{c_{min,j}^2}}\le 1\hfill \\ sin\left({\gamma }_{1,j}\right)+sin\left({\gamma }_{2,j}\right)\le sin\left({\gamma }_{pn}\right)+2·{\displaystyle \frac{r_j}{∥\overrightarrow{p_i{X}_{obs,j}}∥}}\hfill \end{array}\right\}\hfill \end{array}$$

(1)

$$\begin{array}{cc}\hfill {\gamma }_{1,j}& =arccos\left({\displaystyle \frac{\overrightarrow{p_i{p}_{i-1}}·\overrightarrow{p_i{X}_{obs,j}}}{∥\overrightarrow{p_i{p}_{i-1}}∥·∥\overrightarrow{p_i{X}_{obs,j}}∥}}\right),{\gamma }_{2,j}=arccos\left({\displaystyle \frac{\overrightarrow{p_i{p}_{i+1}}·\overrightarrow{p_i{X}_{obs,j}}}{∥\overrightarrow{p_i{p}_{i+1}}∥·∥\overrightarrow{p_i{X}_{obs,j}}∥}}\right)\hfill \end{array}$$

(2)

$$\begin{array}{cc}\hfill {\gamma }_{pn}& =arccos\left({\displaystyle \frac{\overrightarrow{p_i{p}_{i-1}}·\overrightarrow{p_i{p}_{i+1}}}{∥\overrightarrow{p_i{p}_{i-1}}∥·∥\overrightarrow{p_i{p}_{i+1}}∥}}\right)\hfill \end{array}$$

(3)

$$\begin{array}{cc}\hfill c_{maj,i}& =2·\sqrt{{(h+r_j)}^2+d^2},c_{min,i}=2·(h+r_j)\hfill \end{array}$$

(4)

$$\begin{array}{c}\hfill d={\displaystyle \frac{1}{2}}·∥\overrightarrow{p_i{p}_{i+1}}∥,c={\displaystyle \frac{1}{2}}·(∥\overrightarrow{p_i{p}_{i-1}}∥+∥\overrightarrow{p_i{p}_{i+1}}∥),h=\sqrt{c^2-d^2}\end{array}$$

(5)

$$\begin{array}{cc}\hfill p_0(x_0,y_0)& ={\displaystyle \frac{p_{i-1}+p_{i+1}}{2}}\hfill \end{array}$$

(6)

4.1.2. Triangle Inequality Path Reconnection Mechanism

In the realm of path planning, particularly within complex environments, the optimization of local path segments is crucial for enhancing overall path quality. This study introduces the triangle inequality path reconnection mechanism (TIPRM), which refines path points to effectively reduce path length and improve algorithm performance. The mechanism employs different path optimization strategies based on the angle ${\gamma }_{\mathrm{pn}}$ formed by the local path defined by points $p_i$, $p_{i-1}$, and $p_{i+1}$.

For ${\gamma }_{\mathrm{pn}}\in [\pi /3,\pi )$, the mechanism constructs point $p_{\mathrm{creat},i}$ near $X_{\mathrm{nearest}}$ to reduce path length, as shown in Figure 5a. Considering the influence of other obstacles, such as $X_{\mathrm{pre}}$, which may prevent direct connection between $p_i$ and $p_{\mathrm{creat},i}$, the mechanism introduces points $p_{\mathrm{pre},i}$ and $p_{\mathrm{nxt},i}$ to increase the success rate of path optimization. The construction of these points is governed by Equations (7)–(9). Notably, $p_{\mathrm{pre},i}$ and $p_{\mathrm{nxt},i}$ are generated only when the distances from $X_{\mathrm{pre}}$ and $X_{\mathrm{nxt}}$ to the path are less than that of $X_{\mathrm{nearest}}$, under the conditions $h_{\mathrm{pre}}>h_{\mathrm{nst}1}$ and $h_{\mathrm{nxt}}>h_{\mathrm{nst}2}$. The lengths of the movement distances $l_{\mathrm{pre}}$ and $l_{\mathrm{nxt}}$ significantly impact the efficiency of path optimization, with Equation (10) ensuring a balance between the optimization effect and collision avoidance with $X_{\mathrm{pre}}$ and $X_{\mathrm{nxt}}$.

The construction of $p_{\mathrm{creat},i}$ aims to minimize path length while ensuring no collisions with obstacles. This requires an adjustment distance $l_{\mathrm{creat}}$ that is sufficiently close to the obstacle without causing a collision. Equation (11) defines $l_{\mathrm{creat}}$, where the value of n influences the fine-tuning of $l_{\mathrm{creat}}$, affecting the adjustment of path point $p_i$ towards the nearest obstacle $X_{\mathrm{nearest}}$. The selection of n can be based on the distribution of obstacles and the scale of the map.

$$\begin{array}{cc}\hfill p_{pre,i}& =\left\{\begin{array}{cc}{p}_i+l_{pre}·\left({\displaystyle \frac{\overrightarrow{p_i{p}_{i-1}}}{∥\overrightarrow{p_i{p}_{i-1}}∥}}\right)\hfill & ,h_{pre}>h_{nst1}\cap (∥\overrightarrow{p_i{p}_{i-1}}∥\ge (d_{pre}+r_{pre}))\hfill \\ p_{i-1}\hfill & ,else\hfill \end{array}\right.\hfill \end{array}$$

(7)

$$\begin{array}{cc}\hfill p_{nxt,i}& =\left\{\begin{array}{cc}{p}_i+l_{nxt}·\left({\displaystyle \frac{\overrightarrow{p_i{p}_{i+1}}}{∥\overrightarrow{p_i{p}_{i+1}}∥}}\right)\hfill & ,h_{nxt}>h_{nst2}\cap (∥\overrightarrow{p_i{p}_{i+1}}∥\ge (d_{nxt}+r_{nxt}))\hfill \\ p_{i+1}\hfill & ,else\hfill \end{array}\right.\hfill \end{array}$$

(8)

$$\begin{array}{cc}\hfill p_{creat,i}& =\left\{\begin{array}{cc}{p}_i+l_{creat}·\left({\displaystyle \frac{\overrightarrow{p_i{X}_{nearest}}}{∥\overrightarrow{p_i{X}_{nearest}}∥}}\right)\hfill & ,CollisionFree(p_{pre,i},p_{creat,i},p_{nxt,i},X_{filtered})\hfill \\ p_i\hfill & ,else\hfill \end{array}\right.\hfill \end{array}$$

(9)

$$\begin{array}{cc}\hfill l_{pre}& ={\displaystyle \frac{∥\overrightarrow{p_{pre,i}{X}_{pre}}∥-r_{pre}}{cos\left({\gamma }_{pre}\right)}},l_{nxt}={\displaystyle \frac{∥\overrightarrow{p_{nxt,i}{X}_{nxt}}∥-r_{nxt}}{cos\left({\gamma }_{nxt}\right)}}\hfill \end{array}$$

(10)

$$\begin{array}{cc}\hfill l_{creat}& =\left({\displaystyle \frac{n-k_n}{n}}\right)·\left(∥\overrightarrow{p_{creat,i}{X}_{nearest}}∥-r_{nst}\right),k_n=1:1:n-1,n\in {\mathbb{Z}}^{+}\cap n>2\hfill \end{array}$$

(11)

When ${\gamma }_{\mathrm{pn}}$ is within $(0,\pi /3)$, the path optimization strategy becomes more direct, as depicted in Figure 5b. In this case, the adjustment of path points is solely related to $X_{\mathrm{nearest}}$, without reliance on other obstacles. This simplified approach stems from the fact that when the path angle is small, the geometric relationship of the local path segment is relatively simple, and the nearest obstacle $X_{\mathrm{nearest}}$ has the most significant impact on the path. The construction of $p_{\mathrm{pre},i}$ and $p_{\mathrm{nxt},i}$ is defined by Equations (12) and (13), where $l_{\mathrm{pre}}$ and $l_{\mathrm{nxt}}$ are influenced by $\alpha$. A larger $\alpha$ value corresponds to a more pronounced path optimization effect, while a smaller $\alpha$ value represents a trade-off for safer path planning.

$$\begin{array}{cc}\hfill p_{pre,i}& =p_i+l_{pre}·\left({\displaystyle \frac{\overrightarrow{p_i{p}_{i-1}}}{∥\overrightarrow{p_i{p}_{i-1}}∥}}\right)\hfill \end{array}$$

(12)

$$\begin{array}{cc}\hfill p_{nxt,i}& =p_i+l_{nxt}·\left({\displaystyle \frac{\overrightarrow{p_i{p}_{i+1}}}{∥\overrightarrow{p_i{p}_{i+1}}∥}}\right)\hfill \end{array}$$

(13)

$$\begin{array}{cc}\hfill l_{pre}& =\alpha ·{\displaystyle \frac{d_{nearest}-r_{nearest}}{cos\left({\gamma }_1\right)}},\alpha \in [0,1]\hfill \end{array}$$

(14)

$$\begin{array}{cc}\hfill l_{nxt}& =\alpha ·{\displaystyle \frac{d_{nearest}-r_{nearest}}{cos\left({\gamma }_2\right)}},\alpha \in [0,1]\hfill \end{array}$$

(15)

4.2. Pushing Path Points Using the Improved APF Method

4.2.1. Improved APF Method

This section will elaborate in detail on the Improved-APF method, which elevates the proficiency of obstacle evasion by optimizing the direction of the resultant force while advancing from the traditional artificial potential field method. Traditional artificial potential fields typically consist of repulsive and attractive potential fields, with the basic principle illustrated in Figure 6a. Let q be a path point; under the traditional artificial potential field framework, the attractive potential field, Equation (16), the repulsive potential field, Equation (17), and the resultant force field composed of both, Equation (18), are defined, respectively.

$$\begin{array}{cc}\hfill U_{att}\left(q\right)& ={\displaystyle \frac{1}{2}}{k}_a{\rho }_g^2\left(q\right)\hfill \end{array}$$

(16)

$$\begin{array}{cc}\hfill U_{rep}\left(q\right)& =\left\{\begin{array}{cc}{\displaystyle \frac{1}{2}{k}_r{\left(\frac{1}{\rho \left(q\right)}-\frac{1}{{\rho }_0}\right)}^2}\hfill & ,\rho \left(q\right)\le {\rho }_0\hfill \\ 0\hfill & ,\rho \left(q\right)>{\rho }_0\hfill \end{array}\right.\hfill \end{array}$$

(17)

$$\begin{array}{cc}\hfill U_{total}& =\sum U_{att}+\sum U_{rep}\hfill \end{array}$$

(18)

In the equations, we have the following: $k_a$ is the gain constant of the attractive field; $k_r$ represents the gain constant of the repulsive field; $k_a$ is set to 0.3, and $k_r$ is set to 0.1. ${\rho }_0$ (0.2 in this paper) is the maximum safe distance from path point q to the threat range; $\rho \left(q\right)$ and ${\rho }_g$(q), respectively, represent the Euclidean distance between point q and the target point, as well as between point q and the center of the nearest obstacle. The determination of the magnitudes of attraction and repulsion relies on the negative gradients of their respective attractive and repulsive field functions, as illustrated in Equations (19) and (20), respectively. The sum of the repulsive and attractive forces, as depicted in Equation (21), is the resultant force.

$$\begin{array}{cc}\hfill F_{att}\left(q\right)& =-grad{U}_{att}\hfill \end{array}$$

(19)

$$\begin{array}{cc}\hfill F_{rep}\left(q\right)& =-grad{U}_{rep}\hfill \end{array}$$

(20)

$$\begin{array}{cc}\hfill F_{total}& =\sum F_{att}+\sum F_{rep}\hfill \end{array}$$

(21)

As depicted in Figure 6a, in the traditional APF method, when a mobile robot approaches an obstacle, the repulsive force conflicts with the attractive force and dominates the resultant force, leading to drastic changes in the robot’s path and potentially falling into a local, optimal solution. This paper takes the angle, $\lambda$, between attraction and repulsion as the breakthrough point to improve the traditional APF. When $cos\lambda$ in the interval [−1/2, 0], orthogonal repulsion and attraction forces are achieved, ensuring safety while enhancing obstacle avoidance efficiency, as shown in Figure 6b. Particularly, when the angle, $\lambda$, continues to increase and exceeds the set angle threshold, $\mu$, it indicates that the obstacle is close to the linear path between the target position and the mobile robot, posing a significant obstacle to the robot’s movement. In this case, if the repulsive force $F_{rep2}$ opposite to the gravitational force is directly discarded, the gravitational component may dominate and guide the robot to move towards the obstacle, resulting in safety risks, as shown in Figure 6c. Therefore, angle $\theta$ is introduced to dynamically adjust the magnitude and orientation of gravitational $F_{att}$ to $F_{att}^{\prime }$ ( $F_{att}^{\prime }$ in Figure 6d), ensuring that the robot can move in a safe direction. The direction of the adjusted resultant force $F_{total}$ is shown in Figure 6d; it is orthogonal to the direction of the repulsive force $F_{rep}$, maximizing obstacle avoidance efficiency while ensuring path safety.

$$\begin{array}{cc}\hfill cos\lambda & ={\displaystyle \frac{F_{att}^T{F}_{rep}}{∥F_{att}∥∥F_{rep}∥}}\hfill \end{array}$$

(22)

$$\begin{array}{cc}\hfill F_{total}\left(q\right)& =\left\{\begin{array}{cc}{F}_{att}^{\prime }+P_a\times \sum F_{rep}\hfill & ,cos\lambda \in [\mu ,0)\hfill \\ \sum F_{att}+\sum F_{rep}\hfill & ,cos\lambda \in [-1,\mu )\cup [0,1]\hfill \end{array}\right.\hfill \end{array}$$

(23)

$$\begin{array}{cc}\hfill F_{att}^{\prime }& =\left\{\begin{array}{cc}\sum F_{att}\hfill & ,{\displaystyle \frac{∥\sum F_{att}∥}{∥P_a\times \sum F_{rep}∥}}\le tan\left(\theta \right)\hfill \\ P_a\times \sum F_{rep}\times tan\left(\theta \right)\hfill & ,{\displaystyle \frac{∥\sum F_{att}∥}{∥P_a\times \sum F_{rep}∥}}>tan\left(\theta \right)\hfill \end{array}\right.\hfill \end{array}$$

(24)

$$\begin{array}{cc}\hfill P_a& =I-{\displaystyle \frac{F_{att}{F}_{att}^T}{{∥F_{att}∥}^2}}\hfill \end{array}$$

(25)

For $\lambda \in [0,\pi ]$, the optimized resultant force is described by Equation (23), with the adjustment of $F_{att}^{\prime }$ defined by Equation (24). The threshold $\mu$ corresponds to the cosine of the boundary angle, and $P_a$ represents the projection factor given by Equation (25). When $\lambda$ surpasses this boundary, the repulsive component $F_{\mathrm{rep}2}$ is discarded using $P_a$, retaining only the orthogonal component $F_{\mathrm{rep}1}$. By dynamically adjusting the force field, this method enhances avoidance efficiency, accelerates the robot’s speed toward the target, and significantly improves overall motion efficiency. Concurrently, the adjusted $F_{att}\prime$ ensures that resultant forces remain within the safe zone, preventing deviation from the predetermined path or entrapment in a local optimal state due to inappropriate forces.

4.2.2. Drive Critical Path Points

The optimized path obtained through the path reconnection mechanism exhibits reduced cost but is in close proximity to obstacles, posing safety risks. To mitigate this, the reconnected path is first discretized to yield a sequence of discrete path points ($q_1$ to $q_n$ in Figure 7). Then, the improved APF method is used to push the critical path points that are less than the safety radius (0.2 in this paper) from the nearest obstacle; the process of generating new path points is shown in Figure 7a, where the point $q_i$ is subjected to a gravitational force from the subsequent path point $q_{i+1}$ and a repulsive force from all obstacles within a distance of 0.2. The paths before and after the push are shown, for example, in Figure 7b. As you can see, the security of the new path has been greatly improved. The discretization method is direct, including dividing paths at certain intervals or inserting discrete points according to certain criteria. This segmentation allows the improved APF to be applied in a controlled manner, ensuring that each path point is evaluated and adjusted independently. The improved APF effectively keeps path points away from obstacles, resulting in safer navigation routes. In this paper, the number of differences between adjacent two points n is derived from Equation (27), where $q_i$ and $q_{i+1}$ are a pair of the adjacent points of the path, and $d_s$ is the distance between adjacent points.

$$n=\left\{\begin{array}{c}floor\left(1.5\times ⌊{\displaystyle \frac{d}{0.4}}⌋+1\right),d_s>0.4\hfill \\ 1,d_s\le 0.4\hfill \end{array}\right.$$

(26)

$$d_s=q_i-q_{i+1}$$

(27)

In Figure 7, the pushing distance $l_{mov}$ is determined by the magnitude of the total force acting on $q_i$. The maximum force received is $F_{max}$, and Equation (28) defines the pushing distance $l_{mov}$.

$$l_{mov}=\left\{\begin{array}{c}{\displaystyle \frac{l}{F_{max}}\times F_{total},l_{mov}\le \frac{d}{2}}\hfill \\ {\displaystyle \frac{d}{2},l_{mov}>\frac{d}{2}}\hfill \end{array}\right.$$

(28)

In Equation (28), l is the pushing step length, and d is the optimal safe distance between the path point and surrounding obstacles. When $2·l_{mov}$ is greater than d, then the pushing distance is corrected to $d/2$. Let $l_x$ and $l_y$ be the components of $l_{mov}$ on the x and y axes, respectively, and the co-ordinates of $q_{new}$ as ($x_{new},y_{new}$); then, the co-ordinate equation of $q_{new}$ can be obtained, as seen in Equation (29).

$$\left\{\begin{array}{c}{x}_{new}=x+l_x\hfill \\ y_{new}=y+l_y\hfill \end{array}\right.$$

(29)

5. Complexity Analysis

This section is dedicated to comparing the time and space complexities of the proposed DPF-QRRT* algorithm with those of the Q-RRT* algorithm, as shown in

Table 1. Comparison of sampling strategies for RRT variants.

Algorithm	Probabilistic Completeness	Asymptotic Optimality	Time Complexity	Space Complexity
RRT	Yes	No	$O(nlogn)$	$O\left(n\right)$
P-RRT*	Yes	Yes	$O(nlogn)$	$O\left(n\right)$
Q-RRT*	Yes	Yes	$O(nlogn)$	$O\left(n\right)$
DPF-QRRT*	Yes	Yes	$O(nlogn)$	$O\left(n\right)$

. The DPF-QRRT* algorithm incorporates three additional processes: P-Sample, APF-Steer, and Path-Reconnection, and all other processes remain consistent with Q-RRT*. Hence, the analysis in this paper focuses solely on the computational quantities associated with these added processes.

5.1. Space Complexity

Space complexity refers to the amount of memory space required when the algorithm is executed; here, only the Path-Reconnection process added by the DPF-QRRT* algorithm is analyzed. Let $S_{\text{Path-Reconnection}}\left(n\right)$ denote the additional space allocated by the DPF-QRRT* algorithm over n iterations for storing updated sets of vertices, V, and edges, E, along m feasible paths during the Path-Reconnection process.

Lemma 1. 

The space complexity for Path-Reconnection is

$$S_{\text{Path-Reconnection}}\left(n\right)=O(m·k).$$

Proof. 

Let $n_{point,j}$ denote the number of path points on the j-th path. The space required for each j-th path is given by $3·(n_{point,j}-2)$ new vertices and $4·(n_{point,j}-2)$ new edges. Therefore, the additional space complexity can be expressed as

$$\begin{array}{cc}\hfill S_{\text{Path-Reconnection}}\left(n\right)& =O\left(\sum _{j=1}^m(n_{point,,j}-2)·7\right).\hfill \end{array}$$

(30)

In path planning problems, the length of a path (and the number of viable path points) is limited by physical space or computing resources. It is reasonable to assume that there exists a constant k such that for all j, $(n_{point,j}-2)\le k$. We can conclude that

$$\begin{array}{cc}\hfill O\left(\sum _{j=1}m(n_{\mathrm{point},j}-2)·7\right)& \le O\left(\sum _{j=1}^m(k-2)·7\right)\hfill \end{array}$$

(31)

$$\begin{array}{cc}\hfill & =O\left(m·k·7\right)\hfill \end{array}$$

(32)

$$\begin{array}{cc}\hfill & =O\left(m·k\right).\hfill \end{array}$$

(33)

□

Lemma 2. 

The space complexity of the DPF-QRRT* algorithm is given by

$$S_{\text{DPF-QRRT*}}\left(n\right)=S_{\text{Q-RRT*}}\left(n\right)+S_{\text{Path-Reconnection}}\left(n\right)=O(nlogn).$$

Proof. 

The Q-RRT* algorithm maintains a tree $G_n=(V_n,E_n)$ with $|V_n| = n$ and $|E_n| = |V_n| - 1$. Thus, its space complexity can be expressed as follows:

$$\begin{array}{c}\hfill S_{\text{Q-RRT*}}\left(n\right)=O\left(\right|V_n| + |E_n\left|\right)=O(2n-1)=O\left(n\right).\end{array}$$

(34)

Assuming that Lemma 1 holds, in the worst case, when $m=n$, the total space complexity of the DPF-QRRT* algorithm can be expressed as

$$\begin{array}{c}\hfill S_{\text{DPF-QRRT*}}\left(n\right)=O\left(n+n·k\right).\end{array}$$

(35)

Thus, we can conclude that the total space complexity of the DPF-QRRT* algorithm is dominated by the $O\left(n\right)$ term. Therefore, we have

$$\begin{array}{c}\hfill S_{\text{DPF-QRRT*}}\left(n\right)=O\left(n\right).\end{array}$$

(36)

□

5.2. Time Complexity

In the work of Jeong et al. [21], the overall time complexity of the Q-RRT* algorithm has been verified; that is, $T_{\text{Q-RRT*}}\left(n\right)=O(nlogn)$, so only the incremental part of the DPF-QRRT* algorithm is calculated in this section.

5.2.1. P-Sample and APF-Steer Processes

The functions P-Sample and APF-Steer are executed in a fixed number of steps, independent of the number of nodes in the tree structure. Therefore, their time complexity primarily relies on the NearestObs process. The nearest neighbor search is a well-established problem with various algorithms available in the literature. Arya et al. [37] established that the lower bound for algorithmic complexity in nearest neighbor searches is $O(nlogn)$, and they developed an optimal computational algorithm that can identify the nearest obstacle point for a given query point and set of obstacle points within $O(nlogn)$ time. Thus, the time complexity for both processes can be expressed as

$$\begin{array}{c}\hfill T_{\text{P-Sample}}\left(n\right)=T_{\text{APF-Steer}}\left(n\right)=O(logn).\end{array}$$

(37)

5.2.2. Path-Reconnection Process

Lemma 3. 

The time complexity for Path-Reconnection is

$$T_{\text{Path-Reconnection}}\left(n\right)=O\left(n\right).$$

Proof. 

$$\begin{array}{c}\hfill T_{\text{Path-Reconnection}}\left(n\right)=O\left(\sum _{j=1}^m(n_{point,j}-2)·k_n·log{n}_{filtered}\right).\end{array}$$

(38)

where m is the number of calls to Path-Reconnection, $(n_{point,j}-2)$ is the number of path points of the j-th path, and $k_n$ is the number of times CollisionFree is called in GetNewPoint. $n_{filtered}$ is the number of obstacles screened by the Screening-Obs function.

Assuming that Lemma 1 holds, there exists a constant $k\in R$ such that for all j, $(n_{point,j}-2)\le k$. Furthermore, since $k_n$ and $n_{filtered}$ are both bounded by some constant value (in the worst-case scenario, where m is equal to n, this indicates that every iteration has the potential to be within the neighborhood of the target point), the complexity can be simplified as follows:

$$\begin{array}{cc}\hfill T_{\text{Path-Reconnection}}\left(n\right)& =\left(n·k·k_n·log{n}_{filtered}\right)\hfill \end{array}$$

(39)

$$\begin{array}{cc}\hfill & =O\left(n\right).\hfill \end{array}$$

(40)

□

5.2.3. DPF-QRRT*

Lemma 4. 

$$T_{\text{DPF-QRRT*}}\left(n\right)=T_{\text{Q-RRT*}}\left(n\right)+T_{\text{P-Sample}}\left(n\right)+T_{\text{APF-Steer}}\left(n\right)+T_{\text{Path-Reconnection}}\left(n\right)=O(nlogn).$$

Proof. 

According to Lemma 3, the total time complexity of DPF-QRRT* can be summarized as follows:

$$\begin{array}{c}\hfill T_{\text{DPF-QRRT*}}\left(n\right)=O\left(nlogn+logn+logn+n\right).\end{array}$$

(41)

Hence,

$$\begin{array}{c}\hfill T_{\text{DPF-QRRT*}}\left(n\right)=O(nlogn).\end{array}$$

(42)

□

6. Simulation Results and Analysis

A numerical simulation comparison experiment between the proposed strategy and RRT*, P-RRT*, Q-RRT*, and DPF-QRRT* was conducted. The simulations were implemented on an Intel(R) Core(TM) i5-12400 CPU with 32 GB of RAM. The simulation platform was Matlab R2022a.

6.1. Environment Setting and Parameter Design

To intuitively evaluate algorithm performance, this study used three environments in the simulation: sparse Environment A, dense Environment B, and maze Environment C, all sized $120\times 120$. Environment A can be seen in Figure 8a. Representing a simple environment, it consists of three circles forming an obstacle with a radius of 0.8. Environment B in Figure 8b is a complex environment with many circular obstacles of different sizes filling the entire space. Environment C is shown in Figure 8c and is a special environment consisting of a series of regularly arranged circles with a radius of 0.25, forming a labyrinth path of turns.

The simulation parameters include step, $\lambda$, $d_{{obs}^{*}}$, k, and depth, d. All algorithms used uniform parameters, where $step=0.5$, $\lambda =d_{obs}=0.1$, $k=10$. The parameter k represents the maximum number of times the random sampling point moves towards the target point during the P-Sample process. The critical distance $d_{obs}$ represents a small deviation from the obstacle, and $\lambda$ represents a small increment when moving toward the target. During the ChooseParent and Rewire processes, the depth parameter d was set to 2. Additionally, $d_{{obs}^{*}}$ was set to 0.3, which represents the threshold of the nearest obstacle in the APF-Steer process. The parameters $k_{\alpha }$ and $k_{\rho }$ were set to 0.3 and 0.1, respectively. Moreover, ${\rho }_0$ was set to 0.2.

6.2. Performance Metrics

This study evaluates five path planning algorithms through 100 iterative experiments, focusing on three key performance metrics: the initial planned path length ($l_{\mathrm{init}}$), the time required to reach a solution that is $1.05\times$ the optimal solution ($T_{1.05}$), and the shortest distance between a path and an obstacle ($D_{\min }$). The optimal solution is derived from the RRT* algorithm with infinite iterations. Failure is defined as an algorithm’s inability to find a near-optimal solution within 100 s.

6.2.1. Initial Planned Path Length

The initial planned path length $l_{\mathrm{init}}$ serves as a primary indicator of the algorithm’s initial efficiency, reflecting its performance in the non-optimized stage. A shorter $l_{\mathrm{init}}$ indicates a higher efficiency and greater optimization potential, particularly in simpler environments where the algorithms can quickly approach optimal paths. Conversely, in more complex environments, $l_{\mathrm{init}}$ highlights how well the algorithms handle intricate obstacle layouts.

6.2.2. Time to Reach 1.05 Times Optimal Solution

The metric $T_{1.05}$ assesses the responsiveness and practicality of the algorithms in real-world scenarios, where a shorter time signifies the algorithm’s ability to rapidly adapt to environmental changes and provide high-quality solutions.

6.2.3. Safety Index

Additionally, a safety index $D_{\min }$ is introduced to evaluate the safety of the generated paths, measuring the shortest distance between the path and obstacles when the path is $1.05$ times the optimal path. A larger $D_{\min }$ indicates a safer path, reducing collision risks.

6.3. Environment A

Figure 9 compares the paths generated by five algorithms under equivalent iteration in Environment A. RRT* (Figure 9a) generates a path with extended length and numerous turning points, indicating poor quality. However, P-RRT*, which is guided by the potential field for random point sampling (Figure 9b), demonstrates certain optimization effects. In comparison, Q-RRT* (Figure 9c) has fewer path points and a straighter trajectory, resulting in better path quality. Although DPF-QRRT* has the shortest path, it is too close to obstacles, necessitating enhanced safety, as shown in Figure 9d. By applying the IAPF method at key points, the IDAPF-QRRT* strategy effectively improves obstacle avoidance efficiency and ensures the safety of the path, as demonstrated in Figure 9e.

Figure 10a,b present the data analysis results for $l_{init}$ and $T_{1.05}$. In the boxplots, R denotes RRT*, DPF-Q denotes DPF-QRRT*, and IDF-Q denotes IDAPF-QRRT*; it can be observed that the DPF-QRRT* and IDAPF-QRRT* strategies are similar in generating initial paths and in the time taken to achieve 1.05 times the optimal solution, and they significantly outperform the other algorithms, demonstrating a clear advantage in search performance of the improved strategies.

Table 2. Experimental data of five algorithms in Environment A (Std represents the standard deviation).

Algorithm	Performance	Mean	Std	Fail
RRT*	$l_{init}$/km	13.9723	1.2563	0
	$T_{1.05}$/s	4.7288	3.4203
P-RRT*	$l_{init}$/km	13.4749	1.1417	0
	$T_{1.05}$/s	2.2792	1.9855
Q-RRT*	$l_{init}$/km	11.7993	0.4341	0
	$T_{1.05}$/s	1.4651	1.0389
DPF-QRRT*	$l_{init}$/km	11.5130	0.1755	0
	$T_{1.05}$/s	0.2974	0.1711
IDAPF-QRRT*	$l_{init}$/km	11.5862	0.1805	0
	$T_{1.05}$/s	0.3963	0.1634

further compares the performance indicators of each algorithm, showing that the DPF-QRRT* algorithm reduces the initial path length by 17.6% over the RRT* algorithm and saves 4.4314 s in finding 1.05 times the optimal solution. Notably, the DPF-QRRT* algorithm has a lower standard deviation (0.0036 and 0.0961), which is much better than the P-RRT* (1.1417, 1.9855) and QRRT* (0.4341, 1.0389) algorithms, further proving the stability of its performance. Although the IDAPF-QRRT algorithm is slightly higher in initial path length than the DPF-QRRT algorithm, the average values of its two indicators (11.5106, 0.4252 s) are still significantly better than the RRT* (13.9723, 4.7288 s) and P-RRT* (13.4749, 2.2792 s) algorithms. Figure 10c shows the average change in path length over computation time for different algorithms, where “$1.05·$Coptimal” represents 1.05 times the optimal path cost. The comparative analysis shows that DPF-QRRT* has effectively optimized upon QRRT*, significantly shortening the initial path length in sparse environments. The IDAPF-QRRT* strategy, combining the improved APF algorithm, keeps key path points away from obstacles. Although the path length exceeds that achieved by the DPF-QRRT* algorithm, its comprehensive performance remains notably superior to the other three algorithms.

6.4. Environment B

Compared to the open spaces in Environment A, Environment B has denser and more complex obstacles. Figure 11 shows the simulation results of each algorithm at 1.05 times the optimal path length. The RRT* algorithm’s random sampling leads to a blind search, requiring many iterations and generating redundant nodes due to a lack of effective path guidance, as in Figure 11a. The P-RRT* algorithm has some directionality in random point generation but is less efficient in considering local obstacles and generating effective new nodes. Increased obstacle density limits the performance of Q-RRT*, reducing direct connection possibility and weakening its advantage in Environment B, as in Figure 11c. The DPF-QRRT* algorithm considers obstacle repulsion in new node generation, improving efficiency and obstacle avoidance. Its path reconnection mechanism based on triangular inequality optimizes the path near the target and accelerates, reaching 1.05 times the optimal path, with better quality and fewer iterations, as in Figure 11d. The IDAPF-QRRT* algorithm builds on the advantages of DPF-QRRT* and uses improved APF to address safety defects. As shown in Figure 11e, it maintains a safe distance from obstacles with a slight increase in path cost and achieves a balance between quality and safety due to orthogonal repulsion.

Table 3. Experimental data of five algorithms in Environment B.

Algorithm	Performance	Mean	Std	Fail
RRT*	$l_{init}$/km	13.8688	1.2162	3
	$T_{1.05}$/s	7.0452	3.3117
P-RRT*	$l_{init}$/km	13.4814	1.2907	0
	$T_{1.05}$/s	6.5177	4.0513
Q-RRT*	$l_{init}$/km	12.8723	1.1994	0
	$T_{1.05}$/s	4.2432	3.9054
DPF-QRRT*	$l_{init}$/km	12.1453	0.5249	0
	$T_{1.05}$/s	0.7585	0.6872
IDAPF-QRRT*	$l_{init}$/km	12.5741	0.6047	0
	$T_{1.05}$/s	\	\

and Figure 12 present the experimental data and performance comparison of five algorithms. According to the analysis in Table 3, RRT* has the longest average initial path length of 13.8688, with a standard deviation of 1.2162, indicating poor stability. P-RRT*, guided by a potential field, improves path length yet remains longer than Q-RRT* and the enhanced algorithm, indicating limited optimization.The average initial path length of Q-RRT* is 12.8723, which is relatively short. However, due to the increase in obstacle density, its advantage is not obvious in Environment B. The DPF-QRRT* and IDAPF-QRRT* algorithms significantly reduce the initial path length to 12.1453 and 12.5741, respectively, and the standard deviations are 0.5249 and 0.6047, respectively, showing high stability and consistency. This is attributed to their effective use of obstacle repulsion and path reconnection mechanisms in the process of generating new nodes.

RRT* has the longest average time to obtain a 1.05 times optimal solution, which is 7.0452 s. Although P-RRT* (6.5177 s) and Q-RRT* (4.2432 s) have improved, the effect is not obvious. However, DPF-QRRT*, with an average time of 0.7585 s, is significantly better than other algorithms, shortening by 82.12% compared to Q-RRT*, indicating that it can quickly converge to a feasible solution in path planning. This is especially important for complex environments because, in practical applications, the calculation time directly affects the real-time performance and practicability of the algorithm. The box plots in Figure 12a,b further reveal the differences in search performance among the five algorithms. It is noteworthy that as the area and quantity of obstacles grow, the performance of the P-RRT* and Q-RRT* algorithms is markedly influenced. In comparison with other algorithms, a longer time is taken to find the initial solution, which is readily observable in Figure 12c. However, due to the increased path cost driven by the IAPF method, the IDAPF-QRRT* strategy cannot effectively approach 1.05 times the optimal solution in Environment B, which is confirmed in Figure 12b,c. Therefore, in the performance comparison of 1.05 times the optimal solution in Environments B and C, the IDAPF-QRRT* strategy is not considered.

6.5. Environment C

In the maze of Environment C, where the arrangement of obstacles is relatively simple, Q-RRT* significantly improves the efficiency of path acquisition through optimized parent node selection and the Rewire operation. Figure 13 illustrates the initial trajectories formulated by various algorithms, where Q-RRT* outperforms RRT* and P-RRT* in terms of iteration numbers and a reduction in redundant nodes.

Compared to the more complex Environment B, the maze environment’s obstacle layout poses a major challenge to the algorithm’s search performance. P-RRT*, due to gravitational field guidance on random sampling points, fails to consider local obstacles adequately, significantly affecting its adaptability in complex maze structures. When analyzing Figure 13b, the overall sampling points shift towards the target. New node generation from adjacent nodes to sampling points without considering local obstacles further compresses the feasible area on the left, weakening the search performance of P-RRT*, as it has more iterations than RRT*. In new node generation, DPF-QRRT* considers adjacent obstacle repulsion. In the maze scenario, it uses obstacle repulsion to push new nodes to explore the feasible area, enhancing local obstacle avoidance ability and effectively avoiding feasible area reduction. Similar to Q-RRT*, the path reconnection mechanism of DPF-QRRT* based on triangular inequality is effective in regularly arranged maze environments but has the safety hazard of the paths being too close to obstacles (Figure 13d). When analyzing Figure 13e, we can see that when the path-turning amplitude is large, the angle between gravitational and repulsive directions for path points may be large. Traditional APF may cause circuitous paths. The improved repulsive force of APF is orthogonal to the gravitational force, effectively pushing path points away from obstacles and avoiding collisions. The gravitational force stably guides path points towards the target. This orthogonal relationship makes path planning more stable and efficient, ensuring path safety and improving obstacle avoidance efficiency, especially in mazes with obvious path turns.

An analysis of

Table 4. Experimental data of five algorithms in Environment C.

Algorithm	Performance	Mean	Std	Fail
RRT*	$l_{init}$/km	31.9327	3.4368	33
	$T_{1.05}$/s	37.4376	16.9598
P-RRT*	$l_{init}$/km	30.2882	2.7135	14
	$T_{1.05}$/s	33.1918	13.2881
Q-RRT*	$l_{init}$/km	29.0576	2.6475	5
	$T_{1.05}$/s	10.5849	6.6176
DPF-QRRT*	$l_{init}$/km	26.8931	1.5398	0
	$T_{1.05}$/s	6.5983	3.6341
IDAPF-QRRT*	$l_{init}$/km	27.9356	1.5163	0
	$T_{1.05}$/s	\	\

and Figure 14 reveals that the RRT* algorithm has the longest average initial path length of 31.9327 and a standard deviation of 3.4368, indicating poor stability. P-RRT* reduces path length to 30.2882 but still has room for optimization with a standard deviation of 2.7135. Q-RRT* further shortens it to 29.0576, with a standard deviation of 2.6475. In contrast, DPF-QRRT* and IDAPF-QRRT* have averages of 26.8931 and 27.9356, respectively, and low standard deviations of 1.5398 and 1.5163, indicating high stability in path length generation. Figure 14c clearly presents the calculation cost curve of the five algorithms with respect to time.Regarding the time taken to obtain 1.05 times the optimal solution, RRT* has the longest average time of 37.4376 s and a high standard deviation of 16.9598. P-RRT* takes 33.1918 s with a standard deviation of 13.2881. Q-RRT* has an average time of 10.5849 s and a standard deviation of 6.6176. DPF-QRRT* has an average time of 6.5983 s, being significantly shorter. RRT* and P-RRT* failed 33 and 14 times, respectively, while Q-RRT* failed five times. The number of failures for both DPF-QRRT* and IDAPF-QRRT* is 0. In conclusion, DPF-QRRT* and IDAPF-QRRT* stand out in Environment C. They have shorter path lengths and lower failure rates. DPF-QRRT* also has a shorter time to obtain a feasible solution. Compared to RRT*, P-RRT*, and Q-RRT*, these two algorithms offer better performance in terms of stability and reliability.

6.6. Experiment of Path Safety

To thoroughly examine the influence of the enhanced strategy on path safety, a safety metric $D_{min}$ was established, measuring the shortest distance between the generated path and obstacles for each algorithm. Multiple experiments were conducted to obtain $D_{min}$ data under the condition that each algorithm achieves 1.05 times the optimal path.

Table 5. Comparison of the security data of four algorithms.

Environment	Algorithm	Mean of ${\mathit{D}}_{min}$	Std of ${\mathit{D}}_{min}$
Environment A	RRT*	0.1387	0.0557
	P-RRT*	0.1296	0.0461
	Q-RRT*	0.1353	0.0532
	DPF-QRRT*	0.0847	0.0268
	IDAPF-QRRT*	0.2655	0.0309
Environment B	RRT*	0.0325	0.0271
	P-RRT*	0.0319	0.0398
	Q-RRT*	0.0305	0.0258
	DPF-QRRT*	0.0239	0.0194
	IDAPF-QRRT*	0.2378	0.0269
Environment C	RRT*	0.0672	0.0427
	P-RRT*	0.0631	0.0413
	Q-RRT*	0.0573	0.0386
	DPF-QRRT*	0.0224	0.0312
	IDAPF-QRRT*	0.3513	0.0237

examines the path safety of five different algorithms that achieve 1.05 times the optimal solution in three different environments. By taking the more complex Environment B as an example, the IDAPF-QRRT* strategy has the highest average $D_{min}$ value of 0.2655, which is significantly higher than the others. The average $D_{min}$ values of RRT*, P-RRT*, and Q-RRT* are slightly higher than that of the DPF-QRRT* algorithm, but there is still a gap in path safety compared to IDAPF-QRRT*. Moreover, by analyzing the standard deviation of $D_{min}$, it was found that in Environments A and C, IDAPF-QRRT* not only maintained the highest average $D_{min}$ values but also had relatively lower standard deviations (0.0309 and 0.0237), proving that the IDAPF-QRRT* strategy not only ensures path safety but also has good algorithm performance and stability. When analyzing Figure 15 for both Environment A and Environment B, the IDAPF-QRRT* strategy consistently exhibits a significantly higher average $D_{min}$ value at each iteration stage than the other algorithms, and the safe distance is stable at about 0.2. Especially in the complex Environment B, the safety distances of the remaining four algorithms, as depicted in Figure 15b, fall below 0.05, potentially posing a significant hazard to path safety. These results consistently indicate that the IDAPF-QRRT* strategy demonstrates a clear advantage in terms of path safety, with higher average $D_{min}$ values and lower standard deviations, showcasing excellent performance and stability in ensuring path safety.

6.7. Experiment with Different Step Sizes

To better demonstrate the adaptability and reliability of the algorithms, simulation experiments were conducted under three step sizes: 0.2, 0.5, and 1. The specific results are shown in

Table 6. Experimental data of five algorithms for different step sizes (0.2, 0.5, and 1).

Algorithm	Performance	0.2	0.5	1
RRT*	$l_{init}$/km	13.5683	14.0201	13.8443
	$T_{1.05}$/s	9.6987	7.1323	10.9084
P-RRT*	$l_{init}$/km	13.2741	13.5603	13.5164
	$T_{1.05}$/s	8.9653	6.4352	8.6797
Q-RRT*	$l_{init}$/km	12.4502	12.7760	13.0496
	$T_{1.05}$/s	6.0241	4.3593	9.3944
DPF-QRRT*	$l_{init}$/km	11.9635	12.1210	12.3261
	$T_{1.05}$/s	0.8264	0.7568	0.7748
IDAPF-QRRT*	$l_{init}$/km	12.3764	12.5463	12.6844
	$T_{1.05}$/s	\	\	\

and Figure 15. Due to the security conflict with the $T_{1.05}$ index, the IDAPF-QRRT* algorithm did not participate in the related experiment.

From the detailed analysis of Table 6, it can be found that DPF-QRRT* has the shortest initial path for nonsynchronous length. Compared with RRT*, it reduces by at least 10.96% (when the step size is 1). The initial path length of Q-RRT* is better, but it is still at least 4.5% longer than that of DPF-QRRT*. IDAPF-QRRT* sacrifices some advantages in path length and obtains a safer path. The initial path (12.3764, 12.5463, and 12.6844) is slightly longer than that of DPF-QRRT%, but it is still significantly better than the RRT* and P-RRT* algorithms. When the step size is 0.5, the initial solution generated by IDAPF-QRRT* is, on average, 10.52% closer to the optimal value than RRT* and 7.48% closer than P-RRT*. In terms of time efficiency, DPF-QRRT* has an extremely short time. At step sizes of 0.2, 0.5, and 1, DPF-QRRT* takes only 0.8264 s, 0.7568 s, and 0.7748 s to obtain 1.05 times the optimal solution, which is at least 91.47%, 89.39%, and 92.89% less time than RRT*, respectively. Compared to Q-RRT*, DPF-QRRT* takes at least 86.28%, 82.64%, and 91.07% less time.

From Figure 15, in complex obstacle environments, the initial paths of RRT* and P-RRT* are relatively long and influenced by the step size. However, Q-RRT* and DPF-QRRT* exhibit an advantage in terms of shorter path lengths, particularly DPF-QRRT*, which achieves optimal path length performance with outstanding stability. IDAPF-QRRT* ensures path safety while maintaining the advantage in path length. Regarding the time spent to obtain 1.05 times the optimal path, RRT* and P-RRT* are less efficient and significantly affected by changes in the step size. Although Q-RRT* shows some improvement, it is still not satisfactory. DPF-QRRT* rapidly shortens the path length by virtue of its path reconnection mechanism, which shows the high efficiency of its optimized path. Further analysis of Figure 16d–f indicates that under different indexes, the stability of DPF-QRRT* and IDAPF-QRRT* is minimally impacted by changes in step size, and both algorithms outperform others. In particular, when combining Figure 16d,e, it is evident that when step size is 0.2, although the initial path length of Q-RRT* is close to RRT* and P-RRT*, its standard deviation is much higher, indicating poor algorithm stability. The impact of step size on Q-RRT* performance is significant. DPF-QRRT* has a smaller standard deviation and shows optimal stability under the $l_{init}$ index.

As shown in Figure 16b, the performance of P-RRT* and Q-RRT* is significantly affected by step size changes from 0.2 to 1. At a step size of 0.2, P-RRT* and Q-RRT* take as much or even more time than RRT* for the initial path. Similarly, the times taken by DPF-QRRT* and IDAPF-QRRT* to find the initial path also fluctuate significantly. This indicates that a fixed step size has obvious limitations on the algorithm’s performance in obtaining the initial path. Generally, different step sizes affect the algorithm’s exploration mode in the search space. A smaller step size may lead to a more detailed search but increase the search time. While a larger step size can cover the search space more quickly, it may miss some better path choices, resulting in time fluctuations. In complex environments with obstacles and narrow passages, step size selection significantly influences the time to find the initial path. For example, in a narrow passage, a smaller step size may be more likely to find a feasible path but take longer; in contrast, a larger step size may quickly cross a wider area in some cases but may require more adjustments at a narrow passage, leading to time fluctuations.

6.8. Three-Dimensional Environment Simulation Results

After the in-depth study of the performance of five algorithms in 2D simulation environments, the perspective was expanded to a more complex 3D simulation environment. The 3D environment introduces more dimensions and degrees of freedom, requiring the algorithms to consider more spatial factors and potential collision risks when generating paths. This involves higher demands in handling obstacles, the spatial layout, and path safety. Next, this paper will analyze the path planning results of five algorithms in a 3D simulation environment in detail. The objective is to elucidate their respective advantages and limitations in 3D space path planning, thus offering accurate and reliable guidance for practical applications. In the 2D simulation, $x_{rand}$ is obtained through random sampling. To enhance the convergence rate in the 3D environment simulation, a goal-biased sampling strategy was adopted, utilizing Equation (43) for sampling point selection.

$$x_{rand}=\left\{\begin{array}{cc}\mathrm{Random},\hfill & rand\left(\right)\ge \eta \hfill \\ x_{goal},\hfill & \mathrm{else}\hfill \end{array}\right.$$

(43)

Here, $rand\left(\right)$ generates a uniform random number between 0 and 1, with $\eta$ being a pre-determined threshold that determines the direction of expansion of new nodes in the random tree. If $rand\left(\right)\ge \eta$, expansion occurs randomly within the sampling space; if $rand\left(\right)<\eta$, expansion moves towards a specified target position.

Figure 17 presents a comparison of path planning performance in a 3D sparse environment using five different algorithms. As depicted in Figure 17a, RRT* exhibits high redundancy in nodes and inflection points in 3D space, compromising path quality and enhancing instability and uncertainty. In contrast, the Q-RRT algorithm (Figure 17c) significantly shortens the path length by reducing redundant nodes and optimizing trajectory straightness and efficiency. By integrating the improved APF method for path point optimization, the IDAPF-QRRT* strategy achieves high-quality 3D obstacle-avoidance paths, thereby significantly improving the robot’s performance and safety in complex environments, as is evident in Figure 17e.

To further verify the enhanced approach’s efficacy in the 3D environment, the simulation environment was set to include scenes with dense obstacles. Figure 18 shows the results of five algorithms obtaining 1.05 times the shortest path. In Figure 18b, P-RRT* exhibits directionality during exploration, but the efficient generation of new nodes is affected by obstacles, reducing efficiency. In contrast, DPF-QRRT* highlights its advantages in efficient node generation and obstacle avoidance efficiency, meeting path requirements more quickly. Moreover, DPF-QRRT* does not rely on nodes to optimize the path, making it more flexible and efficient in handling high-dimensional complex obstacles with the least number of iterations required, as shown in Figure 18d. The IDAPF-QRRT* strategy generates path lengths comparable to DPF-QRRT* while ensuring greater safety, rendering it a favorable choice for 3D path planning scenarios. These findings provide further validation of the superiority and efficacy of this approach in the realm of 3D path planning.

7. Conclusions

This paper presents an IDAPF-QRRT* strategy aimed at achieving efficiency and safety in mobile robot path planning. Initially, the DPF-QRRT* algorithm rapidly generates an initial path, overcoming the limitations of traditional algorithms in terms of convergence speed and node redundancy. Subsequently, triangle inequality is utilized for path reconnection optimization to shorten the path length. Finally, an improved APF method is adopted to adjust key discrete path points, enhancing path safety. The numerical simulation results indicate that the DPF-QRRT* algorithm reduces the time to achieve 1.05 times the optimal path by at least 82.1% and shortens the initial path length by 5.8% in Environment B. The IDAPF-QRRT* strategy maintains a similar search performance as DPF-QRRT*, while its path safety is at least twice as high as that of other algorithms. At the same time, it demonstrates good performance in various simulation environments, showing excellent environmental adaptability. Although the IDAPF-QRRT* strategy is a promising algorithm, the setting of parameters leads to performance limitations. Therefore, future work will consider improving the strategy in terms of adaptive parameters and step sizes to further improve performance.