Rapidly Exploring Random Trees

So you have a map, then what?

Although maps are pretty in themselves and are proof of a certain level of awareness, there comes a point when you might actually want your robot to do go somewhere, and eventually do something!

There are many ways to plan a path, and some of the best results often come from the simplest approaches like 'try a straight line', or A* search, but sometimes this isn't enough to solve the problem at hand. Typical problems like a car stuck in a dead end, or a 'beetle trap' are problematic for greedy approaches, and sometimes your search space is too large to explore exhaustively.

Here, I'll look at one approach called RRT which searches the space very quickly and can incorporate the motion constaints of the robot.

The simplest form is a Sharp tree.

At each step:

• A random point is chosen in free space.
  
• Find the closest existing point.
  
• If the two can be connected, then connect.
  
• Otherwise, move the new point to furthest safe place and connect.
  
• Test for solution.

The advantage of this type of search is that it quickly explores free space without searching everywhere and progressively increases its coverage. Aditional contraints can be added to keep the minimum distance between points above a given resolution, and make the definition of free space include knowledge about the size of the robot.

The downside is that the result is likely to be far from optimal. To get round this, it can be iterated a few times to see if a shorter path can be found, and then smoothed by looking for shorter lines or curves between points in the path.

However many robots can't turn sharply, or prefer smoother paths. The random approach can be adapted to this by using a slightly different approach:

• Choose a random existing point in the tree.
  
• Choose a random control. (e.g. max speed, gentle left, for 1 second)
  
• If the path is safe, add this to the tree.
  
• Otherwise add the longest safe path.
  
• Test for solution

This method can eaily be adapted to any control space, and subtle optimisations can be made such as only allowing a certain granularity of control, and keeping the tree points a certain distance apart.

The great thing about this method is that you know that the path is possible in terms of the controls of your robot. Still, the path is far from optimal, and may well need iterating, and smoothing of the path and acceleration.

By tweaking the control space to allow reverse, it can solve three point turns and reversing out of tight corridoors.

Again, using reverse frequently is not optimal, so it would be best to use to imply some cost to reversing, and choose a less expensive path if available.

One optimisation that seems to work well, is to grow two trees at the same time, one from your current location and another from the destination.

This often succeeds at finding a solution in many fewer iterations and can help avoid the search getting stuck in beetle traps when the search resolution is too low and points near the exit of the trap are overlooked.

The same technique has been used to solve much more complex problems, where brute force is far too expensive to try.

Related links:

Animations of RRT

Overview

Steven LaValle's free book