PFMS-RRT*: A Progress-Aware Fused-Sampling RRT* with Multi-Level Strategy Extension for Path Planning

Abstract

Sampling-based planners such as RRT* are attractive for robot navigation in complex spaces, but they often suffer from high randomness, low efficiency, slow convergence, and suboptimal path quality in cluttered environments. To address these limitations, this paper proposes PFMS-RRT*, a progress-aware fused-sampling RRT* with a multi-level strategy extension. The method builds on a bidirectional RRT* framework and introduces three main components: (i) a progress-aware fused sampling scheme that adapts an oriented elliptical sampling region based on inter-tree progress and stagnation, mixes locally guided elliptical samples with globally explorative Halton-sequence samples, and dynamically balances exploration and exploitation; (ii) a three-level goal-guided extension mechanism that escalates from direct steering to local probing and then multi-direction detours to maintain forward progress when obstacles block expansion; and (iii) a smooth tangential artificial potential field (APF) extension used as a fallback, with a failure-driven probabilistic switching rule that increases APF usage after repeated extension failures. Simulations in four representative 2D environments (sparse, corridor-like dense, random dense, and narrow passage) show that PFMS-RRT* consistently yields shorter paths, lower and more stable runtime, and fewer nodes than several RRT* variants while maintaining competitive or improved obstacle clearance.

1. Introduction

With the advancement of unmanned technology, mobile robots have attracted widespread attention in the fields of field exploration [1], disaster relief [2], and agricultural production [3]. Path planning aims to generate a collision-free path from the start to the goal, and it is one of the core problems in mobile robotics [4].

Based on their characteristics, path planning algorithms can be categorized into three major types. The first type includes traditional planning algorithms, such as graph-search algorithms [5] and sampling-based algorithms [6]. The second type consists of swarm intelligence optimization algorithms, such as particle swarm optimization (PSO) [7] and genetic algorithms (GA) [8]. The third type comprises learning-based methods, such as reinforcement learning [9].

Due to their outstanding performance in high-dimensional and complex environments, sampling-based algorithms have attracted considerable attention from researchers. Rapidly-exploring Random Tree (RRT) [10] is a classical representative of sampling-based methods. It features a simple and efficient structure as well as probabilistic completeness. However, because it grows through random iterations in the search space and does not explicitly consider path cost or quality, the resulting paths are often complex and of poor quality. RRT* [11] is an extension of RRT that incorporates parent selection and node rewiring strategies, significantly improving path quality. Nevertheless, RRT* still suffers from slow planning speed and long convergence time. To further accelerate the convergence of RRT*, many improved variants have been proposed.

Some improvement approaches focus on optimizing the sampling stage of RRT*, thereby improving algorithmic efficiency. Informed RRT* adopts a non-uniform sampling strategy [12] that reduces search blindness by restricting the sampling region and decreasing redundant samples. SOF-RRT* introduces a spatial probability-weighted sampling strategy [13], assigning higher sampling probability to regions with larger feasible areas. This reduces redundant sampling and improves the effective sampling rate. F-RRT* algorithm optimizes the path cost by creating an exclusive parent node for the random sampling point, rather than selecting a parent node from the existing set of vertices [14]. NRRT* [15] and NIRRT* [16] use neural networks to predict sampling points and perform biased sampling concentrated around these points. Although these algorithms enhance the sampling stage, they lack improvements to the extension stage, which limits their performance in complex environments.

To further improve performance, many algorithms refine RRT* at the extension stage. BI-RRT* adopts a bidirectional exploration mechanism [17], constructing search trees from the start and the goal to improve planning efficiency. E-RRT* replaces line segments with ellipsoids when connecting adjacent nodes, improving performance in narrow environments [18]. FC-RRT* performs bidirectional extension using prior task information [19]. FN-RRT* constrains the tree size using a fixed-node-number control mechanism [20], thereby accelerating convergence. Although these RRT* variants significantly improve extension strategies, they pay insufficient attention to sampling and do not introduce targeted sampling improvements, leading to suboptimal planning performance in scenarios with complex obstacle distributions.

There are also approaches that integrate other techniques and algorithms. IA-RRT* modifies the cost evaluation function of the A* algorithm [21], providing a relatively weak guidance direction for the search. This narrows the search region and prevents the algorithm from converging too quickly and becoming trapped in a locally optimal path. GB-RRT* combines a grid-based search strategy and proposes a minimum-dose path planning scheme suitable for complex environments with obstacles and narrow passages [22]. VPS-APF-RRT* incorporates an improved artificial potential field (APF), further enhancing the algorithm’s obstacle-avoidance capability [23].

To address the issues of high randomness, low efficiency, long search time, and poor path quality of RRT* in complex environments, a Progress-aware Fused Sampling and multi-level strategy Extension RRT* algorithm, namely PFMS-RRT*, is proposed. The main improvements of the proposed algorithm are summarized as follows:

(1)

A progress-aware fused sampling strategy that adaptively reshapes the sampling distribution according to planning progress, reducing redundant samples and improving the probability of generating informative samples.

(2)

A three-level goal-guided extension mechanism with progressively stronger fallback behaviors to improve extension success rate and search stability in cluttered environments.

(3)

A smooth tangential APF-based extension method as a fallback when goal-guided extension is ineffective, which alleviates stagnation near obstacles, reduces invalid expansions, and enhances extension robustness in cluttered environments.

2. Related Works

This section provides a comprehensive overview of the RRT* algorithm, which serves as the foundation for all algorithms proposed in this paper. Subsequently, an in-depth exploration of the Bi-RRT*, APF-RRT*, and Informed RRT* algorithms will be conducted to lay the groundwork for introducing the PFMS-RRT* algorithm.

2.1. RRT*

RRT* is an improved sampling-based motion planning algorithm derived from the classic Rapidly-exploring Random Tree (RRT), designed for path planning in high-dimensional environments with complex obstacles. Unlike RRT, which aims to quickly find a feasible path, RRT* preserves the global exploration capability brought by random sampling while introducing rewiring and choose parent mechanisms. As a result, the generated path is continuously optimized over iterations and enjoys an important property: asymptotic optimality—that is, as the number of samples approaches infinity, RRT* converges to the optimal path with probability 1.

The overall procedure of the RRT* algorithm is shown in Algorithm 1. To facilitate understanding, an explanation of the pseudocode is provided below:

• Sample: In each iteration, a random state $x_{\mathrm{rand}}$ is generated in the state space.
• Nearest: Given the sampled state $x_{\mathrm{rand}}$, the algorithm searches the current vertex set $V\left(T\right)$ and finds the closest vertex $x_{\mathrm{nearest}}$ according to a predefined distance metric.
• Steer: Starting from $x_{\mathrm{nearest}}$, the algorithm steers toward $x_{\mathrm{rand}}$ to generate a new candidate state $x_{\mathrm{new}}$. In practice, the extension is usually limited by a maximum step size $\eta$ to ensure incremental growth and stable collision checking.
• Free: The function Free checks whether the local path connecting $x_{\mathrm{nearest}}$ to $x_{\mathrm{new}}$ lies entirely in the obstacle-free region. If a collision is detected, the candidate is rejected and the algorithm proceeds to the next iteration.
• ChooseParent: After obtaining $x_{\mathrm{new}}$, the algorithm identifies a set of nearby vertices $X_{\mathrm{near}}=\mathrm{Near}(x_{\mathrm{new}},V\left(T\right))$ within a neighborhood radius. Among these candidates, ChooseParentselects the parent vertex $x_{\mathrm{parent}}\in X_{\mathrm{near}}$ that minimizes the total cost from the start to $x_{\mathrm{new}}$. If no better candidate exists, $x_{\mathrm{nearest}}$ is used as the parent.
• Rewire: Once $x_{\mathrm{new}}$ is added to the tree, Rewire attempts to improve the existing structure by reconnecting vertices in $X_{\mathrm{near}}$. For each $x\in X_{\mathrm{near}}$, if routing through $x_{\mathrm{new}}$ yields a lower cost and the edge $(x_{\mathrm{new}},x)$ is collision-free, then the parent of x is updated to $x_{\mathrm{new}}$.

Algorithm 1: RRT*

2.2. Bi-RRT*

Bi-RRT* generates two random trees from the start state and the goal state, respectively, and searches by expanding them toward each other. Owing to its bidirectional search mechanism, Bi-RRT* expands more efficiently than single-tree methods, thereby producing a feasible path more quickly. The pseudocode of this algorithm is shown in Algorithm 2.

Algorithm 2: Bi-RRT*

2.3. Informed-RRT*

