Estimating Probability of Collision for Safe 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 computes an accurate, yet conservative, estimate of the probability of collision based on a priori distributions of the robot state along a given plan. We specifically account for the fact that the probabilities of collision at each stage along the plan are conditioned on the previous stages being collision free, by using a novel method to truncate the a priori state distributions with respect to obstacles and correctly propagating them forward in time. 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.

[1]  Robert F. Stengel,et al.  Optimal Control and Estimation , 1994 .

[2]  N. L. Johnson,et al.  Continuous Univariate Distributions. , 1995 .

[3]  D. Simon.  Optimal State Estimation: Kalman, H Infinity, and Nonlinear Approaches , 2006 .

[4]  Thierry Siméon,et al.  The Stochastic Motion Roadmap: A Sampling Framework for Planning with Markov Motion Uncertainty , 2007, Robotics: Science and Systems.

[5]  Leonidas J. Guibas,et al.  Bounded Uncertainty Roadmaps for Path Planning , 2008, WAFR.

[6]  Dominique Gruyer,et al.  A fast Monte Carlo algorithm for collision probability estimation , 2008, 2008 10th International Conference on Control, Automation, Robotics and Vision.

[7]  David Hsu,et al.  SARSOP: Efficient Point-Based POMDP Planning by Approximating Optimally Reachable Belief Spaces , 2008, Robotics: Science and Systems.

[8]  Marc Toussaint,et al.  Robot trajectory optimization using approximate inference , 2009, ICML '09.

[9]  Matthew B. Greytak,et al.  Integrated motion planning and model learning for mobile robots with application to marine vehicles , 2009 .

[10]  N. Roy,et al.  The Belief Roadmap: Efficient Planning in Belief Space by Factoring the Covariance , 2009, Int. J. Robotics Res..

[11]  Pros and Cons of truncated Gaussian EP in the context of Approximate Inference Control , 2009 .

[12]  William D. Smart,et al.  A Scalable Method for Solving High-Dimensional Continuous POMDPs Using Local Approximation , 2010, UAI.

[13]  Pieter Abbeel,et al.  LQG-Based Planning, Sensing, and Control of Steerable Needles , 2010, WAFR.

[14]  Leslie Pack Kaelbling,et al.  Belief space planning assuming maximum likelihood observations , 2010, Robotics: Science and Systems.

[15]  Joel W. Burdick,et al.  Probabilistic Collision Checking With Chance Constraints , 2011, IEEE Transactions on Robotics.

[16]  Ron Alterovitz,et al.  Motion Planning Under Uncertainty In Highly Deformable Environments , 2011, Robotics: Science and Systems.

[17]  Pieter Abbeel,et al.  LQG-MP: Optimized path planning for robots with motion uncertainty and imperfect state information , 2010, Int. J. Robotics Res..

[18]  C. Tomlin,et al.  Closed-loop belief space planning for linear, Gaussian systems , 2011, 2011 IEEE International Conference on Robotics and Automation.

[19]  Nicholas Roy,et al.  Rapidly-exploring Random Belief Trees for motion planning under uncertainty , 2011, 2011 IEEE International Conference on Robotics and Automation.