key: cord-0779681-aqrfw3of
authors: Yi, Junhui; Yuan, Qingni; Sun, Ruitong; Bai, Huan
title: Path planning of a manipulator based on an improved P_RRT* algorithm
date: 2022-01-21
journal: Complex Intell Systems
DOI: 10.1007/s40747-021-00628-y
sha: 060a1db70454dcc24aa574411bd0824401bd230f
doc_id: 779681
cord_uid: aqrfw3of

Aiming to build upon the slow convergence speed and low search efficiency of the potential function-based rapidly exploring random tree star (RRT*) algorithm (P_RRT*), this paper proposes a path planning method for manipulators with an improved P_RRT* algorithm (defined as improved P_RRT*), which is used to solve the path planning problem for manipulators in three-dimensional space. This method first adopts a random sampling method based on a potential function. Second, based on a probability value, the nearest neighbour node is selected by the nearest Euclidean distance to the random sampling point and the minimum cost function, and in the expansion of new nodes, twice expansion methods are used to accelerate the search efficiency of the algorithm. The first expansion adopts the goal-biased expansion strategy, and the second expansion adopts the strategy of random sampling in a rectangular area. Then, the parent node of the new node is reselected, and the path is rerouted to obtain a clear path from the initial point to the target point. Redundant node deletion and the maximum curvature constraint are used to remove redundant nodes and minimize the curvature on the generated path to reduce the tortuosity of the path. The Bezier curve is used to fit the processed path and obtain the trajectory planning curve for the manipulator. Finally, the improved P_RRT* algorithm is verified experimentally in Python and the Robot Operating System (ROS) and compared with other algorithms. The experimental results verify the effectiveness and superiority of the improved algorithm.

With the introduction of German Industry 4.0, China has successively proposed the "Made in China 2025" strategy to adapt to the gradually increasing requirements for the intelligentization of robotic arms. The term robotic arm is short for robotic intelligent arms. As the name implies, a robotic arm is an important bionic arm that imitates human arms, as shown in Fig. 1 . The roles of robotic arms are important and include lifting, transporting, grasping, and loading heavy objects; they are also used in high-risk, high-precision processing operations and other fields [1] , as shown in Fig. 2 . At present, China's industrial robotic arms are mainly used B Qingni Yuan qnyuan@gzu.edu.cn Junhui Yi 1325736954@qq.com in factories. The logistics industry has developed rapidly in recent years, requiring considerable manpower and material resources, and intelligent warehousing technology has developed rapidly. Robotic arms play a vital role in the intelligent warehousing industry and can be used to reduce production costs and resource waste; they have always been a hot spot in robotics research [2, 3] . Among existing studies in robotic arm research, the motion stability of a robotic arm is a key topic, so developing a method to find an efficient and collision-free path is particularly important for the motion of a robotic arm [4] .

Current motion planning algorithms are based on the grid search method (A*), artificial potential field method (APF), probabilistic roadmap method (PRM) and rapidly exploring random tree (RRT) algorithm [5] . Among them, the grid search method can ensure complete resolution and an optimal solution in path planning, but the flexibility of the algorithm is limited and the calculation efficiency is low; the artificial potential field method is prone to local minima in the path planning algorithm, resulting in the inability to reach the target point; the probability map method of the multiquery Robotic arm is used to carry goods method is probabilistically complete in path planning, but it requires the state space to be known in advance and has low efficiency; the RRT algorithm poses problems, such as large memory usage and low search efficiency in a complex multiobstacle environment [6, 7] . The RRT algorithm randomly samples in the state space without preprocessing the state space. Its randomness proves that the RRT algorithm itself has strong search capabilities and that the algorithm is probabilistically complete. Many scholars have conducted in-depth research on the RRT algorithm [8, 9] .

