Path Planning with Trajectory Libraries

Posted by on December 12, 2015 · 2 mins read

Traditional path planning techniques such as RRT or A* do not do a good job expressing physical constraints like maximum acceleration or turning radius. Even if they are smoothed to find the shortest path, it might not be the quickest path.

rrt Fig 1. An example path found by RRT

Trajectory Optimization will find the fastest path, taking into account physical constraints, but takes a long time to compute, making it difficult to run in real time.

opt Fig 2. The process of Trajectory Optimization

Trajectory Libraries combine the best of both worlds - optimal results in super fast runtimes. The idea is to discretize all possible initial states, and for each possibility build a “library” of possible paths. At runtime, select the fastest path without collisions from the closest library.

opt Fig 3. Picking the fastest valid path from a Library

Building a library for every initial condition sounds crazy, but there’s lots of symmetries that can be exploited to compress the space to just a few dimensions. The 7 dimensions of the problem space start_x, start_y, start_theta, start_velocity_x, start_velocity_y, goal_x, goal_y can be compressed to just 3 with start_velocity_x, start_velocity_y, goal_dist.

opt Fig 4. Exploiting symmetry to reduce the storage space

Against random obstacle scenarios, Trajectory Libraries found a solution 98% of the time, runs in just 1.8ms, and only 4% slower than the path found by running a full Trajectory Optimization.

opt Fig 5. Results

github repo