Contingency Planning Over Probabilistic Obstacle Predictions for Autonomous Road Vehicles

This paper presents a novel optimization-based path planner that is capable of planning multiple contingency paths to directly account for uncertainties in the future trajectories of dynamic obstacles. This planner addresses the particular problem of probabilistic collision avoidance for autonomous road vehicles that are required to safely interact, in close proximity, with other vehicles with unknown intentions. The presented path planner utilizes an efficient spline-based trajectory representation and fast but accurate collision probability bounds to simultaneously optimize multiple continuous contingency paths in real time. These collision probability bounds are efficient enough for real-time evaluation, yet accurate enough to allow for practical close-proximity driving behaviors such as passing an obstacle vehicle in an adjacent lane. An obstacle trajectory clustering algorithm is also presented to enable the path planner to scale to multiple-obstacle scenarios. Simulation results show that the contingency planner allows for a more aggressive driving style than planning a single path without compromising the overall safety of the robot.