In view of the shortcomings of the RRT algorithm, a variety of RRT algorithm variants and other algorithms have emerged to improve the efficiency of path planning. Pérez-Higueras et al. presented an approach for learning navigation behaviours for robots using an optimal rapidly exploring random tree star (RRT*) as the main planner. A new learning algorithm combining both inverse reinforcement learning and RRT* is developed to learn RRT*'s cost function from demonstrations [10] . Li et al. proposed an incremental sampling-based motion planning algorithm, i.e., near-optimal RRT (NoD-RRT). This algorithm aims to solve motion planning problems with nonlinear kinodynamic constraints. To determine the cost/metric between two given states considering the nonlinear constraints, a neural net-work is utilized to predict the cost function. On this basis, a new reconstruction method for the random search tree is designed to achieve a near-optimal solution in the configuration space [11] . Chen et al. proposed a novel approach of RRT* in collaboration with a double-tree structure to separate the extension and optimization procedures [12] . Hidalgo-Paniagua et al. presented quad-RRT, an extension of the bidirectional strategy to speed up the RRT when dealing with large-scale, bidimensional (2D) maps [13] . Chao et al. proposed an algorithm called grid-based RRT* (GB-RRT*) by combining the principle of RRT* with the grid searching strategy. The proposed hybridized algorithm compensates for the weaknesses of RRT* with the strengths of the grid search strategy and is applicable in complex environments with obstacles and narrow areas without relying on predesigned road networks [14] . Ryu et al. proposed an improved informed RRT* algorithm to reduce computational time, even in complex environments. Unlike the use of RRT* for informed RRT* to find an initial solution for the whole configuration space, the gridmap skeletonization approach is applied to generate the initial solution [15] . Kiani et al. presented an improvement on the RRT algorithm, namely, Adapted-RRT, which uses three well-known metaheuristic algorithms, namely, grey wolf optimization (GWO), incremental grey wolf optimization (IGWO), and expanded grey wolf optimization (Ex-GWO). They attempted to find solutions close to the optima without collision while providing comparatively efficient execution time and space complexities [16] . Qi et al. presented an algorithm termed multiobjective dynamic rapidly exploring random (MOD-RRT*), which is suitable for robot navigation in an unknown dynamic environment [17] . Qureshi et al. added the idea of the artificial potential field method to the RRT* algorithm to obtain the P-RRT* algorithm, which significantly improves the shortcoming of slow convergence efficiency of the algorithm and minimizes the use of memory [18] . Jeong et al. proposed the Q-RRT* algorithm, which also considers the parent node of a new node's parent node on the basis of RRT*, expands the ranges of parent vertices, and uses this method in the rewiring operation. This method further improves the search efficiency of the algorithm and the quality of the path [9] . Li et al. proposed a PQ-RRT* algorithm that combines the potential function-based RRT* (P_RRT*) and quick RRT* (Q_RRT*) algorithms to fully utilize the advantages of the two algorithms and further improve the convergence rate and efficiency of the algorithm [7] . Hu et al. proposed a learning scheme with nonlinear model predictive control (NMPC) for mobile robot path tracking. The NMPC strategy utilizes a varying-parameter one-layer projection neural network to solve an online quadratic programming optimization via iteration over a limited receding horizon [19] . Hu et al. developed a 9-degree-of-freedom (DOF) rigid-flexible coupling (RFC) robot to assist coronavirus disease 2019 (COVID-19) oropharyngeal (OP) swab sampling. Compared with a rigid sampling robot, the developed force sensing RFC robot can facilitate OP swab sampling procedures in a safer and more gentle way. In addition, a varying-parameter zeroing neural network-based optimization method was proposed for motion planning of the 9-DOF redundant manipulator [20] . Jordan et al. proposed bidirectional RRT* (B-RRT*) to use the greedy connection heuristic to connect two directional trees to improve the efficiency of the algorithm. However, the algorithm only explores the pure space, and the expansion of the random tree is blind [21] . Wei et al. proposed smooth RRT (S-RRT) and adopted a path optimization strategy based on the maximum curvature constraint to generate a smooth and curved continuous executable path for the robot manipulator. However, for the expansion of the random tree, only the idea of target bias is used for expansion, the search efficiency is still low, and there is a large difference between the fitted path and the original path [22] . Nasir et al. proposed RRT*-Smart, and two new technologies, path optimization and intelligent sampling, were used to improve the efficiency of the RRT*-Smart algorithm. The algorithm only experiments in a two-dimensional environment, the generated path is relatively tortuous, and the proposed method cannot improve the efficiency of the algorithm very well [23] . As combinations of the above algorithms, these algorithms have improved the efficiency and convergence of the path search to a certain extent. In the face of complex obstacle environments, the above algorithms still have low efficiency, slow convergence to the optimal solution, and large computational memory usage. For the motion path of a robotic arm, there are many turning points, and the planned path is not smooth, which seriously affects the service life of the robotic arm.

In response to the above problems, this paper proposes an improved P_RRT* algorithm for path planning of manipulators. This method is used to achieve efficient path planning for manipulators in a complex obstacle environment. Compared with the existing path planning algorithm, the main contributions of the text are as follows:

(1) For the selection of the nearest neighbour nodes, two methods, the minimum Euclidean distance and the minimum cost function, are proposed to improve the convergence speed of the algorithm. (2) For the expansion of new nodes, a two-step expansion strategy is proposed. The first step expansion adopts the idea of target bias for expansion, and the second step expansion adopts the method of random sampling of rectangular regions. This expansion method is used to improve the search speed of the algorithm. (3) Since the generated path has many inflection points and the path curvature is large, the removal operation for redundant nodes and the constraint operation of maximum curvature are used to make the path smoother.

The rest of this article is arranged as follows: The "Definition of the path planning problem" section introduces the basic definition and the problems that need to be solved for path planning; "Basic P_RRT* algorithm" introduces the background knowledge of the improved algorithm presented in this article; "Improved P_RRT* algorithm" introduces the specific implementation steps of the improved algorithm presented in this article; "Analysis" introduces the analysis and optimal solution of the path planning problem; "Deletion of redundant nodes and the maximum curvature constraint" introduces the smoothing operation of the generated path, which includes the deletion of redundant nodes in the path and the maximum curvature constraint operation on the path; "Bezier curve" introduces the use of the Bezier curve to fit the final path; "Experiments and analysis" introduces the Python and robot operating system (ROS) simulation experiment analyses of the improved algorithm presented in this article; "Conclusion" summarizes this article.

This section introduces three problems that need to be solved in path planning. Let Q ⊂ R n denote the state space of the problem described, where R is a set, and r(i) i∈n denotes the mapping from n to R; that is, i ∈ n is mapped to r(i) ∈ R. Q obs ⊂ Q denotes the obstacle space, and Q free Q\Q obs denotes the barrier-free space. Q init and Q end are the starting state and the ending state, respectively. They are both in an obstacle-free space. The obstacle spatial distance (d * obs ) is set: that is, the obstacle model is simplified to a sphere, where the distance that the radius of the sphere expands outward is used. The continuous function α: [0,1] → Q free is a feasible collision-free path [24] .

The problem of path planning is to find a collision-free path, namely, α: [0,1]→Q free . The path that extends from the starting point α (0) ∈ Q init to the ending point α (1) ∈Q end and through the point α(τ ) ∈ Q free for all τ ∈ [0,1] is called a feasible path.

Question 1: For path planning problem {Q init , Q obs , Q end }, if a path is found, it proves that there is a feasible path, and if the path is not found, it proves that a path does not exist.

