RRT & RRT*

RRT is a sampling-based algorithm for motion planning.

What does it sample? A set of all possible transformations of the robot state, often known as the configuration space. If your robot has two degrees of freedom (Suppose it’s a dot that can move in 2D), a possible configuration is a point $(x,y)$, and its configuration space is $\mathbb{R}^2$.

Why sample? Well, think about how large a possible state space can be. For example, say you have a robot that you want to get to a certain position and orientation on a factory floor. You have two options: discretise the space and solve it using A* or Dijkstra's, or try to solve it continuously.

If you discretise it, say you’ve mapped out a square 5m x 5m factory warehouse floor with 5cm resolution (you’ve divided it into tiny 5cm x 5cm squares). The state space of possible movements for this robot, ignoring orientation would be 10,000 squares. Adding orientation within 5 degrees? We now have ($\frac{25m^2}{25cm^2} \times \frac{360^\circ}{5^\circ}$), i.e. 720,000 possible states, for just a 2D robot on a $25m^2$ floor!

Things get even worse for multi-jointed robots. For a 6-jointed robot arm, where each joint has 360 degrees of rotation, at the same 5-degree resolution, the rotation space alone has $72^6 \approx 1.39 \times 10^{11}$ states.

In reality, your robot isn’t confined to those grid cells, and the other option is to try and solve it continuously. What does it even mean for a computer to solve something continuously? What if, instead of discretising the entire space a-priori, we could sample a tiny fraction of the space and build graphs over only those samples?

Naively, say we uniformly sample 1000 points from our configuration space, and connect those points using edges. This has some advantages:

• We choose how many samples to spend instead of a resolution.
• To check if we’ve hit obstacles or the goal, we just ask "is this point in collision/goal?”

However, this is still discretisation. We have to solve using A*, and there is no sense of exploration. If we miss the goal, our only solution is to throw more random points at the problem, none of which are guaranteed to guide us towards the goal.

For this setup to become useful, a sampling-based motion planning algorithm has to achieve two things:

a) Sample the space as needed and avoid creating a billion states.

b) While sampling, explore this space; ideally, explore efficiently, and with speed.

RRT (Rapidly-Exploring Random Trees)

Developed by Steven LaValle and James Kuffner in the 90s, RRT is an algorithm that fulfils the two conditions above, using the data structure of (you guessed it) trees.

Trees give us condition (a) for free. First, as every node has one parent and the root node is the only node with none, every node is connected to the start by construction. Samples are never wasted on an area the robot can’t get to, and we can explore and construct the route at the same time.

Trees themselves were nothing new. LaValle’s and Kuffner’s idea was how to grow one to explore the state space efficiently and with speed (condition (b)).

Given a configuration space $\Omega$, assume the 2D factory floor from above, so $\Omega \subset \mathbb{R}^2$. Someone has spilled boxes over the floor, which we represent as a set of obstacles $\Omega_{obs}$. The space that the robot has to freely move is denoted $\Omega_{free} = \Omega \ \backslash \ \Omega_{obs}$. Represent the location that we want the robot to get to as a point $x_{goal}$. The robot starts at a point $x_{start}$, which serves as the root of the tree.

The algorithm follows the following steps:

T ← Tree(x_start)
repeat:
  x_rand ← Sample(Ω)
  x_nearest ← Nearest(T, x_rand)
  x_new ← Steer(x_nearest, x_rand, η)
  if CollisionFree(x_nearest, x_new):
      T.add(x_new, parent = x_nearest)
  else: continue
  if Distance(x_new, x_goal) ≤ η
     and CollisionFree(x_new, x_goal):
      return Path(T, x_new) + x_goal

As you can see, it looks less like a careful search and more like a lightning bolt striking out in every direction until it hits something useful.

This seems almost absurdly simple, and in some ways it is. You may ask:

Q: “How is this guaranteed to find the goal?”

A: It isn’t. However, it’s extremely likely to find it as $N \rightarrow \infty$. This is because, as the amount of space taken up by the tree is much less than the amount of empty space, there is a greater chance that the randomly sampled point will land there¹. Additionally, as any point sampled in empty space is much more likely to be closer to a “leaf node”, i.e. a node that has no children, the tree appears to expand outwards ². This is why these trees are known as Rapidly Exploring.

Q: “So it’s just as likely to draw a squiggly mess as it is to make a perfect path immediately, or wander around randomly before hitting the goal?”

A: Yes. It’s almost guaranteed to find the goal eventually, but the path it takes there is completely random. Whether the path ends up being a perfect straight line, or decides to circle the entirety of the sample space twice before reaching the goal, there is no way to control it. Unless....

RRT* - Finding the Best Path

In 2011, Sertac Karaman and Emilio Frazzoli sought to make RRT asymptotically optimal. That is to say, as the number of iterations increases, the path drawn between the start and goal will be the shortest possible path, accounting for obstacles. Their key idea was allowing the tree to rewire itself, achieved with two small additions to RRT:

Cost - The length of the unique path from the root of the tree to the new node.

Nearest Neighbours - $X_{near}$, the set of nodes within a circle of radius $r$ around $x_{new}$. This $r$ is separate from $\eta$, the step size³, and $X_{near}$ is separate from $x_{nearest}$, the single closest node, although it usually includes it. This radius shrinks as the tree gets denser ⁴

Each iteration, complete steps 1-4 to obtain $x_{new}$. By construction, the parent of $x_{new}$ is $x_{nearest}$. Then:

5. Choosing the best parent

We ask:

“I am $x_{new}$. Which one of the nodes around me would give me the shortest path back to the start?”

For each node in $X_{near}$ with a collision-free edge to $x_{new}$, add its current cost to the cost of the edge to $x_{new}$. Connect $x_{new}$ to the cheapest one (often still $x_{nearest}$).

6. Choosing the best child

We ask:

"I am $x_{new}$. Can I act as a shortcut for any of my neighbours?" If I become their parent, will their path home become shorter?

If the cost for a node in $X_{near}$ is minimised by having $x_{new}$ as a parent, remove its old edge and make $x_{new}$ its parent.

Unlike RRT, we don't stop when we reach the goal. We run for $N$ iterations and pick the current best path at the end.

Implementations

We visualise how RRT and RRT* handle the same obstacle course:

Notice how the RRT* tree rewires itself over time. Early on the two trees look similar, but RRT* keeps swapping parents until its tree becomes the shortest route around the boxes from the root to almost every point.

We can also visualise the cost/time for RRT and RRT*, averaged over 30 trials:

Q: “But wait, if RRT* rewires itself over time, why does the cost spike at the start?”

A: Survivorship bias. The average only includes trials that have found a path so far. In the first iterations, these paths are lucky, and have reached the goal early via a direct route. As the simulation continues, the 'slower' trials stumble their way to the goal. These latecomers temporarily drag the average cost up, until the RRT* rewiring logic has enough time to prune them all back down to the optimal path.

So, RRT gets you there; RRT* gets you there in style. It does more work per iteration, but in return its path keeps improving instead of being stuck with its early random choices.

However, RRT* is "optimal" only in the limit of infinite samples, so you might be waiting a while for the perfect path for your robot arm to grab your coffee. RRT* converges faster on low-DOF robots, where the state space is small enough. Still, a few thousand samples of rewiring are worth it, because a drunk robot is bad for PR.

[ view my implementation ]