Dynamic path planning strategy based on improved RRT* algorithm

SUO Chao, HE Lile

(College of Electrical & Mechanical Engineering, Xi’an University of Architecture & Technology, Xi’an 710055, China)

Abstract： In order to solve the problem of path planning of mobile robots in a dynamic environment, an improved rapidly-exploring random tree* (RRT*) algorithm is proposed in this paper. First, the target bias sampling is introduced to reduce the randomness of the RRT* algorithm, and then the initial path planning is carried out in a static environment. Secondly, apply the path in a dynamic environment, and use the initially planned path as the path cache. When a new obstacle appears in the path, the invalid path is clipped and the path is replanned. At this time, there is a certain probability to select the point in the path cache as the new node, so that the new path maintains the trend of the original path to a greater extent. Finally, MATLAB is used to carry out simulation experiments for the initial planning and replanning algorithms, respectively. More specifically, compared with the original RRT* algorithm, the simulation results show that the number of nodes used by the new improved algorithm is reduced by 43.19% on average.

Key words： mobile robot; path planning; rapidly-exploring random tree* (RRT*) algorithm; dynamic environment; target bias sampling

0 Introduction

Dynamic path planning is an extremely critical part of mobile robots[1], which can help robots find the best collision free path in the working environment. Related scholars have done a lot of related research, and the uniform sampling based rapid-exploring random tree (RRT) is widely used because of its probability completeness[2-3]. However, the path planed out by RRT has strong randomness and poor stability. Therefore, for most cases, neither local nor global path planning can be directly applied. Many researchers have put forward a variety of improvement schemes from different directions.

Choudhury[4]and Iram[5]et al. improved the convergence rate by limiting the sampling area. Wang[6]et al. used variable step RRT to generate the initial path, and then deleted the redundant nodes. The optimized nodes tended to be biased toward the barrier vertices, which achieved good results in UAV global path planning. Hyejeong[7]et al. proposed an environmental modeling method to counteract the randomness of RRT*algorithm and reduce the computation time by skeleton grid map. Shiarlis[8]used inverse reinforcement learning (IRL) to reduce the computational complexity of the algorithm, but the IRL framework itself was more complex and needed further optimization. Ichter[9]et al. used offline learning to find possible areas of a route, sample the area offset and improve the speed of route planning. Frazzoli[10]et al. proposed the RRT*algorithm, which optimized the node expansion process so that it could find a certain degree of optimal solution, but the computational load would increase dramatically as the nodes increased.

All the above-mentioned algorithms have achieved good results in static route planning, but because static route planning needs to input global environment information in advance, which can not be adapted to the dynamic environment. For this reason, route caching and replanning[11]combined with bidirectional random tree[12]were used to adapt to the dynamic environment, and achieved good results in dynamic path planning[13]. However, the bidirectional random tree method is still based on random sampling, and it is difficult to find the optimal solution in the current environment.

In this paper, combined with target bias and path cache, RRT*can be used to get a more optimized path to enhance its search performance in dynamic environment. The effectiveness of the improved RRT*algorithm is verified in MATLAB.

1 RRT* algorithm combined with target bias and path cache

The RRT algorithm takes the starting pointxinitas the root node of the random treeT, and setsVto store the nodexiwhich satisfiesxi∈Xfree, whereXfreeis the free area in the tree, and then randomly selects nodes in the free area to guide the direction of random tree growth. When a child node reaches the target neighborhood, the search is completed and the path is retrieved to the root node. Because the RRT algorithm uses uniform random sampling, it not only produces a large number of redundant nodes, but also cannot obtain the optimal path through a finite number of operations.

1.1 RRT* algorithm

The RRT*algorithm optimizes the node expansion process of RRT algorithm. First, the distance from each node to the root node is recorded as the weight of each node. Then, the node with the smallest weight valuexminis found whenxnewis connected within the neighborhood ofxnear.xminis used as the new parent node ofxnew. Ifxmindoes not exist, the original parent node is retained, so that the node is always the current optimal one. The optimization process is shown in Fig.1.

(a) Set node neighborhood

Although the RRT*algorithm can obtain an optimal solution in a certain degree, when the number of nodes is large, the computational load will increase exponentially and the computational speed will decrease dramatically.Target bias is used to limit the sampling area of RRT*algorithm to reduce node usage, and path caching and replanning are used to improve its adaptability in dynamic environments in this paper.

