Research on optimal path sampling algorithm of manipulator based on potential function

Abstract

Aiming at the problems of low success rate, long time and tortuous path of the traditional Rapidly-exploring Random Trees series of algorithms for path planning, this paper proposes the optimal path sampling algorithm based on the potential function (AP-RRT*), which solves the path planning problem of the manipulator in three-dimensional space. First, the potential function is defined and the concept of sampling termination distance is proposed. Second, a secondary sampling strategy is proposed in combination with the potential function to improve the planning speed and increase the coverage rate. Third, adaptive weights and adaptive step size are used to dynamically adjust the planning direction and distance, thereby improving the planning efficiency. Moreover, when performing node reconnection, dynamic retrieval circles are set to ensure path quality while reducing computational consumption. Finally, the improved algorithm, along with other algorithms, is simulated and experimentally verified in MATLAB and ROS. The results show that the AP-RRT* algorithm is superior in terms of path length, planning time, iterations, number of waypoints and success rate.

1 Introduction

In recent years, the manipulator has become a research hotspot in the field of robotics (Suarez et al. 2018). The traditional teaching methods are time-consuming and laborious.

With the growing demand for intelligent solutions, the field of manipulator path planning has transitioned from its historical reliance on teaching pendants and manual drag teaching to autonomous path planning. As a result, autonomous path planning for manipulators has emerged as a prominent research topic.

Path planning refers to the process of finding an optimal, collision-free path from the starting point to the target point, considering the given environment and constraints (Sang et al. 2021). Efficient and safe path planning is of utmost importance for manipulators, mobile robots, and other systems. Based on different search strategies, path planning algorithms can be categorized into graph-based algorithms, sampling-based algorithms, and intelligent algorithms.

The most typical of graph-based algorithms are Dijkstra (Noto and Sato 2000). Subsequently, based on Dijkstra, A*(Ju et al. 2020) cleverly design heuristic functions according to prior information about the starting and ending points. Korf have improved the A*, with IDA*(Korf 1985) using a limited iterative deepening algorithm and optimizing pruning through the evaluation function, reducing search memory consumption. Stentz proposed the D*(Stentz 1997), based on the efficiency of reusing already searched nodes, which is higher than the efficiency of recalculating. Although graph search algorithms have been well developed, these algorithms require grid representation of the map, discretizing the configuration space, significantly reducing the application scope of the algorithms (Wang et al. 2020).

Genetic algorithm is the earliest intelligent optimization algorithm applied to optimization problems, and this algorithm and its derivatives have been applied in the research of robot path planning (Hao et al. 2022). Subsequently, inspired by the release of pheromones to find paths during ant foraging, Dorigo et al. proposed the ACO (ant colony algorithm) (Tang 2009). PSO (Kennedy and Eberhart 1995) was initially proposed by Kennedy and Eberhardt, the PSO algorithm was conceived and formulated from observations of the social behaviour of flocks of birds and schools of fish. Zhang (2018) proposed A novel particle swarm optimization based on prey–predator relationship. Ye et al. (2017) proposed A novel multi-swarm particle swarm optimization with dynamic learning strategy.

When Zhao et al. (2018) use the ACO for manipulator path planning, they first carry out exhaustive mathematical modelling of the 3D space, and then use the particle swarm optimization algorithm (Houssein et al. 2021) to train the parameters in the ant colony algorithm to record the optimal parameter combinations, and then finally search for the optimum through the ant colony algorithm. As the algorithm initially finds the potential optimal solution through random selection, it takes a long time to play the role of positive feedback, which leads to slower convergence at the initial stage of the algorithm, in addition, the algorithm needs a large amount of training data to ensure the reliability, which leads to a significant increase in consumption. Nevertheless, while positive feedback enhances the algorithm's convergence speed, the algorithm may encounter challenges when subjected to minor perturbations, especially when a sub-optimal solution becomes more favourable. In such cases, the positive feedback may expedite the adoption of the sub-optimal solution, causing the algorithm to become trapped in a local optimum, thereby hindering its ability to escape from local minima.

