In my first post on robotics path planning, we have discussed the graph search algorithms. As we mentioned in that post that the common approaches we have for path planning are various:
- Bug algorithms: Bug[0-2], Tangent Bug
- Graph Search (fixed graph)
- Depth first search, Breadth first search, Dijkstra, A*, Greedy best-first search
- Sampling-based Search (build graph)
- Probabilistic Road Maps, Rapidly-exploring Random Trees
- Optimization (local search)
- Gradient descent, potential fields, Wavefront
So in this post, I am going to discuss Sampling based Search - Rapidly-exploring Random Trees (RRT)
RRT
- What is RRT algorithm?
- RRT stands for Rapidly-exploring Random Tree. It is an algorithm designed to efficiently search nonconvex, high-dimensional spaces by randomly building a space-filling tree.
- An RRT grows a tree rooted at the starting configuration by using random samples from the search space. As each sample is drawn, a connection is attempted between it and the nearest state in the tree.
- Original paper: Rapidly-Exploring Random Trees: A New Tool for Path Planning
- Paper that presents the approach to trajectory planning for robots: Randomized Kinodynamic Planning
Pseudocode
As shown above, this is the basic RRT construction algorithm, and it runs for K times to keep extending the tree and return three different flags when adding new configurations. This pseudocode is from the paper for robotics planning instead of the original RRT paper.
- Firstly we initialize the tree with a initial configuration. We can either run the for loop for K times or have other conditions to stop the extending of the tree.
- The
RANDOM_CONFIG()will randomly generate a configration in the C-space and assign to $q_{rand}$. Then we extend the tree from the new generated point. NEAREST_NEIGHBOR(q, T)will find a configuration $q_{near}$ in the tree that is nearest to the $q_{rand}$.- Then
NEW_CONFIGwill select a new configuration $q_{new}$ by moving an incremental distance $\epsilon$ from $q_{near}$ in the direction of $q_{rand}$. - If $q_{new}$ is valid, we add the new configuration to the tree and connected to $q_{near}$. As the image shows below.
![]()
Demo
| Empty | Obstacle Map |
|---|---|
 |  |
| Iterations: 2363 | Iterations: 10000+ |
From the demo, we can tell that there is a problem with this algorithm. It is not goal-oriented. The search is so random that it takes too long to find the goal. For the obstacle map, due to the randomness, the algorithm could not find a solution within 10000 iterations, so we stopped. Though the algorithm is probabilistically complete but not optimal.
RRT-Connect
The paper RRT-Connect: An Efficient Approach to Single-Query Path Planning introduced the RRT-Connect algorithm. This algorithm combines Rapidly-exploring Random Trees (RRTs) with a simple greedy heuristic that aggressively tries to connect two trees, one from the initial configuration and the other from the goal. RRT-Connect finds solutions faster than RRT.
Pseudocode
- As shown in the image above, instead of attempting to extend an RRT by a single step, the
CONNECTheuristic iterates theEXTENDstep until q or an obstacle is reached. - Under
RRT_CONNECT_PLANNER, the two trees $T_a$ and $T_b$ are maintained at all times until they become connnected and a solution is found. - One tree keeps extending until
- Reach the goal and return the solution, or
- Hit obstacles and swap the two trees. For example, if it was $T_a$ extending, then it is $T_b$’s turn to grow.
Demo
| Empty | Obstacle Map |
|---|---|
 |  |
| Iterations: 1 | Iterations: 1279 |
As we can see from the demo, with no obstacle, the algorithm is able to find a path directly to the goal. And with obstacles, the search attempts are still more concentrated toward the goal than the RRT. However, even with a faster searching ability, RRT-connect still does not give optimal solutions.
RRT*
RRT* is an optimized version of RRT, which is provably asymptotically optimal,
- Paper on RRT* in robotic motion planning: Anytime Motion Planning using the RRT*
- Original paper: Sampling-based Algorithms for Optimal Motion Planning
- Here is great YouTube video explaining the RRT* algorithm.
Pseudocode
- RRT_star works similarly to RRT, but after finding the new configuration $q_{new}$, we will check in the neighborhood, namely the K-Nearest-Neighbors, which one has the lowest cost to the $q_{new}$, and choose that neighbor to be its parent.
- Then, in reverse, take the new configuration $q_{new}$ as the parent, and compare the costs to the configurations in the neighborhood. If take $q_{new}$ as the new parent has a lower cost, then assign $q_{new}$ to be the new parent to them.
Demo
| Empty | Obstacle Map |
|---|---|
 |  |
| Iterations: 748 | Iterations: 7858 |
Comparing the search map between RRT* and RRT, we can clearly see that RRT* has cleaner branches because it keeps updating the parent-child relations.