RRT

Colin Green (cjgreen@cmu.edu)
16-735
HW7 - 11/29/2006

Images:
example1 example2 example2
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:
  1. Add current vehicle pose to graph.
  2. While(time < endTime)
    1. q_rand = generateRandomConfiguration.
    2. q_near = findNearestNeighbor(q_rand, graph).
    3. if(rrtConnect(q_near, q_rand))
      1. add q_near to graph
  1. 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.