Estimating probability of collision for safe motion planning under Gaussian motion and sensing uncertainty

We present a fast, analytical method for estimating the probability of collision of a motion plan for a mobile robot operating under the assumptions of Gaussian motion and sensing uncertainty. Estimating the probability of collision is an integral step in many algorithms for motion planning under uncertainty and is crucial for characterizing the safety of motion plans. Our method is computationally fast, enabling its use in online motion planning, and provides conservative estimates to promote safety. To improve accuracy, we use a novel method to truncate estimated a priori state distributions to account for the fact that the probability of collision at each stage along a plan is conditioned on the previous stages being collision free. Our method can be directly applied within a variety of existing motion planners to improve their performance and the quality of computed plans. We apply our method to a car-like mobile robot with second order dynamics and to a steerable medical needle in 3D and demonstrate that our method for estimating the probability of collision is orders of magnitude faster than naïve Monte Carlo sampling methods and reduces estimation error by more than 25% compared to prior methods.