The sampling-based algorithm possesses probabilistic completeness and progressive optimality (Wu et al. 2019; Qi et al. 2020), meaning that if a feasible path exists, by employing appropriate strategies and conducting a sufficient number of iterations, a progressively optimal path can be discovered. The RRT series algorithms have become a hot spot in the field of robot path planning due to their ability to avoid modelling the environment space and their effectiveness in high-dimensional space planning. RRT is an algorithm proposed by LaValle and Steven to facilitate path search in non-convex high-dimensional space by constructing tree nodes (LaValle 1998). The algorithm continuously samples and detects in the search space from the starting point, and integrates the collision-free path points into the node tree until the target node is searched. However, RRT still faces certain challenges, including issues related to poor orientation, slow convergence speed, and low efficiency. To address these issues, numerous scholars have conducted research on these algorithms. Karaman and Frazzoli proposed RRT* algorithm with progressive optimality (Karaman et al. 2010), which adds optimization strategies such as reselection of parent nodes to ensure the optimality of the current node. Akgun and Stilman proposed the Bidirectional RRT* (Bi-RRT*) algorithm, which performs a two-way search from the starting point and the end point to improve the convergence speed (Akgun et al. 2011). At the same time, heuristic methods are used to selectively generate new nodes to improve the path. Informed RRT* increases the optimization of the sampling space, and introduces the elliptical sampling space to improve the sampling efficiency (Gammell et al. 2014). Furthermore, in response to these challenges, several optimization algorithms have been developed, including PQ-RRT*, RRT-connect, DT-RRT, and others (Li et al. 2020; Kang et al. 2021; Moon and Chung 2014).

The research on RRT and its improved algorithms is limitless. Each algorithm is tailored to a specific application scenario, thereby mitigating certain issues to a certain extent. Therefore, optimizing the algorithm for a specific environment, especially for manipulator path planning, holds significant importance. This paper proposes an optimal path sampling planning algorithm based on a potential function (AP-RRT*). In response to some shortcomings of the RRT and its variant algorithms mentioned above, the improvements of AP-RRT* are as follows.

In terms of planning efficiency:

• (a) Propose sampling termination distance to accelerate planning process.

• (b) Improve path planning speed by generating new nodes twice.

In terms of planning quality:

• (a) Generating directional guidance for nodes through potential fields to improve effective sampling rate.

• (b) Effectively adjusting the planning process through adaptive strategies to shorten planning length.

In the end, the effectiveness of the AP-RRT* algorithm and other algorithms is verified through simulations and experiments conducted on the MATLAB and ROS platform.

The main contributions of the paper are summarized as follows:

• 1. A optimal path sampling algorithm based on the potential function (AP-RRT*) are proposed for the path planning issues of robotic manipulators in three-dimensional space. Building upon sampling principles, we propose optimization strategies and integrate a potential field to guide the sampling process.

• 2. The concepts of termination sampling distance, secondary node expansion, and adaptive strategy are introduced, aiming to expedite the planning process, reduce planning time, and alleviate path convolution.

• 3. A simulation validation environment and experimental setup were established in MATLAB and ROS, providing tools and platforms for substantiating the algorithm's superiority.

2 Basic principle introduction

2.1 RRT* (rapidly-exploring random trees star)

RRT* improves the path length by optimizing the master–slave relationship of nodes based on RRT (Zhang et al. 2023). Two main improvements have been made: reselecting the parent node and regional node reconnection (Mohammed et al. 2021).

Figure 1 illustrates the process of reselecting the parent node. A circle is generated with $q_{new}$ as the center and $r_1$ as the radius, and the center is connected with the nodes inside the circle. According to the principle of the nearest total distance from $q_{start}$ to $q_{new}$, the parent node for $q_{new}$ is re-selected. The original path is:$s \to a \to b \to c \to d \to n$, with a length of 25. The adjusted path is: $s \to e \to f \to n$, whose length is 14. Therefore, the parent node of $q_{new}$ is adjusted from $q_{nearest}$ to $f$.

