Efficient Path Planning of Robotic Arms Based on the Improved Informed-RRT* Algorithm

Abstract

To address the limitations of the standard Informed-RRT* algorithm in manipulator path planning, including low initial path search efficiency, susceptibility to local optima in complex obstacle environments, and high path redundancy, this paper proposes an improved Informed-RRT* algorithm tailored for manipulator applications. First, we construct a phased adaptive sampling framework that separates the initial path search and path optimization stages. A target region constraint strategy is introduced, and the sampling confidence probability is dynamically adjusted based on the current search phase and real-time path quality. This design significantly enhances the efficiency of feasible path discovery while effectively preventing premature convergence to local optima. Second, an adaptive step size mechanism guided by gravitational–repulsive coordination is developed. This mechanism dynamically adjusts the extension step size according to the local obstacle distribution, reducing invalid sampling and increasing the number of effective sampling points while strictly ensuring obstacle avoidance safety, thereby accelerating both path search and optimization processes. Finally, a dichotomy-based dynamic boundary path smoothing strategy is integrated to generate smooth intermediate path points near obstacle boundaries. This strategy eliminates redundant inflection points and reduces path length while maintaining a safe distance from obstacles. The performance of the proposed algorithm is comprehensively verified through multiple sets of comparative experiments in both 2D grid maps and ROS-based manipulator simulation environments. The experimental results demonstrate that compared with the standard Informed-RRT* algorithm, the proposed method achieves a relative reduction of 77.17% in the average time to first initial path in complex environments. The path planning success rate increases from 21% to 95%, corresponding to an absolute increase of 74 percentage points and a relative increase of 352.38%. Additionally, the average path length is relatively reduced by 24.81%.

1. Introduction

As the core execution component of robots, the path planning technology of manipulators directly determines the performance of complex planning tasks. The core objective of path planning is to find a path that enables the end-effector of the manipulator to move smoothly, stably and efficiently from the starting point to the target point while effectively avoiding obstacles [1,2]. However, in high-dimensional complex spaces, manipulator path planning still faces challenges such as low search efficiency and poor path quality.

To address these problems, scholars at home and abroad have proposed various path planning algorithms. Traditional graph search algorithms, including Dijkstra’s algorithm [3] and A* algorithm [4,5], can guarantee an optimal path, but their computational complexity rises sharply with the increase in spatial dimensions, making them unsuitable for high-dimensional manipulator planning problems. In addition, evolutionary algorithms inspired by biological behaviors, such as genetic algorithms [6] and ant colony algorithms [7,8], have certain advantages in path optimization, but also suffer from inherent drawbacks: genetic algorithms involve heavy computation and tend to premature convergence, while ant colony algorithms exhibit blind search in the early stage and easily fall into local optima.

Sampling-based planning algorithms, especially the Rapidly-exploring Random Tree (RRT) algorithm [9], have reduced the complexity of high-dimensional space planning with probabilistic completeness and become a research hotspot in manipulator path planning. Nevertheless, the classical RRT algorithm suffers from strong path randomness, slow convergence and low path quality. To this end, Karaman et al. [10] proposed the RRT* algorithm, which achieves asymptotic optimality by introducing parent node rewiring and rewiring mechanisms, yet the convergence speed of the initial solution remains slow. The Informed-RRT* algorithm proposed by Gammell et al. [11] further improved this method. After obtaining the initial path, it restricts the sampling region to a dynamically shrinking elliptical subset, significantly accelerating the convergence speed of optimization.

Subsequent scholars have carried out extensive improvements to enhance the quality of initial solutions and convergence speed. The Q-RRT* algorithm [12] expands the selection range of potential parent nodes by checking neighboring nodes and their ancestors using the triangle inequality, thus obtaining a better initial path. The F-RRT* algorithm [13] adopts a novel strategy that creates new nodes directly instead of selecting existing ones, and optimizes the path cost through binary search.

Despite the progress made in the above studies, existing methods still have three core limitations that have not been systematically addressed in high-dimensional complex manipulator path planning scenarios:

Low initial path search efficiency: Even with the elliptical sampling constraint of Informed-RRT*, it remains difficult to quickly obtain a feasible initial solution in heavily occluded environments.

Poor adaptability of fixed-step sampling: A large number of invalid samples are easily generated in dense obstacle and narrow passage areas, leading to a high planning failure rate.

Insufficient path optimization capability: The generated paths contain numerous redundant inflection points and detours, making them difficult to be directly applied to smooth motion control of manipulators.

To address the above problems, this paper proposes an improved Informed-RRT* algorithm integrating multi-module collaborative optimization, which carries out systematic innovations in three core links: sampling strategy, step size adjustment and path smoothing.

The main contributions of this paper can be summarized as follows:

A target region-guided phased dynamic adaptive sampling strategy is proposed, which divides the sampling process into exploration and optimization stages. The sampling distribution is dynamically adjusted according to search feedback and path quality, fundamentally solving the problem of blind global search in traditional algorithms.

An adaptive step size strategy guided by gravity–repulsion collaboration is designed, which combines the gravity–repulsion mechanism of artificial potential field with sampling step size adjustment. Dynamic step size adjustment can be achieved through only one line segment collision detection, greatly improving sampling effectiveness.

A dichotomy-based dynamic boundary smoothing path optimization strategy is proposed. Through obstacle centroid positioning and two dichotomy searches, smooth transition nodes are generated as close to obstacles as possible within the safety threshold, achieving simultaneous reduction in path length and improvement in smoothness.

A complete progressive ablation experiment system and high-dimensional verification platform are constructed. Through prototype experiments on 2D grid maps and ROS simulation experiments on 6-degree-of-freedom manipulators, the independent contributions and synergistic enhancement effects of each innovation module are systematically verified, proving the engineering practicability of the algorithm.

Compared with existing improved Informed-RRT* methods, the core novelties of this paper are reflected in the following aspects:

For the first time, a full-link collaborative optimization framework of global direction guidance, local step size adjustment and terminal path optimization is proposed, rather than isolated improvements in a single link, achieving simultaneous enhancements in the three dimensions of search speed, reliability and path quality.

The artificial potential field concept is innovatively applied only to step size adjustment rather than path generation, which not only retains the local guidance capability of the artificial potential field, but also completely avoids its inherent defect of being prone to falling into local optima.

A dynamic boundary smoothing method based on obstacle centroid is proposed, which can automatically generate paths close to obstacle boundaries while ensuring a safe distance, solving the problem that traditional smoothing methods tend to produce redundant detours.

Verified by grid map and ROS simulation experiments, the algorithm proposed in this paper significantly outperforms the traditional Informed-RRT* algorithm in terms of initial path time consumption, planning success rate and average path length, demonstrating its effectiveness and advancement in high-dimensional complex environments.

2. Informed-RRT* Algorithm

The RRT algorithm [14,15,16] conducts search through sampling in a random configuration space. Given a start point and a goal point, the algorithm randomly generates nodes in their neighborhood, keeps exploring toward the goal point, avoids obstacles, and prevents trapping in local minima. The principle of the RRT algorithm is shown in Figure 1.

Assuming the target environment is known, the algorithm takes Xstart as the root node of an empty tree T. It then randomly samples a new point Xrand on the map, and finds the nearest node to this point in the existing tree, denoted as Xnear. On this basis, a new node Xnew in the tree is generated by extending a fixed length toward Xrand. After that, the segment connecting Xnew and Xnear is checked for collision with any obstacles. If no collision is detected, Xnew is added to the tree, and a new node is thus included in the tree.

This step is executed repeatedly until the newly obtained node is sufficiently close to the goal Xgoal. Since it is rare for Xnew to coincide exactly with Xgoal, a certain range is set near the goal Xgoal. When the new node falls within this range, it is considered to have reached Xgoal.

The RRT* [17,18,19] (Rapidly-exploring Random Tree Star, an optimized variant of the Rapidly-exploring Random Tree, RRT) algorithm achieves continuous reduction in the total path length through node rewiring and reconnection operations. As shown in Figure 2, after generating a new node Xnew, the algorithm searches for three neighboring nodes a, b and c within a circular area centered at Xnew with radius R. It calculates the distances from each neighboring node to the new node Xnew, compares these distances among existing nodes, and finally selects node a, which has the shortest distance to Xnew, as the parent node of Xnew. After that, it checks whether other nearby nodes can shorten their distance to the start node by taking Xnew as their parent node. If so, the parent nodes of these existing nodes are reset. Since all existing nodes near Xnew tend to shorten their own paths, the parent nodes of nodes b and c are set to Xnew.

Informed-RRT* [20,21,22] first employs RRT* to search for an initial path. It then takes Xstart and Xgoal as the two foci of an ellipse, the shortest found path C_best as the major axis of the ellipse, and the straight-line distance between the two foci is denoted as C_min. Let a be the semi-major axis of the ellipse, b the semi-minor axis, and c half the distance between the two foci.

$$\mathrm{a}={\displaystyle \frac{{\mathrm{C}}_{\mathrm{b}\mathrm{e}\mathrm{s}\mathrm{t}}}{2}}$$

(1)

$$\mathrm{c}={\displaystyle \frac{{\mathrm{C}}_{\mathrm{m}\mathrm{i}\mathrm{n}}}{2}}$$

(2)

$$\mathrm{b}={\displaystyle \frac{\sqrt{{\mathrm{C}}_{\mathrm{best}}^2{-\mathrm{C}}_{\min }^2}}{2}}$$

(3)

With all parameters of the ellipse equation determined, a sampling space confined within the ellipse is constructed to restrict path generation, which helps the algorithm locate and converge to the solution set. A point is sampled inside the ellipse, and the RRT* method is used on the search tree to optimize the current path, thus completing one iteration. The algorithm then checks whether the newly obtained optimal path is better than the previous result. If so, C_best is updated to this new improved value. Through repeated iterations, the size of the ellipse is gradually reduced, narrowing the sampling range continuously. After sufficient iterations, the algorithm asymptotically converges to a near-optimal path, as illustrated in Figure 3.

3. Improved Informed-RRT* Algorithm

3.1. Target Region-Guided Phased Dynamic Adaptive Sampling Strategy

3.1.1. Target Point Region Constraint

The traditional target bias strategy combines sampling with the target point as the sampling point and random sampling.

For random sampling, for example, in a 2D map of size 30 × 50, this paper adopts the following strategy to generate random points: Randomly select coordinates from a certain buffer area inside the map to determine the position of the point. Specifically, the range of the map in the x-axis direction is [0, 30], and the range in the y-axis direction is [0, 50]. A buffer distance delta is set. The x-coordinate of the random point is obtained by uniform random sampling in the interval [delta, 30-delta], and the y-coordinate is obtained by uniform random sampling in the interval [delta, 50-delta]. Finally, the random point is returned in the form of a node, whose coordinates are the sampled (x, y) coordinates. The buffer safety distance delta set in this paper is 0.5.

For target bias sampling, the confidence probability p is generally set to 0.8; for each sampling, a point α in the interval [0, 1] is taken, and the sampling strategy β is determined according to the value range of α.