1.2 Target bias sampling

To solve the randomness problem of RRT*algorithm in sampling, target bias sampling is added to guide random tree growth. As shown in Fig.2, target bias sampling can make the tree grow more toward the target, reduce the search in unused areas and save computing time.

Fig.2 Target bias sampling

In this case, the growth of random trees mainly depends on the target bias probability. In open maps, this parameter can be increased appropriately so that it can pass through barrier-free areas more quickly; in maps with dense obstacles, it can be reduced appropriately so that random trees can find more paths to avoid obstacles. Therefore,it can partly reduce the amount of calculation caused by RRT*algorithm by adjusting the target bias probability according to different map characteristics.

1.3 Path replanning

RRT*algorithm needs to adjust the dynamic environment due to new obstacles. The idea used in this paper is to replan the existing routes in time when they are about to cross obstacles that are not included in the original map. The newly planned routes should not only avoid the current obstacles, but also be as close as possible to the original routes.

1.3.1 Creating path cache

In order to achieve the functions mentioned above to deflect the new path from its original counterpart, the path cache is set up in the plan. Path caching refers to the preservation of points on the original planned path, which has a certain probability to be selected asxnewnodes to guide the growth of random trees when the new route plan starts sampling.

Based on the random tree growth strategy described in Fig.3, the improved growth guidance functionF(xnear) of new nodexnewgrowing fromxnearis

F(xnear)=R(xnear)+G(xnear)+P(xnear),

(1)

whereR(xnear) is the random sampling function for nodes;G(xnear) is the target bias function, andP(xnear) is the path cache bias function.

Gdenotes the force of target vectorxgoalonxnearas

(2)

(3)

(a) Path cache

(c) Toward path cache

Similarly, a path cache bias function can be constructed by

(4)

The random extension function of the basic RRT algorithm[2]is

(5)

Substituting Eqs.(3)-(5) into Eq.(1), the growth guidance function can be obtained by

(6)

Further, the formula for generating the new node is

(7)

In practice, define three probabilitypgoal,ppathandprandas the probability of random tree growth toward target, path cache point and random growth, respectively, where

pgoal+ppath+prand=1.

(8)

This section mainly modifies the new node selection function in the RRT*algorithm. This operation starts with a random numberpand a random integeri, wherepis selected from 0 to 1, andiis selected from 1 toN, andNis the number of nodes in the path cache. Ifppgoalandp

(9)

wherexgoalis the coordinate of the target;xpath,iis the coordinate of theith point in path cache, andxfreeis the coordinate of a random point in sampling space.

1.3.2 Trimming and rebuilding of random trees

The introduction of the path cache reduces the inconsistency of the results of each route planning caused by the randomness of the RRT*algorithm. In global planning, the RRT*algorithm can be combined to obtain the characteristics of the optimal path, and a more stable optimal path can be planned. In dynamic path planning, new obstacle in the environment or moving obstacle may block the original path, which can be regarded as new obstacles blocking the original path. At this time, the original path needs to be trimmed and replanned. Therefore, dynamic route planning is mainly divided into two parts： initial planning and replanning. Initial planning refers to route planning by entering environmental information before a robot enters the space to move. Replanning refers to the emergence of new obstacle during the course of the robot’s movement along this route, which makes the original remaining path unavailable.

(a) Initial planning

(c) Replanning

As shown in Fig.4, when new obstacle appears within a certain distance in the direction of the mobile robot’s movement, the current location is used as the starting point for the replanning of the path.

Due to the introduction of path cache in the replanning, the new route will not deviate from the original route too much. Combined with the target bias, the efficiency of the replanning is improved, and the resource consumption in the process of calculation, including robot advancing, is further reduced. Therefore, the improved RRT*algorithm can better adapt to the path planning in dynamic environment. The flow chart of path planning proposed in this paper is shown in Fig.5.

Fig.5 Flow chart of dynamic path planning

2 Simulation

In order to verify the validity of the algorithm in practical application, the algorithm is simulated in both static and dynamic environments in MATLAB. First, the validity of target bias and path cache bias in three typical environments is tested in a static environment. Then, the original algorithm and the improved algorithm are applied in different environments for 100 simulation experiments, and their performance indicators are analyzed.