Fig. 1

Reselect the parent node

Figure 2 illustrates the process of regional node reconnection. The original path is $s \to a \to b \to c$ with a length of 15. Using $q_{new}$ as the center and $r_2$ as the radius, draw a circle and connect it with the tree nodes inside. Now, the path length from $q_{new}$ to $c$ node is shorter than the original path, that is:$s \to d \to q_{new} \to c$ with a length of 11. Therefore, the parent of node $c$ is adjusted from b to $q_{new}$.

Fig. 2

Regional reconnection

The pseudo-code of the algorithm is shown in Table 1.

Table 1 RRT* pseudo-code

2.2 APF (artificial potential filed)

APF assumes that the object moves under the influence of virtual field forces: the target point generates a gravitational field, guiding the object to move towards it, while obstacles generate a repulsive field, pushing the object away to avoid collisions (Hsueh et al. 2022). At each point, the resultant force on an object is the sum of all gravitational and repulsive forces. The potential field is constructed as follows:

2.2.1 Gravitational filed

$$U_{att} (q) = \frac{1}{2}\xi \rho^2 (q,q_{goal} )$$

(1)

where:$\xi$ denotes the gravitational scale factor,$\rho (q,q_{goal} )$ denotes the distance between the current state of the object and the target state. The gravitational function is obtained by taking the gradient of the gravitational field:

$$F_{att} (q) = - \nabla U_{att} (q) = \xi \rho (q,q_{goal} )$$

(2)

2.2.2 Repulsion field