Informed-RRT* improves upon RRT* by refining the sampling strategy: instead of sampling over the entire map, it focuses on an elliptical region defined by the start and goal states. This informed, targeted sampling substantially reduces the search space, thereby accelerating convergence toward the optimal solution and improving overall sampling efficiency. The detailed procedure is presented in Algorithm 3.

The specific implementation of the SampleInformed function in Algorithm 3 is given in Algorithm 4.

Algorithm 3: Informed-RRT*

Algorithm 4: Sample Informed

2.4. APF-RRT*

APF-RRT* is an improved variant of RRT* that incorporates the guiding idea of the Artificial Potential Field method. During the sampling or expansion stage, it introduces a goal-attractive force and an obstacle-repulsive force, causing the random tree to grow preferentially toward the goal while avoiding obstacles, thereby reducing blind exploration and ineffective sampling.

3. PFMS-RRT* Algorithm

As shown in Figure 1, PFMS-RRT* inherits the dual-tree expansion mechanism of Bi-RRT* and introduces three core improvements: (1) in the sampling phase, progress-aware fused sampling replaces random sampling to generate informative samples; (2) in the expansion phase, a three-level goal-guided extension mechanism is proposed to ensure robust tree growth of the algorithm in cluttered environments with obstacles; and (3) an improved smooth tangential APF is integrated as a fallback extension strategy when the goal-guided extension fails. In addition, a failure-driven probabilistic switching scheme adaptively increases the chance of invoking the APF extender when repeated extension failures are observed. The pseudocode of PFMS-RRT* is presented in Algorithm 5. Firstly, the start tree $T\_s$ and goal tree $T\_g$ are initialized. the progress metric $d_{\mathrm{prev}}$ is initialized to the Euclidean distance between the start state $x_{\mathrm{start}}$ and the goal state $x_{\mathrm{goal}}$, and the stagnation countersis reset to 0. Secondly, an informative sample x is generated via the progress-aware fused sampling strategy. Thirdly, the extension strategy switching probability $p_{\mathrm{sw}}^{\left(X\right)}$ driven by the failure metric is updated. According to $p_{\mathrm{sw}}^{\left(X\right)}$, the algorithm determines whether to skip the three-level goal-guided extension. If it is skipped, the smooth tangential APF is directly employed for extension. Otherwise, the three-level goal-guided extension is executed first, and the smooth tangential APF is used as a fallback strategy if the extension fails. The following sections provide a detailed introduction to each stage.

3.1. Progress-Aware Fused Sampling

Progress-aware fused sampling dynamically concentrates samples in regions that are more likely to reduce the separation between the two trees while preserving global exploration. At each iteration, the nearest inter-tree vertex pair $(x_s^{*},x_g^{*})$ is computed, and the current progress metric is defined as

$$d_k={\parallel x_s^{*}-x_g^{*}\parallel }_2.$$

(1)

where $T_s$ and $T_g$ denote the start and goal trees, respectively; $(x_s^{*},x_g^{*})$ is the nearest inter-tree vertex pair (one vertex from each tree); $x_s^{*}\in V\left(T_s\right)$ and $x_g^{*}\in V\left(T_g\right)$; ${\parallel ·\parallel }_2$ denotes the Euclidean norm; $d_k$ is the inter-tree separation (progress metric) at iteration k. If $d_k$ decreases compared with the previous value $d_{\mathrm{prev}}$, the planner is considered to make progress and the stagnation counter s is reset; otherwise, s is increased by one.

Figure 1. Overall diagram of PFMS-RRT*.

Figure 1. Overall diagram of PFMS-RRT*.

Algorithm 5: PFMS-RRT*$\left(x_{\mathrm{start}},x_{\mathrm{goal}},\mathcal{O},K_{max}\right)$

To adapt the sampling domain to the planning progress, as shown in Figure 2, an oriented elliptical region is constructed whose semi-axes $a_k$ and $b_k$ are scaled by $d_k$ and an additional stagnation amplification factor ${\gamma }_k$ (line 11–12 in Algorithm 6). When the stagnation counter exceeds a threshold $s_{\mathrm{th}}$, a stagnation level ${\mathit{\ell }}_k$ is increased (line 8–10 in Algorithm 6), which enlarges ${\gamma }_k$ and thus expands the ellipse. This expansion encourages broader exploration and helps the planner escape local trapping near cluttered obstacles.

To mitigate the bias induced by a fixed ellipse center, as shown in Figure 3, the center $c_k$ is randomly switched (with probability 0.5) between (i) the midpoint of the nearest inter-tree pair and (ii) the midpoint of the start–goal pair. The ellipse orientation ${\varphi }_k$ aligns with the corresponding pair direction. A locally guided sample $q_{\mathrm{ell}}$ is then drawn uniformly inside the oriented ellipse (line 13–20 in Algorithm 6).

Meanwhile, a globally explorative sample $q_g$ is generated using a Halton low-discrepancy sequence, which improves space-filling properties over purely random sampling. The sequence index is denoted by $n\in {\mathbb{N}}^{+}$ and is incremented by one each time a global sample is generated. The n-th Halton point is defined as

$$(u_n,v_n)=\left(H_2\left(n\right),H_3\left(n\right)\right)\in {(0,1)}^2,$$

(2)

where $H_b\left(n\right)$ denotes the Halton radical-inverse function in base b. The point $(u_n,v_n)$ is mapped to the workspace bounding box $[x_{min},x_{max}]\times [y_{min},y_{max}]$ as

$$x_n=x_{min}+u_n(x_{max}-x_{min}),y_n=y_{min}+v_n(y_{max}-y_{min}).$$

(3)

No scrambling is applied. Skipping is used only through different initial indices for the two trees: the start tree uses $n_0^s=1$ and the goal tree uses $n_0^g=5000$.

The final sample q is selected from $\{q_{\mathrm{ell}},q_g\}$ by a mixing probability $p_{g,k}$ (line 27–31 in Algorithm 6). To strengthen global exploration during stagnation, $p_{g,k}$ increases with the stagnation level ${\ell }_k$, thereby achieving an adaptive balance between fast local convergence and escaping local minima.

Algorithm 6: Progress Aware Fusion Sample $(T_s,T_g,d_{\mathrm{prev}},s)$

   Notation for Algorithm 6. $V\left(T\right)$ denotes the vertex set of a tree T. $d_{\mathrm{prev}}$ is the progress metric value from the previous iteration. s is the stagnation counter. $s_{\mathrm{th}}$ is the stagnation threshold used to trigger stagnation-level increase. ${\ell }_k$ is the stagnation level at iteration k. ${\gamma }_k$ is the stagnation amplification factor, bounded by ${\gamma }_{max}$. (If the implementation uses a slope coefficient, denote it explicitly as ${\gamma }_k$ and use ${\gamma }_k=min({\gamma }_{max},1+{\gamma }_k{\ell }_k)$.) $a_k$ and $b_k$ are the semi-major and semi-minor axes of the oriented elliptical sampling region. $a_{min},a_{max}$ and $b_{min},b_{max}$ are the lower/upper bounds of $a_k$ and $b_k$, respectively. $c_k^{\left(p\right)}={\displaystyle \frac{1}{2}}(x_s^{*}+x_g^{*})$ is the midpoint of the nearest inter-tree pair. $c_k^{\left(sg\right)}={\displaystyle \frac{1}{2}}(x_{\mathrm{start}}+x_{\mathrm{goal}})$ is the midpoint of the start–goal pair. $c_k$ is the ellipse center, randomly chosen between $c_k^{\left(p\right)}$ and $c_k^{\left(sg\right)}$ with probability $0.5$. ${\phi }_k$ is the ellipse orientation angle computed by $\mathrm{atan}2(·,·)$. $R\left({\phi }_k\right)$ is the $2\times 2$ planar rotation matrix with angle ${\phi }_k$. $u_1,u_2\sim \mathcal{U}(0,1)$ are i.i.d. uniform random variables; $r=\sqrt{u_1}$ and $\theta =2\pi u_2$ are polar coordinates for uniform sampling in an ellipse. $q_{\ell }$ is the locally guided sample drawn uniformly inside the oriented ellipse. n is the index of the Halton sequence. $H_2\left(n\right)$ and $H_3\left(n\right)$ are the 2nd and 3rd dimensions of the Halton sequence, producing $u_n$ and $v_n$. $x_{min},x_{max},y_{min},y_{max}$ define the workspace bounding box. $q_g\left(n\right)$ is the global sample generated by the Halton sequence mapping to the workspace. $p_{g,k}$ is the mixing probability for choosing the global sample, with increment coefficient $\Delta p$. $p_{g,0}>0$ is a small global-sampling probability. q is the final sample selected from $\{q_{\ell },q_g\}$.

