GPU Accelerated Batch Trajectory Optimization for Autonomous Navigation

Trajectory optimizations encountered in mobile robot navigation are non-convex, and thus the solution process is prone to get stuck at poor local optima, resulting in collisions with the environment. A conceptually simple workaround is to simply run the optimizer from several initializations in parallel and choose the best solution. But realizing this simple trick with off-the-shelf optimizers is challenging since they are not customized for parallel/batch operation. We fill this gap by proposing a novel batchable and GPU accelerated trajectory optimizer for autonomous navigation. Our batch optimizer can run several hundred instances of the problem in parallel in real time. We improve the state-of-the-art in the following respects. First, we show that parallel initialization naturally discovers a distribution of locally optimal trajectories residing in different homotopies. Second, we improve the navigation quality (success rate, tracking) compared to the baseline approach that relies on computing a single locally optimal trajectory at each control loop. Finally, we show that when initialized with trajectory samples from a Gaussian distribution, our batch optimizer outperforms the state-of-the-art cross-entropy method in solution quality. Codes: https://tinyurl.com/a3b99m8, Video: https://www.youtube.com/watch?v=ZlWJk-w03d8