RRT
Colin Green (cjgreen@cmu.edu)
16-735
HW7 - 11/29/2006
Images:
First image - vehicle in an open area. Second image - vehicle on a
trail (slopes on either side). Third image - same as second but with D*
path shown from every leaf in the tree.
A few things about these images. First, black is low cost and white is
high cost. The "goal" is at least a few hundred meters away and is not
shown, the blue line running off the end of the image is the D* path to
the goal. The blue lines showing the RRT are a 2D projection of a 5D
space.
Algorithm
The algorithm used in this assignment follows from the basic RRT algorithm as presented here. First, a few things about the system I used.
For
this simulator the world is represented (in workspace) by a discretized
cost field (each cell can have a value between 16-65535). In this
particular example the cost field was generated from overhead imagery
and elevation data of a test site. Low cost cells are black and higher
cost cells are white.
The vehicle model used consists of a 1st order model for both linear
and angular velocities (the acceleration is proportional to the
difference between current and desired velocity) as well as a variety
of thresholds and other gains. At the highest level the model takes a
sequence of control inputs (curvature and velocity) and return a
trajectory (x,y,z,roll,pitch,yaw,curvature,velocity,time) that the
vehicle will follow.
Outside of this program another module exists which provides a field of
path costs. These costs are produced by running field D* lite on the
full global cost map (within D* the configuration space costs are
computed by assuming the vehicle is a circle). The result of this is
that the path to the goal (a waypoint that could be kilometers away)
and the cost of the path to the goal (in a simpler 2 dimensional space)
can be extracted from any point.
To compute the configuration space cost of a trajectory, the vehicle
body is convolved along that trajectory and the sum of all of the cells
the vehicle touched is computed. As with the collision checking talked
about in class, this is the most expensive operation in the planner.
A trajectory generator is used which takes in a start and goal
configuration (x,y,theta,curvature,velocity) and return the sequence of
commands which when passed through the vehicle model will move the
vehicle from the start to the goal.
Now, I wrote the RRT to replace the local planner in this planning
system, operating in a 5D configuration space
(x,y,theta,curvature,velocity). The local planner's job is to find the
best dynamically feasible trajectory while only looking at the next
20ish meters of the world. Best is basically defined as the sum of the
convolution cost of the trajectory, penalties based on the integral of
curvature and velocity along the trajectory, and the D* distance to
goal from the endpoint of the trajectory. Think of D* as providing a
heuristic estimate for the remaining distance to goal from the end of
the trajectory. An additional difficulty is that the D* path cost is
always lower than any dynamically feasible path (in addition to its
simplifying assumption of a circular vehicle) so following the
description above the best trajectory is always the shortest. To avoid
this problem an additional penalty was applied based on the length of
the trajectory.
Now to the heart of the program where the RRT is used. The RRT depends
on a few important functions, each of which is described below.
generateRandomConfiguation - This function selects a random point in
the 5D configuration space. Curvature is limited to +-1 and the
velocity limits are modulated by other parts of the system. The
velocity is allowed to be negative. x and y are limited to a ~35mx35m
region around the vehicle. Theta is freely selected from 0-2PI.
findNearestNeighbor - This function searches all of the nodes in the
graph to find the one that is "closest" to the current node. In this
case closest is difficult to define. What we would like to minimize is
the length of the trajectory connecting the configurations. This is
approximated by the Euclidean distance between them plus a gain term
times the difference in heading and velocities. In addition, because
the world is a cost field (not just binary obstacles) we want to
connect to a node that does not have a high cost so far. So a portion
of the current convolution is added. This biases the tree to avoid
growing over particularly high cost areas of the world. All of these
gains must be set to reasonable values to get good results.
rrtConnect - This function takes two configurations and tries to find a
trajectory connecting them using the trajectory generator. If it
succeeds (failure can be caused by many things, one of which is that
there are no simple, dynamically feasible, solutions) the convolution
cost of the trajectory is computed, along with curvature and velocity
penalties, and the node is added to the graph.
Now because the cost field is constantly changing (the on-vehicle
perception system continues to gain information, and the vehicle is
always moving into new areas), the local planner should run in real
time (< 1second). In simulation this does not matter. In reality to
generate enough nodes to create a nice expansive tree took much more
then 1 second (more like 30 seconds+). Anyway, pretending that things
run in real time, the main RRT loop is based on a time limit which,
when reached, will cause the best trajectory to be returned. The main
loop looks something like this:
- Add current vehicle pose to graph.
- While(time < endTime)
- q_rand = generateRandomConfiguration.
- q_near = findNearestNeighbor(q_rand, graph).
- if(rrtConnect(q_near, q_rand))
- add q_near to graph
- Return best trajectory (a trajectory is made up of the path from the root to a leaf).
The benefits of using an RRT is that it produces a tree which
expands quickly to explore the space around the vehicle (the local
planners domain). Given enough time it would find a good path through
any environment in which a good path exists (although that much time is
never available). The biasing of the nearest neighbor with the
convolution costs helped to focus the search in the most reasonable
portions of the map. Another thing I wanted to do, but did not, is to
bias the generation of the random configurations based on the D* cost
to goal. This would also have the effect of focusing the search in the
most likely portions of the map.
There are some problems with using an RRT as the local planner inside
this planning system. The connect function (including convolution) is
too slow. Completely separate from this assignment, speeding up
convolution is high on my list of this to work on so this was no
surprise. In order to get reasonable behavior out of the system a
couple of parameters had to be added. This is disappointing since one
of the nice things about RRT's is that they only have 1 real parameter.
Finally the trajectories produced by the RRT are far from smooth, which
makes them difficult to follow and would not look good if the vehicle
could execute them correctly. This was also expected, and smoothing is
the obvious way to fix this. But smoothing is very difficult in this
case. What we have is a sequence of controls that we must modify to
generate a smoother trajectory which still has a low total cost. This
can be done (in much the same way the trajectories are generated in the
first place) by computing a Jacobian and running a gradient descent
search, but it is hard and very subject to local minimum.