3.2. Three-Level Goal-Guided Extension Mechanism

After obtaining the sample in Section 3.1, the tree is expanded using the three-level goal-guided extension mechanism. The three-level goal-guided extension is the core strategy of PFMS-RRT* in the expansion phase, which aims to efficiently generate collision-free nodes toward the goal direction through hierarchical exploration. Its pseudocode is presented in Algorithm 7, and the core workflow is outlined below.

The tree is expanded using a three-level goal-guided extension mechanism, which follows a coarse-to-fine strategy to balance fast convergence and robust obstacle avoidance. Given the current node $x_k$, the guiding target g, and obstacles O, as shown in Figure 4, the first level performs the fastest direct goal steering by advancing one step along the unit direction ${\widehat{h}}_k=(g-x_k)/{\parallel g-x_k\parallel }_2$, i.e., $x_{k+1}^{\left(1\right)}=x_k+\eta {\widehat{h}}_k$. If the segment $(x_k,x_{k+1}^{\left(1\right)})$ is collision-free, the new node is accepted immediately.

As shown in Figure 5, when direct steering is blocked, the second level conducts a local small-range probing search. First, the lateral shift direction $s\in \{-1,+1\}$ is selected, where $s=-1$ corresponds to a leftward shift and $s=1$ corresponds to a rightward shift. Given the lateral displacement set $\Delta =\{0.3\eta ,0.8\eta \}$ and the forward advance set $\Lambda =\{0.6\eta ,\eta \}$, the lateral shift base point is first constructed:

$$x_{\mathrm{base}}(s,\delta )=x_k+s\delta {\widehat{v}}_k$$

(4)

then the candidate point is obtained:

$$x_{\mathrm{try}}(s,\delta ,\lambda )=x_{\mathrm{base}}(s,\delta )+\lambda {\widehat{h}}_k$$

(5)

where $x_k$ is the current node (state) to be extended; g is the guiding target (typically the sampled state or the opposite-tree target); ${\widehat{h}}_k=(g-x_k)/{\parallel g-x_k\parallel }_2$ is the unit direction from $x_k$ to g; ${\widehat{v}}_k={[-{\widehat{h}}_{k,y},{\widehat{h}}_{k,x}]}^{\top }$ is the lateral unit vector orthogonal to ${\widehat{h}}_k$; $s\in \{-1,+1\}$ indicates left ($-1$) or right ($+1$) lateral shift; $\delta \in \Delta$ is the lateral displacement magnitude, and $\lambda \in \Lambda$ is the forward step size; $\eta$ is the nominal extension step length.

A pure lateral shift may cause the expansion tree to drift back and forth locally. Therefore, the forward progress along the goal direction is defined as

$$\mathrm{prog}\left(x_{\mathrm{try}}\right)=\langle x_{\mathrm{try}}-x_k,{\widehat{h}}_k\rangle$$

(6)

with the constraint

$$\mathrm{prog}\left(x_{\mathrm{try}}\right)\ge \tau \eta$$

(7)

This constraint ensures that the extension bypasses obstacles while continuing to approach the goal, rather than performing a purely lateral movement. A one-step look-ahead point is added in front of the candidate point:

$$x_{\mathrm{look}}=x_{\mathrm{try}}+\eta {\widehat{h}}_k$$

(8)

and define

$$L=\mathrm{Free}(x_{\mathrm{try}},x_{\mathrm{look}})\in \{0,1\}$$

(9)

where $\langle ·,·\rangle$ denotes the Euclidean inner product; $\mathrm{prog}\left(x_{\mathrm{try}}\right)$ measures the forward progress of candidate $x_{\mathrm{try}}$ along ${\widehat{h}}_k$; $\tau \in (0,1]$ is a progress threshold coefficient enforcing minimum forward progress $\tau \eta$; $x_{\mathrm{look}}$ is a one-step look-ahead point used for feasibility checking; L is a binary feasibility indicator.

By comprehensively considering the goal distance, progress effect, look-ahead feasibility, and lateral shift cost, an evaluation function $J\left(x_{\mathrm{try}}\right)$ is constructed. The candidate that minimizes this function among all feasible candidates is selected as $x_{k+1}^{\left(2\right)}$:

$$\begin{array}{cc}\hfill J\left(x_{\mathrm{try}}\right)& =\parallel g-x_{\mathrm{try}}{\parallel }_2-w_p·\mathrm{prog}\left(x_{\mathrm{try}}\right)\hfill \\ \hfill & -w_L·L\eta +w_{\delta }·\delta \hfill \end{array}$$

(10)

$$x_{k+1}^{\left(2\right)}=arg\underset{\begin{array}{c}s\in \{\pm 1\},\delta \in \Delta ,\lambda \in \Lambda \\ F\left(x_{\mathrm{try}}(s,\delta ,\lambda )\right)\end{array}}{min}J\left(x_{\mathrm{try}}(s,\delta ,\lambda )\right)$$

(11)

where $J\left(x_{\mathrm{try}}\right)$ is the candidate evaluation function; $w_p$, $w_L$, and $w_{\delta }$ are nonnegative weights for progress reward, look-ahead feasibility reward, and lateral-shift penalty, respectively; $\parallel g-x_{\mathrm{try}}{\parallel }_2$ is the Euclidean distance to the guiding target; $\Delta$ and $\Lambda$ are discrete sets of lateral and forward step candidates. In (9), $x_{k+1}^{\left(2\right)}$ is the best second-level candidate selected by minimizing $J(·)$ over all feasible $(s,\delta ,\lambda )$ combinations. The overall procedure is detailed in Lines 7–24 of Algorithm 7.

As shown in Figure 6, if no feasible node is found in the second level, the third level activates a stronger multi-direction detour fallback by sampling multiple rotated directions $\widehat{d}(\theta ,\sigma )=R\left(\sigma \theta \right){\widehat{h}}_k$ with discrete angles $\Theta$ and step sizes $\mathcal{S}$, $\Theta$ denotes a discrete set of steering angles around the goal direction, and $\mathcal{S}$ denotes a discrete set of step sizes used to generate candidate detour motions. Among all collision-free candidates, the one minimizing $J_{\det }\left(x_{try}\right)=\parallel g-x_{try}{\parallel }_2+{\lambda }_{\theta }\left|\theta \right|$ is chosen. This three-level mechanism improves the extension success rate and maintains continuous forward progress in cluttered environments.

Algorithm 7: Three Level Goal Guided Extend $(x_k,g,\mathcal{O})$

3.3. Smooth Tangential APF Extension

To improve extension stability and obstacle-avoidance efficiency near obstacles, a smooth tangential artificial potential field is introduced on top of the conventional APF. The conventional APF consists of an attractive potential $U_{\mathrm{att}}$, a repulsive potential $U_{\mathrm{rep}}$, and the total potential $U_{\mathrm{total}}$, where the acting forces are given by the negative gradients of the corresponding potentials. However, the repulsive field may vary sharply around the boundary of the obstacle influence region, which can cause abrupt changes in the resultant force direction, leading to jittery extensions and ineffective samples. In narrow passages or near concave obstacles, the planner may also suffer from local minima or inefficient detours, thus degrading the extension success rate. The pseudocode of the smooth tangential APF extension is shown in Algorithm 8.

For each obstacle o, a smoothing factor is defined as

$$s_o\left(x\right)=min\left(max\left({\displaystyle \frac{{\rho }_0-{\rho }_o\left(x\right)}{{\rho }_0}},0\right),1\right),$$

(12)

$$w_o\left(x\right)=s_o^2\left(x\right),$$

(13)

where ${\rho }_o\left(x\right)$ denotes the distance from point x to obstacle o. In this work, this distance is computed based on the exact geometric boundary of each obstacle. ${\rho }_0>0$ is the obstacle influence radius (threshold distance), $s_o\left(x\right)\in [0,1]$ is a smoothing factor, and $w_o\left(x\right)=s_o^2\left(x\right)$ is its squared weight, so that the tangential effect changes continuously at the boundary of the influence region. Let ${\widehat{n}}_o\left(x\right)$ denote the outward unit normal at the expansion point with respect to obstacle o, and define the tangential unit vector as

$${\tilde{t}}_o\left(x\right)=\left[\begin{array}{c}-{\widehat{n}}_{o,y}\left(x\right)\\ {\widehat{n}}_{o,x}\left(x\right)\end{array}\right].$$

(14)

