Collision-Free Path Planning Method for Robots Based on an Improved Rapidly-Exploring Random Tree Algorithm

Featured Application

The new method can be used to guide the path planning of robots with any number of degrees of freedom in complex environments.

Abstract

Sampling-based methods are popular in the motion planning of robots, especially in high-dimensional spaces. Among the many such methods, the Rapidly-exploring Random Tree (RRT) algorithm has been widely used in multi-degree-of-freedom manipulators and has yielded good results. However, existing RRT planners have low exploration efficiency and slow convergence speed and have been unable to meet the requirements of the intelligence level in the Industry 4.0 mode. To solve these problems, a general autonomous path planning algorithm of Node Control (NC-RRT) is proposed in this paper based on the architecture of the RRT algorithm. Firstly, a method of gradually changing the sampling area is proposed to guide exploration, thereby effectively improving the search speed. In addition, the node control mechanism is introduced to constrain the extended nodes of the tree and thus reduce the extension of invalid nodes and extract boundary nodes (or near-boundary nodes). By changing the value of the node control factor, the random tree is prevented from falling into a so-called “local trap” phenomenon, and boundary nodes are selected as extended nodes. The proposed algorithm is simulated in different environments. Results reveal that the algorithm greatly reduces the invalid exploration in the configuration space and significantly improves planning efficiency. In addition, because this method can efficiently use boundary nodes, it has a stronger applicability to narrow environments compared with existing RRT algorithms and can effectively improve the success rate of exploration.

1. Introduction

With the development of computer technology and modern manufacturing, particularly in the context of Industry 4.0, the intelligence level requirements continue to increase and the application scenarios of multi-Degree-Of-Freedom (DOF) robots are becoming increasingly complex. These conditions pose challenges for the motion planning of manipulators. Up to now, scholars have extensively researched motion planning in high-dimensional spaces and unstructured complex environments. Sampling-based methods mainly seek a collision-free path from the start point to the goal point by sampling in the Configuration space (C-space). It is unnecessary to model the entire space, and the methods have a probabilistic completeness [1,2,3]. Therefore, sampling-based methods are widely used in the motion planning of high-dimensional spaces owing to their unique advantages. For example, the Rapidly-exploring Random Tree (RRT) [4] and Probabilistic Roadmap Method (PRM) [5,6] are the most popular and commonly used techniques.

Basic-RRT, as shown in Algorithm 1, has been widely used in many fields, including robot motion planning, as a single-query planner since it was proposed by LaValle et al. in 1998. A large number of algorithms derived from RRT have been proposed to solve different problems. Improvements in RRT have mainly focused on sampling strategies and their guidance for exploring areas, the selection of extension nodes, the directions and step sizes of extensions, selection of metrics, and the collision detection algorithm and local connection method, all of which have many studies available for reference [7]. These improved methods enhance the performance of Basic-RRT from various aspects, but the obtained solutions are still highly suboptimal in most cases. Therefore, various methods of pruning and smoothing [8,9,10,11] have been proposed to implement path post processing.

Algorithm 1 Basic-RRT algorithm.

1: $T\leftarrow$ InitTree($q_{start}$);

2: for $i=$ 1 to n do

3: $q_{rand}\leftarrow$ RandomSample(i);

4: $q_{near}\leftarrow$ NearestNeighbor($q_{rand},T$);

5: $q_{new}\leftarrow$ Extend($q_{rand},q_{near},\epsilon$);

6: if CollisionFree($q_{near},q_{new}$) then

7:  AddNewNode($T,q_{new}$);

8: end if

9: if Distance($q_{new},q_{goal})<{\rho }_{min}$ then

10:  return T;

11:  end if

12: end for

13: return $Failed$;

