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.