$$\mathsf{\beta }=\{\begin{array}{ll}\mathrm{A}& \mathsf{\alpha }\ge \mathrm{p}\\ \mathrm{B}& \mathsf{\alpha }<\mathrm{p}\end{array}$$

(4)

In contrast, the improved bias strategy proposed in this paper combines random sampling with generating sampling points that can be directly connected to the target point within a circle with the target point as the center and radius R. The confidence probability is set to p; for each sampling, a point α in the interval [0, 1] is taken, and the sampling strategy β is determined according to the value range of α.

$$\mathsf{\beta }=\{\begin{array}{ll}\mathrm{A}& \mathsf{\alpha }\ge \mathrm{p}\\ \mathrm{C}& \mathsf{\alpha }<\mathrm{p}\end{array}$$

(5)

Among them, A is random sampling, B is sampling with the target point as the sampling point, and C is sampling with the target point region as the sampling area.

This sampling strategy can achieve rapid path generation by generating sentinel nodes for transit when the target point is severely occluded, as shown in Figure 4. The sentinel nodes are a and b, which are generated by sampling in the target point area and can be directly connected to the target point. It is only necessary to judge whether they can be connected to the random sampling tree to quickly find the target. If only the target point is used as the sampling point, a large number of nodes will be generated in front of obstacles, and the target node can only be found by random sampling.

3.1.2. Phased Adaptive Sampling Strategy

The target point region constraint strategy guides sampling through a geometric region (circle), which is more robust than simple target point bias. However, its parameter, the confidence probability p, is fixed during each run, meaning it lacks the ability to respond to dynamic changes in the environment. For example, when obstacles in front of the target point are very dense, a fixed confidence probability may still generate a large number of invalid sentinel nodes even with target circular region sampling. For the Informed-RRT* algorithm, the sampling process can be divided into two stages: the exploration stage when no initial path is found, and the optimization stage when the initial path is found. For different stages, the generation of p should consider different perspectives, and different confidence probability generation methods should be adopted. Even in the same stage, p should be dynamically changed to improve the ability to respond to dynamic changes in the environment.

To this end, this paper proposes making the confidence probability p an adaptive variable deeply coupled with the algorithm’s search stage and real-time state.

In the exploration stage, when no initial path is found, the goal is to quickly discover a feasible solution. At this time, p should be dynamically adjusted according to search feedback (such as the number of consecutive failures) to maintain an intelligent balance between adhering to target orientation and random exploration.

In the optimization stage, after finding the initial path, the goal is efficient optimization. At this time, p should be linked to the current path quality and dynamically adjusted to maintain an intelligent balance between exploring new path topologies and optimizing existing paths.

• Adaptive Confidence Probability p in the Exploration Stage

When the algorithm fails to find a path for a long time in the exploration stage, reduce the probability p of target bias and increase the proportion of random sampling to avoid falling into local minima (e.g., when the target point is severely occluded).

p is initially set to a high initial value p_initial (e.g., 0.8). A failure counter is defined to record the number of consecutive target bias samplings that fail to expand tree nodes. Once the algorithm is stuck (i.e., failures increase), it indicates that the target direction may be severely occluded. At this point, the algorithm should automatically reduce its focus on the target and increase the intensity of random exploration. The following formula is used to dynamically calculate p:

$$\begin{array}{c}\mathrm{p}=\max \left({\mathrm{p}}_{\begin{array}{c}\mathrm{m}\mathrm{i}\mathrm{n}\end{array}},{\mathrm{p}}_{\mathrm{initial}}\times \exp (-\mathsf{\lambda }\times \mathrm{failures})\right)\end{array}$$

(6)

where p_initial is the initial confidence probability (e.g., 0.8), λ is the attenuation coefficient controlling the speed of probability decline, failures is the number of consecutive failures, and p_min is the minimum confidence probability (e.g., 0.5) to ensure that a certain target orientation is always retained. If a sampling expansion is successful, the failure counter is set to half of its original value instead of resetting to 0, which can avoid the influence of accidental sampling success events on the current confidence probability in some dense obstacle situations.

2.

Adaptive Confidence Probability p in the Optimization Stage

After finding the initial path, the algorithm enters the optimization stage. The traditional Informed-RRT* algorithm and most of its improved variants completely restrict sampling to the elliptical subset. However, this study finds that there is still room for optimization in the search strategy at this stage. The size of the elliptical subset is determined by the current optimal path cost C_best, which itself contains rich information about path quality.

When the area of the elliptical subset is large, it indicates that the currently found path C_best is long and the path has severe detours. This means there is significant optimization potential, and multiple distinct and shorter feasible paths may exist in the search space. At this time, the sampling strategy of the exploration stage should be implemented within the elliptical subset, as shown in Equation (7). The only difference is that the sampling at this stage is constrained within the elliptical subset.

$$\mathsf{\beta }=\{\begin{array}{ll}\mathrm{A}& \mathsf{\alpha }\ge \mathrm{p}\\ \mathrm{B}& \mathsf{\alpha }<\mathrm{p}\end{array}$$

(7)

Among them, A is random sampling within the ellipse, and B is sampling in the intersection of the target point region and the ellipse.

When the area of the elliptical subset is small, it indicates that the current path C_best is already short and close to the global optimal solution. At this time, the most effective strategy is to concentrate resources on fine search near the existing path inside the ellipse to further shorten the path.

To match the sampling strategy in the optimization stage with the path optimization potential, this paper innovatively proposes that the sampling strategy in the optimization stage should be intelligently adjusted based on the current path quality, and designs an adaptive confidence probability adjustment method based on the size of the elliptical subset.

The sampling strategy in the optimization stage combines the sampling strategy of the exploration stage within the entire elliptical subset and randomly selecting a point on the existing optimal path to generate a new sample by random offset within a circle of radius r.

Set the confidence probability as p; for each sampling, take a point α in the interval [0, 1] to determine the sampling strategy β according to the value range of α, as shown in Equation (8):

$$\mathsf{\beta }=\{\begin{array}{ll}\mathrm{A}& \mathsf{\alpha }\ge \mathrm{p}\\ \mathrm{B}& \mathsf{\alpha }<\mathrm{p}\end{array}$$

(8)

Among them, A represents the sampling strategy for the exploration phase within the elliptical subset, while B denotes the biased sampling within the circle defined by the optimal path neighborhood radius r.

The confidence probability p is intelligently adjusted according to the quality of the ellipse, as shown in the following formula:

$$\begin{array}{c}\mathrm{p}={\mathrm{p}}_{\mathrm{m}\mathrm{i}\mathrm{n}}+\left({\mathrm{p}}_{\mathrm{m}\mathrm{a}\mathrm{x}}-{\mathrm{p}}_{\mathrm{m}\mathrm{i}\mathrm{n}}\right)\left[1-\mathrm{e}\mathrm{x}\mathrm{p}\left(-\mathsf{\beta }\cdot {\displaystyle \frac{{\mathrm{C}}_{\mathrm{i}\mathrm{n}\mathrm{i}\mathrm{t}}-{\mathrm{C}}_{\mathrm{b}\mathrm{e}\mathrm{s}\mathrm{t}}}{{\mathrm{C}}_{\mathrm{i}\mathrm{n}\mathrm{i}\mathrm{t}}-{\mathrm{C}}_{\mathrm{m}\mathrm{i}\mathrm{n}}}}\right)\right]\end{array}$$

(9)

where C_best is the current optimal path length; C_min is the Euclidean distance between the start point and the end point, which is the theoretical lower bound of the path length; C_init is the length of the first feasible path searched by the algorithm, serving as the initial benchmark for path optimization; and β is the smoothing adjustment factor that controls the sensitivity of probability change with path quality.

When the path quality is poor (i.e., $({\mathrm{C}}_{\mathrm{b}\mathrm{e}\mathrm{s}\mathrm{t}}$) is close to $({\mathrm{C}}_{\mathrm{i}\mathrm{n}\mathrm{i}\mathrm{t}}$) with a large ellipse area), the ratio $\left({\displaystyle \frac{{\mathrm{C}}_{\mathrm{i}\mathrm{n}\mathrm{i}\mathrm{t}}-{\mathrm{C}}_{\mathrm{b}\mathrm{e}\mathrm{s}\mathrm{t}}}{{\mathrm{C}}_{\mathrm{i}\mathrm{n}\mathrm{i}\mathrm{t}}-{\mathrm{C}}_{\mathrm{m}\mathrm{i}\mathrm{n}}}}\right)$ approaches 0, and the exponential term tends to 1. Consequently, $(\mathrm{p}\approx {\mathrm{p}}_{\mathrm{m}\mathrm{i}\mathrm{n}})$. At this time, the algorithm adopts an exploration-phase sampling strategy within the elliptical subset with high probability to actively search for new paths that can greatly shorten the path length.

When the path quality is good (i.e., $({\mathrm{C}}_{\mathrm{b}\mathrm{e}\mathrm{s}\mathrm{t}}$) is close to $\left({\mathrm{C}}_{\mathrm{m}\mathrm{i}\mathrm{n}}\right)$ with a small ellipse area), the ratio $\left({\displaystyle \frac{{\mathrm{C}}_{\mathrm{i}\mathrm{n}\mathrm{i}\mathrm{t}}-{\mathrm{C}}_{\mathrm{b}\mathrm{e}\mathrm{s}\mathrm{t}}}{{\mathrm{C}}_{\mathrm{i}\mathrm{n}\mathrm{i}\mathrm{t}}-{\mathrm{C}}_{\mathrm{m}\mathrm{i}\mathrm{n}}}}\right)$ approaches 1 and the exponential term reaches its minimum value. In this case, $(\mathrm{p}\approx {\mathrm{p}}_{\mathrm{m}\mathrm{a}\mathrm{x}})$. The algorithm then performs local biased sampling in the neighborhood of the current optimal path with high probability, focusing on fine-tuning the existing path to accelerate convergence toward the optimal solution.

The adaptability of the proposed formula to the variability and extreme cases of the initial solution $\left({\mathrm{C}}_{\mathrm{i}\mathrm{n}\mathrm{i}\mathrm{t}}\right)$ is analyzed as follows.

$\left({\mathrm{C}}_{\mathrm{i}\mathrm{n}\mathrm{i}\mathrm{t}}\right)$, the first feasible solution obtained by the algorithm in the current scenario, inherently reflects the inherent difficulty and optimization potential of the scenario. The optimization space $\left(\right({\mathrm{C}}_{\mathrm{i}\mathrm{n}\mathrm{i}\mathrm{t}}-{\mathrm{C}}_{\mathrm{m}\mathrm{i}\mathrm{n}}\left)\right)$ corresponds to the maximum theoretical reduction in path length for the given scenario. Taking this as the benchmark enables adaptive matching between the sampling strategy and the optimization potential of the scenario.

To cope with the variability and extreme cases of $\left({\mathrm{C}}_{\mathrm{i}\mathrm{n}\mathrm{i}\mathrm{t}}\right)$, a triple robustness guarantee mechanism is embedded into the algorithm design to fundamentally avoid pathological behaviors:

Hard boundary constraints of confidence probability (ultimate safety net) Equation (9) explicitly set upper and lower bounds for the confidence probability: $({\mathrm{p}}_{\mathrm{m}\mathrm{i}\mathrm{n},\mathrm{o}\mathrm{p}\mathrm{t}\mathrm{i}\mathrm{m}\mathrm{i}\mathrm{z}\mathrm{e}}=0.2)$ and $({\mathrm{p}}_{\mathrm{m}\mathrm{a}\mathrm{x},\mathrm{o}\mathrm{p}\mathrm{t}\mathrm{i}\mathrm{m}\mathrm{i}\mathrm{z}\mathrm{e}}=0.8)$. Regardless of the variation in $\left({\mathrm{C}}_{\mathrm{i}\mathrm{n}\mathrm{i}\mathrm{t}}\right)$, the value of p is always restricted within this interval. This hard constraint acts as an ultimate safety net for all extreme cases, ensuring that the algorithm never exhibits pathological behaviors of over-reliance on a single sampling strategy under any circumstances.

Nonlinear smoothing property of the exponential function Equation (9) employs an exponential function for adaptive adjustment. The nonlinear characteristic of the exponential function naturally provides a smoothing effect, which effectively buffers fluctuations in $\left({\mathrm{C}}_{\mathrm{b}\mathrm{e}\mathrm{s}\mathrm{t}}\right)$. Through sensitivity analysis, the optimal smoothing factor $(\mathsf{\gamma }=3.0)$ is determined. This value enables the adjustment curve to be sufficiently sensitive to respond to changes in path quality while remaining smooth enough to resist random fluctuations of the initial solution.

Phase separation and independent operation mechanism $\left({\mathrm{C}}_{\mathrm{i}\mathrm{n}\mathrm{i}\mathrm{t}}\right)$ is only used in the optimization phase, whereas the sampling strategy in the exploration phase is completely independent of $\left({\mathrm{C}}_{\mathrm{i}\mathrm{n}\mathrm{i}\mathrm{t}}\right)$. No matter when the initial solution is found, the dynamic confidence probability adjustment mechanism in the exploration phase operates stably to ensure rapid discovery of the first feasible path. This design confines the variability of the initial solution to the optimization phase without affecting the initial path-searching performance, which is critical for the algorithm.

Quantitative analysis and experimental validation are conducted for extreme cases:

Strong random-seed-induced variability of the first feasible solution: When $\left({\mathrm{C}}_{\mathrm{i}\mathrm{n}\mathrm{i}\mathrm{t}}\right)$ is significantly suboptimal ($({\mathrm{C}}_{\mathrm{i}\mathrm{n}\mathrm{i}\mathrm{t}}\gg {\mathrm{C}}_{\mathrm{b}\mathrm{e}\mathrm{s}\mathrm{t}})$), the normalized factor $({\displaystyle \frac{{\mathrm{C}}_{\mathrm{i}\mathrm{n}\mathrm{i}\mathrm{t}}-{\mathrm{C}}_{\mathrm{b}\mathrm{e}\mathrm{s}\mathrm{t}}}{{\mathrm{C}}_{\mathrm{i}\mathrm{n}\mathrm{i}\mathrm{t}}-{\mathrm{C}}_{\mathrm{m}\mathrm{i}\mathrm{n}}}}\approx 0)$, and thus $(\mathrm{p}\approx 0.2)$. The algorithm performs global exploration with an 80% probability to actively search for better path topologies, which matches our expected behavior. As $\left({\mathrm{C}}_{\mathrm{b}\mathrm{e}\mathrm{s}\mathrm{t}}\right)$ gradually decreases, p rises smoothly and the proportion of local optimization increases progressively, achieving a natural transition from global exploration to local optimization.

When $\left({\mathrm{C}}_{\mathrm{i}\mathrm{n}\mathrm{i}\mathrm{t}}\right)$ is close to $\left({\mathrm{C}}_{\mathrm{b}\mathrm{e}\mathrm{s}\mathrm{t}}\right):$ When the first feasible solution is nearly optimal ($({\mathrm{C}}_{\mathrm{i}\mathrm{n}\mathrm{i}\mathrm{t}}\approx {\mathrm{C}}_{\mathrm{b}\mathrm{e}\mathrm{s}\mathrm{t}})$), the normalized factor $({\displaystyle \frac{{\mathrm{C}}_{\mathrm{i}\mathrm{n}\mathrm{i}\mathrm{t}}-{\mathrm{C}}_{\mathrm{b}\mathrm{e}\mathrm{s}\mathrm{t}}}{{\mathrm{C}}_{\mathrm{i}\mathrm{n}\mathrm{i}\mathrm{t}}-{\mathrm{C}}_{\mathrm{m}\mathrm{i}\mathrm{n}}}}\approx 0)$, leading to $(\mathrm{p}\approx 0.2)$.

The algorithm performs global exploration with an 80% probability to actively search for superior path topologies, during which the value of $\left({\mathrm{C}}_{\mathrm{b}\mathrm{e}\mathrm{s}\mathrm{t}}\right)$ rarely changes. Nevertheless, the 20% path optimization sampling ensures that the algorithm never abandons refinement entirely. Even if the initial solution is a local optimum, there remains an opportunity to discover a better global path.

To verify the feasibility of the above analysis under the condition where the initial solution is strongly affected by random seeds, 100 repetitive experiments are conducted in a complex obstacle environment of size $(100\times 150)$, with the results presented as follows (

Table 1. Robustness verification and ill-conditioned behavior control of confidence probability parameter under different initial C values.

Parameter	Value	Relative Fluctuation (Coefficient of Variation)
Average Initial Path Length (m)	167.82 ± 5.23	3.12%
Average Final Path Length (m)	143.85 ± 4.42	3.07%
Average Initial Path Generation Time (s)	2.89 ± 1.58	54.67%
Average Runtime per Valid Experiment (s)	42.75 ± 3.56	8.33%

):

The initial solution is indeed strongly affected by random seeds:

The absolute standard deviation of the average initial path generation time is 1.58 s, with a relative fluctuation as high as 54.67%. This is an inherent characteristic of all stochastic sampling-based algorithms and is entirely within normal expectations.

The final performance of the algorithm is extremely stable: Despite large fluctuations in the initial solution, the absolute standard deviation of the average final path length is only 4.42 m, corresponding to a relative fluctuation of merely 3.07%. This indicates that regardless of whether the first obtained solution is good or poor, the final path length stabilizes within the range of 139.43 m to 148.27 m after optimization by the proposed algorithm, achieving highly consistent path quality.

The overall runtime fluctuation is controllable: The absolute standard deviation of the average runtime for each valid experiment is 3.56 s, with a relative fluctuation of only 8.33%. This demonstrates that the overall computational time of the algorithm remains within an acceptably stable range without extreme outliers.

These results strongly verify that the triple robustness mechanism proposed in this paper (hard boundary constraints, exponential smoothing adjustment, and phase separation) can effectively eliminate random disturbances from the initial solution, compressing large-amplitude fluctuations in the initial stage into a narrow range for the final outputs, and guarantee the reliability of the algorithm in practical applications.

3.1.3. Proof of Probabilistic Completeness and Asymptotic Optimality of the Improved Sampling Strategy

Probabilistic completeness and asymptotic optimality are the core theoretical foundations of sampling-based path planning algorithms. In this section, we systematically prove that the phased dynamic adaptive sampling strategy proposed in this paper fully preserves all convergence properties of the original Informed-RRT* algorithm.

Preliminary Knowledge and Core Sampling-Related Convergence Conditions

We first define the two core convergence properties and the critical sampling-dependent conditions for the convergence of the original Informed-RRT* algorithm:

Definition 1 

(probabilistic completeness). Let  $\left(X_{free}\right)$ denote the free configuration space. For any feasible path $(\pi \subset X_{free})$, if the probability that the algorithm finds at least one feasible path approaches 1 as the number of iterations $(n\to \infty )$, the algorithm is said to be probabilistically complete.

Definition 2 

(asymptotic optimality). Let  $\left(c^{*}\right)$ be the cost of the globally optimal path, and $\left(c_n\right)$ be the cost of the optimal path found by the algorithm at the n-th iteration. If the probability that $(c_n\to c^{*})$ approaches 1 as $(n\to \infty )$, the algorithm is said to be asymptotically optimal.

At the sampling level, the convergence of the original Informed-RRT* algorithm relies on two indispensable core conditions:

Global sampling ergodicity: Any point in the free space $\left({\mathrm{X}}_{\mathrm{f}\mathrm{r}\mathrm{e}\mathrm{e}}\right)$ can be sampled infinitely often.

Optimal region coverage: The region containing the globally optimal path is continuously sampled, ensuring that points on the optimal path have infinite chances to be sampled.

The phased dynamic adaptive sampling strategy in this paper strictly satisfies the above two core conditions, thus completely inheriting the convergence properties of the original Informed-RRT* algorithm.

Proof of Probabilistic Completeness for the Improved Sampling Strategy

Theorem 1.

The proposed phased dynamic adaptive sampling strategy satisfies the requirement of probabilistic completeness.

Proof. 

The sampling strategy is divided into the exploration phase (no initial path obtained) and the optimization phase (initial path obtained), both of which satisfy global sampling ergodicity. □

Global sampling ergodicity in the exploration phase: Sampling in the exploration phase covers the entire free space $\left({\mathrm{X}}_{\mathrm{f}\mathrm{r}\mathrm{e}\mathrm{e}}\right)$, and only the confidence probability p of sampling within the target region is dynamically adjusted. According to the parameter settings in this paper, the minimum value of p satisfies $({\mathrm{p}}_{\mathrm{m}\mathrm{i}\mathrm{n},\mathrm{e}\mathrm{x}\mathrm{p}\mathrm{l}\mathrm{o}\mathrm{r}\mathrm{e}}=0.5>0)$. This means the probability of sampling the target region is always no less than 50%, and the probability of global random sampling is always no more than 50%.

By the Borel–Cantelli lemma, if an event occurs with a non-zero probability in each independent trial, it will occur infinitely often as the number of trials tends to infinity. Therefore, any point in $\left({\mathrm{X}}_{\mathrm{f}\mathrm{r}\mathrm{e}\mathrm{e}}\right)$ has a non-zero probability of being selected in each global random sampling, and will inevitably be sampled infinitely often as $(\mathrm{n}\to \mathrm{\infty }).$

Global sampling ergodicity in the optimization phase: Sampling in the optimization phase is mainly concentrated in a dynamically shrinking elliptical region with the start and goal points as foci and the current optimal path length as the major axis. To ensure global sampling ergodicity, a global random sampling ratio of $({\mathrm{p}}_{\mathrm{m}\mathrm{i}\mathrm{n},\mathrm{o}\mathrm{p}\mathrm{t}\mathrm{i}\mathrm{m}\mathrm{i}\mathrm{z}\mathrm{e}}=0.2)$ is retained in the optimization phase.

This indicates that even in the optimization phase, any point in $\left({\mathrm{X}}_{\mathrm{f}\mathrm{r}\mathrm{e}\mathrm{e}}\right)$ still has a non-zero probability of being sampled. According to the Borel–Cantelli lemma, these points will be sampled infinitely often as $(\mathrm{n}\to \mathrm{\infty })$.

In summary, whether in the exploration or optimization phase, the proposed sampling strategy ensures that any point in the free space can be sampled infinitely often, satisfying the core condition for probabilistic completeness.

Proof of Asymptotic Optimality for the Improved Sampling Strategy

Theorem 2.

The proposed phased dynamic adaptive sampling strategy satisfies the requirement of asymptotic optimality.

Proof. 

The core of asymptotic optimality is to guarantee that any point on the globally optimal path can be sampled infinitely often, so that parent node reselection and rewiring mechanisms can gradually optimize path costs and eventually converge to the global optimum. The proposed sampling strategy ensures this from three perspectives. □

Global sampling ergodicity enables sampling of optimal path points: The proposed sampling strategy retains a global random sampling ratio in all phases. Accordingly, any point on the globally optimal path has a non-zero sampling probability and will be sampled infinitely often as $(\mathrm{n}\to \mathrm{\infty })$, which meets the necessary condition for asymptotic optimality.

Dynamically shrinking elliptical region ensures continuous coverage of the optimal region: The sampling region in the optimization phase is a dynamically shrinking ellipse determined by the current optimal path cost $\left({\mathrm{C}}_{\mathrm{b}\mathrm{e}\mathrm{s}\mathrm{t}}\right)$. Since $\left({\mathrm{C}}_{\mathrm{b}\mathrm{e}\mathrm{s}\mathrm{t}}\right)$ is monotonically decreasing, the size of the elliptical region decreases monotonically and finally converges to the minimal ellipse enclosing the globally optimal path.

This means the region containing the globally optimal path is always taken as the primary sampling area, and more sampling resources are allocated to this region. This does not undermine asymptotic optimality; on the contrary, it accelerates the exploration of the optimal path and enables the rewiring mechanism to discover superior path topologies more rapidly.

Path-neighbor biased sampling does not damage convergence: The path-neighbor biased sampling introduced in the optimization phase only allocates partial sampling resources to the neighborhood of the current optimal path, without eliminating global random sampling. Hence, it does not affect global sampling ergodicity or cause the algorithm to fall into local optima.

In conclusion, the proposed sampling strategy ensures that any point on the globally optimal path can be sampled infinitely often, satisfying the core condition for asymptotic optimality.

3.2. Adaptive Step Size Guided by Gravity–Repulsion Synergy

In the traditional Informed-RRT* algorithm, the sampling step size is designed as a fixed value. If the candidate node Xnew generated with this step size collides with obstacles, this sampling will be directly invalidated. This mechanism is prone to generating a large number of invalid samples, resulting in a lack of random tree nodes and sparse distribution, making it difficult to efficiently complete initial path generation and subsequent path optimization.

Most existing improvement schemes for step size only adopt passive adjustment methods such as binary halving sampling and dynamic scaling of step size according to the number of iterations or collisions, which always stay at the shallow optimization level of step size values. Such methods completely fail to start from the essence of spatial geometric constraints and explore the positional relationship among the nearest node, random node and obstacles. They cannot fundamentally avoid invalid sampling and accurately guide the generation of effective nodes, thus making it difficult to solve the core problems of low sampling efficiency and poor path planning performance in complex obstacle environments.

To this end, this paper proposes an adaptive step size strategy guided by gravity–repulsion synergy [23,24,25], which integrates the gravity–repulsion idea of the artificial potential field method with the sampling direction guidance mechanism. It innovatively focuses on the positional relationship among the nearest node, random node and obstacles, and realizes dynamic step size adjustment through only two core collision detections. While ensuring environmental adaptability, it controls the time complexity at the O (1) level, greatly improving sampling efficiency, as shown in Figure 5.

First, define the core parameters: let ${\mathrm{X}}_{\mathrm{near}}({\mathrm{x}}_{\mathrm{near}},{\mathrm{y}}_{\mathrm{near}})$ be the node closest to the random sampling point ${\mathrm{X}}_{\mathrm{rand}}({\mathrm{x}}_{\mathrm{rand}},{\mathrm{y}}_{\mathrm{rand}})$ in the random tree (extendable to $({\mathrm{x}}_{\mathrm{near}},{\mathrm{y}}_{\mathrm{near}},{\mathrm{z}}_{\mathrm{near}})$ for 3D scenarios); ${\mathrm{L}}_{\mathrm{init}}$ is the initial step size between Xnear and Xrand (i.e., the Euclidean distance between the two points); ${\mathrm{l}}_{\max }$ is the preset maximum step size (to avoid collision risks caused by excessive step size); ${\mathrm{l}}_{\min }$ is the preset minimum step size (to avoid iterative redundancy caused by too-small step size); ${\mathrm{d}}_{\mathrm{obs},\mathrm{path}}$ is the shortest distance from the nearest obstacle on the path ${\mathrm{X}}_{\mathrm{near}}\to {\mathrm{X}}_{\mathrm{rand}}$ to X_near; ${\mathrm{k}}_{\mathrm{g}}$ is the gravity coefficient (adjusting the guidance strength of the sampling point distance on the step size); ${\mathrm{k}}_{\mathrm{r}}$ is the repulsion coefficient (adjusting the constraint strength of obstacles on the path on the step size); $\mathrm{collision}(\mathrm{u},\mathrm{v})$ is the collision detection function (returns 0 when there is no conflict and 1 when there is a conflict); and ${\mathrm{X}}_{\mathrm{new}}$ is the finally generated effective sampling node.

The specific process and core formulas of the adaptive step size guided by gravity–repulsion synergy are as follows:

Step 1: Calculation of initial step size and basic direction. First, calculate the initial step size ${\mathrm{L}}_{\mathrm{i}\mathrm{n}\mathrm{i}\mathrm{t}}$ between ${\mathrm{X}}_{\mathrm{n}\mathrm{e}\mathrm{a}\mathrm{r}}$ and ${\mathrm{X}}_{\mathrm{r}\mathrm{a}\mathrm{n}\mathrm{d}}$; the formula is:

$${\mathrm{L}}_{\mathrm{i}\mathrm{n}\mathrm{i}\mathrm{t}}=\sqrt{({\mathrm{x}}_{\mathrm{r}\mathrm{a}\mathrm{n}\mathrm{d}}-{\mathrm{x}}_{\mathrm{n}\mathrm{e}\mathrm{a}\mathrm{r}}{)}^2+({\mathrm{y}}_{\mathrm{r}\mathrm{a}\mathrm{n}\mathrm{d}}-{\mathrm{y}}_{\mathrm{n}\mathrm{e}\mathrm{a}\mathrm{r}}{)}^2}$$

(10)

At the same time, calculate the basic direction vector ${\overrightarrow{\mathrm{d}}}_0$ from ${\mathrm{X}}_{\mathrm{n}\mathrm{e}\mathrm{a}\mathrm{r}}$ to ${\mathrm{X}}_{\mathrm{r}\mathrm{a}\mathrm{n}\mathrm{d}}$; the formula is:

$${\overrightarrow{\mathrm{d}}}_0=\left({\displaystyle \frac{{\mathrm{x}}_{\mathrm{r}\mathrm{a}\mathrm{n}\mathrm{d}}-{\mathrm{x}}_{\mathrm{n}\mathrm{e}\mathrm{a}\mathrm{r}}}{{\mathrm{L}}_{\mathrm{i}\mathrm{n}\mathrm{i}\mathrm{t}}}},{\displaystyle \frac{{\mathrm{y}}_{\mathrm{r}\mathrm{a}\mathrm{n}\mathrm{d}}-{\mathrm{y}}_{\mathrm{n}\mathrm{e}\mathrm{a}\mathrm{r}}}{{\mathrm{L}}_{\mathrm{i}\mathrm{n}\mathrm{i}\mathrm{t}}}}\right)$$

(11)

Step 2: Initial direct connection judgment and gravity–repulsion calculation. First, judge whether ${\mathrm{X}}_{\mathrm{near}}$ and ${\mathrm{X}}_{\mathrm{rand}}$ can be directly connected without conflict: if $\mathrm{collision}({\mathrm{X}}_{\mathrm{near}},{\mathrm{X}}_{\mathrm{rand}})=0$, directly determine ${\mathrm{X}}_{\mathrm{new}}={\mathrm{X}}_{\mathrm{rand}}$ and complete this sampling; if there is a conflict, continue to perform gravity and repulsion calculation. Among them, gravity is generated by the sampling point ${\mathrm{X}}_{\mathrm{rand}}$, which is used to guide the step size to adapt to the direction of the sampling point, and its strength is proportional to the distance from ${\mathrm{X}}_{\mathrm{near}}$ to ${\mathrm{X}}_{\mathrm{rand}}$ (the farther the distance, the stronger the gravity, and the step size tends to increase to fit the sampling direction); repulsion is generated by obstacles on the path ${\mathrm{X}}_{\mathrm{near}}\to {\mathrm{X}}_{\mathrm{rand}}$, which is used to constrain the step size, and its strength is inversely proportional to the distance from the obstacle to ${\mathrm{X}}_{\mathrm{near}}$ (the closer the distance, the stronger the repulsion, and the step size tends to decrease to ensure safety).

Gravity strength ${\mathrm{F}}_{\mathrm{g}}$ calculation formula:

$${\mathrm{F}}_{\mathrm{g}}={\mathrm{k}}_{\mathrm{g}}\times \left(\mathrm{d}({\mathrm{X}}_{\mathrm{near}},{\mathrm{X}}_{\mathrm{rand}})+\mathsf{ϵ}\right)$$

(12)

where $\mathsf{ϵ}=10^{-3}$ is a small constant to avoid gravity being zero when $\mathrm{d}({\mathrm{X}}_{\mathrm{near}},{\mathrm{X}}_{\mathrm{rand}})=0$, and $\mathrm{d}({\mathrm{X}}_{\mathrm{near}},{\mathrm{X}}_{\mathrm{rand}})$ is the Euclidean distance between ${\mathrm{X}}_{\mathrm{near}}$ and ${\mathrm{X}}_{\mathrm{rand}}$.

Repulsion strength ${\mathrm{F}}_{\mathrm{r}}$ calculation formula: Obtain the distance ${\mathrm{d}}_{\mathrm{o}\mathrm{b}\mathrm{s},\mathrm{p}\mathrm{a}\mathrm{t}\mathrm{h}}$ from the nearest obstacle on the path ${\mathrm{X}}_{\mathrm{n}\mathrm{e}\mathrm{a}\mathrm{r}}\to {\mathrm{X}}_{\mathrm{r}\mathrm{a}\mathrm{n}\mathrm{d}}$ to ${\mathrm{X}}_{\mathrm{n}\mathrm{e}\mathrm{a}\mathrm{r}}$ along the path, and substitute it into the following formula to calculate repulsion:

$$\begin{array}{c}{\mathrm{F}}_{\mathrm{r}}=\{\begin{array}{ll}\mathrm{Abandon}\mathrm{this}\mathrm{sampling},& {\mathrm{d}}_{\mathrm{obs},\mathrm{path}}<{\mathrm{d}}_{\mathrm{safe}}\\ {\mathrm{k}}_{\mathrm{r}}·{\displaystyle \frac{{\mathrm{d}}_{\mathrm{safe}}}{{\mathrm{d}}_{\mathrm{obs},\mathrm{path}}}},& {\mathrm{d}}_{\mathrm{obs},\mathrm{path}}\ge {\mathrm{d}}_{\mathrm{safe}}\end{array}\end{array}$$

(13)

where ${\mathrm{d}}_{\mathrm{s}\mathrm{a}\mathrm{f}\mathrm{e}}$ is the preset safety distance. The core logic is that the closer the obstacle on the path ${\mathrm{X}}_{\mathrm{n}\mathrm{e}\mathrm{a}\mathrm{r}}\to {\mathrm{X}}_{\mathrm{r}\mathrm{a}\mathrm{n}\mathrm{d}}$ (the smaller ${\mathrm{d}}_{\mathrm{o}\mathrm{b}\mathrm{s},\mathrm{p}\mathrm{a}\mathrm{t}\mathrm{h}}$), the stronger the repulsion; when the obstacle distance on the path is less than ${\mathrm{d}}_{\mathrm{s}\mathrm{a}\mathrm{f}\mathrm{e}}$, abandon this sampling.

In this paper, the distance to the nearest obstacle along the path $\left({\mathrm{d}}_{\mathrm{o}\mathrm{b}\mathrm{s},\mathrm{p}\mathrm{a}\mathrm{t}\mathrm{h}}\right)$ adopted for attractive–repulsive step size adjustment is precisely defined as the Euclidean distance from $\left({\mathrm{X}}_{\mathrm{n}\mathrm{e}\mathrm{a}\mathrm{r}}\right)$ to the surface of the first obstacle along the direction of the line segment $\left({\mathrm{X}}_{\mathrm{n}\mathrm{e}\mathrm{a}\mathrm{r}}{\mathrm{X}}_{\mathrm{r}\mathrm{a}\mathrm{n}\mathrm{d}}\right)$. If this line segment does not intersect any obstacle, $({\mathrm{d}}_{\mathrm{o}\mathrm{b}\mathrm{s},\mathrm{p}\mathrm{a}\mathrm{t}\mathrm{h}}=+\mathrm{\infty })$.

All distance calculations are uniformly performed in the Cartesian workspace rather than the joint configuration space of the manipulator. This is the core design principle enabling the proposed algorithm to be efficiently extended to manipulators with arbitrary degrees of freedom (DoFs):

Distances in the workspace have explicit physical meaning, directly corresponding to the actual moving distance of the manipulator end-effector;

Calculation results are independent of the number of manipulator DoFs, ensuring cross-platform generality of the algorithm;

The exponential complexity of distance computation in high-dimensional configuration spaces is avoided.

Calculation Workflow for Different Scenarios

The Bresenham line traversal algorithm is used for distance calculation in all scenarios in this paper. As an efficient line traversal algorithm dedicated to discrete grid spaces, it uses only integer arithmetic with no floating-point computational overhead, and is a standard approach for collision detection in industrial robots.

The core idea of the Bresenham algorithm is to determine grids intersected by a straight line point-by-point via error-term accumulation:

Calculate the length differences in the line segment along the x- and y-axes (2D) or x-, y- and z-axes (3D).

Initialize the error term to 0 and start from the grid containing the starting point.

Add the directional length difference to the error term in each iteration. When the error term exceeds 0.5, move one grid in the corresponding direction and subtract 1 from the error term.

Repeat the above process until reaching the grid of the end point.

The advantages of this algorithm are as follows: it relies only on integer addition and comparison operations, achieving extremely fast computation; it traverses all grids actually crossed by the straight line without omission or repetition; and its computational complexity is only related to the length of the line segment, and completely independent of obstacle shape and complexity.

Since all obstacles are fixed and known in static environments, all grids are pre-classified into free grids and obstacle grids during environment initialization.

Prototype Experiments on 2D Grid Maps

The 2D Bresenham line traversal algorithm is implemented as follows:

Convert $\left({\mathrm{X}}_{\mathrm{n}\mathrm{e}\mathrm{a}\mathrm{r}}\right)$ and $\left({\mathrm{X}}_{\mathrm{r}\mathrm{a}\mathrm{n}\mathrm{d}}\right)$ into grid coordinates;

Starting from the grid containing $\left({\mathrm{X}}_{\mathrm{n}\mathrm{e}\mathrm{a}\mathrm{r}}\right)$, sequentially traverse all grids intersected by the line segment using the 2D Bresenham algorithm;

When the first obstacle grid is encountered, compute the Euclidean distance from the edge of this grid to $\left({\mathrm{X}}_{\mathrm{n}\mathrm{e}\mathrm{a}\mathrm{r}}\right)$, which is defined as $\left({\mathrm{d}}_{\mathrm{o}\mathrm{b}\mathrm{s},\mathrm{p}\mathrm{a}\mathrm{t}\mathrm{h}}\right)$;

If no obstacle is detected after traversing all grids, set $({\mathrm{d}}_{\mathrm{o}\mathrm{b}\mathrm{s},\mathrm{p}\mathrm{a}\mathrm{t}\mathrm{h}}=+\mathrm{\infty })$.

In a 2D grid with 1 m resolution, each traversal requires only 8–10 integer operations on average, with negligible computational overhead.

Simulation Experiments on Multi-DoF Manipulators

Following the exact same principle as the 2D scenario, the 3D workspace is discretized into uniform cubic grids, each pre-marked as free space or obstacle. Distance calculation adopts the 3D Bresenham ray-traversal algorithm, a direct extension of the 2D version with additional error-term calculation along the z-axis:

Convert $\left({\mathrm{X}}_{\mathrm{n}\mathrm{e}\mathrm{a}\mathrm{r}}\right)$ and $\left({\mathrm{X}}_{\mathrm{r}\mathrm{a}\mathrm{n}\mathrm{d}}\right)$ into 3D grid coordinates;

Starting from the grid containing $\left({\mathrm{X}}_{\mathrm{n}\mathrm{e}\mathrm{a}\mathrm{r}}\right)$, sequentially traverse all 3D grids intersected by the ray using the 3D Bresenham algorithm;

When the first obstacle grid is encountered, compute the Euclidean distance from the surface of this grid to $\left({\mathrm{X}}_{\mathrm{n}\mathrm{e}\mathrm{a}\mathrm{r}}\right)$, which is defined as $\left({\mathrm{d}}_{\mathrm{o}\mathrm{b}\mathrm{s},\mathrm{p}\mathrm{a}\mathrm{t}\mathrm{h}}\right)$;

If no obstacle is detected after traversing all grids along the ray, set $({\mathrm{d}}_{\mathrm{o}\mathrm{b}\mathrm{s},\mathrm{p}\mathrm{a}\mathrm{t}\mathrm{h}}=+\mathrm{\infty })$.

Handling of Arbitrary Non-Convex Obstacles

The grid-based method combined with the Bresenham algorithm inherently supports non-convex obstacles of arbitrary shapes without special processing:

Both convex and non-convex obstacles can be accurately represented by marking their spatial occupancy through 3D grids;

The ray-traversal algorithm only checks whether a grid is an obstacle, regardless of the specific shape and topological structure of obstacles;

Even for complex non-convex obstacles with holes and indentations, the algorithm can accurately locate the first collision point.

Step 3: Adaptive step size calculation and candidate node generation. Integrate the effects of gravity and repulsion to calculate the final adaptive step size ${\mathrm{l}}_{\mathrm{a}\mathrm{d}\mathrm{a}\mathrm{p}\mathrm{t}}$. Considering that gravity ${\mathrm{F}}_{\mathrm{g}}$ is a proportional function of distance (the value may be large), normalization processing is introduced to ensure the rationality of step size adjustment. The formula is:

$${\mathrm{l}}_{\mathrm{a}\mathrm{d}\mathrm{a}\mathrm{p}\mathrm{t}}=\min \left({\mathrm{l}}_{\mathrm{m}\mathrm{a}\mathrm{x}},\max \left({\mathrm{l}}_{\mathrm{m}\mathrm{i}\mathrm{n}},{\mathrm{L}}_{\mathrm{i}\mathrm{n}\mathrm{i}\mathrm{t}}\cdot {\displaystyle \frac{1+{\mathrm{F}}_{\mathrm{g}}/{\mathrm{F}}_{\mathrm{g},\mathrm{m}\mathrm{a}\mathrm{x}}}{1+{\mathrm{F}}_{\mathrm{r}}}}\right)\right)$$

(14)

(Note: ${\mathrm{F}}_{\mathrm{g},\mathrm{m}\mathrm{a}\mathrm{x}}$ is the preset maximum gravity threshold, used to normalize ${\mathrm{F}}_{\mathrm{g}}$ to the $\left[\mathrm{0,1}\right]$ interval to avoid step size runaway caused by excessive gravity; the min and max functions constrain the step size within the range $[{\mathrm{l}}_{\mathrm{m}\mathrm{i}\mathrm{n}},{\mathrm{l}}_{\mathrm{m}\mathrm{a}\mathrm{x}}]$; and the term $1+{\mathrm{F}}_{\mathrm{g}}/{\mathrm{F}}_{\mathrm{g},\mathrm{m}\mathrm{a}\mathrm{x}}$ in the formula is associated with the distance characteristic between ${\mathrm{X}}_{\mathrm{n}\mathrm{e}\mathrm{a}\mathrm{r}}$ and ${\mathrm{X}}_{\mathrm{r}\mathrm{a}\mathrm{n}\mathrm{d}}$. The farther the distance, the greater the normalized gravity, and the step size tends to increase; the term $1+{\mathrm{F}}_{\mathrm{r}}$ is associated with the distribution of path obstacles. The closer the obstacle, the greater the repulsion, and the step size tends to decrease. The two work together to achieve dynamic adaptation of comprehensive distance and obstacles.)

Based on the basic direction vector ${\overrightarrow{\mathrm{d}}}_0$ and the adaptive step size ${\mathrm{l}}_{\mathrm{a}\mathrm{d}\mathrm{a}\mathrm{p}\mathrm{t}}$, generate the candidate node ${\mathrm{X}}_{\mathrm{c}\mathrm{a}\mathrm{n}\mathrm{d}}$; the formula is:

$${\mathrm{X}}_{\mathrm{c}\mathrm{a}\mathrm{n}\mathrm{d}}=\left({\mathrm{x}}_{\mathrm{n}\mathrm{e}\mathrm{a}\mathrm{r}}+{\mathrm{l}}_{\mathrm{a}\mathrm{d}\mathrm{a}\mathrm{p}\mathrm{t}}\cdot {\overrightarrow{\mathrm{d}}}_0.\mathrm{x},{\mathrm{y}}_{\mathrm{n}\mathrm{e}\mathrm{a}\mathrm{r}}+{\mathrm{l}}_{\mathrm{a}\mathrm{d}\mathrm{a}\mathrm{p}\mathrm{t}}\cdot {\overrightarrow{\mathrm{d}}}_0.\mathrm{y}\right)$$

(15)

Step 4: Conflict verification and iteration termination. Perform collision detection on the candidate node ${\mathrm{X}}_{\mathrm{c}\mathrm{a}\mathrm{n}\mathrm{d}}$ (the second core detection), and judge the subsequent operation according to the return value of $\mathrm{c}\mathrm{o}\mathrm{l}\mathrm{l}\mathrm{i}\mathrm{s}\mathrm{i}\mathrm{o}\mathrm{n}({\mathrm{X}}_{\mathrm{n}\mathrm{e}\mathrm{a}\mathrm{r}},{\mathrm{X}}_{\mathrm{c}\mathrm{a}\mathrm{n}\mathrm{d}})$:

If $\mathrm{c}\mathrm{o}\mathrm{l}\mathrm{l}\mathrm{i}\mathrm{s}\mathrm{i}\mathrm{o}\mathrm{n}({\mathrm{X}}_{\mathrm{n}\mathrm{e}\mathrm{a}\mathrm{r}},{\mathrm{X}}_{\mathrm{c}\mathrm{a}\mathrm{n}\mathrm{d}})=0$ (no conflict), determine ${\mathrm{X}}_{\mathrm{n}\mathrm{e}\mathrm{w}}={\mathrm{X}}_{\mathrm{c}\mathrm{a}\mathrm{n}\mathrm{d}}$ and complete this sampling;

If $\mathrm{c}\mathrm{o}\mathrm{l}\mathrm{l}\mathrm{i}\mathrm{s}\mathrm{i}\mathrm{o}\mathrm{n}({\mathrm{X}}_{\mathrm{n}\mathrm{e}\mathrm{a}\mathrm{r}},{\mathrm{X}}_{\mathrm{c}\mathrm{a}\mathrm{n}\mathrm{d}})=1$ (conflict), reduce the step size to 1/2 of the current ${\mathrm{l}}_{\mathrm{a}\mathrm{d}\mathrm{a}\mathrm{p}\mathrm{t}}$, regenerate the candidate node and perform collision detection until the step size is less than ${\mathrm{l}}_{\mathrm{m}\mathrm{i}\mathrm{n}}$ (at this time, abandon this sampling).

3.3. Dynamic Boundary Smoothing Path Optimization Strategy Based on Dichotomy

Path cost optimization can be achieved by introducing the FindReachest process and CreateNode process of the F-RRT* algorithm. To further improve path smoothness, this paper proposes a smoothing improvement strategy based on the dichotomy algorithm to generate intermediate nodes q and p, whose core principle is shown in Figure 6. The specific generation strategy is as follows.

First, define the core parameters: Let the key nodes of the original path be $\mathrm{a}({\mathrm{x}}_{\mathrm{a}},{\mathrm{y}}_{\mathrm{a}})$, $\mathrm{b}({\mathrm{x}}_{\mathrm{b}},{\mathrm{y}}_{\mathrm{b}})$, $\mathrm{c}({\mathrm{x}}_{\mathrm{c}},{\mathrm{y}}_{\mathrm{c}})$, and the centroid of the obstacle be $\mathrm{d}({\mathrm{x}}_{\mathrm{d}},{\mathrm{y}}_{\mathrm{d}})$; ${\mathrm{L}}_{\mathrm{u}\mathrm{v}}$ denotes the Euclidean distance length of line segment $\overline{\mathrm{u}\mathrm{v}}$, ${\mathrm{l}}_{\mathrm{k}}$ is the step size of the k-th dichotomy iteration, ${\mathrm{l}}_{\mathrm{t}\mathrm{h}}$ is the termination boundary of the dichotomy algorithm, $\mathrm{m}\mathrm{i}{\mathrm{d}}_{\mathrm{k}}$ is the search length of the k-th iteration; $\mathrm{c}\mathrm{o}\mathrm{l}\mathrm{l}\mathrm{i}\mathrm{s}\mathrm{i}\mathrm{o}\mathrm{n}(\mathrm{u},\mathrm{v})$ is the collision detection function, where $\mathrm{c}\mathrm{o}\mathrm{l}\mathrm{l}\mathrm{i}\mathrm{s}\mathrm{i}\mathrm{o}\mathrm{n}(\mathrm{u},\mathrm{v})=1$ when line segment $\overline{\mathrm{u}\mathrm{v}}$ conflicts with obstacles, and $\mathrm{c}\mathrm{o}\mathrm{l}\mathrm{l}\mathrm{i}\mathrm{s}\mathrm{i}\mathrm{o}\mathrm{n}(\mathrm{u},\mathrm{v})=0$ when there is no conflict; the finally generated boundary nodes are $\mathrm{x}\left({\mathrm{x}}_{\mathrm{x}},{\mathrm{y}}_{\mathrm{x}}\right),\mathrm{y}({\mathrm{x}}_{\mathrm{y}},{\mathrm{y}}_{\mathrm{y}})$ and the smooth intermediate nodes are $\mathrm{p}({\mathrm{x}}_{\mathrm{p}},{\mathrm{y}}_{\mathrm{p}})$, $\mathrm{q}({\mathrm{x}}_{\mathrm{q}},{\mathrm{y}}_{\mathrm{q}})$.

Step 1: Obstacle centroid localization. The centroid of an obstacle is its geometric center. If the obstacle is an n-sided polygon (with vertex coordinates ${\mathrm{d}}_1({\mathrm{x}}_1,{\mathrm{y}}_1),{\mathrm{d}}_2({\mathrm{x}}_2,{\mathrm{y}}_2),\dots ,{\mathrm{d}}_{\mathrm{n}}({\mathrm{x}}_{\mathrm{n}},{\mathrm{y}}_{\mathrm{n}})$, and ${\mathrm{x}}_{\mathrm{n}+1}={\mathrm{x}}_1,{\mathrm{y}}_{\mathrm{n}+1}={\mathrm{y}}_1$), the calculation of its centroid $\mathrm{d}({\mathrm{x}}_{\mathrm{d}},{\mathrm{y}}_{\mathrm{d}})$ requires first solving the polygon area S, and then obtaining it through weighted summation. The formulas are as follows:

$$\mathrm{S}={\displaystyle \frac{1}{2}}\left|{\displaystyle \sum _{\mathrm{i}=1}^{\mathrm{n}}}({\mathrm{x}}_{\mathrm{i}}{\mathrm{y}}_{\mathrm{i}+1}-{\mathrm{x}}_{\mathrm{i}+1}{\mathrm{y}}_{\mathrm{i}})\right|$$

(16)

$$\{\begin{array}{l}{\mathrm{x}}_{\mathrm{d}}={\displaystyle \frac{1}{6\mathrm{S}}}{\displaystyle \sum _{\mathrm{i}=1}^{\mathrm{n}}}({\mathrm{x}}_{\mathrm{i}}+{\mathrm{x}}_{\mathrm{i}+1}\left)\right({\mathrm{x}}_{\mathrm{i}}{\mathrm{y}}_{\mathrm{i}+1}-{\mathrm{x}}_{\mathrm{i}+1}{\mathrm{y}}_{\mathrm{i}})\\ {\mathrm{y}}_{\mathrm{d}}={\displaystyle \frac{1}{6\mathrm{S}}}{\displaystyle \sum _{\mathrm{i}=1}^{\mathrm{n}}}({\mathrm{y}}_{\mathrm{i}}+{\mathrm{y}}_{\mathrm{i}+1}\left)\right({\mathrm{x}}_{\mathrm{i}}{\mathrm{y}}_{\mathrm{i}+1}-{\mathrm{x}}_{\mathrm{i}+1}{\mathrm{y}}_{\mathrm{i}})\end{array}$$

(17)

Step 2: Generation of boundary node x by binary search. Node x is located on line segment $\overline{\mathrm{a}\mathrm{d}}$, with the goal of making line segment $\overline{\mathrm{b}\mathrm{x}}$ conflict-free with obstacles. The specific search process is as follows:

(1)

Initial parameter calculation: First, calculate the total length ${\mathrm{L}}_{\mathrm{a}\mathrm{d}}$ of line segment $\overline{\mathrm{a}\mathrm{d}}$ (denoted as the initial length len); the formula is:

$${\mathrm{L}}_{\mathrm{a}\mathrm{d}}=\mathrm{l}\mathrm{e}\mathrm{n}=\sqrt{({\mathrm{x}}_{\mathrm{d}}-{\mathrm{x}}_{\mathrm{a}}{)}^2+({\mathrm{y}}_{\mathrm{d}}-{\mathrm{y}}_{\mathrm{a}}{)}^2}$$

(18)

(2)

Iteration initialization: Set the initial search length $\mathrm{m}\mathrm{i}\mathrm{d}$ to 1/2 of the total length of line segment $\overline{\mathrm{a}\mathrm{d}}$, and halve the step size $\mathrm{l}\mathrm{e}\mathrm{n}$ at the same time. The formulas are:

$$\{\begin{array}{l}\mathrm{m}\mathrm{i}{\mathrm{d}}_0={\displaystyle \frac{\mathrm{l}\mathrm{e}\mathrm{n}}{2}}\\ \mathrm{l}\mathrm{e}\mathrm{n}={\displaystyle \frac{\mathrm{l}\mathrm{e}\mathrm{n}}{2}}\end{array}$$

(19)

(3)

Iteration adjustment: Starting from node a, search for a candidate point m along the direction of $\overline{\mathrm{a}\mathrm{d}}$ that is at a length of mid from node a. Judge the collision status of $\overline{\mathrm{b}\mathrm{m}}$ and adjust mid, then halve the step size len again. The formulas for step size update and mid adjustment in the k-th iteration are:

$${\mathrm{len}}_{\mathrm{k}}={\displaystyle \frac{{\mathrm{len}}_{\mathrm{k}-1}}{2}}$$

(20)

$${\mathrm{mid}}_{\mathrm{k}}=\{\begin{array}{ll}{\mathrm{mid}}_{\mathrm{k}-1}-{\mathrm{len}}_{\mathrm{k}},& \mathrm{if}\mathrm{collision}(\mathrm{b},{\mathrm{m}}_{\mathrm{k}-1})=1\\ {\mathrm{mid}}_{\mathrm{k}-1}+{\mathrm{len}}_{\mathrm{k}},& \mathrm{if}\mathrm{collision}(\mathrm{b},{\mathrm{m}}_{\mathrm{k}-1})=0\end{array}$$

(21)

$${\overrightarrow{\mathrm{e}}}_{\mathrm{ad}}=\left({\displaystyle \frac{{\mathrm{x}}_{\mathrm{d}}-{\mathrm{x}}_{\mathrm{a}}}{{\mathrm{L}}_{\mathrm{ad}}}},{\displaystyle \frac{{\mathrm{y}}_{\mathrm{d}}-{\mathrm{y}}_{\mathrm{a}}}{{\mathrm{L}}_{\mathrm{ad}}}}\right)$$

(22)

$$\{\begin{array}{l}{\mathrm{x}}_{\mathrm{x}}={\mathrm{x}}_{\mathrm{a}}+{\mathrm{L}}_{\mathrm{ax}}·{\overrightarrow{\mathrm{e}}}_{\mathrm{ad}}\left(\mathrm{x}\right)\\ {\mathrm{y}}_{\mathrm{x}}={\mathrm{y}}_{\mathrm{a}}+{\mathrm{L}}_{\mathrm{ax}}·{\overrightarrow{\mathrm{e}}}_{\mathrm{ad}}\left(\mathrm{y}\right)\end{array}$$

(23)

where ${\overrightarrow{\mathrm{e}}}_{\mathrm{a}\mathrm{d}}$ is the unit vector of line segment $\overline{\mathrm{a}\mathrm{d}}$. Through such binary search operations, the conflict-free ${\mathrm{L}}_{\mathrm{b}\mathrm{x}}$ is obtained, and ${\mathrm{L}}_{\mathrm{c}\mathrm{y}}$ can be obtained in the same way.

Step 3: Generation of smooth intermediate nodes p and q. Taking node x as the reference, solve for node p on line segment $\overline{\mathrm{y}\mathrm{c}}$ via binary search to ensure that line segment $\overline{\mathrm{p}\mathrm{x}}$ is collision-free. The length formula of line segment $\overline{\mathrm{y}\mathrm{c}}$ is:

$${\mathrm{L}}_{\mathrm{y}\mathrm{c}}=\sqrt{({\mathrm{x}}_{\mathrm{c}}-{\mathrm{x}}_{\mathrm{y}}{)}^2+({\mathrm{y}}_{\mathrm{c}}-{\mathrm{y}}_{\mathrm{y}}{)}^2}$$

(24)

Then, taking node p as the reference, solve for node q on line segment $\overline{\mathrm{x}\mathrm{b}}$ via binary search to ensure that line segment $\overline{\mathrm{p}\mathrm{q}}$ is collision-free. The length formula of line segment $\overline{\mathrm{x}\mathrm{b}}$ is:

$${\mathrm{L}}_{\mathrm{xb}}=\sqrt{({\mathrm{x}}_{\mathrm{b}}-{\mathrm{x}}_{\mathrm{x}}{)}^2+({\mathrm{y}}_{\mathrm{b}}-{\mathrm{y}}_{\mathrm{x}}{)}^2}$$

(25)

The coordinate solving logic of nodes p and q is consistent with that of x and y.

Step 4: Smooth path construction. Replace node a in the original path with the generated intermediate nodes q and p, and construct new smooth line segments $\overline{\mathrm{b}\mathrm{q}}$, $\overline{\mathrm{q}\mathrm{p}}$, and $\overline{\mathrm{p}\mathrm{c}}$. The line segment set of the final smooth path is: ${\mathrm{P}\mathrm{a}\mathrm{t}\mathrm{h}}_{\mathrm{s}\mathrm{m}\mathrm{o}\mathrm{o}\mathrm{t}\mathrm{h}}=\{\overline{\mathrm{b}\mathrm{q}},\overline{\mathrm{q}\mathrm{p}},\overline{\mathrm{p}\mathrm{c}}\}$. Compared with the original path $\{\overline{\mathrm{b}\mathrm{a}},\overline{\mathrm{a}\mathrm{c}}\}$, the proposed path achieves improved smoothness and optimized obstacle proximity.

4. Experiments and Analysis

4.1. Experimental Setup

A hybrid simulation platform is adopted in the experiments, including a self-designed grid map simulation environment and the standard robot operating system (ROS) [26] (Robot Operating System, Version Noetic Ninjemys) simulation environment, so as to ensure the comprehensiveness of the validation.

Hardware Platform: All simulation experiments on grid maps are conducted on a computer equipped with an Intel Core i7-9750F CPU @ 2.60 GHz and 16 GB RAM (Intel Corporation, Santa Clara, CA, USA).

Software Environment: Grid map simulation: The operating system is Windows 11 (Microsoft Corporation, Redmond, WA, USA), the programming language is Python 3.8 (Python Software Foundation, Wilmington, DE, USA), and the integrated development environment is PyCharm 2022.3.3 (JetBrains s.r.o., Prague, Czech Republic). This environment is used for rapid prototype verification and quantitative comparison of the core performance of the algorithm.

ROS simulation: The operating system is Ubuntu 20.04, with ROS Noetic as the robot operating system. The MoveIt! framework is applied to motion planning of the manipulator, and the algorithm proposed in this paper is integrated into the Open Motion Planning Library (OMPL) for invocation. This environment is used to verify the practical performance of the algorithm in manipulator control tasks close to real scenarios.

4.2. Experimental Parameter Analysis of the Improved Algorithm

In this section, sensitivity analysis is conducted on the key parameters of the improved algorithm. The control variable method is employed to compare the performance of different parameter values. The parameters investigated and their experimental values are presented in

Table 2. Key parameter experiment settings of the improved algorithm.

Parameter	Symbol	Experimental Value
Minimum Step Size	${\mathrm{l}}_{\min }$	0.1 m to 1 m with an interval of 0.1 m
Maximum Step Size	${\mathrm{l}}_{\max }$	5 m to 10 m with an interval of 0.5 m
Dichotomy Iteration Termination Boundary	${\mathrm{l}}_{\mathrm{th}}$	0.1 m to 1 m with an interval of 0.1 m
Decay Coefficient in Exploration Stage	$\mathsf{\lambda }$	0.1 to 1 with an interval of 0.1
Smoothing Adjustment Factor in Optimization Stage	β	1.0 to 5.0 with an interval of 0.5
Circular Sampling Radius of Target Area	R	5 m to 10 m with an interval of 0.5 m

.

According to the comparative experimental results, the optimal parameter settings for the improved algorithm are determined, and the rest of the parameters are set to empirical values, as listed in

Table 3. Optimal parameter configuration of the improved algorithm.

Parameter	Symbol	Set Value
Gravitational Coefficient	kg	1.0
Repulsive Coefficient	kr	3.0
Obstacle Avoidance Safety Distance	dsafe	0.5 m
Maximum Repulsive Force Limit	Fr,max	10
Minimum Step Size	$l_{\min }$	0.7 m
Maximum Step Size	$l_{\max }$	8 m
Maximum Gravitational Threshold	Fg,max	10
Dichotomy Iteration Termination Boundary	$l_{\mathrm{th}}$	0.5 m
Initial Confidence Probability in Exploration Stage	pinitial	0.8
Minimum Confidence Probability in Exploration Stage	Pmin	0.5
Maximum Confidence Probability in Optimization Stage	Pmax	0.8
Minimum Confidence Probability in Optimization Stage	Pmin	0.2
Decay Coefficient in Exploration Stage	$\lambda$	0.5
Smoothing Adjustment Factor in Optimization Stage	β	3.0
Circular Sampling Radius of Target Area	R	8.5 m

.

4.3. Theoretical Basis and Scale Uniqueness of Parameter Optimality

For multi-hyperparameter random path planning algorithms, the transparency of the parameter tuning process and the generalization ability of parameters are core indicators for evaluating the scientific nature of the algorithm. If parameters are only empirically fine-tuned for specific scenarios, it is difficult to distinguish whether the algorithm performance improvement comes from the innovation of the method itself or overfitting to a specific dataset. To address this issue, all hyperparameters in this paper are not empirically adjusted for specific obstacle distributions. Instead, they are divided into two categories, spatial geometric parameters and algorithm mechanism parameters, which are uniquely determined by objective laws and theoretical derivations. Their optimal values are completely independent of the position, density, and shape of obstacles.

Spatial Geometric Parameters: Uniquely Determined by Map Scale and Digital Precision

Spatial geometric parameters directly define the basic granularity of the algorithm’s operation in the workspace. Their optimal values are only related to the physical size of the workspace and digital precision (grid resolution). In maps of the same size and resolution, the values of such parameters have theoretical uniqueness:

Minimum step size: Determines the highest precision of path planning. If the step size is smaller than the grid resolution, it will lead to redundant sampling and waste of computing resources; if the step size is too large, it will reduce the geometric accuracy of the path and easily miss narrow passages.

Maximum step size: This setting ensures that a single sampling can cover a sufficiently large spatial range while avoiding a sharp increase in collision probability caused by an excessively large step size.

Binary iteration termination boundary: Ensures the convergence and accuracy consistency of the path optimization process, independent of obstacle distribution.

Circular sampling radius of the target area: Ensures that the sampling coverage around the target point reaches a reasonable level, only related to the overall size of the map.

Algorithm Mechanism Parameters: Derived from Convergence and Balance Theory

Algorithm mechanism parameters are determined by the design logic and convergence characteristics of the algorithm itself. Their optimal values are the results of theoretical derivations and are independent of specific obstacle distributions:

Phased sampling attenuation coefficient: Theoretically, this parameter should ensure that the target confidence probability smoothly decreases from the initial value to the preset minimum value after 10 consecutive sampling failures, thereby achieving the optimal balance between exploration and exploitation. This convergence speed is derived based on the general exploration theory of random sampling algorithms, ensuring that the algorithm can quickly adjust the search strategy under any obstacle distribution.

Smoothing factor β in the optimization stage: Theoretically, this parameter should ensure that the sampling strategy smoothly switches from global exploration to local optimization when the path quality is optimized to 110% of the theoretical optimal value. This switching curve is designed based on the convergence characteristics of asymptotically optimal algorithms, ensuring that the algorithm does not fall into local optima while guaranteeing convergence speed.

Attraction coefficient and repulsion coefficient: Theoretical derivations show that when the ratio of the repulsion coefficient to the attraction coefficient is 3:1, the step size adjustment can achieve the optimal balance between obstacle avoidance constraints and goal orientation. This ratio is an inherent property of the gravity–repulsion step size adjustment mechanism and is independent of the specific position and density of obstacles.

Environmental Robustness Guarantee of Adaptive Mechanisms

One of the core innovations of the algorithm in this paper is the introduction of environmental adaptive adjustment mechanisms. These mechanisms are designed to cope with different obstacle distributions, so there is no need to adjust any external parameters with changes in obstacles:

Phased dynamic confidence probability mechanism: Automatically adjusts the goal bias intensity according to the number of consecutive sampling failures. When obstacles are dense, the number of consecutive failures increases, the confidence probability automatically decreases, and the proportion of global exploration increases; when obstacles are sparse, the number of consecutive failures decreases, the confidence probability automatically increases, and goal-oriented search is accelerated. This adaptive process is completely completed by the internal feedback mechanism of the algorithm without manual intervention.

Gravity–repulsion collaborative adaptive step size mechanism: Automatically adjusts the step size according to the real-time detected local obstacle distance. The closer to the obstacle, the stronger the repulsion and the smaller the step size; the farther from the obstacle, the stronger the attraction and the larger the step size. This adjustment is based on real-time perception of the local environment and is independent of the overall obstacle distribution, so it can maintain optimal sampling efficiency under any obstacle layout.

In summary, the external parameters of the algorithm in this paper only need to be set once according to the map size and grid resolution, and can automatically adapt to all obstacle distributions in maps of the same size. All parameter values have strict theoretical support rather than empirical tuning for specific scenarios, fundamentally eliminating the possibility of parameter overfitting and ensuring the generalization ability and scientific nature of the algorithm.

4.4. Experimental Procedures

Environment initialization: For each scenario, the coordinates of the start point and goal point, as well as the layout of obstacles (circular or rectangular obstacles), are set.

Initial path search stage: Starting from the start point, the algorithm generates random points under the constraint of the target region. New nodes are obtained using the adaptive step size strategy guided by gravitational and repulsive force coordination. Path optimization is then carried out via the dynamic boundary path smoothing strategy based on the dichotomy method to obtain the initial path.

Path optimization stage: After the initial path is obtained, the sampling strategy for the optimization stage is activated to sample within the elliptical subset. The dynamic boundary path smoothing strategy based on the dichotomy method is applied again for further path optimization to acquire the optimal path.

Termination condition: The algorithm terminates when the number of iterations reaches the preset value.

Recording of evaluation metrics: The path length, computation time, number of successful samplings and number of sampling points on the final path are recorded in real time for each experiment. The average path length, average generation time of the initial path, average number of successful samplings, average number of sampling points and planning success rate (number of successful path findings/total number of experiments) are then calculated statistically.

4.5. Explanation of Algorithm State Space and Dimensionality Independence

The research object of this paper is the path planning problem of general-purpose manipulators. The core innovation mechanisms have dimensionality independence and can be directly applied to manipulator systems with any degrees of freedom. To clearly verify the core principles of the algorithm and ensure engineering practicability, this paper adopts a progressive research paradigm of two-dimensional prototype verification and 6-degree-of-freedom manipulator verification. The state space design is as follows:

In the prototype verification stage, a two-dimensional grid map configuration space is adopted, and the motion of the manipulator is simplified to the planar movement of the end-effector. The core purpose of this design is to isolate the interference of non-algorithmic factors such as inverse kinematics solution, joint constraints, and multi-link collisions, and purely quantify the independent contributions and synergistic effects of the three innovation modules. All core algorithm logics are implemented in the Cartesian workspace and do not depend on the dimensionality of the configuration space.

In the engineering verification stage, a 6-degree-of-freedom joint configuration space is adopted, and the actual deployment of the algorithm is implemented based on the ROS MoveIt! platform. Since the three core innovations of this paper (phased adaptive sampling, gravity–repulsion adaptive step size, and dichotomy-based dynamic boundary smoothing) are all designed based on the geometric characteristics of the workspace and are independent of the number of degrees of freedom of the manipulator, the conclusions of the two-dimensional prototype verification can be completely migrated to high-dimensional manipulator scenarios. The results of the ROS simulation experiments also prove that the algorithm in this paper has also achieved significant performance improvements in 6-degree-of-freedom manipulator path planning.

4.6. Grid Map Experiments

Three experimental scenarios are designed in this paper, namely the ablation experiment in a 100 × 150 scenario for verifying the incremental gains of each individual improvement module, the 100 × 150 scenario for validating the dynamic boundary path smoothing optimization strategy based on the dichotomy method, and the 100 × 150 scenario with the integrated sampling strategy for verifying the comprehensive improvement effects.

Both the improved algorithm proposed in this paper and the Informed-RRT* algorithm are iterative algorithms. Compared with algorithms that only obtain an initial path, these algorithms can generate more accurate paths but also incur higher computational costs. Therefore, with a fixed number of iterations as the benchmark, this paper compares and analyzes the performance of different algorithms in the same environment through experiments to verify the effectiveness of the improved algorithm.

4.6.1. Validation of Ablation Experiment Modules

To accurately quantify the incremental contributions and layer-by-layer synergistic effects of the three core innovation modules proposed in this paper and eliminate the interference of parameter engineering tuning, a progressive controlled-variable ablation experiment is designed in this subsection. The experiment is conducted in a unified 100 × 150 2D grid map with complex obstacle environments, with the start point set at (25, 12) and the target point at (127, 90). The number of iterations is fixed at 1500. All experimental groups adopt exactly the same obstacle distribution, hardware platform and software environment, and each group of experiments is independently repeated 100 times to take the average value. The experiment adopts a design of gradually superimposing innovation modules, adding each core improvement sequentially starting from the baseline algorithm. The test indicators cover the average initial path generation time, average path length, planning success rate, number of sampling points on the shortest path and average time consumption per valid experiment.

A total of 4 experimental groups are set up in this experiment, gradually superimposing the core innovation modules of this paper starting from the baseline algorithm. The specific configurations are as follows:

Baseline group: Standard Informed-RRT* algorithm with a fixed step size of 10 and no additional path smoothing optimization;

Ablation group 1: Baseline group + phased dynamic adaptive sampling strategy;

Ablation group 2: Ablation group 1 + gravity–repulsion collaborative adaptive step size strategy;

Ablation group 3 (complete improved algorithm): Ablation group 2 + dichotomy-based dynamic boundary smoothing strategy.

The experimental results are shown in the following figure and table, which clearly demonstrate the incremental performance changes in each innovation module based on the existing improvements.

The incremental contribution analysis of each innovation module is as follows.

• Incremental Contribution Analysis of Each Innovation Module

Phased dynamic adaptive sampling strategy (first-stage increment): As the basic improvement of the algorithm, this module achieves an order-of-magnitude improvement in initial path search efficiency at a very small total time cost. Compared with the baseline group, after only adding this strategy:

As shown in

Table 4. Ablation experiment data table.

Experimental Group	Average Initial Path Generation Time/s	Average Path Length/m	Planning Success Rate	Number of Sampling Points on the Shortest Path	Average Time Consumption Per Valid Experiment/s
Baseline Group	12.66 ± 2.14	192.22 ± 8.76	21	26	32.24 ± 4.31
Ablation Group 1	1.92 ± 1.41	156.17 ± 5.94	85	16	35.68 ± 3.21
Ablation Group 2	2.26 ± 1.47	151.54 ± 4.85	94	28	37.89 ± 3.16
Ablation Group 3	2.89 ± 1.58	143.85 ± 4.42	95	14	42.75 ± 3.56

and Figure 7. The average initial path generation time drops significantly from 12.66 s to 1.92 s, a relative decrease of 84.83%. This is because the dynamically adjusted target area sampling confidence probability avoids blind global search, enabling the algorithm to quickly locate the feasible path direction.

The planning success rate increases from 21% to 85%, an absolute increase of 64 percentage points and a relative increase of 304.76%, proving that this strategy can still effectively find feasible solutions in complex occluded environments.

The average path length shortens from 192.22 m to 156.17 m, a relative decrease of 18.75%, indicating that phased sampling can discover better path topologies earlier.

The number of sampling points on the shortest path reduces from 26 to 16, a relative decrease of 38.46%. This is because focused sampling in the optimization stage reduces redundant inflection points in the path.

The average time consumption per valid experiment rises from 32.24 s to 35.68 s, a relative increase of 10.67%. There are two core reasons for the increased time consumption: First, the module itself needs to dynamically calculate the sampling confidence probability, introducing a small amount of computational overhead. Second, the target bias strategy significantly narrows the sampling range, making the growth of the random tree more concentrated in high-value areas, resulting in a significant increase in the number of valid sampling points that can be searched by subsequent rewiring and reconnection operations, and the computational load of a single iteration increases slightly. However, compared with the 84.83% improvement in initial path search efficiency, this time increase is completely within an acceptable range.

Gravity–repulsion collaborative adaptive step size strategy (second-stage increment): On the basis of phased sampling, this module further improves sampling effectiveness and path robustness, and controls the computational overhead at an extremely low level. Compared with ablation group 1, after adding this strategy:

The average initial path generation time rises slightly from 1.92 s to 2.26 s, a relative increase of 17.71%. The core reason for the minimal increase is that step size adjustment only requires performing a line segment collision detection along the path from Xnear to Xrand once to find the nearest collision point, without global distance search or multiple iterations.

The planning success rate further increases from 85% to 94%, an absolute increase of 9 percentage points and a relative increase of 10.59%. The adaptive step size enables the algorithm to pass through narrow passages more flexibly, solving the problem that fixed step size tends to get stuck in dense obstacle areas.

The average path length shortens from 156.17 m to 151.54 m, a relative decrease of 2.96%. The dynamically adjusted step size can more accurately approach the obstacle boundary, reducing unnecessary detours.

The number of sampling points on the shortest path increases from 16 to 28, a relative increase of 75%. This is not an increase in redundant points, but the adaptive step size generates more continuous intermediate valid nodes, making the geometric continuity of the path better and laying a foundation for subsequent smoothing optimization.

The average time consumption per valid experiment rises from 35.68 s to 37.89 s, a relative increase of 6.19%. In addition to the tiny overhead of step size adjustment itself, the improvement in effective sampling rate makes more valid nodes generated per iteration, and the processing volume of rewiring operations increases slightly accordingly, leading to a slow rise in total time consumption. However, the overall increase is far lower than the performance improvement range.

Dichotomy-based dynamic boundary smoothing strategy (third-stage increment): As the final optimization link of the algorithm, this module achieves a significant improvement in path quality without sacrificing core planning efficiency. Compared with ablation group 2, after adding this strategy:

The average initial path generation time rises from 2.26 s to 2.89 s, a relative increase of 27.88%. The planning success rate increases slightly from 94% to 95%, an absolute increase of 1 percentage point and a relative increase of 1.06%. The fine adjustment of the path boundary during the smoothing process solves the traffic problem in some edge scenarios.

The average path length further shortens from 151.54 m to 143.85 m, a relative decrease of 5.07%. This is because the smoothing strategy can approach the obstacles to the maximum extent within the safety threshold, eliminating redundant detours in the path.

The number of sampling points on the shortest path drops significantly from 28 to 14, a relative decrease of 50%. This is the core contribution of this module. By generating continuous smooth transition nodes, all sharp inflection points in the path are effectively deleted.

The average time consumption per valid experiment rises from 37.89 s to 42.75 s, a relative increase of 12.83%. The increased time consumption mainly comes from two aspects: First, dichotomy smoothing requires performing multiple line segment collision detection and node generation operations. Second, the higher quality of the smoothed path makes the rewiring operations in the subsequent optimization stage adjust the path topology more finely, bringing a small amount of additional computational load. However, compared with the significant improvement in path smoothness and length, this overhead is completely controllable.

2.

Overall Synergistic Enhancement Effect and Performance Trade-off Analysis

The experimental results show that the layer-by-layer superposition of the three innovation modules produces a significant 1 + 1 + 1 > 3 synergistic enhancement effect, and the increase in total time consumption is completely within a reasonable range acceptable for engineering.

Compared with the baseline group, the complete improved algorithm reduces the average initial path generation time by 77.17% relatively, achieving an order-of-magnitude efficiency improvement.

The planning success rate increases from 21% to 95%, an absolute increase of 74 percentage points and a relative increase of 352.38%, basically solving the problem of planning failure in complex environments.

The average path length is shortened by 25.16% relatively, achieving the global optimization of path efficiency.

The number of sampling points on the shortest path is reduced by 46.15% relatively, while ensuring the smoothness and simplicity of the path.

The average time consumption per valid experiment increases by 32.61% relatively, but this increase is far smaller than the improvement range of various core performance indicators, which is a typical engineering optimization of exchanging controllable costs for significant benefits.

4.6.2. Verification of Path Smoothness in 100 × 150 Scenario

Path planning is carried out in a 2D grid map with a circular obstacle environment, where the start point is set to (20, 5), the goal point to (128, 5), and the number of iterations is 1000. All conditions are kept the same except for the path smoothing and sampling strategies. Goal-biased sampling is adopted for all algorithms with a bias rate of 0.8 and a fixed step size of 10. Paths before and after smoothing are compared, and key data including path length and the number of sampling points on the path are recorded over 100 independent experiments.

The performance of each algorithm in this environment is shown in Figure 8. As can be clearly observed from

Table 5. Data results of five algorithms in the 100 × 150 scenario.

Algorithm	Average Path Length/m	Number of Sampling Points on Path
RRT	248.82 ± 11.23	28 ± 8
RRT*	231.33 ± 8.47	26 ± 7
Q-RRT*	214.87 ± 6.82	8 ± 3
F-RRT*	197.63 ± 3.64	19 ± 4
The Improved Algorithm	189.95 ± 2.71	25 ± 4

, compared with conventional algorithms, the improved algorithm in this paper achieves significantly enhanced path smoothness. The generated path approaches obstacles within the safety threshold, which not only greatly improves smoothness but also considerably shortens the path distance. The path length is reduced by approximately 23.66% compared with the RRT* algorithm.

In terms of the number of sampling points on the path, the RRT and RRT* algorithms produce numerous redundant sampling points due to the lack of path smoothness optimization. The Q-RRT* [27] algorithm optimizes the path through the FindReachest and CreatNode procedures and greatly reduces redundant points, but its path smoothness remains insufficient. The F-RRT* [28,29,30] algorithm improves path smoothness by adding the CreatNode procedure to generate new sampling points for path optimization. The improved algorithm in this paper further enhances path smoothness by creating two intermediate nodes via dichotomy-based path smoothing, and also achieves a shorter path length than F-RRT*.

In terms of average computation time, compared with F-RRT*, the proposed algorithm improves path quality without excessive increase in computational cost and even reduces the planning time.

4.6.3. 100 × 150 Scenario for Verifying the Comprehensive Improvement with the Integrated Sampling Strategy

In the 100 × 150 scenario, (25, 12) is the starting point and (127, 90) is the target point. For the fixed sampling strategy, the sampling step size is set to 10 and the number of iterations is set to 2000. Path planning simulations are carried out in an environment with circular and rectangular obstacles, and the average path length, average time for generating the initial path, and number of successful path findings are recorded through 100 experiments.

In the 100 × 150 scenario, it can be seen from Figure 9a that without a bias strategy, sampling points are scattered, making it difficult to reach the target point. Even if the target point is found, the scattered distribution of sampling points hinders iterative path optimization. In contrast, Figure 9b,c show that with a bias strategy, sampling points are concentrated and biased toward the target point, which improves algorithm efficiency. However, this leads to long initial path search time and a tendency to fall into local optima. As shown in Figure 9e, with the improved bias strategy proposed in this paper, sampling points remain concentrated, the time for searching the initial path is greatly reduced, and the length of the final generated path is also shortened.

Comparative experiments are carried out with Improved Algorithm A [31] as the benchmark. To tackle the problems of traditional RRT*, such as inadequate biased random sampling, low convergence efficiency, slow path-searching performance and poor path smoothness, Improved Algorithm A is proposed with multiple optimizations. Firstly, a dynamic ellipsoidal sampling strategy is introduced to accelerate path-space exploration by adaptively adjusting the sampling area. In addition, bidirectional RRT* is adopted to build two alternately growing search trees for bidirectional search, which effectively improves convergence speed. Secondly, a dynamic goal-oriented strategy greedily guides the random tree to grow rapidly toward the target point to enhance planning efficiency. A heuristic search mechanism is combined with RRT* to further speed up convergence. A stochastic sampling expansion strategy guides tree expansion toward unexplored areas to avoid local minima while ensuring global search capability. Local rewiring optimization is used to reduce the cumulative path cost of new nodes, balancing path length, smoothness and safety. To decrease iteration times, an improved artificial potential field method is integrated into the growth process of bidirectional random search trees to provide directional guidance for their expansion. Finally, path pruning is adopted to eliminate redundant nodes in the initial path, and cubic B-spline interpolation is applied to smooth the pruned path, generating the final trajectory with continuous curvature suitable for tracking.

As shown in Figure 9 and

Table 6. Data results of four algorithms in the 100 × 150 scenario.

Algorithm	Average Path Length (m)	Average Initial Path Generation Time (s)	Number of Successful Path Finds
Unbiased Informed-RRT*	191.32	12.66	21
Target sampling rate of 0.8	171.09	5.20	44
Artificial potential field guided	169.66	4.89	48
Comprehensive performance of Improved Algorithm A	148.22 ± 5.21	3.15 ± 1.88	87
Proposed improved algorithm	143.85	2.89	95

. The experimental results clearly show that the proposed algorithm comprehensively outperforms Improved Algorithm A in average path length, initial path generation efficiency and planning success rate. Although Improved Algorithm A integrates various strategies including bidirectional RRT* and artificial potential field guidance, it lacks sufficient global exploration capability in complex obstacle-rich environments, making it prone to premature convergence and local optima, and thus cannot fully explore better path topologies. Meanwhile, its path optimization module only depends on basic local rewiring and smoothing, with weak refined optimization capacity that fails to further compress path length. As a result, it achieves an average path length of 148.22 m with a standard deviation of 5.21 m, featuring longer paths and poorer stability. In comparison, the proposed algorithm avoids local optima in complex environments through adaptive attractive–repulsive step size regulation and the triple-robust confidence probability mechanism, balancing large-scale global exploration and fine local optimization. Moreover, equipped with a more efficient path optimization strategy, it deeply refines path cost, reducing the average path length to 143.85 m with a standard deviation of 4.42 m. In addition, the superposition of multiple strategies in Improved Algorithm A causes unnecessary computational redundancy, leading to an initial path generation time of 3.15 s with larger fluctuations. In contrast, relying on the fast Bresenham grid-based detection algorithm, the proposed algorithm takes only 2.89 s to generate the initial path. It obtains 95 successful planning runs in 100 trials, far more than the 87 successes of Improved Algorithm A, which fully demonstrates its superior overall performance and robustness in complex obstacle environments.

Compared with the unbiased Informed-RRT* algorithm, the proposed algorithm achieves a relative reduction of 24.81% in average path length and an absolute reduction of 6.13 s in average initial path generation time. In 100 repetitive experiments, the number of successful planning runs rises from 32 to 98, corresponding to a 66-percentage-point increase in planning success rate (from 32% to 98%), which verifies a significant overall performance improvement.

4.7. ROS Simulation Experiment

This paper conducts path simulation for the manipulator based on ROS, with the operating system being Ubuntu 20.04 and the ROS version Noetic. MoveIt is a robotic development library dedicated to manipulators, which integrates the Open Motion Planning Library (OMPL). The improved Informed-RRT* algorithm proposed in this paper is implemented in OMPL and can be invoked by MoveIt.

To enable the improved algorithm to function effectively in practical manipulator path planning, this section establishes a simulation environment containing cuboid obstacles on MoveIt. Identical obstacles, start and goal configurations are used to evaluate the performance of the algorithm on a 6-DOF manipulator. For an intuitive understanding of the planning success rate and path performance, the maximum planning time is set to 2 s. The success rate of motion planning is calculated for each trial, and the results are recorded in the user interface of the MoveIt client over 100 repeated experiments.

Compared with the path planned by the standard Informed-RRT* algorithm (with goal-biased sampling and a sampling rate of 0.8), the path generated by the improved algorithm is shorter and smoother, as shown in Figure 10. Subfigure (a) illustrates the start and goal states of the manipulator, which is required to move from its vertical initial configuration to the goal configuration through three obstacles. The manipulator in this scenario has 6 degrees of freedom. Subfigure (b) shows the path planned by the original Informed-RRT* algorithm, which features long search time, long path length and high motion cost for the manipulator. Subfigure (c) presents the path planning result of the improved Informed-RRT* algorithm, where the path length is clearly shortened, thus reducing the motion cost of the manipulator.

The relevant experimental data are presented in

Table 7. Comparison of ROS simulation environment data.

Algorithm	Average Path Length (m)	Average Time to First Solution (s)	Number of Successful Runs
Standard Informed-RRT*	3.56 ± 0.62	1.86 s ± 0.57	53
Proposed Algorithm	2.37± 0.37	1.05 s ± 0.42	92

. The average first path search time of the Informed-RRT* algorithm is 1.86 s, while that of the improved algorithm is 1.05 s, representing a relative reduction of 43.55%. The path search success rate of the Informed-RRT* algorithm is 53%, which increases to 92% after improvement, an absolute increase of 39 percentage points. In terms of average path length, it is 3.56 m before improvement and decreases to 2.37 m after improvement, with a relative reduction of 33.43%.

5. Conclusions

This paper proposes an improved Informed-RRT* algorithm, which mainly carries out innovative improvements from three aspects: sampling strategy, step size growth strategy and path optimization.

In the path search link, the target point region constraint strategy and phased adaptive sampling strategy are added, and the sampling stage is innovatively divided into the search stage and the optimization stage. A phased adaptive confidence probability mechanism is designed, which significantly improves the path search speed of the Informed-RRT* algorithm in complex environments. Using the dichotomy-based dynamic boundary smoothing path optimization strategy, smooth intermediate path points are generated within the obstacle safety boundaries, which effectively reduces path redundancy, relatively shortens the path length while significantly improving path smoothness, and can meet the requirements of smooth manipulator motion. In addition, in terms of the step size growth strategy, the optimal sampling step size is determined through the gravity–repulsion collaborative guided adaptive step size strategy, which increases the number of effective sampling points and further improves the comprehensive performance of the algorithm.

The experimental results show that compared with the standard Informed-RRT* algorithm, the proposed method achieves a relative reduction of 77.17% in the average time to obtain the initial path for the first time in complex environments; the path planning success rate increases from 21% to 95%, with an absolute increase of 74 percentage points and a relative increase of 352.38%; and the average path length achieves a relative reduction of 24.81%. The experimental results verify that the improved Informed-RRT* algorithm proposed in this paper can significantly improve path search efficiency and generate shorter and smoother paths. The generated smooth trajectories can provide good reference orbits for manipulator motion, have strong engineering practical value, and are suitable for manipulator trajectory planning design.

The next step will be to obtain the environmental information around the manipulator based on a depth camera, perform real-time modeling and tracking of dynamic obstacles, realize efficient trajectory planning of the manipulator in an accurate dynamic obstacle environment, and quickly find the globally optimal feasible motion path on the premise of minimizing manipulator motion energy consumption and joint wear.