$$U_{rep} (q) = \left\{ {\begin{array}{*{20}c} {\frac{1}{2}\eta (\frac{1}{{\rho (q,q_{obs} )}} - \frac{1}{\rho_0 })^2 ,if\rho (q,q_{obs} ) \le \rho_0 } \\ {0,if\rho (q,q_{obs} ) > \rho_0 } \\ \end{array} } \right.$$

(3)

where:$\eta$ is the repulsive scale factor,$\rho (q,q_{obs} )$ is the distance between the object and the obstacle, and $\rho_0$ is the influence range of the obstacle. The object is not affected by the repulsive force when the distance between the object and the obstacle exceeds a certain range. The repulsion function is obtained by taking the gradient of the repulsion field:

$$\begin{gathered} F_{rep} (q) = - \nabla U_{rep} (q) \\ = \left\{ {\begin{array}{*{20}c} {\eta \left( {\frac{1}{{\rho (q,q_{obs} )}} - \frac{1}{\rho_0 }} \right) \cdot \frac{1}{{\rho^2 (q,q_{obs} )}}\nabla \rho (q,q_{obs} ),if\rho (q,q_{obs} ) \le \rho_0 } \\ {0,if\rho (q,q_{obs} ) > \rho_0 } \\ \end{array} } \right. \\ \end{gathered}$$

(4)

The resultant force on the object, combining both the gravitational and repulsive fields, is expressed as follows:

$$F(q) = F_{att} (q) + F_{rep} (q)$$

(5)

Although the artificial potential field method offers the advantage of fast planning speed, it is rarely used alone in path planning due to the following disadvantages:

• In certain scenarios, a balance between gravity and repulsion can lead to the planning getting stuck in a local optimal solution, resulting in planning failure;

• In complex obstacle environments, local positions may cause path oscillations, leading to an increase in path length;

• When the obstacle is in close proximity to the target, its repulsive force outweighs the gravitational force of the target, rendering the target unreachable.

3 Research on optimal path sampling algorithm based on potential

Existing algorithms still encounter challenges in achieving satisfactory planning results in complex scenarios. To address this, we propose an optimal path sampling algorithm, AP-RRT*, based on the potential function. This algorithm combines potential field concepts with random tree sampling, ensuring a balance between random sampling and target guidance during the path planning process.

3.1 Define the planner

The AP-RRT* algorithm needs to define the potential function in the state space and the sampling termination distance first.

3.1.1 Potential function

When obstacles surround the target, the traditional potential field may result in issues, such as unreachable targets. To address this, this paper proposes redefining the global gravitational field and segmenting the gravitational field.

The definition of gravitational field is shown in Eq. (6):

$$U_{att} (q) = \left\{ {\begin{array}{*{20}c} { - \xi \cos \left( {\frac{\pi }{{2d_{\min } }} \cdot \rho (q,q_{goal} )} \right),if\rho (q,q_{goal} ) \le d_{\min } } \\ {\frac{1}{2}\xi \rho^2 (q,q_{goal} ),if\rho (q,q_{goal} ) > d_{\min } } \\ \end{array} } \right.$$

(6)

In the formula,$\xi$ represents the gravitational scale factor,$d_{\min }$ represents the boundary distance of the gravitational field,$\rho (q,q_{goal} )$ represents the distance between the current object and the target state, and the gravitational function is:

$$F_{att} (q) = \left\{ {\begin{array}{*{20}c} {\frac{\pi \xi }{{2d_{\min } }}\sin \left( {\frac{\pi }{{2d_{\min } }} \cdot \rho (q,q_{goal} )} \right),if\rho (q,q_{goal} ) \le d_{\min } } \\ {\xi \rho ,if\rho (q,q_{goal} ) > d_{\min } } \\ \end{array} } \right.$$

(7)

3.1.2 Sampling termination distance

The traditional algorithm determines the termination of the plan by checking whether the state tree contains the target node. In this paper, the termination distance is calculated as the minimum Euclidean distance between the target and the obstacle in the state space. The pseudo-code of the algorithm is shown in Table 2.

Table 2 Sampling termination distance

Calculate the Euclidean distance $d_i$ between the target node and all obstacles, then subtract the obstacle radius $r_i$, and take the minimum value in $(d_i - r_i )$ to obtain the termination sampling distance $d_{\min }$. When the path search reaches this range, the node and the target node are directly connected to complete the path planning, as shown in Fig. 3.

Fig. 3

Define the termination distance

3.2 New node generation

In the process of expanding new nodes, the AP-RRT* algorithm adopts a secondary sampling strategy.

The first sampling is based on the target-bias. When selecting $q_{rand}$, there is a probability $p$ to randomly sample in the state space, and the probability $(1 - p)$ to selects the target node. It is shown in Eq. (8):

$$q_{rand} = \left\{ {\begin{array}{*{20}c} {q_{goal} ,(x \ge P)} \\ {randSample(),(x < P)} \\ \end{array} } \right.$$

(8)

After obtaining $q_{rand}$, we find $q_{nearest}$ and generate the first node $q_{new1}$, using Eq. (9):

$$q_{new1} = q_{nearest} + \lambda (\frac{{\overset{\lower0.5em\hbox{$\smash{\scriptscriptstyle\rightharpoonup}$}}{{q_{rand} }} - \overset{\lower0.5em\hbox{$\smash{\scriptscriptstyle\rightharpoonup}$}}{{q_{nearest} }} }}{{\left| {\overset{\lower0.5em\hbox{$\smash{\scriptscriptstyle\rightharpoonup}$}}{{q_{rand} }} - \overset{\lower0.5em\hbox{$\smash{\scriptscriptstyle\rightharpoonup}$}}{{q_{nearest} }} } \right|}})$$

(9)

where:$\lambda$ denotes the first sampling step.

The second sampling is based on the potential field. Starting from $q_{new1}$, the step size $\lambda$ is extended along the direction of the resultant force of the gravitational and the repulsive field to generate the second node $q_{new2}$. The second sampling process is shown in Fig. 4. The calculation of $q_{new2}$ is shown in Eq. (10):

$$q_{new2} = q_{new1} + \lambda \frac{{\overset{\lower0.5em\hbox{$\smash{\scriptscriptstyle\rightharpoonup}$}}{{F_{att} }} + \overset{\lower0.5em\hbox{$\smash{\scriptscriptstyle\rightharpoonup}$}}{{F_{rep} }} }}{{\left| {\overset{\lower0.5em\hbox{$\smash{\scriptscriptstyle\rightharpoonup}$}}{{F_{att} }} + \overset{\lower0.5em\hbox{$\smash{\scriptscriptstyle\rightharpoonup}$}}{{F_{rep} }} } \right|}}$$

(10)

where,$\overset{\lower0.5em\hbox{$\smash{\scriptscriptstyle\rightharpoonup}$}}{{F_{att} }}$ represents the gravitation,$\overset{\lower0.5em\hbox{$\smash{\scriptscriptstyle\rightharpoonup}$}}{{F_{rep} }}$ represents the repulsion,$\lambda$ is the second step size.

Fig. 4

The second sampling process

3.3 Adaptive strategy

In the planning process, the AP-RRT* algorithm dynamically adjusts the target-bias coefficient, step size, and search radius to enhance its adaptability and search efficiency.

3.3.1 Adaptive step-size

When the iterations is less than 100 times, the distance between $q_{rand}$ and $q_{nearest}$ is calculated. If it is less than 5,$q_{rand}$ is taken as $q_{new1}$, otherwise $q_{new1}$ is generated with 5 as the step size. When the iteration is greater than 100: the step size is determined by the formula. The definition of step size is as Eq. (11):

$$\lambda = \left\{ {\begin{array}{*{20}c} {\max (5,\left| {\overset{\lower0.5em\hbox{$\smash{\scriptscriptstyle\rightharpoonup}$}}{{q_{rand} }} - \overset{\lower0.5em\hbox{$\smash{\scriptscriptstyle\rightharpoonup}$}}{{q_{nearest} }} } \right|),if:iter < 100} \\ {\min (5,\frac{{\left| {\overset{\lower0.5em\hbox{$\smash{\scriptscriptstyle\rightharpoonup}$}}{{q_{goal} }} - \overset{\lower0.5em\hbox{$\smash{\scriptscriptstyle\rightharpoonup}$}}{{q_{rand} }} } \right|}}{{d_{\min } }}),if:iter \ge 100} \\ \end{array} } \right.$$

(11)

where:$iter$ is the iterations;$d_{\min }$ is the termination distance.

By employing the aforementioned settings, the algorithm achieves rapid expansion in the early planning stages with a large step size, significantly improving planning speed. During the middle and late stages of planning, the step size is dynamically adjusted based on the distance between the random point and the target. This approach effectively enhances node utilization and prevents issues such as endpoint escape due to excessively large step sizes.

3.3.2 Adaptive weight

By setting the decreasing function, it is ensured that the algorithm has a strong guiding role in the early stage of planning, avoids the disorderly diffusion of nodes, gradually reduces the bias probability in the middle and late stages, avoids falling into the local optimum, and provides a diversified path (Hara and Tomono 2020). The weight formula is as follows (12):

$$P = P_0 - \frac{iter}{{iter_{\max } }} \cdot \left( {P_0 - P_{\min } } \right)$$

(12)

Here,$P_0$ represents the initial bias rate,$P_{\min }$ represents the minimum bias rate, $iter,iter_{\max }$ represent the current iterations and the maximum iterations, respectively. Then, combining Eq. (8) to guide the generation of random nodes.

3.3.3 Dynamic retrieval radius

This paper proposes a dynamic retrieval radius (Eq. 13), which balances optimization effectiveness and computational efficiency during parent node and regional node reconnection:

$$r = 300 \cdot \sqrt {{\frac{{\log n_{node} }}{{n_{node} }}}}$$

(13)

Here,$n_{node}$ denotes the number of nodes in the current tree, and $r$ is the retrieval radius.

Based on the aforementioned definition, it is observed that as the number of nodes increases, the search radius gradually decreases and converges to a constant, as depicted in Fig. 5.

Fig. 5

Retrieve radius variation curve

4 Algorithm implementation scheme

The specific implementation steps of AP-RRT* are as follows:

• 1) Initialize the state space and the potential function, and calculate the termination distance $d_{\min }$;

• 2) Based on the principle of sampling algorithm, the random point $q_{rand}$ is generated according to the Eq. (8);

• 3) Find the nearest $q_{nearest}$ to $q_{rand}$, determine the step $\lambda$ according to Eq. (11), and obtain $q_{new1}$ according to Eq. (9). If the segment does not collide with obstacles,$q_{new1}$ is included in the node tree and continues; otherwise return to step 2;

• 4) With $q_{new1}$ as the center of the circle and $r$ as the radius, the regional node reconnection and parent node reconnection are performed, and the optimization results are saved. If $\left| {q_{new1} - q_{goal} } \right|$ is less than $d_{\min }$, then directly connect $q_{new1}$ and $q_{goal}$ to complete the planning. Otherwise continue to execute;

• 5) The resultant force of the gravity and repulsion at the $q_{new1}$ node is calculated, and $q_{new2}$ is obtained according to Eq. (10).

• 6) With $q_{new2}$ as the center of the circle and $r$ as the radius, and the regional node reconnection and parent node reconnection are performed, and the optimization results are saved. If $\left| {q_{new2} - q_{goal} } \right|$ is less than $d_{\min }$, directly connect $q_{new2}$ and $q_{goal}$ to complete the planning. Otherwise return to step 2.