2.1 Static environment simulation

The map size is set to [1 000,1 000], the starting coordinate is [0,1 000], and the ending coordinate is [800,100]. The three environments are sparse environment with rectangular obstacles only in the northwest and southeast corners of the map, narrow environment with larger obstacles in the center of the map, concave obstacle environment with U-shaped deadlock easily caused by obstacles near the starting point.

2.1.1 RRT*algorithm

The RRT*algorithm is used to simulate in three environments. In the diagram, the cross indicates the sampling point, the dark rectangles are obstacles, the highlighted route is the final path, the black line is the random tree in the process of operation, and the thin line is the connection between the new node of RRT*algorithm when expanding the node and other nodes in its domain.

(a) Sparse environment

(b) Narrow environment

(c) Concave obstacle environment

2.1.2 Target bias sampling

Adding target bias sampling to RRT*algorithm, it can be seen that a large number of unnecessary nodes are reduced due to the stronger search target, the oscillation of the path near the target is significantly reduced, and the operation efficiency is improved compared with the original RRT*algorithm in the search process.

(a) Sparse environment

(b) Narrow environment

(c) Concave obstacle environment

In different environments, the probability of target bias sampling can be adjusted appropriately. For example, the proportion in sparse environment can be adjusted appropriately to make the path converge to the target point faster; the proportion in the narrow environment or concave obstacle environment can be adjusted appropriately to explore as many regions as possible during path search.

To make the path converge faster, the target bias probability in the three environments is set to 0.8.It will increase the risk of operation falling into U-shaped deadlock by using higher target bias probability in concave obstacle environment, however, it also means that once the exit of this deadlock area is explored, it can converge to the target point faster at the same time.

Table 1 shows the node usage of different algorithms in three environments.Nrefers to the number of nodes used;Nprefers to the number of path nodes;g=0 indicates no target bias is used;g=1 indicates target bias is used, anduNis the node utilization.

Table 1 Simulation result by two algorithms in three environments

2.1.3 Path cache

Due to the characteristics of RRT*algorithm, it is difficult to plan the same path several times under the same constraints. The results of the first path planning are used as the path cache of the next planning, so that each planning can be avoided to start in a completely random way, and the emergence of useless nodes can be further avoided. In addition, RRT*algorithm can find the optimal solution under the current random tree, so combined with path caching, it can be closer to the global optimal path.

In order to verify the validity of the path cache, it is used to calculate multiple times for the same map in the static environment. The obtained path and node usage are shown in Fig.8 and Table 2.

It can be seen from Fig.8 and Table 2 that the effective paths obtained by path planning with path cache are consistent to a great extent. At the same time, due to the reduction of randomness in sampling, the exploration of useless nodes is also less. However, the path nodes have not been greatly reduced, which is determined by the distance between the expansion step and the starting point and the end point. When the step size is fixed, the path nodes and path length under the same conditions can not be reduced without limit. However, due to the introduction of path cache, node calculation is saved and the node utilization is improved.

(a) Initial planning

(b) 1st path planning

(c) 2nd path planning

(d) 3rd path planning

(e) 4th path planning

(f) 5th path planning

Table 2 Simulation result with path cache

2.2 Replanning in dynamic environment

In the dynamic environment, there are two kinds of obstacles. One is that the moving obstacles appear in front of the mobile robot, blocking its original route; the other is that there are obstacles that are not in the environment. Both of them can be regarded as new obstacles in front of the robot’s running path. Next, 100 simulation experiments are carried out for the three typical environments mentioned above, and the proposed algorithm with path cache and target bias sampling is used in the replanning. At the same time, it is compared with the RRT*algorithm which only uses target bias sampling. The node usage of the two methods is compared, and the typical experimental results are selected for analysis.

2.2.1 Sparse environment

The characteristic of sparse environment is that the obstacles are far away and the robot has a large space to move. Therefore, the target bias probability in initial planning and path cache bias probability in replanning can be appropriately increased.

In the two experiments,pgoalis 0.7 in the initial planning and 0.5 in the replanning, in which the path cache bias is added in the second replanning,ppath=0.3. It can be seen from Fig.9 that the results obtained in the initial planning of the two experiments are similar, but the path direction in the replanning is quite different. Although the path in Fig.9(d) is longer than that in Fig.9(b), the former explores less areas in searching paths, so compared with the latter, the calculation time is greatly reduced.