Algorithm improvement, regardless of the method, has only two purposes: to reduce the path cost and to reduce the running time of the algorithm. For the former, the typical method is RRT*, which is an optimal planning method based on RRT and proposed by Karaman and Frazzoli [12]. It guarantees the asymptotic optimality of the algorithm by adding two processes to Basic-RRT: “near vertices” and “rewire”. However, the planning process is time consuming and thus impractical for planning tasks that have certain time requirements. Many methods [13,14,15] have been subsequently proposed to accelerate the convergence of RRT*, but the computational time still cannot meet the requirements of the actual application.

This paper focuses on the latter purpose of algorithm improvement, namely to decrease the computational time of the algorithm on the premise that the algorithm can be applied to complex environments. The usual approach involves reducing the number of nodes in the tree and the number of collision detections. RRT-biased [16] is a simple way to improve efficiency. As shown in Algorithm 2, it improves the performance of the algorithm through goal bias sampling with a certain probability (usually 5–10%). However, although the number of nodes in the random tree is appropriately reduced, the computational cost remains large. Gitae Kang et al. [17] proposed a method based on goal-oriented sampling for the motion planning of a manipulator; this method can limit the distribution of sampling points to improve the search speed, but the invalid exploration area is large. Brendan Burns et al. [18] proposed a utility function for selecting the extension node and direction; this function, which determines the maximum expected extension step of the planner on the basis of the obtained state space information, has improved efficiency to some extent. In addition, Dong-Hyung Kim et al. [19] described a method of adaptive body selection based on the complexity of planning; this method provides another description of the variable-dimensional C-space for high-DOF articulated robots, thereby improving efficiency from the perspective of dimensionality reduction.

Algorithm 2 GoalBiasedSample()

1: $num\leftarrow$ RandomNumber;  RandomNumber $\in [0,1]$

2: if $num<$ k then

3: $q_{rand}=q_{goal}$;

4: else

5: $q_{rand}=$ RandomSample();

6: end if

7: return $q_{rand}$;

These methods and many existing RRT variants that guide sampling and expansion have improved efficiency from different aspects, but there is no good general solution for complex environments, such as “narrow” ones. Haojian Zhang et al. [20] proposed an improved method combining the regression and boundary expansion mechanisms; this technique improves efficiency by providing a corresponding solution process for a specific problem. However, although the boundary node can be identified during the exploration process, this method also limits the use of boundary nodes. In addition, some methods [21,22,23] dedicated to improving the success rate of the algorithm by changing the sampling strategy have been proposed specifically to solve the problem of narrow environments, but their universality is also weak.

It can be seen from the above that an efficient and universal algorithm that is suitable for complex environments is needed to ensure the completion of planning tasks. Therefore, this paper proposes an autonomous path planning algorithm of Node Control based on the architecture of RRT (NC-RRT). Firstly, a method of gradually changing the sampling area is proposed to guide exploration, thereby effectively improving the search speed. In addition, unlike existing methods, which aim to explore new sampling strategies, a node control mechanism is proposed to constrain the extended nodes of the tree and, thus, enhance the applicability of the environment. The results reveal that the algorithm greatly reduces the invalid exploration in the C-space and significantly improves planning efficiency. Moreover, compared with most existing RRT algorithms, NC-RRT is universal for different environments by appropriately adjusting the parameters. For the convenience of explanation, the algorithm will be first described and verified in a two-dimensional space and then applied in a high-dimensional space.

The rest of the paper is organized in the following manner. Section 2 introduces the proposed improved RRT algorithm in two parts. The method of gradually changing the sampling area is proposed in the first part, and we present the node control mechanism in the second part. Section 3 explains the simulation process and results of the algorithm. The proposed algorithm is simulated in a two-dimensional space and then applied to a 6-DOF manipulator. Finally, the conclusions and future work of this paper are provided in Section 4.

2. The NC-RRT Method

2.1. The Method of Gradually Changing the Sampling Area

We propose the method of gradually Changing the Sampling Area based on RRT (CSA-RRT) to guide exploration; the process is shown in Algorithm 3. As in [17], we need to calculate the distance (the Euclidean distance is used in this paper) between the goal configuration point $q_{goal}$ and the configuration point that is initially the farthest from the goal $q_f$ in the C-space, which is represented by $D_{farthest}$ here, to ensure the completeness of the solution space. Assuming that the dimension of the C-space is s, $D_{farthest}$ is solved as follows in Equation (1):

