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.

[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]  Steven M. LaValle,et al.  Planning algorithms , 2006 .

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

[20]  Gregory S. Chirikjian,et al.  Robotic Needle Steering: Design, Modeling, Planning, and Image Guidance , 2011 .

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