Robotic Path Planning: RRT and RRT*
Exploring the optimized version of a orthodox path planning algorithm
The robotic path planning problem is a classic. A robot, with certain dimensions, is attempting to navigate between point A and point B while avoiding the set of all obstacles, Cobs. The robot is able to move through the open area, Cfree, which is not necessarily discretized.
Unlike most path planning algorithms, there are two main challenges that are imposed by this problem. First, the robot does not have existing nodes to travel between. If one considers the famous Dijkstra’s algorithm, that problem includes a graph. With a free continuous space, a graph of edges and vertices needs to be created. This fosters questions on the characteristics a graph should have for such a problem. Secondly, one must determine how a shortest path will be determined. The problem of building a graph and navigating are not necessarily solved by the same algorithm.
There exist numerous path planning algorithms that address the navigation problem. Rapidly-exploring random trees (RRT) is a common option that both creates a graph and finds a path. The path will not necessarily be optimal. RRT*, popularized by Dr. Karaman and Dr. Frazzoli, is an optimized modified algorithm that aims to achieve a shortest path, whether by distance or other metrics. Both are implemented in python and observed in this article. To demonstrate the idea, the algorithms will be implemented in a 2D space with bounds. However, both algorithms can be built into any real continuous dimensional space. A video demonstration of these algorithms, as well as additional algorithms, is at the end of this article.
RRT
The premise of RRT is actually quite straight forward. Points are randomly generated and connected to the closest available node. Each time a vertex is created, a check must be made that the vertex lies outside of an obstacle. Furthermore, chaining the vertex to its closest neighbor must also avoid obstacles. The algorithm ends when a node is generated within the goal region, or a limit is hit.
RRT Pseudo CodeQgoal //region that identifies success
Counter = 0 //keeps track of iterations
lim = n //number of iterations algorithm should run for
G(V,E) //Graph containing edges and vertices, initialized as empty
While counter < lim:
Xnew = RandomPosition()
if IsInObstacle(Xnew) == True:
continue
Xnearest = Nearest(G(V,E),Xnew) //find nearest vertex
Link = Chain(Xnew,Xnearest)
G.append(Link)
if Xnew in Qgoal:
Return G
Return G
The method of determining a random position is a design decision. Simple methods, such as using built in random number generators can be used. For basic applications, such approaches suffice. Yet, developers should understand that random number generators are not truly random and do contain a degree of bias. An entire topic of study, known as Sampling Theory (La Valle, 195) , exists for the curious.
Additionally, the method of chaining the randomly generated vertex is customizable. One method involves calculating the vector that forms the shortest distance between the new vertex and the closest edge. At the point of intersection, a new node is added to the edge and connected to the randomly generated vertex. Alternatively, the vertex can be attached to the closest node by chaining a link of discretized nodes to it. This method requires less computation and is simpler to implement, but requires more points to be stored.
RRT produces very cubic graphs. This is expected as nodes are attached to their nearest neighbor. The structural nature of these graphs hinders the probability of finding an optimal path. Instead of taking the hypotenuse between two points, the two legs of a triangle are navigated across. This is evidently a longer distance. The cubic nature and irregular paths generated by RRT are addressed by RRT*.
The benefit of the algorithm is its speed and implementation. Compared to other path planning algorithms, RRT is fairly quick. The costliest part of the algorithm is finding its closest neighbor as this process grows depending on the number of vertices that have been generated.
RRT*
RRT* is an optimized version of RRT. When the number of nodes approaches infinity, the RRT* algorithm will deliver the shortest possible path to the goal. While realistically unfeasible, this statement suggests that the algorithm does work to develop a shortest path. The basic principle of RRT* is the same as RRT, but two key additions to the algorithm result in significantly different results.
First, RRT* records the distance each vertex has traveled relative to its parent vertex. This is referred to as the cost()of the vertex. After the closest node is found in the graph, a neighborhood of vertices in a fixed radius from the new node are examined. If a node with a cheaper cost() than the proximal node is found, the cheaper node replaces the proximal node. The effect of this feature can be seen with the addition of fan shaped twigs in the tree structure. The cubic structure of RRT is eliminated.
The second difference RRT* adds is the rewiring of the tree. After a vertex has been connected to the cheapest neighbor, the neighbors are again examined. Neighbors are checked if being rewired to the newly added vertex will make their cost decrease. If the cost does indeed decrease, the neighbor is rewired to the newly added vertex. This feature makes the path more smooth.
RRT* creates incredibly straight paths. Additionally, its graphs are characteristically different from those of RRT. For finding an optimal path, especially in a dense field of obstacles, the structure of RRT* is incredibly useful. The graph vines around objects, finding shorter paths in comparison to RRT. If the destination were to change, the original graph can still be used as it represents the quickest path to most locations in the region.
While not apparent from the images, RRT* suffers from a reduction in performance. Due to examining neighboring nodes and rewiring the graph, my implementation of RRT* took nearly eight times longer to complete a single path on average than the default version.
The majority of computing effort came from obstacle avoidance. Obstacle avoidance must be checked when a node is placed, when a node is connected to its neighbor, and for each node that is to be rewired. This is a considerable number of checks to make. Yet, one cannot deny the success of the generated paths.
A final note to make is on the simplifications made in this article. I aimed to filter out much of the mathematics and details in this article, while retaining the key points. The attempt at brevity should help those who are purely interested in path planning algorithms. I greatly encourage for the curious to read more about these two algorithms and other path planning algorithms from the original source.
RRT* Pseudo CodeRad = r
G(V,E) //Graph containing edges and vertices
For itr in range(0…n)
Xnew = RandomPosition()
If Obstacle(Xnew) == True, try again
Xnearest = Nearest(G(V,E),Xnew)
Cost(Xnew) = Distance(Xnew,Xnearest)
Xbest,Xneighbors = findNeighbors(G(V,E),Xnew,Rad)
Link = Chain(Xnew,Xbest)
For x’ in Xneighbors
If Cost(Xnew) + Distance(Xnew,x’) < Cost(x’)
Cost(x’) = Cost(Xnew)+Distance(Xnew,x’)
Parent(x’) = Xnew
G += {Xnew,x’}
G += Link
Return G
I discuss an alternative method of path planning, PRM and PRM* here!