$$D_{farthest}=\sqrt{{(q_{goal\left(1\right)}-q_{f\left(1\right)})}^2+...+{(q_{goal\left(s\right)}-q_{f\left(s\right)})}^2}.$$

(1)

Algorithm 3 CSA-RRT algorithm.

1: $T\leftarrow$ InitTree($q_{start}$);

2: $R\leftarrow$$D_{farthest}$;

3: for $i=$ 1 to n do

4: $q_{rand}\leftarrow$ RandomSample(i);

5: if Distance$(q_{rand},q_{goal})>R$ then

6:  continue;

7: end if

8: $q_{near}\leftarrow$ NearestNeighbor($q_{rand},T$);

9: $q_{new}\leftarrow$ Extend($q_{rand},q_{near},\epsilon$);

10: if CollisionFree($q_{near},q_{new}$) then

11:  AddNewNode($T,q_{new}$);

12:  $R=$ Distance$(q_{new},q_{goal})$;

13: else

14:  $R=R+k\times \epsilon$;

15:  continue;

16: end if

17: if Distance($q_{new},q_{goal})<{\rho }_{min}$ then

18:  return T;

19: end if

20: end for

21: return $Failed$;

We initialize the sampling radius R to $D_{farthest}$, and the sampling range in the subsequent sampling process is limited to the area within R of the goal configuration point. If a new node $q_{new}$ is successfully added in a certain iteration, that is there is no collision with obstacles during the generation of $q_{new}$, then the value of R is changed to the distance between $q_{new}$ and $q_{goal}$. On the contrary, if there is a collision, then the sampling radius is increased as follows:

$$R=R+k\times \epsilon ,$$

(2)

where k is the coefficient used to change the range of the sampling domain; its value is a positive integer and can be adjusted according to the complexity of the environment. $\epsilon$ is the step size of the extension. We can find that, if there is no obstacle in the environment or there are obstacles, but no collision, the sampling domain will be reduced to an area $\Vert q_{new}-q_{goal}\Vert$ away from the goal node each time a new node is added to the tree; once obstacles are encountered during expansion, the sampling domain will be expanded to find a new sampling point and the corresponding $q_{near}$ node. When $q_{new}$ is successfully added, the random tree will be restored to the no obstacle state and continue to explore. This gradual change in the sampling area will drive the tree to grow continuously toward the goal node. Figure 1 shows the performance comparison of Basic-RRT and CSA-RRT in the same environment. Obviously, the latter has much fewer nodes. The specific experiment and analysis are provided in Section 3.

2.2. The Node Control Mechanism

When the CSA-RRT algorithm selects the extended nodes, the nodes in the tree should be traversed to find the node with the smallest distance from the sampling configuration point in the C-space, but this traversal is usually redundant and time consuming. Under the premise of maintaining the performance of the CSA method, a node control mechanism is introduced in this part to further reduce the extension of invalid nodes and extract the boundary nodes (or near-boundary nodes), thereby further improving the speed of the algorithm and enhancing the adaptability of the environment. On the basis of this mechanism, the “local trap” phenomenon in the process of random tree expansion is proposed. We update and record the expansion state of each node when the random tree expands in the C-space. The value of the node control factor is changed according to whether the “local trap” phenomenon occurs. Then, the selection of the extension node is adjusted, such that the tree is expanded by the boundary nodes only or by nodes close to the boundary in most cases. The specific process is as follows.