(a) Initial planning

(b) Replanning

(c) Initial planning using target bias sampling

(d) Replanning with target bias and path cache

2.2.2 Narrow environment

In the narrow environment, the distance between obstacles is close, so the target bias probability in the initial planning should be reduced appropriately. In the process of replanning, it is difficult for the new path to be consistent with the original path after the new obstacle blocks the original path. Therefore, the path cache bias probability should be appropriately reduced.

In the narrow environment, the initial planningpgoalof the two experiments is 0.7, while the replanning ispgoal=0.2. In the second experiment, the path cache bias is added to the replanning, andppath=0.2. At this time, the two probabilities are appropriately lowered because the distance of obstacles in narrow environment is relatively close, and the mobile space of robot is compressed. The dynamic obstacle avoidance in this environment has the risk of falling into a U-shaped deadlock. In order to make the random tree explore as much as possible, the two probabilities are appropriately reduced. As can be seen from Fig.10, the new obstacle encountered during the replanning has formed a U-shaped deadlock with the original obstacles, and the robot is located in the deadlock area.

(a) Initial planning using target bias sampling

(b) Replanning with target bias sampling

(c) Initial planning using target bias sampling

(d) Replanning with target bias and path cache

2.2.3 Concave obstacle environment

Concave obstacle environment is a relatively easy to form a U-shaped deadlock environment, but once the robot can jump out of the deadlock area, it can quickly find an effective path in the open area. Therefore, the two probabilities keep the values used in sparse environment in the experiment.

(a) Initial planning using target bias sampling

(b) Replanning with target bias sampling

(c) Initial planning using target bias sampling

(d) Replanning with target bias and path cache

Due to the introduction of target bias sampling in the initial planning, although it is easy to fall into a U-shaped deadlock environment, as long as the random tree finds the exit of the deadlock area, it can quickly expand towards the target and get the available path. However, in the replanning, it is obvious that the random tree with path cache tends to be more centralized, and the exploration of useless areas is less.

3 Results and discussion

Because of the characteristics of RRT*algorithm, better routes have been obtained at the beginning of planning. Therefore, the quality of the routes can be further improved by introducing the path cache and replanning with RRT*algorithm. However, generally the path length between two points cannot be infinitely compressed in the same environment, especially in narrow environments, as can be seen from Fig.10, effective paths are limited to two main directions when replanning. Therefore, although the new algorithm can shorten the path length, the improvement in the path length is limited compared with the original algorithm. However, it can be seen from the graph that even if the resulting path lengths are similar, the improved algorithm greatly reduces the running time and overall improves the efficiency. The average path length in the three environments was reduced by 13.71%, 2.20% and 8.73%, respectively.

(a) Sparse environment

(b) Narrow environment

Table 3 Sparse environment

Table 4 Narrow environment

In order to preserve the probability completeness of the algorithm, the probability of random sampling cannot be compressed indefinitely, so the new algorithm still has the probability of random sampling in operation, which makes the performance indicators far from the average in rare cases. Nevertheless, it still has some advantages over the original algorithm. The new algorithm sacrifices some stability and improves the overall operation efficiency and the quality of the results.

Table 5 Concave environment

4 Conclusions

According to the characteristics of RRT*algorithm, this paper proposes an improved RRT*algorithm and applies it to obstacle avoidance strategy in dynamic environment. Firstly, the static path planning is carried out in the static environment, and the target bias sampling is introduced in the initial planning to limit the sampling area and reduce the exploration of the useless area. Secondly, on the basis of maintaining the target bias sampling in the path replanning, the path cache bias sampling is added to further reduce the randomness of the original algorithm. At the same time, the two probabilities can be adjusted for different environments to achieve the optimal effect. In this paper, MATLAB is used to simulate the initial planning and replanning compared with the traditional RRT*algorithm. The simulation results show that the number of nodes used in the new algorithm is reduced by 43.19%, the calculation time is reduced by 62.20%, and the average path length is reduced by 8.21%. The simulation results verify the effectiveness of the new improved algorithm. At the same time, the research structure of this paper also lays a good foundation for the subsequent study of parameter optimization in different types of environments.