where ${\widehat{n}}_o\left(x\right)$ is the outward unit normal direction at x with respect to obstacle o; ${\tilde{t}}_o\left(x\right)$ is the tangential unit vector obtained by rotating ${\widehat{n}}_o\left(x\right)$ by ${90}^{\circ }$; ${\widehat{t}}_o\left(x\right)$ is the final tangential direction chosen to be consistent with the goal-guiding direction ${\widehat{h}}_k$. To avoid moving along an undesired tangential direction, the sign is selected according to the goal-guiding direction ${\widehat{h}}_k$, yielding the final tangential direction ${\widehat{t}}_o\left(x\right)$. The tangential force is then defined by

$$F_{\tan ,o}\left(x\right)=k_{\tan }{w}_o\left(x\right)\left({\displaystyle \frac{1}{{\rho }_o\left(x\right)}}-{\displaystyle \frac{1}{{\rho }_0}}\right){\widehat{t}}_o\left(x\right),$$

(15)

$$F_{\tan }\left(x\right)=\sum _{o\in O}{F}_{\tan ,o}\left(x\right),$$

(16)

The resultant force is

$$F\left(x\right)=F_{\mathrm{att}}\left(x\right)+F_{\mathrm{rep}}\left(x\right)+F_{\tan }\left(x\right),$$

(17)

$$\widehat{u}\left(x\right)={\displaystyle \frac{F\left(x\right)}{{\parallel F\left(x\right)\parallel }_2}},$$

(18)

where $k_{\tan }$ is the tangential force gain, $k_r$ is the repulsive force gain, and $k_a$ is the attractive force gain. $F_{\tan ,o}\left(x\right)$ is the tangential force induced by obstacle o and $F_{\tan }\left(x\right)={\sum }_{o\in O}{F}_{\tan ,o}\left(x\right)$. $F_{\mathrm{att}}\left(x\right)$ and $F_{\mathrm{rep}}\left(x\right)$ are the attractive and repulsive forces, respectively; $F\left(x\right)$ is the resultant force; and $\widehat{u}\left(x\right)=F\left(x\right)/{\parallel F\left(x\right)\parallel }_2$ is its unit direction. $\eta$ is the step length used to generate the next node $x_{k+1}=x_k+\eta \widehat{u}\left(x\right)$.

Algorithm 8: Smooth Tangential APF Extend $(x_k,g,\mathcal{O})$

3.4. Failure-Driven Switching Probability Update

PFMS-RRT* integrates two extension strategies: three-level goal-guided extension mechanism and smooth tangential APF extension. The three-level goal-guided extension mechanism mainly advances toward the guiding target and converges faster in open areas, but it may suffer frequent collisions and stagnation in cluttered regions or around narrow-passage entrances. In contrast, the smooth tangential APF extension introduces a smooth tangential component within the obstacle influence region, enabling boundary-following motion instead of being pushed away purely by normal repulsion, and is thus more robust in locally obstacle-constrained regions. Based on this complementarity, a failure-driven adaptive switching scheme is adopted: when repeated failures indicate that goal-guided probing is ineffective locally, the probability of activating the smooth tangential APF extension is increased to enhance obstacle avoidance and restore forward progress. The failure-driven adaptive switching scheme is shown in Algorithm 9.

A binary success indicator $S^{\left(X\right)}\in \{0,1\}$ is defined for each extension attempt, where $S^{\left(X\right)}=1$ denotes success and $S^{\left(X\right)}=0$ denotes failure (i.e., returning ⌀). A failure counter $f^{\left(X\right)}$ is maintained as

