Literature Review

Before implementing our improvements, we did a literature review on several published papers that are related to RRT algorithm in order to get some ideas from those algorithms that are already existed and have been implemented. Based on the algorithms and concepts we have found in literature reviews, we formulated three general rules to make our own improvements on RRT as described in the problem formation section.

Article One: “A Comparison of RRT, RRT* and RRT*-Smart Path Planning Algorithms”

The first paper that we reviewed is “A Comparison of RRT, RRT* and RRT*-Smart Path Planning Algorithms”. In this paper, two versions of RRT are introduced. The first one is called RRT* algorithm. The RRT* has all the properties that RRT has, and in addition, the RRT* algorithm has two improvements. The first improvement is near neighbor search[1]. This improvement will let the new node to select the best parent node[1]. Another improvement is the rewiring operation. This operation will rebuild the tree within a radius of the area in order to keep the minimum cost of the tree and decrease the number of nodes[1]. The second version of RRT is called RRT*-Smart. The RRT*-Smart algorithm inherits all the features of RRT*. However, it has two improvements. The first one is that it has a path optimization process, which means that the algorithm will remove unnecessary nodes from the initial path found[1]. The second improvement of RRT*-Smart is that this algorithm will towards the nodes of the optimized path. Therefore, not like the other version of RRT generating nodes randomly, RRT*-Smart has a direction when generating nodes[1].

Article Two: “Autonomous navigation using received signal strength and bearing-only pseudogradient interpolation.”

The second paper that we reviewed is “Autonomous navigation using received signal strength and bearing-only pseudo gradient interpolation.” In this paper, the concept of received signal strength is introduced, and it indicates the intensity of the wireless signal in WSN communication where the target has the highest signal intensity and the intensity decreased as the area gets far away from the target [2]. This concept gives us the inspirations about the signal heuristic.

Article Three: “Sampling-based Motion Planning for Robotic Information Gathering”

The third paper that we looked at is “Sampling-based Motion Planning for Robotic Information Gathering”. In this paper, the Rapidly-exploring information gathering algorithm extends ideas from RRT and combines them with branch and bound techniques to achieve efficient optimization of information gathering and path planning while also allowing for operation in continuous space with motion constraints[3].

Problem Formulation:

Problem definition:

The RRT algorithm currently runs extremely slow. Generally, for the graph in Figure 2, it will take over two minutes to find the path from start to goal. This algorithm is not time efficient and therefore, our goal is to make the RRT algorithm runs faster. In order to improve the RRT algorithm, we plan to design a density checker, a heuristic and a signal strength. We expect that after the implementation of improvements on RRT, the running time will be reduced over 20% and the number of nodes generated will be reduced over 30%.

Improvement attempts:

We made several improvements and implementation. We did experiments and found that only one out of three improvements will work in all scenario.

Density checks:

Our first attempt was density checks. We see a lot of branches colliding with each other. This makes exploring the space very inefficient. We want to cover the whole area with nodes as even as possible. After applying density check, nodes are spaced out much more evenly. However, because density check will loop through the list of existing nodes every time a new node is trying to be created, the cost of this action goes up linearly as the tree grows.

Direction heuristic:

What if the tree knows where the goal is and tries to walk toward it as much as possible? We did experiment with RRT that knows the goal and use the coordinate to generate directional heuristic. This works perfectly if the map is simple and the next node has the direct line of sight. However, this doesn't always work as the closest node sometimes can be blocked. For example, a starting point surrounded by three walls and the only opening is opposite to the goal. In this case, the tree will try to grow towards the goal. But once hitting the wall, it will grow randomly until at some point the closest node to the goal has the direct line of sight. We abandoned this solution because it only works in some certain situations. However, this solution gave us hits that lead us to our current solution that improves every time.

Hit count randomness:

This attempt is a follow up to the previous attempts. The previous solution works only when the closest node has the direct line of sight to the goal. What if the tree will try to grow towards the goal. If attempts were made and there were no new progress, the tree should grow randomly and try such attempt again. We used a hit count that counts how many times the tree tried to grow toward the goal but failed. If the hit count goes up to a certain amount, we grow randomly and try again. We did experiment with such implementation. But again, this only works in some certain situations. The advantage of RRT was flooded over the area with speed and find the path with the shortest amount of time. Using hit count compromises the randomness of RRT,and making it actually slower than random RRT in a map with complicated obstacles.

Proposed Approach:

Approach overview