We represent the extended state value of the node as $\delta$ and redefine the following concept that is different from the past: the path that each leaf node of the tree traces back to the initial node $q_{start}$ is called a branch. Then, the change strategy of the node state value is as follows. Its $\delta$ value is set to zero each time a new node is successfully added to the tree. Then, starting from the parent node and following the reverse path, one is added to the state value of each node in the branch where this new node is located until it traces back to the initial node. As shown in Figure 2, any node $q_n$ in the process of random tree exploration is regarded as an example. Assuming its $\delta$ value is recorded as m at this time, if a child node $q_{n+1}$ is generated with $q_n$ as the parent node in a certain iteration, then the value of $\delta$ for $q_{n+1}$ is set to zero; then, starting from the parent node (i.e., $q_n$) of $q_{n+1}$ and following the reverse path, one is added to the value of $\delta$ for all nodes in the branch where $q_{n+1}$ is located until it traces back to the $q_{start}$ node. At this time, the $\delta$ value of $q_n$ becomes $m+1$. If the new node $q_{n+2}$ generated in a subsequent iteration is still a child node of $q_n$, the above process is repeated, and the $\delta$ value of $q_n$ becomes $m+2$. However, because the $q_{n+1}$ node is not in the same branch as the $q_{n+2}$ node, the $\delta$ value of the $q_{n+1}$ node remains unchanged in this iteration.

According to the state value of each node, a node control factor $control$ can be introduced to guide the selection of the expanded node. For each extension, only the node that has a state value less than $control$ and lies closest to the sampling node is selected as the extension node. To reduce the invalid exploration in the C-space sufficiently, we set $control$ to one by default. However, the introduction of this control factor creates a problem. Because only the $\delta$ value of the leaf nodes in the random tree is zero and the value of $control$ remains one, it means that the last node in the tree will always be used as the extension node. Combined with the proposed CSA sampling method, this technique will drive the tree to reach the target configuration extremely rapidly if there are no obstacles in the environment. As shown in Figure 3a, we can see that all nodes in the tree exist as “valid nodes” in the final query path. However, once there are obstacles in the environment, the tree easily falls into the “local trap” state, as shown in Figure 3b. The random tree will keep this state, which can be expanded in local areas only until it reaches the set maximum number of failures.

Therefore, we need to change the value of $control$ at the appropriate time to increase the number of optional extension nodes to prevent this problem. This phenomenon is likely to occur when the tree encounters obstacles. Thus, we consider the collision occurrence in the expansion process approximately as the judgment condition for the occurrence of the “local trap” phenomenon, that is the condition for changing the value of the node control factor, similar to the change condition of the sampling radius in the CSA-RRT method. When collision occurs, the $control$ value is increased, and the tree can effectively escape from this area. After the new node is successfully added, the $control$ value is restored to one to continue the exploration. At this point, the NC-RRT algorithm proposed in this paper is obtained, and its pseudocode is shown in Algorithm 4.

Algorithm 4 NC-RRT algorithm.

1: $T\leftarrow$ InitTree($q_{start}$);

2: ${\Delta }_T\leftarrow$$\left\{\delta \left(q_{start}\right)=0\right\}$;

3: $R\leftarrow$$D_{farthest}$;

4: $control\leftarrow$ 1;

5: for $i=$ 1 to n do

6: $q_{rand}\leftarrow$ RandomSample(i);

7: if Distance$(q_{rand},q_{goal})>R$ then

8:  continue;

9: end if

10: $T_{ctrl}\leftarrow$ LessThanControl(${\Delta }_T,control,T$);

11: $q_{near}\leftarrow$ NearestNeighbor($q_{rand},T_{ctrl}$);

12: $q_{new}\leftarrow$ Extend($q_{rand},q_{near},\epsilon$);

13: if CollisionFree($q_{near},q_{new}$) then

14:  AddNewNode($T,q_{new}$);

15:  ${\Delta }_T\leftarrow$$\left\{\delta \left(q_{new}\right)=0\right\}$;

16:  ${\Delta }_T\leftarrow$ BranchNodeUpdate(${\Delta }_T,q_{new},T$);

17:  $R=$ Distance$(q_{new},q_{goal})$;

18:  $control=1$;

19: else

20:  $R=R+k\times \epsilon$;

