From Footprints to Beliefprints: Motion Planning under Uncertainty for Maneuvering Automated Vehicles in Dense Scenarios

Motion planning for car-like robots is one of the major challenges in automated driving. It requires to solve a two-point boundary value problem that connects a start and a goal configuration with a collision-free trajectory. This paper introduces a novel approach for motion planning under uncertainty resulting in motion plans with higher robustness due to a bounded risk of collision. To that end, the state of the art G
¹
and G
²
continuous steering functions, namely Dubins, Reeds-Shepp, Hybrid Curvature, and Continuous Curvature Steer, are extended to belief space, where an expected vehicle state is augmented by its associated uncertainty. The so-called beliefs are obtained by propagating a Gaussian probability distribution along the nominal path of the steering function while taking into account the control and localization uncertainty. A novel concept for collision avoidance in Gaussian belief space is then introduced that considers the shape of the vehicle and the full information about the uncertainty of a state. Together with the sampling-based motion planner RRT*, the proposed realtime capable methodology reduces the collision probability by an order of magnitude in three challenging automated driving scenarios.