Until now, we have mostly studied collision-free paths for robots with velocity controls for each degree of freedom. In this video, I will describe a popular sampling-based motion planner that applies to robots with arbitrary dynamics of the form x-dot equals f of (x,u). The planner is based on rapidly exploring random trees, or RRTs for short.

Let's start with a partially formed search tree T. x_start is the initial state, capital X is the state space, and any state in X_goal is an acceptable final state.

The RRT planner chooses a random state x_samp, finds the nearest node x_nearest already in the tree, finds a collision-free motion from x_nearest to x_new, which may or may not be the same as x_samp, but at least is in the direction of x_samp, then updates the search tree. The process repeats until a node is created inside the goal region X_goal. Every so often the sample x_samp should be chosen inside of X_goal to try to complete the planning problem.

This video shows an RRT generated for a 2-dimensional space. Samples are chosen uniformly randomly from the space, and the motion planner from the nearest node goes directly toward the sampled node, up to a maximum step size. The randomly chosen samples "pull" the tree to explore the state space.

Compare this to a random walk, where at each step the tree is grown from a randomly chosen node by the maximum step size in a randomly chosen direction. After the same number of nodes, the random walk has not explored very much of the state space.

This is pseudocode for the RRT algorithm. In line 3, we sample from the state space. In line 4, we find the nearest node already in the tree. In line 5, we use a local planner to find a motion from this nearest node to a state closer to the sampled state. In lines 6 and 7, a new edge is added to the tree if the motion from line 5 is collision free. Finally, in lines 8 and 9, we check to see if the new node is in the goal region, and if so, the planner has succeeded. The plan is reconstructed by following the sequence of parent nodes backward from the node in the goal region.

Let's focus on lines 3, 4, and 5, as they offer a lot of flexibility to customize the algorithm, and they are critical to the efficiency of the algorithm. The sampler in line 3 could choose states uniformly randomly from the state space X.

But other options are possible, including deterministic sampling schemes. For example, Van der Corput sampling could be used on a one-dimensional state space. The Van der Corput sequence is a deterministic sequence that jumps around the interval, providing a progressively finer, approximately uniform, sampling of the interval. This is attractive, since it results in something like multi-resolution sampling that increases in resolution until a solution is found. The generalization of the Van der Corput sequence to higher-dimensional spaces is called the Halton sequence.

The algorithm designer can choose the sampling algorithm as best suited for the task. The sampler should also occasionally sample states in the goal region, to try to complete the planning process.

Returning to the algorithm, line 4 chooses the node in the tree that is closest to the new sample. Various data structures and algorithms can be employed to make this operation efficient, but we first have to have a sensible definition of the distance between two states. For example, if the configuration consists of linear and angular coordinates, how do we compare a distance of one radian to a distance of one meter?

As another example, this blue car represents a sampled configuration of a car and these white cars represent configurations of nodes already in the tree. Which of these configurations is closest to the sample? I would say that this configuration is probably the closest, since a path to the sample probably takes less time than paths from the other configurations, considering the car's motion constraints.

Returning again to the algorithm, line 5 is the local planner that finds a motion from a node in the tree to a new state that is closer to the sampled state. The algorithm designer has a lot of flexibility in how to choose this local planner, but it should run fast. Line 6 checks whether the planned motion is collision-free, so we don't need to worry about collisions in the local planner.

The simplest local planner is one that returns a straight-line path for fully actuated kinematic robots with velocities as the inputs. For robots with more general dynamics x-dot equals f of (x,u), we can discretize the set of controls, integrate each one forward a fixed amount of time, and choose the new state x_new as the one that comes closest to the sampled state. For example, a car-like robot has 2 controls: the linear velocity v and the angular velocity omega. For a car with a bounded linear speed and a bounded turning radius, the bounds on the controls look like this bowtie. We can discretize this control set as 6 velocities, including forward motion, backward motion, and turns at the tightest turning radius. The integrals of these controls are shown here. If the sampled state is here, then the closest integrated state is shown here. If this path is collision free, then x_new will be added to the RRT. This local planner is attractive for its simplicity and generality, since it can be applied to any robot. Finally, we could use a local planner specifically tailored to the robot. For a car-like robot, Reeds-Shepp curves are paths that minimize the path-length between two configurations, without considering obstacles. Reeds-Shepp curves are good candidates for local plans.

RRTs are simple to code and customizable, and variations of RRTs have been used for many different applications. They are typically designed to try to solve complex motion planning problems quickly, but without regard to the quality of the solution. If you wanted to improve the quality of the solution, you could continue to grow the RRT after an initial solution is found, and keep the best solution, as you see in this figure. The path indicated is the best path found so far to the green goal region after generating 5000 nodes in the tree. This process does not result in solutions that tend to an optimal solution, however. A modification to the basic algorithm, called RRT-star, continually rewires the search tree so that the solution tends to the optimal solution as the number of nodes in the tree goes to infinity. RRT-star cannot be applied to arbitrary robot dynamics, however.

Sampling-based methods such as RRTs, PRMs, and related algorithms are popular because of their simplicity and their performance on some complex motion planning problems. While implementations continue to get faster, traditionally they have been used as offline planners. In the next video, I will introduce a method for real-time trajectory generation based on artificial potential fields.