21:  $control=$ c;  (c $=2,3,...)$

22:  continue;

23: end if

24: if Distance($q_{new},q_{goal})<{\rho }_{min}$ then

25:  return T;

26: end if

27: end for

28: return $Failed$;

3. Simulation and Analysis

Computation in a two-dimensional space is small and yields an intuitive effect. Therefore, several algorithms were firstly simulated in a two-dimensional environment in this section to evaluate the performance of the proposed algorithm. Basic-RRT, CSA-RRT, and NC-RRT were analyzed and evaluated by comparing their simulation results in various environments. Subsequently, each algorithm was applied to a 6-DOF serial manipulator and simulated in an environment full of obstacles. Simulations of all algorithms were performed using MATLAB 2015b on a Windows 10 system with an Intel Core i7-8750H 2.2 GHz CPU, and 8 GB of RAM. In addition, a virtual prototype experiment of the 6-DOF manipulator was completed by using ADAMS. The simulation results in the two-dimensional space and the virtual prototype experiments of the 6-DOF manipulator were the average values of 50 runs.

3.1. Simulations in a Two-Dimensional Environment

Each algorithm was simulated in three different environments in a two-dimensional space. As shown in Figure 4, three typical maps were used: a map cluttered with obstacles, a trapped environment, and a narrow passage. The environment dimensions were $500\times 500$ in all cases. The black areas in the map indicate obstacles, and the start and goal points were set separately in each map. The step size $\epsilon$ was set to 15, and the maximum number of failures nwas set to 2000. In addition, the k and c values in all the algorithms could be adjusted appropriately according to the different environments.

Figure 5, Figure 6 and Figure 7 present the performance of the three algorithms in the cluttered, trapped, and narrow environments, respectively. The entire exploration process is denoted by blue lines, and the resulting query path is denoted by red lines. When the Basic-RRT algorithm was simulated, the nodes of the random tree almost filled the entire free space in each map. By contrast, the number of nodes was greatly reduced after the proposed CSA-RRT and NC-RRT algorithms were used. In addition, as seen in Figure 6c and Figure 7c, the extended nodes in the tree were distributed more around the obstacles because of the node control mechanism in the NC-RRT algorithm.

Table 1. Simulation results of the three algorithms in the cluttered environment. $(k=1,c=2).$

Algorithm	Average Computational Time (s)	Average Number of Nodes in the Tree	Average Number of Collision Detections	Average Path Length	The Success Rate of the Algorithm
Basic-RRT	0.463	844.520	38,026	954.369	96%
CSA-RRT	0.058	92.900	4860.1	915.982	100%
NC-RRT	0.055	82.860	3941.7	977.667	100%

,

Table 2. Simulation results of the three algorithms in the trapped environment. $(k=3,c=2).$

Algorithm	Average Computational Time (s)	Average Number of Nodes in the Tree	Average Number of Collision Detections	Average Path Length	The Success Rate of the Algorithm
Basic-RRT	0.471	928.680	35,821	865.236	84%
CSA-RRT	0.118	122.909	9705.3	805.751	88%
NC-RRT	0.159	126.817	9214.6	992.595	88%

and

Table 3. Simulation results of the three algorithms in the narrow environment. $(k=1,c=2).$

Algorithm	Average Computational Time (s)	Average Number of Nodes in the Tree	Average Number of Collision Detections	Average Path Length	The Success Rate of the Algorithm
Basic-RRT	0.550	1145.600	40,159	871.825	78%
CSA-RRT	0.106	172.732	8867.2	868.609	82%
NC-RRT	0.117	126.245	6746.4	942.615	98%

present the simulation results of the three algorithms with respect to the average computational time, average number of nodes in the tree, average number of collision detections, average path length, and success rate in each environment. The results revealed that the proposed CSA-RRT and NC-RRT had good effects in each environment. Compared with that of Basic-RRT, the running times of the proposed algorithm were greatly reduced, and the numbers of nodes in the random tree and the numbers of collision detections were decreased.