RRT is not best used as the shortest pathfinder, but rather, a fast pathfinder. Density check is our final solution, as it improves the RRT performance drastically by exploring space efficiently. The number of existing nodes is one of the factors that slows down the expansion if density check is not applied. Therefore, it is very important to only explore places with high heuristic first before anything else. In short, we want RRT to "flood" unknown spaces that have high heuristic first before "flooding" spaces that have low heuristic.

Implementations:

Density checks:

The purpose of density check is to make sure that branches are not colliding with each other. If we image that every node has a radius of explorations, we want the areas of exploration of each node has no overlaps. This made sure the nodes are exploring the space efficiently. To do this, we simply ran a for loop that checks the euclidean distance from the next node to list of the existing nodes. If the distance is shorter than the unit of the node branch, we abandon this node.

Signal heuristic:

We simulated signal heuristic by combining distance to the goal, number of obstacles and the total thickness of obstacle it shoots through if connected directly to the goal. This is a very simple simulation but works reasonably well. The heuristic function is modularized, meaning it can be replaced by any heuristic that desired based on the application

Growing RRT:

Because we used density checks, we will have to check the new node against all existing nodes. This means that as the tree gets bigger, it will cost more time to expand the next node. To avoid this expansive cost, we need to expand our nodes wisely. The algorithm will randomly pick 10 points (just like regular RRT). These regular 10 points have to be valid expansion points. Each of the points satisfied the criteria of not being in any obstacle and at least one expansion unit away from any existing nodes. We called these 10 valid expandable nodes, candidate nodes.

We then feed these 10 expandable nodes through the heuristic function, which is modularized if the different heuristic is needed. The node with the highest heuristic will be chosen as the next expansion.

Pseudo code for density check and growing RRT:

Heuristic RRT efficiency comparison:

As shown in figure 2 and figure 3, heuristic RRT generated way fewer nodes compare to completely random original RRT. The key here is our heuristic RRT trying to "flood" toward the goal while the original RRT "floods" everywhere possible. Not only heuristic RRT generated way less node, but 27 times faster finding a path. The animation for heuristic RRT ran way faster even though the animation for original RRT is speeded up 8 times. To see the performance comparison, please check the evaluation section below.

Results from evaluation:

Results:

In short, our improved RRT with heuristic has promising performance improvements.

The heuristic RRT runs 12.37 times faster on average and generates 5.31 fewer nodes compare to Original RRT with density check. It is at minimal 2.27times faster and generates at least 1.8 times fewer nodes to reach the goal. The maximum improvements are stunning, 29.89 times faster and 11 times fewer nodes generated.

Test Maps and paths generated:

We designed 10 maps to evaluate our heuristic-RRT. We compare our heuristic-RRT with original RRT with density check. We use original RRT + density check rather than just the original RRT because of a low success rate of finding the path in a reasonable amount of time. We believe that the density check should be a necessity for RRT with will guarantee to find the path in a finite space. Without density checks, there is a possibility that it never find paths if the goal was never hit by random.

box.txt results: 3.92 times less nodes generated, 7.45 times faster

corner.txt results: 7.89 times less nodes generated, 29.89 times faster

triangle.txt results: 9.63 times less nodes generated, 27.79 times faster

maze.txt results: 1.89 times less nodes generated, 3.37 times faster

drillfield.txt results: 5.19 times less nodes generated, 7.73 times faster

military_base.txt results: 2.53 times less nodes generated, 3.91 times faster

prison.txt results: 2.26 times less nodes generated, 2.27 times faster

ruins.txt results: 6.30 times less nodes generated, 8.71 times faster

school.txt results: 11.00 times less nodes generated, 28.76 times faster

Resources:

Github link: https://github.com/TheoLong/RRT-heuristic

References:

1. Noreen, I., Khan, A., & Habib, Z. (2016, October 10). A Comparison of RRT, RRT*, and RRT*-Smart Path Planning Algorithms. Retrieved December 11, 2018, from http://paper.ijcsns.org/07_book/201610/20161004.pdf

2. Nikhil Deshpande, Edward Grant, Thomas C. Henderson, Mark T. Draelos, Autonomous navigation using received signal strength and bearing-only pseudogradient interpolation, Robotics and Autonomous Systems, Volume 75, Part B, 2016, Pages 129-144, ISSN 0921-8890, https://doi.org/10.1016/j.robot.2015.10.009.(http://www.sciencedirect.com/science/article/pii/S0921889015002432)

3. Sampling-based Motion Planning for Robotic Information Gathering, Geoffrey A. Hollinger, Gaurav S. Sukhatme