Question 2: For all the path sets , feasible represents all feasible paths, and C(.) represents the cost function measured by the Euclidean distance; then, given the path planning problem {Q init , Q obs , Q end }, find a path α* ∈ feasible ; if there is C (α*) min{C (α): α ∈ feasible }, there is an optimal path; otherwise, the optimal path planning process fails.

Question 3: For t ∈ T , which represents the time required for the algorithm to find a collision-free path, the algorithm is required to find the optimal path in the shortest time t ∈ T [7, 9, 18] .

The classic RRT algorithm [5] is an efficient multidimensional space planning method. An initial node is taken as the root node (q init ); then, a random sampling point (q rand ) in the state space.

is selected, and the node (q nearest ) in the random tree that is closest to q rand in terms of the Euclidean distance according to the Get_Nearest function is selected. Then, q nearest is extended to q rand by a step (ρ) distance, a new node q new is generated, and it is judged whether q new collides with any obstacles. If there is a collision, q new is deleted, and the random points are resampled. If there is no collision, q new is added to the random tree, and the parent node of q new is assigned as q nearest , as shown in Fig. 3 . The next random sampling is performed, and the above steps are repeated. When the distance between q new and the target point q end is less than the step length ρ, q new and the target point are connected to obtain a collision-free path from the starting point to the ending point.

The RRT* algorithm [25] is an improved algorithm based on the traditional RRT algorithm; it reselects the parent node in the nearest neighbour area (Reselect_Parent_Node) and rewires the node in the nearest neighbour area (Rewire_Node). In the RRT algorithm, the parent node of q new is assigned as q nearest , and the RRT* algorithm improves the selection of the parent node of the new node. Specifically, the set (Q(potential_parent)) of all the adjacent potential parent nodes of q new in the tree within a circle, where q new is the centre and r 1 is the radius, is found. Then, whether there is a node in Q(potential_parent) that is more suitable than q nearest to be the parent node of q new is determined; that is, the Euclidean distance from q new to q init is determined and used as the cost function. This method first calculates the Euclidean distance cost value, Cost(q new ), with q nearest as the parent node, selects the potential parent node in Q(potential_parent) to connect with the new node, and then judges the potential parent node as the parent of the new node. It is determined whether the Euclidean distance cost value of the node, Cost(Q(potential_parent)), is less than Cost(q new ); if it is greater than Cost(q new ), the potential parent node will be discarded, and the next potential parent node will be calculated. If it is less than Cost(q new ) and the connection between the potential parent node and the new node does not collide with an obstacle, then the parent node of q new is reselected and connected. If it collides with an obstacle, the parent node of the new node remains unchanged as q nearest . The nodes are compared in sequence until all potential parent nodes are compared, as shown in Fig. 4 . The RRT* algorithm also uses the rewiring of nodes in the nearest neighbour area. The idea is to use q new as the parent node of all potential child nodes Q(potential_child) in an adjacent circle with r 2 as the radius. If the Euclidean distance cost value, Cost(Q(potential_child)), of a parent node with q new as a potential child node is smaller than the original Euclidean distance cost value of the potential child node and if it does not collide with obstacles, the original parent node of the potential child node is discarded, and the new node is used as its parent node for rewiring; otherwise, the parent node remains unchanged, as shown in Fig. 5 . The codes for reselecting the parent node and the rewiring operation of the RRT* algorithm are shown in Algorithm 1 and Algorithm 2, respectively. The Euclidean distance cost value is the sum of the Euclidean distance of the path from this point to the root node.

q parent q nearest 6. return q parent

The P_RRT* algorithm uses a potential function to improve the selection of random sampling points q rand on the basis of RRT* and is defined as the potential function random sampling point selection method (RGD(q rand )), and the RGD(q rand ) code is shown in Algorithm 3. Specifically, RGD(q rand ) introduces the idea of the artificial potential field method in the operation of selecting random nodes (q rand ). APG stands for the attraction gradient [18] . The target point generates attraction, and a potential random sample point is defined as q prand . q prand is a point from q rand along the decreasing direction of the attractive potential field gradient; that is, the point is biased towards the target direction, and it is obtained by moving downhill with a small step λ. When q prand is used instead of q rand as a random sampling point, the potential field gradient of gravity will decrease upon approaching the target point. During the expansion process, the Euclidean distance to the nearest obstacle is judged in real time. If it is less than the obstacle spatial distance (d* obs ), the program will be terminated immediately, and q prand will be returned. Otherwise, the next expansion will be performed with a limited number of k, in which the values of the parameters λ, k and d* obs need to be adjusted. This article ignores the adjustment of the parameters and sets λ 0.02, k 80, and d* obs 0.1.

The basic P_RRT* algorithm improves the selection of random nodes in the state space, which makes it more biased towards the location of the target point, reduces the probability of generating invalid nodes in a tree, saves calculation space and reduces memory usage. However, the efficiency of a successful path search and the convergence rate of a search are still very low. To improve the search efficiency and achieve a higher convergence rate, in this paper, the selection of nearest neighbour nodes and the expansion of new nodes in the P_RRT* algorithm are further improved.

This paper improves the selection of the nearest neighbour node in the P_RRT* algorithm (Near-est_Neighbour(q prand ,q end )). The traditional nearest neighbour node (q nearest ) selection method selects the node with the closest Euclidean distance to the random sampling point as the nearest neighbour node (q nearest ). In this paper, searching for the smallest value of the cost function C(q) in the tree as the nearest neighbour node (q nearest ), C(q) is defined as