However, as can be seen from Table 1, Table 2 and Table 3, the average computational time of CSA-RRT and NC-RRT in the cluttered environment was 0.058 and 0.055 s, respectively; the average computational time in the trapped environment was 0.118 and 0.159 s, respectively; and the average computational time in the narrow environment was 0.106 and 0.117 s, respectively. Thus, the computing efficiency of the two algorithms in each environment was nearly the same. Moreover, the CSA method could reduce the path length to some extent, but the path length increased slightly after the node control mechanism was added. The reason was that the random tree in the NC-RRT method usually expanded along the boundary of obstacles. However, it was irrelevant for our purposes because the node control mechanism proposed in this paper was concerned more about how to improve the environmental adaptability of the algorithm by using boundary nodes than about the path cost. Furthermore, all the above-mentioned algorithms would eventually need to complete path pruning and smoothing when used in practice, so this cost could be ignored. In addition, we could see that the success rates of NC-RRT in the cluttered and trapped environments were not significantly different compared with those of the other two algorithms. However, in the narrow environment, the success rate was effectively increased, and the occurrence of pathological cases was reduced due to the excellent boundary property of the NC-RRT algorithm. Therefore, the proposed node control mechanism was necessary. In other words, the NC-RRT algorithm, which combined the CSA method and the node control mechanism, could effectively improve the efficiency of planning and was more suitable for complex environments than other algorithms.

3.2. Simulation of the 6-DOF Manipulator

In this section, the above algorithms are applied to a 6-DOF serial manipulator. As presented in Figure 8, the manipulator was surrounded by obstacles (blue solid blocks) mainly distributed in the upper and lower parts of the manipulator workspace. These two parts restricted the path of the manipulator through a tunnel. Each algorithm was implemented and applied to the manipulator to complete the task of passing through the tunnel from the initial configuration to the target configuration. Because the success rate of using uniform sampling for planning in high-dimensional spaces was almost zero, all the algorithms in this experiment adopted the sampling strategy with a $10\%$ goal bias to maintain the consistency of the conditions. The step size $\epsilon$ was set to $2^{\circ }$, and the maximum number of failures n was set to 2000. $k=15,c=2$ were set in the NC-RRT and CSA-RRT algorithms, and the average time obtained after 50 runs of each algorithm is presented in

Table 4. Average computational time for each algorithm applied to the 6-DOF manipulator ($10\%$ goal bias, $100\%$ success rate).

Algorithm	Basic-RRT	CSA-RRT	NC-RRT
Average computational time (s)	3.658	1.541	1.469

. It could be seen that compared with RRT with the $10\%$ goal bias, the algorithm proposed in this paper significantly enhanced the efficiency.

4. Conclusions and Future Work

To address the problem of existing sampling-based planners having low exploration efficiency and poor environmental adaptability and the increasingly sophisticated level of intelligence requirements in the Industry 4.0 era not being met, this study proposed a path planning algorithm that was based on the architecture of the RRT algorithm and suited complex environments. The algorithm included a method of gradually changing the sampling area and a node control mechanism, which were used to guide the random tree exploration and reduce the expansion of invalid nodes, respectively. Furthermore, the node control mechanism could extract boundary nodes to improve the environmental adaptability. The algorithm was tested in three scenarios in two-dimensional space and was applied to a 6-DOF manipulator. The results revealed that the algorithm was effective and universal. It could significantly improve the planning efficiency and had a stronger applicability to complex environments, particularly narrow environments, compared with the traditional RRT algorithm.

However, the proposed method had certain limitations. The selection of the parameters k and c in the algorithm was a problem. Although the algorithm was universal, these parameters sometimes needed to be adjusted appropriately for different environments in order to obtain the best results. In addition, the path quality needed to be improved. This issue will be further researched by attempting an adaptive adjustment of parameters and considering the introduction of curvature constraints and other kinematic and dynamic constraints to improve the performance of the algorithm.