The algorithm flow is shown in Fig. 6. The AP-RRT* algorithm pseudocode is shown in Table 3.

Fig. 6

Algorithm flow chart

Table 3 AP-RRT* pseudo-code

5 Experimental analysis

To verify the significant improvement of the AP-RRT* algorithm in planning effectiveness compared to other planning algorithms (APF, RRT, RRT*, RRT*-Connect (Jia et al. 2023) and PQ-RRT* (Li et al. 2020)), simulation analysis and experimental verification are carried out on MATLAB and ROS-Moveit platforms respectively.

5.1 MATLAB simulation analysis

To objectively compare the planning effectiveness of different algorithms, we set unified parameters as follows: the dimensions of the three-dimensional space are 500 × 500 × 500, the starting node and target node are at position (0, 0, 0) and (500, 500, 500) respectively, and three different obstacle map ranging from simple to complex are set.

5.2 Path planning results analysis

An algorithm simulation was conducted under the aforementioned conditions. The starting node and the target node are represented by red and blue respectively. The red line segment represents the collision-free feasible path discovered upon completing the planning. The blue line segment represents the exploration path in the planning process. The three different obstacle environments are shown in the Fig. 7. The planning results under three different obstacle environments using various algorithms are illustrated in Figs. 8, 9, 10.

