Multi-Modal Model Predictive Control Through Batch Non-Holonomic Trajectory Optimization: Application to Highway Driving

Standard Model Predictive Control (MPC) or trajectory optimization approaches perform only a local search to solve complex non-convex problem. As result, they cannot capture the multi-modal characteristic of human driving. A global optimizer can be potential solution but is computationally intractable in real-time setting. In this letter, we present MPC capable searching over different driving modalities. Our basic idea simple: run several goal-directed parallel optimizations and score resulting trajectories based on user-defined meta cost functions. This allows us locally optimal motion plans. Although conceptually straightforward, realizing with existing optimizers highly challenging from technical computational standpoints. With motivation, novel batch non-holonomic whose underlying matrix algebra easily parallelizable across problem instances reduces computing large matrix-vector products. structure, turn, achieved by deriving linearization-free multi-convex reformulation kinematics collision avoidance constraints. We extensively validate our approach using both synthetic real data sets (NGSIM) traffic scenarios. highlight how algorithm automatically takes lane-change overtaking decisions defined function. achieves lower cost, up 6x faster than competing baselines.