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.
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.
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.
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
.
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.
Fig 5. Results