Fig. 7

Obstacle Environments

Fig. 8

Map 1 Planning Result

Fig. 9

Map 2 Planning Result

Fig. 10

Map 3 Planning Result

Observing the planning results of the three maps above, the RRT algorithm lacks target guidance, as it performs a random search in the global scope, leading to low efficiency and generating paths of poor quality. While the artificial potential field accelerates the approach to the target node, localized oscillations arise at specific positions due to the approximation of gravity and repulsion, leading to an increased path length. While the retrieval strategy significantly enhances the planning effectiveness of RRT*, challenges persist in terms of decentralized paths and low search efficiency. The RRT*-Connect algorithm generates node trees separately from the starting point and the goal, improving planning speed. However, it encounters challenges of low effective sampling rates and longer paths []. The PQ-RRT* algorithm incorporates strategies involving potential field guidance and random sampling, resulting in a significant enhancement of planning efficiency and an improvement in path length []. The AP-RRT* algorithm, in comparison to previous algorithms, concentrates sampling points more towards the goal, resulting in superior path quality and substantial improvements in planning efficiency.

5.3 Simulation data analysis

Under the same experimental parameters, the APF, RRT, RRT*, RRT*-Connect, PQ-RRT* and AP-RRT* algorithms are tested 40 times respectively. The path length, planning time, number of waypoints, iterations are compared horizontally. The simulation data for each of the three maps are shown in Figs. 11, 12, 13, and the average planning effect of the algorithms is shown in Tables 4, 5, 6.