$$f^{\left(X\right)}\leftarrow \left\{\begin{array}{cc}0,\hfill & S^{\left(X\right)}=1,\hfill \\ f^{\left(X\right)}+1,\hfill & S^{\left(X\right)}=0.\hfill \end{array}\right.$$

(19)

The counter is then mapped to a probability term via an exponential decay model

$$p_g^{\left(X\right)}=exp\left(-\beta f^{\left(X\right)}\right),\beta >0,$$

(20)

and the switching probability is defined as

$$p_{sw}^{\left(X\right)}=1-p_g^{\left(X\right)}.$$

(21)

where $f^{\left(X\right)}$ is the accumulated failure counter; $\beta >0$ is the exponential decay rate controlling the sensitivity to failures; $p_g^{\left(X\right)}$ is the probability term mapped from the failure counter; $p_{sw}^{\left(X\right)}$ is the switching probability of invoking the APF-based extension.

Consequently, accumulated failures monotonically increase $p_{sw}^{\left(X\right)}$, making the planner more likely to invoke smooth tangential APF extension. Once a feasible extension is found, $f^{\left(X\right)}$ is reset and $p_{sw}^{\left(X\right)}$ drops quickly, allowing for the planner to return to the more efficient three-level goal-guided extension mechanism.

Algorithm 9: Update Switch Prob $(f^{\left(X\right)},S^{\left(X\right)})$

3.5. Probabilistic Completeness and Asymptotic Optimality Discussion

Let the state of PFMS-RRT* at iteration k be

$$X_k=\left(V_k^s,V_k^g\right),$$

(22)

where $V_k^s$ and $V_k^g$ denote the vertex sets of the two search trees grown from the start and goal, respectively. Let X be the state space and $X_{\mathrm{free}}\subseteq X$ be the free space. At each iteration, PFMS-RRT* generates a sample $q_k$ from a mixture distribution

$$q_k\sim {\mu }_k=(1-p_{g,k}){\mu }_k^{\mathrm{ell}}+p_{g,k}{\mu }^{\mathrm{global}},$$

(23)

where ${\mu }_k^{\mathrm{ell}}$ is the uniform distribution over the oriented ellipse constructed from the current progress information, ${\mu }^{\mathrm{global}}$ is a global distribution covering the workspace (e.g., Halton low-discrepancy samples mapped to X), and $p_{g,k}\in (0,1]$ is the global-sampling probability that can be adaptively adjusted according to stagnation.

(Probabilistic completeness). Assume that for any measurable set $A\subseteq X_{\mathrm{free}}$ with $\mathrm{Vol}\left(A\right)>0$, the global distribution satisfies ${\mu }^{\mathrm{global}}\left(A\right)>0$, and there exists a constant $p_{min}>0$ such that $p_{g,k}\ge p_{min}$ for all k (i.e., global sampling is triggered infinitely often with a non-zero lower-bounded probability). Then, for any $\epsilon >0$ and any $q_{\mathrm{free}}\in X_{\mathrm{free}}$, with probability one there exist infinitely many iterations whose samples fall into the $\epsilon$-neighborhood of $q_{\mathrm{free}}$, i.e.,

$$\mathbb{P}\left(\exists k:\parallel q_k-q_{\mathrm{free}}\parallel <\epsilon \right)=1.$$

(24)

Consequently, together with bidirectional tree growth and collision checking/connection, if a feasible path exists, PFMS-RRT* is probabilistically complete in the sense that

$$\underset{k\to \infty }{lim}\mathbb{P}\left(X_k\mathrm{contains}\mathrm{a}\mathrm{feasible}\mathrm{start}-\mathrm{goal}\mathrm{connection}\right)=1.$$

(25)

Note that this conclusion relies on the condition that the global sampling component continuously provides non-zero coverage of $X_{\mathrm{free}}$; if $p_{g,k}$ is set to zero or the sampling support is restricted to a strict subset of $X_{\mathrm{free}}$ for all sufficiently large k, completeness may be weakened.

(Asymptotic optimality). Let $c_k$ denote the best path cost found by PFMS-RRT* up to iteration k (once a connection exists), and let $c^{*}$ be the optimal cost. Since PFMS-RRT* retains the RRT* parent selection and rewiring operations, the best-so-far cost is non-increasing, i.e., $c_{k+1}\le c_k$ when rewiring improves the solution. Moreover, under the same assumptions above (global sampling has full support over $X_{\mathrm{free}}$ and is invoked infinitely often with $p_{g,k}\ge p_{min}>0$), and assuming that a standard goal-steering/steer attempt is executed with non-zero probability (so that the algorithm does not exclude the cost-improving events used in the classical RRT* convergence argument), PFMS-RRT* preserves asymptotic optimality in the sense that

$$\mathbb{P}\left(\underset{k\to \infty }{lim}{c}_k=c^{*}\right)=1.$$

(26)

4. Experiment

In this section, the proposed PFMS-RRT* algorithm will be compared to GB-RRT* [22], F-RRT* [14], BI-RRT* [17], BI-APF-RRT* [24] and IBI-P-RRT* [25] algorithms. The simulation platform is MATLAB R2024b. All experiments were conducted on a desktop PC with CPU: 13th Gen Intel(R) Core(TM) i5-13600KF, RAM: 32 GB, and OS: Windows 10.

4.1. Experiment Configuration

Four representative 2D obstacle environments are used in the experiments, as shown in Figure 7a–d (Environment A–D).

Environment A (sparse mixed-obstacle scenario): the number of obstacles is relatively small, with rectangles and ellipses mixed in the workspace. The free space is largely open with a few local occlusions, and this scenario is mainly used to evaluate convergence speed and path quality under general obstacle distributions.

Environment B (structured corridor with dense small obstacles): The layout is highly structured, where long rectangular obstacles form boundary and corridor constraints along the top/bottom and sides, while many small elliptical obstacles are placed inside. This setting reflects dense obstacle interference and frequent collision checking, and is used to assess extension success rate and stability in cluttered environments.

Environment C (random dense mixed-obstacle scenario): Obstacles are more numerous and distributed more randomly, with rectangles and ellipses interleaved. The connectivity of the free space becomes more complex, often requiring detours through locally constrained regions, and this scenario is used to evaluate exploration capability and robust obstacle avoidance in unstructured complex environments.

Environment D (narrow-passage scenario): Multiple large rectangular obstacles form a typical narrow-passage structure, leaving only a slender traversable corridor with local bottlenecks. This is a classic challenging scenario for testing whether the sampling and extension strategies can pass through narrow passages and avoid stagnation.

To ensure a fair comparison, all environments share the same map scale and coordinate setting: the planar workspace is a $100\times 100$ square with $(x,y)\in [0,100]\times [0,100]\mathrm{m}$. Obstacles are represented by gray occupied regions, and the remaining area is free space for feasible sampling and tree expansion. The start and goal configurations are fixed for all environments as $x_{\mathrm{start}}=(0,0),x_{\mathrm{goal}}=(100,100)$. The initial step size is set to 3. All algorithms were executed independently for 30 runs per environment, with a maximum iteration limit of 3000 for each algorithm.

4.2. Parameter Settings

As summarized in

Table 1. Key parameter settings used in the experiments.

Parameter (Symbol)	Value
Step size ($\eta$)	3
Stagnation threshold ($s_{\mathrm{th}}$)	4
Amplification upper bound (${\gamma }_{max}$)	3.0
Global-sampling increment ($\Delta p$)	0.15
Ellipse bounds ($a_{min},a_{max};b_{min},b_{max}$)	$(12, 90; 8, 90)$
Progress reward weight ($w_p$)	0.8
Look-ahead feasibility reward weight ($w_L$)	0.8
Lateral-shift penalty weight ($w_{\delta }$)	0.05
Switching decay rate ($\beta$)	0.8
Repulsive force gain ($k_r$)	1.5
Tangential force gain ($k_{\tan }$)	1.2
Attractive force gain ($k_a$)	0.5
Influence radius (${\rho }_0$)	10

, the key parameters of PFMS-RRT* were fixed across all experiments to ensure reproducibility and fair comparison. The step size $\eta$ was set to balance exploration efficiency and local collision avoidance, while the stagnation threshold $s_{\mathrm{th}}$ and the amplification upper bound ${\gamma }_{max}$ were used to detect persistent lack of progress and to enlarge the elliptical sampling corridor in a controlled manner. The global-sampling increment $\Delta p$ regulates how quickly the planner increases global exploration when stagnation occurs, preventing premature over-exploration while still enabling escape from difficult regions. The ellipse bounds $(a_{min},a_{max};b_{min},b_{max})$ constrain the locally guided sampling region to avoid over-concentration or excessive expansion, thus maintaining both efficiency and robustness. For the weights for progress reward, look-ahead feasibility reward, lateral-shift penalty $w_p$, $w_L$, and $w_{\delta }$ were set to fixed values ($w_p=0.8$, $w_L=0.8$, $w_{\delta }=0.05$), and then kept unchanged in all reported experiments. These values were chosen based on term scaling and intended behavior: the forward progress term $\mathrm{prog}\left(x_{\mathrm{try}}\right)$ is typically on the order of the step size $\eta$, and the look-ahead feasibility reward is bounded by one step through $L\eta$ with $L\in 0,1$; therefore, setting $w_p=w_L=0.8$ assigns comparable importance to promoting progress and preferring candidates that admit a feasible one-step continuation, while the distance-to-goal term $|g-x_{\mathrm{try}}|2$ still provides global guidance. In contrast, a smaller lateral-shift penalty $w_{\delta }=0.05$ discourages unnecessary detours without suppressing lateral motion, which is essential for escaping local dead-ends when direct steering is blocked. For the failure-driven switching mechanism, the decay rate $\beta$ controls the sensitivity of the switching probability to consecutive failures, enabling an adaptive balance between goal-guided extension and APF-based extension. In the APF module, the gains $(k_a,k_r,k_{\tan })$ and influence radius ${\rho }_0$ were chosen to provide stable obstacle avoidance and smooth boundary-following behavior without introducing excessive detours.

As summarized in

Table 2. Method-specific baseline parameters used in the experiments.

Baseline	Method-Specific Parameters	Value
GB-RRT*	Goal bias probability ($p_{\mathrm{goal}}$)	0.2
F-RRT*	Goal bias in random branch ($P_{\mathrm{goal}}$)	0.2
	Bisection tolerance (${\epsilon }_{\mathrm{bisect}}$)	0.25
	Max bisection steps ($N_{\mathrm{bisect}}$)	30
	Constraint radius factor ($\alpha$)	1.0
	Max constraint tries ($N_{\mathrm{tries}}$)	200
BI-RRT*	None	–
BI-APF-RRT*	Attractive force gain ($k_a$)	0.5
	Repulsive force gain ($k_r$)	30
	Influence radius (${\rho }_0$)	10
	Random attraction gain ($k_{\mathrm{rand}}$)	10
IBI-P-RRT*	Attractive force gain ($k_a$)	0.2
	Repulsive force gain ($k_r$)	0.5
	Influence radius (${\rho }_0$)	5
	Repulsive exponent (n)	2
	Random attraction gain ($k_{\mathrm{rand}}$)	15

, for the compared baseline planners, we followed the parameter settings reported in their original papers to maintain consistency with prior work. In addition, common experimental settings such as the workspace bounds, termination conditions, and collision-checking procedures were kept the same for all methods, so that performance differences primarily reflect the algorithmic designs rather than parameter discrepancies.

4.3. Metric Definitions

In this paper, four metrics are reported to evaluate planning performance: Path length, Runtime, Number of nodes, and Avgdistance.

4.3.1. Path Length

Let the planned path be an ordered waypoint sequence $P={\left\{p_i\right\}}_{i=0}^N$. The path length is

$$L\left(P\right)=\sum _{i=0}^{N-1}{\parallel p_{i+1}-p_i\parallel }_2.$$

(27)

4.3.2. Runtime

Runtime is the wall-clock planning time measured from the start of the main planning loop until a feasible start–goal connection is found (or the termination budget is reached). Unless otherwise stated, it includes sampling, collision checking, and RRT* rewiring, and excludes visualization.

4.3.3. Number of Nodes

The number of nodes is the total number of vertices in the final tree(s) when the algorithm terminates. For bidirectional planners, it is reported as the sum of nodes in the start tree and the goal tree.

4.3.4. Avgdistance

Let the path $P={\left\{p_i\right\}}_{i=0}^N$ form N line segments $e_i=[p_i,p_{i+1}]$. We evaluate clearance by sampling the path on segments (not vertices only). For a segment e and obstacle set O, define

$$d_{min}\left(e\right)=\underset{o\in O}{min}d(e,o),$$

(28)

where $d(e,o)=0$ if e intersects obstacle o, otherwise $d(e,o)$ is the Euclidean distance between segment e and the obstacle boundary $\partial o$ (segment-to-circle for circular obstacles and segment-to-polygon-edge for rectangular obstacles). The average clearance is

$$\mathrm{Avgdistance}={\displaystyle \frac{1}{N}}\sum _{i=0}^{N-1}{d}_{min}\left(e_i\right).$$

(29)

All methods (PFMS-RRT* and baselines) are evaluated using the same definitions and distance computation for fair comparison.

4.4. Algorithm Performance Analysis

As shown in Figure 8, in Environment A, all six path-planning algorithms are able to successfully find a collision-free feasible path from the start to the goal. The red curve denotes the final planned path, while the blue and pink curves indicate the bidirectional search trees expanded from the start and the goal, respectively. However, the differences mainly lie in sampling efficiency and path quality. For GB-RRT* and F-RRT*, the search trees are more dispersed and exhibit more ineffective expansions in the vicinity of obstacles, manifested by repeated growth of the blue and pink branches near obstacles with insufficient utilization of the truly traversable regions. This behavior leads to unnecessary detours and polyline-like corrections in the resulting path. In contrast, PFMS-RRT* concentrates its tree expansion in effective feasible regions, producing more coherent obstacle-avoidance expansions with fewer rollbacks, and thus generating a more compact and smoother path. This phenomenon is directly related to the proposed progress-aware fusion sampling strategy. As a result, ineffective sampling around obstacle neighborhoods and redundant nodes are significantly reduced, improving both convergence efficiency and path quality.

The quantitative results in

Table 3. Performance comparison of different algorithms in Environment A. Values are reported as mean (standard deviation).

Algorithm	Path Length (m)	Runtime (s)	Number of Nodes	Avgdistance (m)
PFMS-RRT*	149.61 (0.69)	0.09 (0.01)	53.60 (1.22)	1.26 (0.04)
BI-APF-RRT*	156.49 (5.19)	0.29 (0.15)	136.97 (45.93)	1.18 (0.27)
BI-RRT*	156.41 (4.49)	0.21 (0.06)	110.57 (21.74)	1.13 (0.35)
F-RRT*	151.03 (3.00)	0.16 (0.03)	92.87 (15.12)	0.98 (0.27)
GB-RRT*	151.50 (3.38)	0.45 (0.19)	253.70 (86.36)	0.92 (0.33)
IBI-P-RRT*	155.95 (5.18)	0.24 (0.10)	119.97 (32.31)	1.20 (0.29)

further support the above observations. In Environment A, PFMS-RRT* achieves an average path length of $149.61 \mathrm{m}$, an average runtime of $0.09 \mathrm{s}$, an average node count of $53.60$, and an average minimum distance from path points to obstacles of $1.26 \mathrm{m}$. Compared with BI-APF-RRT*, BI-RRT*, and IBI-P-RRT*, PFMS-RRT* reduces the path length by $4.40\%$, $4.35\%$, and $4.07\%$, respectively; compared with F-RRT* and GB-RRT*, the path length is reduced by $0.94\%$ and $1.25\%$, respectively. In terms of efficiency, PFMS-RRT* decreases runtime by $68.97\%$, $57.14\%$, $43.75\%$, $80.00\%$, and $62.50\%$ relative to BI-APF-RRT*, BI-RRT*, F-RRT*, GB-RRT*, and IBI-P-RRT*, respectively; meanwhile, the average node count is reduced by $60.87\%$, $51.52\%$, $42.28\%$, $78.87\%$, and $55.32\%$, respectively. Regarding safety, PFMS-RRT* improves the average minimum obstacle clearance by $6.78\%$, $11.50\%$, $28.57\%$, $36.96\%$, and $5.00\%$ over BI-APF-RRT*, BI-RRT*, F-RRT*, GB-RRT*, and IBI-P-RRT*, respectively. Overall, progress-aware fusion sampling significantly reduces runtime and tree size by eliminating ineffective samples and redundant expansions, while the more focused exploration also contributes to shorter, smoother paths with stronger clearance performance.

As shown in Figure 9, Environment B contains multiple narrow passages and corner-like structures, imposing higher requirements on reachability and extension stability. In this scenario, the baseline planners are more likely to exhibit path jitter at passage entrances and corners, which is reflected by frequent changes in expansion direction and unstable node distributions inside the corridor. As a consequence, the resulting paths often include additional polyline turns and detours before and after entering the narrow passages. In contrast, PFMS-RRT* can enter and traverse the narrow passages more stably, showing better convergence during search and significantly improved path continuity compared with the baselines.

These improvements are directly attributed to the proposed extension strategy. During expansion, PFMS-RRT* employs a three-level fallback goal-guided mechanism: when the standard goal-directed extension is blocked, the planner can still maintain forward progress via progressively strengthened detour behaviors, thereby reducing the probability of local failures and mitigating stagnation around corridor entrances. Moreover, when goal-guided probing becomes ineffective (or when the switching condition is triggered), the planner switches to the smooth tangential APF extension, which encourages boundary-following growth and smoother steering near obstacle boundaries. This makes the node expansion better aligned with corridor boundaries and yields more stable turning behaviors, ultimately improving passage-traversal stability and safety margin in Environment B.

The quantitative results in

Table 4. Performance comparison of different algorithms in Environment B. Values are reported as mean (standard deviation).

Algorithm	Path Length (m)	Runtime (s)	Number of Nodes	Avgdistance (m)
PFMS-RRT*	150.90 (0.93)	0.10 (0.01)	54.13 (1.11)	1.02 (0.09)
BI-APF-RRT*	157.70 (6.41)	0.54 (1.09)	165.33 (109.11)	0.99 (0.28)
BI-RRT*	159.11 (7.61)	0.23 (0.08)	114.80 (23.63)	0.90 (0.18)
F-RRT*	155.25 (5.89)	0.17 (0.04)	93.73 (19.24)	0.67 (0.20)
GB-RRT*	154.37 (2.93)	0.40 (0.26)	219.37 (112.16)	0.70 (0.27)
IBI-P-RRT*	158.48 (3.65)	0.43 (0.40)	164.87 (93.95)	0.86 (0.26)

further validate the above observations. PFMS-RRT* achieves an average path length of $150.90 \mathrm{m}$, an average runtime of $0.10 \mathrm{s}$, an average node count of $54.13$, and an average minimum obstacle clearance (Avgdistance) of $1.02 \mathrm{m}$. Compared with BI-APF-RRT*, BI-RRT*, and IBI-P-RRT*, PFMS-RRT* reduces the path length by $4.31\%$, $5.16\%$, and $4.78\%$, respectively; compared with F-RRT* and GB-RRT*, the path length is reduced by $2.80\%$ and $2.25\%$, respectively. In terms of efficiency, PFMS-RRT* decreases runtime by 81.48%, 56.52%, 41.18%, 75.00%, and 76.74% relative to BI-APF-RRT*, BI-RRT*, F-RRT*, GB-RRT*, and IBI-P-RRT*, respectively; meanwhile, the average node count is reduced by 67.26%, 52.85%, 42.25%, 75.32%, and 67.17%, respectively. Regarding safety, PFMS-RRT* improves Avgdistance by 3.03%, 13.33%, 52.24%, 45.71%, and 18.60% over the corresponding baselines. Overall, the strong reductions in runtime and node count indicate that progress-aware fused sampling effectively suppresses redundant exploration in dense obstacle fields, while the improved clearance reflects more stable obstacle-neighborhood expansions.

As shown in Figure 10, Environment C features highly fragmented free space, which can easily induce redundant sampling and detour-prone paths. The baseline planners generally suffer from increased ineffective exploration in this scenario. In particular, IBI-P-RRT* exhibits rollback behaviors, indicating that it is more prone to repeated probing in regions with poor local connectivity, which leads to less natural path shapes and degraded search efficiency. In contrast, PFMS-RRT* concentrates its sampling distribution more clearly on effective regions and produces more natural obstacle-avoidance trajectories, resulting in improvements in both overall search efficiency and path quality.

These results highlight the synergy between fused sampling and multi-level strategy extension. On the one hand, the progress-aware elliptical sampling suppresses repeated sampling in low-value regions by progressively focusing on informative areas. On the other hand, the three-level extension together with the smooth tangential APF provides more stable local guidance in fragmented spaces, thereby reducing the likelihood of rollbacks and unnecessary detours.

As reported in

Table 5. Performance comparison of different algorithms in Environment C. Values are reported as mean (standard deviation).

Algorithm	Path Length (m)	Runtime (s)	Number of Nodes	Avgdistance (m)
PFMS-RRT*	148.97 (0.89)	0.10 (0.01)	52.53 (1.11)	1.44 (0.03)
BI-APF-RRT*	161.82 (8.41)	0.32 (0.14)	144.33 (37.78)	1.40 (0.27)
BI-RRT*	161.86 (9.58)	0.24 (0.08)	121.43 (28.91)	1.32 (0.25)
F-RRT*	152.00 (3.71)	0.19 (0.06)	111.17 (32.17)	1.23 (0.19)
GB-RRT*	153.28 (3.96)	0.82 (0.44)	399.70 (163.71)	1.22 (0.24)
IBI-P-RRT*	159.76 (8.01)	0.27 (0.10)	124.43 (29.36)	1.36 (0.23)

, PFMS-RRT* attains an average path length of $148.97 \mathrm{m}$, an average runtime of $0.10 \mathrm{s}$, an average node count of $52.53$, and an Avgdistance of $1.44 \mathrm{m}$. Compared with BI-APF-RRT*, BI-RRT*, and IBI-P-RRT*, PFMS-RRT* reduces the path length by 7.94%, 7.96%, and 6.75%, respectively; compared with F-RRT* and GB-RRT*, the path length is reduced by 1.99% and 2.81%, respectively. For efficiency, PFMS-RRT* decreases runtime by 68.75%, 58.33%, 47.37%, 87.80%, and 62.96% relative to BI-APF-RRT*, BI-RRT*, F-RRT*, GB-RRT*, and IBI-P-RRT*, respectively; the average node count is reduced by 63.60%, 56.74%, 52.75%, 86.86%, and 57.78%, respectively. In terms of safety, PFMS-RRT* improves Avgdistance by 2.86%, 9.09%, 17.07%, 18.03%, and 5.88% over the corresponding baselines. These gains indicate that the proposed sampling improvement drives substantial reductions in tree size and runtime in fragmented clutter, while the robust extension behaviors help maintain strong clearance and avoid inefficient stagnation.

As shown in Figure 11, Environment D is a typical strongly constrained narrow-passage scenario, where the feasible corridor is extremely limited. In this setting, most baseline planners exhibit unstable extensions or noticeable path oscillations inside the passage. Their search trees repeatedly grow within the narrow region but often fail to form a continuous and effective through-corridor connection, so the resulting paths tend to include multiple corrections or boundary-hugging segments. In contrast, PFMS-RRT* can expand stably along the passage and generates a smoother path that better conforms to the corridor structure, demonstrating stronger robustness and adaptability to constrained environments.

This advantage is closely related to the introduction of the smooth tangential APF extension. Under tight boundary constraints, the tangential potential field is more effective at guiding the expansion to follow obstacle boundaries smoothly, thereby reducing collision-prone probing and abrupt direction changes within the passage. Consequently, PFMS-RRT* improves both reachability and safety in Environment D.

As summarized in

Table 6. Performance comparison of different algorithms in Environment D. Values are reported as mean (standard deviation).

Algorithm	Path Length (m)	Runtime (s)	Number of Nodes	Avgdistance (m)
PFMS-RRT*	177.39 (0.64)	0.11 (0.01)	62.33 (1.60)	0.86 (0.07)
BI-APF-RRT*	208.25 (34.67)	1.56 (1.42)	223.30 (76.35)	0.83 (0.18)
BI-RRT*	198.42 (28.78)	1.14 (1.50)	183.70 (79.55)	0.81 (0.14)
F-RRT*	202.48 (31.90)	0.44 (0.17)	220.67 (72.49)	0.76 (0.20)
GB-RRT*	195.61 (25.24)	0.56 (0.14)	285.57 (52.72)	0.73 (0.13)
IBI-P-RRT*	195.87 (26.94)	0.98 (0.74)	181.83 (54.77)	0.82 (0.14)

, PFMS-RRT* achieves an average path length of $177.39 \mathrm{m}$, an average runtime of $0.11 \mathrm{s}$, an average node count of $62.33$, and an Avgdistance of $0.86 \mathrm{m}$. Compared with BI-APF-RRT*, BI-RRT*, and IBI-P-RRT*, PFMS-RRT* reduces the path length by 14.82%, 10.60%, and 9.43%, respectively; compared with F-RRT* and GB-RRT*, the path length is reduced by 12.39% and 9.31%, respectively. In terms of efficiency, PFMS-RRT* decreases runtime by 92.95%, 90.35%, 75.00%, 80.36%, and 88.78% relative to BI-APF-RRT*, BI-RRT*, F-RRT*, GB-RRT*, and IBI-P-RRT*, respectively; the average node count is reduced by 72.09%, 66.07%, 71.75%, 78.17%, and 65.72%, respectively. Regarding safety, PFMS-RRT* improves Avgdistance by 3.61%, 6.17%, 13.16%, 17.81%, and 4.88% over BI-APF-RRT*, BI-RRT*, F-RRT*, GB-RRT*, and IBI-P-RRT*, respectively. Overall, the pronounced reductions in runtime and node count strongly suggest that the proposed sampling and multi-strategy extension effectively mitigate stagnation in narrow passages, while the tangential APF fallback helps maintain stable boundary-following behavior and competitive clearance under severe local constraints.

Figure 12 compares the core performance metrics of six RRT* variant algorithms across the four environments using bar charts with overlaid scatter points and 95% confidence-interval error bars. PFMS-RRT* achieves significantly shorter path lengths in all scenarios, with a particularly pronounced advantage in the challenging Environment D. F-RRT exhibits the largest variation in path length, and its mean value in Environment D is much higher than that of the other algorithms. PFMS-RRT* maintains relatively low runtime in most scenarios, with only a slight increase in Environment D. In contrast, GB-RRT* shows notably higher runtime in Environment B, and its scatter points are highly concentrated, indicating unstable efficiency in that scenario. GB-RRT* also produces consistently larger node counts across all environments, suggesting substantial redundancy in its sampling and exploration. By comparison, PFMS-RRT* maintains a low node count throughout, reflecting a more efficient sampling strategy. Moreover, the average distance between nodes and obstacles for PFMS-RRT* varies little across environments, indicating stronger planning stability, whereas F-RRT shows the most dispersed scatter distribution and the largest performance fluctuation. Overall, PFMS-RRT* delivers the best comprehensive performance over the four environments, producing shorter paths with fewer nodes, lower and more stable runtime, and improved robustness. GB-RRT* suffers from high exploration redundancy, and F-RRT exhibits large performance variability. The bidirectional variants perform between PFMS-RRT* and the single-tree methods, achieving a relatively good balance between efficiency and stability.

4.5. Ablation Study

PFMS-RRT* integrates multiple mechanisms, including progress-aware fused sampling, multi-level strategy extension, and a smooth tangential APF extension. To quantify the contribution of each component, ablation variants are evaluated by disabling one component at a time while keeping all other settings unchanged.

Ablation Variants

• PFMS-RRT*: The complete method.
• No-FS: Without fused sampling improvement.
• No-ML: Without multi-level strategy extension.
• No-APF: Without smooth tangential APF extension.

In this ablation study, Environments A and D are selected as representative cases. Environment A is a relatively simple scenario that reflects typical open-space navigation, while Environment D is the most challenging environment with denser obstacles and tighter passages. Evaluating both allows for the contribution of each module to be observed in both easy and difficult settings under the same experimental budget.

Table 7. Ablation results in Environment A.

Variant	Avg Path Length (m)	Avg Node Count	Avg Time (s)	Avg Dist (m)
PFMS-RRT*	149.61	53.60	0.09	1.26
No-FS	150.95	56.40	0.11	1.07
No-ML	151.29	145.20	0.23	1.13
No-APF	150.52	56.10	0.11	1.17

and

Table 8. Ablation results in Environment D.

Variant	Avg Path Length (m)	Avg Node Count	Avg Time (s)	Avg Dist (m)
PFMS-RRT*	177.39	62.33	0.11	0.86
No-FS	178.56	66.23	0.13	0.81
No-ML	183.29	126.23	0.24	0.74
No-APF	178.36	65.13	0.13	0.82

report ablation results in the representative Environments A and D. Overall, disabling the multi-level extension (No-ML) causes the largest performance degradation across both environments, indicating that the multi-level strategy extension is the primary contributor to efficiency and robustness in cluttered scenes. In Environment A, No-ML increases the average node count from 53.60 to 145.20 (+170.9%) and runtime from 0.09 s to 0.23 s (+155.6%), showing substantially more ineffective expansions when the hierarchical obstacle-bypassing mechanism is removed. In the more challenging Environment D, No-ML similarly increases the node count from 62.33 to 126.23 (+102.5%) and runtime from 0.11 s to 0.24 s (+118.2%), and also leads to a larger path-length increase (177.39 m → 183.29 m, +3.33%) and reduced average clearance (0.86 m → 0.74 m, $-14.0$%). These results confirm that the multi-level extension effectively improves obstacle bypassing and reduces repeated failed attempts in constrained regions.

Removing fused sampling (No-FS) and removing the APF (No-APF) lead to moderate but consistent degradations, suggesting that these components provide complementary benefits. In particular, No-FS noticeably reduces the average clearance Avgdistance (1.26 m → 1.07 m, $-15.1$% in Environment A; 0.86 m → 0.81 m, $-5.8$% in Environment D), indicating that progress-aware fused sampling improves the quality of sampled guidance and tends to yield safer paths with larger obstacle clearance. Importantly, disabling the APF (No-APF) also degrades both safety and efficiency: Avgdistance decreases from 1.26 m to 1.17 m ($-7.1$%) in Environment A and from 0.86 m to 0.82 m ($-4.7$%) in Environment D, accompanied by longer paths (e.g., 149.61 m → 150.52 m in Environment A and 177.39 m → 178.36 m in Environment D). This indicates that the smooth tangential APF provides a useful robustness-enhancing backup. Overall, the ablation study demonstrates that PFMS-RRT* benefits from complementary contributions of the three components.

4.6. Sensitivity Experiment

To evaluate the influence of the weight parameters in Equation (10), a sensitivity experiment was conducted for $w_p$, $w_L$, and $w_{\delta }$. These three parameters respectively control the reward of forward progress, the reward of one-step look-ahead feasibility, and the penalty on lateral offset during the secondary local probing stage. Their values jointly determine the balance among target-oriented expansion, local feasibility maintenance, and obstacle-avoidance flexibility.

The sensitivity results of $w_p$ are reported in

Table 9. Sensitivity analysis results of $w_p$.

${\mathit{w}}_{\mathit{p}}$	Path Length (m)	Runtime (s)	Number of Nodes	Avgdistance (m)
0.40	149.29	0.09	53.60	1.27
0.60	149.41	0.09	53.60	1.27
0.80	149.61	0.09	53.60	1.26
1.00	150.06	0.09	53.60	1.24
1.20	153.21	0.09	53.56	1.18
1.40	155.05	0.09	53.60	1.13

. It can be observed that when $w_p$ varies from 0.4 to 0.8, the overall performance remains highly stable. The path length is maintained around 149.29–149.61 m, the runtime remains approximately 0.09 s, the number of nodes is almost unchanged at 53.60, and the average obstacle distance stays near 1.26–1.27 m. This indicates that within a moderate range, adjusting the forward-progress reward does not significantly alter the global planning behavior, suggesting that the proposed evaluation mechanism is relatively robust to small perturbations of $w_p$.

However, when $w_p$ further increases to 1.0, 1.2, and 1.4, the path length gradually rises from 150.06 m to 153.21 m and finally 155.05 m, while the average distance to obstacles decreases from 1.24 m to 1.18 m and then 1.13 m. This trend indicates that an excessively large $w_p$ causes the local probing strategy to overemphasize direct forward movement, thereby weakening its ability to maintain a proper balance between aggressive advancement and safe detouring. In other words, an overly large forward-progress reward makes the candidate selection more radical, which may reduce local adaptability and eventually deteriorate path quality.

Overall, the results suggest that $w_p$ has a relatively wide stable interval, and values in the range of 0.4–0.8 can provide both short paths and stable safety margins. The setting $w_p=0.8$ achieves a good compromise between planning efficiency and local adaptability.

The results for $w_L$ are listed in

Table 10. Sensitivity analysis results of $w_L$.

${\mathit{w}}_{\mathit{L}}$	Path Length (m)	Runtime (s)	Number of Nodes	Avgdistance (m)
0.40	153.20	0.09	53.60	1.47
0.60	153.20	0.09	53.60	1.47
0.80	149.61	0.09	53.60	1.26
1.00	149.60	0.09	53.60	1.26
1.20	149.60	0.09	53.60	1.26
1.40	149.60	0.09	53.60	1.26

. Compared with $w_p$, the influence of $w_L$ shows a more distinct threshold behavior. When $w_L$ is set to 0.4 or 0.6, the path length remains relatively large at 153.20 m, and the average obstacle distance reaches 1.47 m, which is noticeably larger than that obtained with higher $w_L$ values. This suggests that when the look-ahead feasibility reward is insufficient, the probing process tends to generate more conservative local expansions, resulting in longer paths and larger clearance from obstacles.

Once $w_L$ increases to 0.8, the path length drops sharply to 149.61 m, while the average obstacle distance decreases to about 1.26 m. Further increasing $w_L$ to 1.0, 1.2, and 1.4 produces almost identical results, with the path length stabilized around 149.60 m and the node number unchanged. This implies that after the one-step look-ahead feasibility term reaches a sufficient weight, it can reliably guide the local probing toward candidates with better continuation potential, thereby significantly improving path compactness. At the same time, the absence of noticeable performance degradation at larger $w_L$ values indicates that the proposed strategy is not overly sensitive to this parameter once it exceeds a certain effective threshold.

Therefore, the experimental results demonstrate that $w_L$ mainly acts as an enabling factor for local feasibility awareness. A value that is too small weakens the look-ahead guidance effect, whereas values from 0.8 upward yield stable and favorable performance. Considering both effectiveness and consistency, $w_L=0.8$ is a reasonable and economical choice.

The sensitivity analysis of $w_{\delta }$ is summarized in

Table 11. Sensitivity analysis results of $w_{\delta }$.

${\mathit{w}}_{\mathit{\delta }}$	Path Length (m)	Runtime (s)	Number of Nodes	Avgdistance (m)
0.01	149.59	0.09	53.60	1.29
0.05	149.61	0.09	53.60	1.29
0.10	149.98	0.09	53.60	1.26
0.15	150.28	0.09	53.60	1.26
0.20	151.04	0.09	53.60	1.21

. Unlike $w_p$ and $w_L$, the role of $w_{\delta }$ is to penalize lateral deviation during candidate evaluation. The results show that the influence of $w_{\delta }$ is comparatively mild within a small range. Specifically, when $w_{\delta }$ increases from 0.01 to 0.05, the path length only changes slightly from 149.59 m to 149.61 m, and the average obstacle distance remains nearly constant at 1.29 m. This indicates that small variations of the lateral-offset penalty do not substantially affect the local decision process.

When $w_{\delta }$ is further increased to 0.10 and 0.15, the path length rises to 149.98 m, and the average distance to obstacles drops to 1.26 m. At $w_{\delta }=0.20$, the path length further increases to 151.04 m, while the average obstacle distance decreases to 1.21 m. These results suggest that an excessively large lateral penalty suppresses necessary sideways adjustment during local obstacle avoidance, making the probing process less flexible and slightly reducing path quality and safety margin.

In general, $w_{\delta }$ behaves as a weak regularization term rather than a dominant decision factor. The algorithm is relatively insensitive to small perturbations of this parameter, but too large a value may gradually limit local detouring capability. Therefore, a small-value interval such as 0.01–0.05 is sufficient, and $w_{\delta }=0.05$ provides a suitable balance between suppressing redundant lateral oscillations and preserving necessary obstacle-avoidance flexibility.

Combining the above observations, the three parameters exhibit different sensitivity patterns. The parameter $w_p$ mainly affects the aggressiveness of forward expansion, and overly large values may lead to longer paths and reduced safety margins. The parameter $w_L$ plays a more decisive role in enabling one-step feasibility awareness, and its performance improves significantly once it reaches a sufficient level. In contrast, $w_{\delta }$ acts as a mild regularizer, and its influence is relatively limited unless it becomes too large.

More importantly, the results in Table 9, Table 10 and Table 11 show that the proposed method maintains stable runtime and node number under different parameter settings, indicating that the overall framework is robust and does not rely on delicate parameter tuning. Based on the sensitivity experiment, the final parameter configuration is set to

$$w_p=0.8,w_L=0.8,w_{\delta }=0.05,$$

(30)

which achieves a favorable trade-off among path quality, runtime, node scale, and obstacle clearance.

5. Conclusions

This paper proposes PFMS-RRT*, a progress-aware fused-sampling RRT* with multi-level strategy extension, to address the high randomness, low efficiency, long search time, and poor path quality of RRT* in complex obstacle environments. The method introduces three key components: (1) progress-aware fused sampling that adaptively reshapes the sampling distribution according to planning progress to reduce redundant samples and generate more informative ones; (2) a three-level goal-guided extension with progressively stronger fallback behaviors to improve extension success rate and stability in clutter; and (3) a smooth tangential APF-based fallback extension (with failure-driven probabilistic switching) to alleviate stagnation near obstacles and enhance robustness under local constraints.

Extensive simulations in four representative 2D environments, including structured corridors, dense random clutter, and narrow passages, demonstrate that PFMS-RRT* consistently delivers superior overall performance compared with several RRT* variants. In particular, PFMS-RRT* achieves shorter paths, lower and more stable runtime, and substantially fewer nodes, while maintaining competitive or improved obstacle clearance (Avgdistance), indicating stronger planning stability across scenarios.

Future work includes incorporating robot footprint and kinodynamic constraints (e.g., minimum turning radius and velocity/acceleration limits), extending the framework to higher-dimensional planning (the sampling region generalizes from an ellipse to an ellipsoid), and validating the method in more realistic settings with uncertainty and dynamic obstacles.