where q end − q is the Euclidean distance between the target point q end and the current node q, w d is the distance proportional coefficient, w c is the mixed proportional coefficient, and k represents the number of obstacles in the circle near the current node. The values of w d and w c are obtained through the distribution of obstacles and experimental analysis. In this article, the values are w d 1 and w c 4. To improve the ability of the model to avoid obstacles, the radius of the circle that determines the value of k in formula (1) is twice the length of r 1 in the RRT* algorithm, and the nearest neighbour node chooses the smallest C (q) in the tree.

Nearest_Neighbor(q prand ,q end ) 1.q prand RGD(q rand ); 2.rand Random(0,1); 3.if rand < p nearest 4. q nearest Get_nearest(T, q prand ); 5.else 6. C(q) C(q end ,q) 7. q nearest min(C(q)) 8. return q nearest

Let rand be a uniformly distributed random number in the interval [0,1]. To improve the obstacle avoidance ability of the algorithm, according to the experimental analysis, the value of p nearest 0.5. P nearest represents the probability of selecting the node with the smallest Euclidean distance from the random sampling point in the tree as the q nearest ; that is, if rand < p nearest , then the node with the smallest Euclidean distance from the random sampling point (q prand ) is selected as the nearest neighbour node (q nearest ). If rand > p nearest , the point with the smallest cost function C (q) in the tree is selected as the nearest neighbour node (q nearest ). The corresponding code is shown in Algorithm 4.

The traditional P_RRT* algorithm first samples random points based on the potential function, finds the node (q nearest ) closest to the random node and expands along the direction of the random point to obtain q new . The traditional algorithm only expands the new node once, which is not efficient. This article improves the expansion of the new node in the P_RRT* algorithm, and the strategy of expanding the new node twice is adopted to speed up convergence and improve the efficiency of the algorithm. The specific steps are as follows:

The new node (q 1new ) in the first step is expanded with the goal-biased expansion method [26, 27] : that is, the idea of gravitation in the artificial potential field method is introduced on the expanded step size so that it grows towards the target point, as shown in Fig. 6 . q 1new is defined as:

where k p is the gravitational coefficient, ρ is the growth step of the random tree, q prand is the random sampling point, q nearest is the nearest neighbour node, q end − q nearest is the Euclidean distance from the target point (q end ) to the nearest neighbour node (q nearest ), and q prand − q nearest is the Euclidean distance from the random sampling point (q prand ) to the nearest neighbour node (q nearest ). Whether the path between q 1new and q nearest collides with obstacles is determined; if there is a collision, the expansion of q 1new will be abandoned, and the random sampling point will be selected again using the method of selecting a random sampling point in the P_RRT* algorithm. If there is no conflict, q 1new is added to the random tree, and its parent is assigned to q nearest . [q 2new x ,q 2new y ,q 2new z =[q 1new x , q 1new y ,q 1new z ]+ r * [rand(q 1new x →q prand x ,rand(q 1new y →q prand y , rand(q 1new z →q prand (z))]

The second step of the expansion of the new node (q 2new ) adopts the strategy of random sampling from q 1new to q prand in a rectangular area (New_Node_2(q 1new ,q prand )). That is, a point is randomly selected in the rectangular area formed by the new node q 1new of the first expansion to the last expansion point q rand and is assigned as the new node q 2new of the second expansion, as shown in Fig. 7 . Let rand(x) be a randomly selected value in the range of x; then, the new node (q 2new ) can be expressed as:

[q 1new (x), q 1new (y), q 1new (z)] + r *[rand (q 1new (x) → q prand (x)),rand(q 1new (y) → q prand (y)),

In the formula, q 2new (), q 1new (), and q prand () represent the coordinate values of q 2new , q 1new , and q prand , respectively; r represents the expansion ratio coefficient; and r 0.01 according to the experimental analysis. In addition, whether the connection between q 2new and q 1new collides with obstacles is determined; if they collide, the expansion of q 2new is abandoned, and q 2new is resampled. If there is no collision, q 2new is added to the random tree, and its parent node is assigned to q 1new . The code is shown in Algorithm 5.

According to the principle of the above improved P_RRT* algorithm, the specific implementation steps can be summarized as follows:

Step 1: Initialize each parameter in the random tree, including inserting the starting point, target point, obstacles, step length, target bias step length, and obstacles;

Step 2: Use the random sampling point selection method RGD (q rand ) of the potential function in the P_RRT* algorithm to obtain the randomly sampled point q prand ;

Step 3: Obtain the random number probability (rand) that obeys the uniform distribution, and select the nearest neighbour node (q nearest ). If the random number probability (rand) is less than the nearest neighbour node probability p nearest 0.5, then the nearest neighbour node is the node in the tree that has the closest Euclidean distance to the random sampling point (q prand ); if the random probability rand is greater than p nearest 0.5, formula (1) is used to calculate the cost function C (q), and the point with the smallest cost function C (q) in the tree is selected as the nearest neighbour node;

Step 4: Use the obtained nearest neighbour node for expansion. The first step is to expand and use the target bias strategy, obtain q 1new through formula (2) , and judge whether the new node q 1new collides with obstacles. If collision with an obstacle occurs, then return to Step 2; otherwise, go to Step 5;

Step 5: On the expanded new node q 1new , the second step of expansion is carried out by formula (3), which uses a randomly selected point in the rectangular area as the second expanded new node to obtain q 2new and judges whether the new node q 2new collides with an obstacle or whether the line between two new nodes (q 1new , q 2new ) collides with an obstacle. If there is a collision, execute Step 5 to randomly sample the new node q 2new again; otherwise, execute Step 6;

Step 6: Perform the operation of reselecting the parent node in the RRT* algorithm for the new node in the adjacent circle (see Algorithm 1) and find the parent node of the new node with the smallest Euclidean distance cost value that does not collide with an obstacle;

Step 7: The new node acts as the parent node of the node in the adjacent circle to perform the rewiring operation in the adjacent circle in the RRT* algorithm (see Algorithm 2) . Find the node in the adjacent circle whose Euclidean distance cost value with the new node as its parent node is less than its own Euclidean distance cost value and that does not collide with an obstacle to perform the rerouting operation to find the optimal path;

Step 8: Check whether the Euclidean distance between the new node and the target point is less than the step length ρ. If it is, connect the new node and the target point to obtain a complete and clear path, and the algorithm ends; otherwise, go to Step 2. Algorithm 6 Improved P_RRT* 1.initialization 2.T.( q init ); 3.while true do 4. q rand Random(); 5. q prand RGD(q rand ) 6. rand Random(0,1); 7. if rand < p nearest 8.

q nearest Get_nearest_list_index(node_list, q prand ); 9. else 10.

C(q) C(q end ,q) 11. q nearest min(C(q)) 12. q 1new Extend(q nearest ,q prand ) 13. if collision_check(q 1new ) 14.

T.add(q 1new ) 15.

while true do 16.

rand() Random(); 17 18 .

if collision_check(q 2new ,q 1new ) 19.

T continue 37. return T [q 2new (x),q 2new (y),q 2new (z)]=[q 1new (x), q 1new (y),q 1new (z)]+ r * [rand(q 1new (x) →q prand (x) ,rand(q 1new (y) →q prand (y) , rand(q 1new (z) →q prand (z))] w d * q end −q +w c * ( k q end −q )

According to the above steps, the flow chart of the improved P_RRT* algorithm is shown in Fig. 8 , and the code of the algorithm is shown in Algorithm 6.

This section analyses the probabilistic completeness, asymptotic optimality, fast convergence, and computational complexity of the improved P_RRT* algorithm. Let ALG represent an algorithm. V n ALG represents the vertex of the tree generated after n iterations of the ALG algorithm. Y ALG n represents the minimum path cost after n iterations of the ALG algorithm. S n ALG represents the number of steps after n iterations of the ALG algorithm.

For Question 1 (mentioned in the definition of the path planning problem), the probabilistic completeness of most sampling-based algorithms can be guaranteed. The form of probabilistic completeness is as follows:

Definition 1 (Probabilistic completeness) For path planning problem {Q init , Q obs , Q end }, if the probability of finding a feasible path as the number of iterations increases is 1 for ALG, that is, The q nearest is selected as the point with the shortest Euclidean distance to the random sampling point

The selection of the nearest neighbor node adopts the minimum cost function C(q) to select Returns the nearest neighbor node (q nearest ) New node q 1new ,q prand obtained by the action of RGD(q rand )

On the basis of q 1new , a random sampling method in a rectangular area is used to expand the new node q 2new Construct a rectangular area formed by q 1new to q prand start end end then the ALG algorithm generates a complete path connecting root q init to q end ∈ Q end . The ALG algorithm has the characteristics of probabilistic completeness.

The probabilistic completeness of the RRT algorithm has been proven in detail, and the RRT* algorithm, as a variant of RRT, inherits the probabilistic completeness of the RRT algorithm [25] . At the same time, the probabilistic completeness of the P_RRT* algorithm has also been explained [18] . This paper explains the probabilistic completeness of the improved P_RRT* algorithm proposed in Theorem 1.

For any feasible path planning problem {Q init , Q obs , Q end }, as the number of iterations increases, the probability of finding a feasible path is close to 1, that is, lim n→8 P(∃q end ∈ V ImprovedP_RRT* n ∩ Q end such that q init is connected to q end ∈ Q end ) 1

Proof of Theorem 1 This article uses the same sampling method as the P-RRT* algorithm, and the improved P_RRT* algorithm only improved the selection of its nearest neighbour nodes and the expansion strategy on the basis of the P_RRT* algorithm, which changed the growth trend of the tree but did not change the connectivity of the tree. Therefore, the improved P_RRT* algorithm has the same probabilistic completeness as the P_RRT* algorithm.

For Question 2 (mentioned in the definition of the path planning problem), if the algorithm has a continuous path minimum cost solution α*: [0,1] such that α* (0) Q init and α*(1) ∈ Q end , and there is no collision in the complex obstacle environment, then the algorithm is asymptotically optimal.

Let β ∈ R + , for the state q ∈ Q free , where H q, β represents the closed ball area with radius β centred on q. If the ball area is completely located in the barrier-free space, then for any q ∈ Q free , it can be expressed as β-internal state (Q intβ ); and if the ball area is partially located in the barrier-free space, for any q ∈ Q free , it can be defined as β-external state (Q extβ ). Q intβ and Q extβ represent a subset of the barrier-free space Q free . Then, Q intβ : {q ∈ Q free : H q, β ⊆ Q free } and Q extβ : Q free \Q intβ [18] .

[0,1], only when all points on the path belong to Q intβ, that is, α(s) ∈ Q intβ ; ∀s ∈ [0,1], the path has strong η-clearance. where M* represents the optimal path cost. P_RRT* is only an improvement upon the random sampling method of the RRT* algorithm based on the RRT* algorithm, and this operation does not affect the asymptotic optimality of the algorithm. Therefore, P-RRT* inherits the property of RRT* asymptotic optimality [18] . The improved P_RRT* algorithm in this paper is an optimized version of the P-RRT* algorithm, so the improved P_RRT* in this paper also has the property of asymptotic optimality.

For Question 3 (mentioned in the definition of the path planning problem), the algorithm is required to find the optimal path in the shortest time t ∈ T .

Definition 5 (Optimal path planning) If a path α* is collision-free and has weak β-clearance, then the path is the optimal path. Theorem 2 ((Potential guided sampling heuristic RGD(q)The RGD(q) step in P_RRT* guides random samples to the target area for sampling so that P(q prand ∈ Q extβ ) > 0.

According to Theorem 2 and Definition 5, P_RRT* has the property of quickly converging to the optimal solution [7, 18] . The improved P_RRT* in this paper further optimizes the search process of P_RRT*. The search time is reduced while remaining unchanged with the rest of the P_RRT* process. Therefore, the improved P_RRT* and P_RRT* have the same property of quickly converging to the optimal path solution.

Computational complexity analysis of the improved P_RRT* algorithm is performed. Theorem 3 shows that the improved P_RRT* algorithm has a computational complexity similar to that of P_RRT*.

There is a constant a ∈ R + , such that the following formula remains true:

Since the improved P_RRT* algorithm only improves the selection method and expansion strategy of the nearest neighbour node of the P_RRT* algorithm, it does not increase the number of samples but optimizes the P_RRT* algorithm, which does not greatly affect the complexity, so the improved P_RRT* algorithm and P_RRT* algorithm have similar computational complexity.

Due to the random expansion of the algorithm, it will inevitably cause the path to become tortuous and include many unnecessary turning points. For a robotic arm, the redundant turning points will cause unnecessary energy loss, cause wear and reduce the service life of the robotic arm. Therefore, this article adopts an operation to remove redundant points [28, 29] . The redundant node removal operation is performed on the generated path. The specific operation starts at the starting point, connects the subsequent way points, abandons the second way point, and connects to the third way point. If the path does not collide with obstacles, the second node is determined to be redundant and is deleted. The starting node is then connected to the fourth node to determine whether there is a collision; if there is no collision, the Fig. 9 Operation diagram of the path with the redundant nodes deleted third node is deleted. The process continues according to this method; if there is a collision, the node is retained and used as the starting point to make the above decisions until the target point is reached. This will generate a path that removes the redundant nodes, as shown in Fig. 9 .

In the redundant node deletion operation section of this article, a path after removing redundant nodes is obtained, and the path is smoother. The obtained path may have an excessively large rotation angle, which may cause impact damage to the robotic arm during operation, can seriously affect the service life of the robotic arm and is not suitable for an actual application of a robotic arm. Therefore, the maximum curvature constraint method [30, 31] is used to smooth the path. According to the data analysis, the cosine value of the maximum corner angle is 0.707. The cosine value of each angle between the paths is calculated in order from the starting point of the obtained path with the redundant points removed. If the value is greater than 0.707, the child node on the right side and the parent node on the left side of this vertex from the original path are added as standards; that is, the parent nodes on the left side and the child nodes on the right side are added. As shown in Fig. 10b , where the curvature is relatively large, the left and right adjacent points of this point from the original path are added to the left and right sides of the point, as shown in Fig. 10a . This is a preparatory step for the Bezier curve fitting method described below.

The path after the deletion of the redundant nodes and the maximum curvature constraint operation is already a relatively smooth path, as shown in Fig. 11b , but in practical applications, it is necessary to minimize the impact damage at the turning point. Therefore, in this section, the Bezier curve method [32, 33] is used for the final smoothing of the path; it makes the path smoother and more suitable for the application of a robotic arm in actual scenes. With a Bezier curve of order n, which includes n + 1 nodes, the formula C(u) is as follows:

In the formula, p i represents n + 1 points in the space, the weight coefficient B n,i (u) with the parameter u represents the Bernstein basis function, and the formula is calculated as follows:

The final generated curve is composed of n + 1 nodes, and these nodes are called control points. When u 0 and u 1, they are located at the start and end points, respectively. The obtained path diagram fitted by the Bezier curve is shown in Fig. 11a . It can be determined from the diagram that there is a slight difference between the fitted path and the original path, which may lead to collisions. Under numerous experimental tests, the probability of collision is 0.

In this section, the improved P_RRT* algorithm is experimentally verified, the improved P_RRT* algorithm is compared with the existing RRT, RRT* and P_RRT* algorithms in the same three-dimensional environment, and the superiority, effectiveness and reliability of the improved algorithm are verified. This experiment is a Python and ROS simulation experiment conducted in the Windows 10 environment of an HP Intel (R) Core (TM) i5-6500 CPU @3.20 GHz and 3.19 GHz with 4.

GB memory in the laboratory. The initial node coordinates are set to [0,0,0], the end node coordinates are [8, 10, 10] , the random expansion step length ρ is 1.0, λ 0.02, k 80, d* obs 0.1, the adjacent circle radius r and other parameters are the same, the radius of the adjacent circle used for reselecting the parent node is 2 (r 1 2), and the radius of the adjacent circle used for the rewiring operation is 1 (r 2 1). Then, an efficient and collision-free path is planned from the start point to the end point. Furthermore, this paper uses the UR5 manipulator of UAO to implement and verify the simulation experiment. When the search time exceeds 100 s, the search is unsuccessful; otherwise, it is successful. This article mainly compares the average search time, average number of sampling nodes, average path length, search success rate and other indicators with those of the RRT, RRT* and P_RRT* algorithms for comparative experimental analyses.

The superiority of the proposed algorithm will be verified through comparison. Specific experimental contents are as follows: the RRT, RRT*, and P_RRT* algorithms are compared in environments with the same number of randomly distributed obstacles. Each group of experiments is performed 200 times. Due to the randomness of the algorithm, the average value is taken for comparison, as shown in Table 1 .

According to the data in Table 1 , the average search time and the average number of sampling nodes of the classic RRT algorithm are very large compared to those of the other three algorithms. The average length of the path is also the longest, and the search success rate is 70%. The search time of the RRT* algorithm is the second slowest. Compared with the RRT algorithm, the average search time and the average number of sampling nodes were significantly reduced. The average search time of the P_RRT* algorithm compared to those of the RRT and RRT* algorithms also changed significantly, but the average number of sampling nodes remained high, which inevitably led to the requirement for more memory and consumption of more calculation space by the algorithm. Compared with the other three algorithms, the improved P_RRT* algorithm shows a significant reduction in the average search time and average number of sampling nodes, and its average path length is also the shortest. The superiority of the improved algorithm is, therefore, obvious.

The effectiveness of the proposed algorithm will be compared. Specific experimental contents are as follows: the RRT, RRT*, and P_RRT* algorithms are compared in environments with different numbers of randomly distributed obstacles. Each group of experiments was carried out 200 times, and the average of the results was taken, as shown in Table 2 . Table 2 shows that the search time of the classic RRT algorithm is relatively small when there are few obstacles; when there are many obstacles, the average search time and the average number of sampling nodes increase rapidly, and the search success rate is significantly reduced. With the gradual increase in the number of obstacles, the average search time of the RRT* algorithm increases significantly, the search success rate is reduced, and the average number of sampling nodes is increased. Compared with the RRT and RRT* algorithms, the average search time and average path length of the P_RRT* algorithm are significantly improved; however, as the number of obstacles gradually increases, the average number of sampling nodes also greatly increases, the convergence is reduced, and an increasing amount of memory space is consumed. The improved P_RRT* algorithm can still maintain its search efficiency as the number of obstacles gradually increases. The average search time is small and stable. The average number of sampling nodes is also significantly reduced. The algorithm has a faster convergence rate, and its average path length is greatly reduced compared with the RRT, RRT* and P_RRT* algorithms. The effectiveness and excellent results of the improved algorithm are obvious.

The reliability of the proposed algorithm will be compared with existing algorithms. A comparative experiment with each algorithm is carried out on two environmental maps. The two environmental map models are shown in Fig. 12 , and an experimental analysis is performed with the RRT, RRT* and P_RRT* algorithms, as shown below.

Under the condition that the starting point and target point are the same, the results of the RRT, RRT*, and P_RRT* algorithms are compared. Each group of experiments is carried out 200 times, and the average value of the results is taken, as shown in Table 3 . The path diagrams generated by different algorithms are shown in Fig. 13a -d.

Under the condition that the starting point and target point are the same, the results of the RRT, RRT*, and P_RRT* Table 4 . The path diagrams generated by different algorithms are shown in Fig. 14a -d.

According to the data in Tables 3 and 4 , when there are many obstacles on Maps 1 and 2 and the environment is complex, the average search time of the RRT algorithm is very large. The required times are 76.38 s and 83.89 s for Maps 1 and 2, respectively. The RRT* algorithm has search times of 52.99 s and 62.72 s, the P_RRT* algorithm has search times of 29.2 s and 46.36 s, and the improved P_RRT* algorithm has minimum search times of 4.51 s and 8.3 s for Maps 1 and 2, respectively. The planning efficiency and search success rate of the RRT algorithm are very low, and the average number of sampling nodes is very high, resulting in substantial memory usage. Compared with the RRT algorithm, the average search time and average sampling node number of the RRT* algorithm are reduced; however, compared with P_RRT* and the improved P_RRT* algorithm, they are still very high, and the search success rate is also low. The P_RRT* algorithm has improved average search times and average path lengths compared to those of the RRT and RRT* algorithms, but the average number of sampling nodes remains very large. Compared with the improved P_RRT* algorithm proposed in this paper, the P_RRT* algorithm still exhibits the shortcomings of low efficiency, a large memory footprint, and long tortuous paths. In this paper, the improved P_RRT* algorithm has a smaller average search time and average number of sampling nodes. It shows a more prominent effect in a complex multiobstacle environment, and its search path is shorter and smoother. The reliability of the improved algorithm is obvious through the comparative analysis of two different environment map experiments.

To combine theory with reality and to verify the feasibility of the algorithm in this article, this section uses the UR5 manip- ulator of UAO to perform a simulation analysis in the ROS. The UR5 manipulator is a six-degree-of-freedom articulated manipulator. First, the environment scene is built in MoveIt, the rviz tool in ROS is used to visualize the demonstration, the robotic arm model is loaded, and the error transformation matrix is used to compensate for the docking error of the mobile robotic arm. The obstacles, starting point pose, and target point pose are set. Here, the starting point and target point pose are the poses after error compensation. The improved P_RRT* algorithm is added to the Open Motion Planning Library (OMPL), and the corresponding ymal file is modified. The Kinematics and Dynamics Library (KDL) solver in MoveIt is used to solve each joint angle, and finally, a collision-free path that meets the requirements is obtained, as shown in Fig. 15b . In Fig. 15a, 1 represents the target and 2, 3, and 4 represent the obstacles near the target. The horizontal manipulator represents the initial pose and reaches the target pose that coincides with the yellow pose. The position values of each joint at the start and end points of the robotic arm are shown in Table 5 . The trajectory of the robotic arm is shown in Fig. 15b . Figure 16 shows the position change diagram of each joint during the simulation of the manipulator. The figure shows that the manipulator runs smoothly in the simulation experiment of the improved P_RRT* algorithm, and the algorithm meets the movement needs of a real manipulator. The simulation experiment verifies the feasibility of the algorithm in this paper. To verify the superiority of the improved algorithm, the proposed algorithm was compared with the RRT, RRT* and P_RRT* algorithms. Under the same obstacle conditions mentioned above, 20 simulation experiments were performed for each algorithm, and the average of the results was used, as shown in Table 6 . According to the table, the improved P_RRT* algorithm still has high motion planning efficiency in the simulation experiment. The superiority of the improved algorithm is obvious.

The improved P_RRT* algorithm proposed in this paper improves the P_RRT* algorithm's shortcomings of low search efficiency and slow convergence speed. The superiority, effectiveness, reliability and feasibility of the improved algorithm in this paper are verified through experimental comparisons and analyses. The steps of the improved algorithm are as follows:

(1) The same random sampling strategy as P_RRT* is used;

(2) The selection strategy of the nearest neighbour node is introduced. With a certain probability, the traditional Euclidean distance method and the minimum cost function C (q) are used alternately to select the nearest neighbour node so that the algorithm can avoid obstacles more effectively; (3) The two expansion strategies for new nodes are introduced, and on the basis of the traditional new nodes adopting a target-biased strategy expansion, the second expansion of new nodes is performed, which more effectively improves the efficiency of the algorithm; (4) The reselection of the parent node from the RRT* algorithm and the rerouting operation from the RRT* algorithm are performed on the new node, and an optimal path is obtained. (5) The redundant node removal operation and the maximum curvature constraint operation are performed on the path after reselecting the parent node and the rerouting operation. The redundant nodes in the path are removed, and the maximum curvature constraint makes the path smoother by adding points from the original path to the left and right sides of the turning point at a corner with a larger curvature. (6) After the removal of redundant points and the maximum curvature constraint operation, the path is fitted with the Bezier curve, and a smooth path that is more consistent with the manipulator motion is obtained.

Through Python experiments, it can be determined that the improved algorithm has higher search efficiency and higher convergence rates, the search time and path length are effectively reduced, and the average number of sampling nodes is significantly reduced, which reduces the computer's memory consumption. The comparison of the average search times, the average search success rates, and the motion trajectories of the robotic arm obtained by the other algorithms in the ROS simulation experiment also proves the superiority of the improved algorithm.

This paper studies the spatial path planning of a robotic arm; the method can be applied in unstructured environments, such as logistics storage, material stacking, and cargo handling. In the future, the dynamic path planning of a robotic arm will be studied in a dynamic environment.

Open Access This article is licensed under a Creative Commons Attribution 4.0 International License, which permits use, sharing, adaptation, distribution and reproduction in any medium or format, as long as you give appropriate credit to the original author(s) and the source, provide a link to the Creative Commons licence, and indicate if changes were made. The images or other third party material in this article are included in the article's Creative Commons licence, unless indicated otherwise in a credit line to the material. If material is not included in the article's Creative Commons licence and your intended use is not permitted by statutory regulation or exceeds the permitted use, you will need to obtain permission directly from the copyright holder. To view a copy of this licence, visit http://creativecomm ons.org/licenses/by/4.0/.

Physical-virtual impedance control in ultralightweight and compliant dual-arm aerial manipulators

Multi-view self-supervised deep learning for 6D pose estimation in the Amazon Picking Challenge

Dexterous grasping by manipulability selection for mobile manipulator with visual guidance

Continuous trajectory point control research of six degree of freedom mechanical intelligent arm position

Rapidly-exploring random trees: a new tool for path planning

Mobile manipulator motion planning based on sparse node RRT

PQ-RRT*: an improved path planning algorithm for mobile robots

A learning-based multi-RRT approach for robot path planning in narrow passages

Quick-RRT*: triangular inequality-based implementation of RRT* with improved initial solution and convergence rate

Teaching robot navigation behaviors to optimal RRT planners

Neural network approximation based nearoptimal motion planning with kinodynamic constraints using RRT

A fast and efficient double-tree RRT*-like sampling-based planner applying on mobile robotic systems

Quad-RRT: a real-time GPU-based global path planner in largescale real environments

Grid-based RRT* for minimum dose walking path-planning in complex radioactive environments

Improved informed RRT* using gridmap skeletonization for mobile robot path planning

Adapted-RRT: novel hybrid method to solve three-dimensional path planning problem using sampling and metaheuristic-based algorithms

MOD-RRT*: a sampling-based algorithm for robot path planning in dynamic environment

Potential functions based sampling heuristic for optimal path planning

Nonlinear model predictive control for mobile medical robot using neural optimization

Design and control of a highly redundant rigid-flexible coupling robot to assist the COVID-19 oropharyngeal-swab sampling

Optimal bidirectional rapidly-exploring random trees

A method on dynamic path planning for robotic manipulator autonomous obstacle avoidance based on an improved RRT algorithm

RRT*-SMART: a rapid convergence implementation of RRT

Robot path planning based on improved RRT * FN algorithm

Sampling-based algorithms for optimal motion planning

A heuristic rapidly-exploring random trees method for manipulator motion planning

Path planning of mobile robot based on improved RRT* algorithm

Research on intelligent vehicle path planning based on rapidly-exploring random tree

Development of an improved rapidly exploring random trees algorithm for static obstacle avoidance in autonomous vehicles

Clothoids composition method for smooth path generation of car-like vehicle navigation

Curvature-constrained path elongation with expected length for Dubins vehicle

Application improvement of A* algorithm in intelligent vehicle trajectory planning

Planning and control for collisionfree cooperative aerial transportation

Publisher's Note Springer Nature remains neutral with regard to jurisdictional claims in published maps and institutional affiliations

Author contributions JY contributed to the conception of the study; contributed significantly to analysis and manuscript preparation; performed the data analyses and wrote the manuscript. QY helped perform the analysis with constructive discussions. RS helped perform the analysis with constructive discussions. HB helped perform the analysis with constructive discussions.