Fig. 11

Simulation data comparison of Map1

Fig. 12

Simulation data comparison of Map2

Fig. 13

Simulation data comparison of Map3

Table 4 Data summary of Map1

Table 5 Data summary of Map2

Table 6 Data summary of Map3

In summary, the AP-RRT* algorithm outperforms other algorithms in various aspects, including path length, planning speed, the number of iterations and so on, as depicted in the charts for three different obstacle environments. The only exception lies in planning time, where it falls short of the APF algorithm. The exclusive impact of potential field forces allows APF to possess a unique solution at a specific point in three-dimensional space, rendering path exploration unnecessary.

5.4 ROS-Moveit experimental verification

This section validates the efficacy of the AP-RRT* algorithm for manipulator path planning through experimental verification on the manipulator development platform Moveit in ROS. The experiment utilizes the FrankaR3 robot, which benefits from the completed development of an underlying driver adapted to ROS, enabling real-time control through the libfranka library of the FCI client. The size parameters of the robotic arm are shown in the Fig. 14.

Fig. 14

Robotic arm reachable space parameter

The AP-RRT* algorithm is integrated into the OMPL (Open Motion Planning Library) in the form of a plug-in for Moveit to call. The KDL (Kinematics and Dynamics Library) serves as the kinematics solver for the manipulator, conducting forward and inverse solution operations, while cuboid obstacles are introduced into the planning scene. The collision detection algorithm calls FCL (Flexible Collision Library) through ROS, and the basic idea of detection is to convert the object model into simple geometric bodies, and then perform geometric calculations on these simple geometric bodies.

Obstacle configurations is 40 × 50 × 70 cm, the joint angle of the initial pose and the target pose are as following:

$$q_{start} = [52^\circ ,39^\circ , - 31^\circ , - 116^\circ ,51^\circ ,132^\circ ,30^\circ ]$$

$q_{goal} = [5^\circ ,36^\circ , - 42^\circ , - 121^\circ ,33^\circ ,136^\circ , - 75^\circ ]$

Experiments for robotic arm path planning were conducted 40 times each using the six different algorithms, and the planning results are illustrated in Fig. 15. An analysis of the planning time, path length, and success rate is presented in Table 7.

Fig. 15

Simulation Results for Six Algorithms

Table 7 Simulation data analysis

In the same robotic arm simulation environment, among the six algorithms, the AP-RRT* algorithm exhibits the highest success rate, as well as the shortest average planning time and path length.

After the path planning simulation in Rviz, the AP-RRT* algorithm is verified on the real FrankaR3 robot. A cuboid obstacle of 40 × 50 × 70 cm is placed in the workspace of the manipulator, 40 cm from the front of the base of the manipulator.

Moveit operates on the host computer, establishing a connection to the FrankaR3 robot control cabinet through the network port. The motion trajectory points are transmitted to the control cabinet, directly controlling the manipulator's movement. The experimental setup is shown in Fig. 16.

Fig. 16

Experimental Setup

The AP-RRT* algorithm successfully plans a smooth path for the manipulator, avoiding static obstacles. The path sequence is depicted in Fig. 17. It is evident from Fig. 18 that the path generated by the AP-RRT* algorithm is both executable and effective, with each joint angle changing smoothly.

Fig. 17

Path implementation process

Fig. 18

Variation of each joint angle

6 Conclusion

This paper presents an optimal path sampling algorithm based on the potential function to address issues encountered in applying RRT series algorithms to manipulator path planning, including inadequate guidance, low sampling efficiency, prolonged computation time, and convoluted paths. The simulation and experimental results show that compared with the traditional algorithms, the AP-RRT* algorithm improves significantly in planning time, path length and other parameters. Subsequently, utilizing the ROS-Moveit development platform in conjunction with external sensors, the visual camera performs environmental obstacle scans, thereby further validating the algorithm's feasibility and reliability in a more intricate working environment.