LQG-MP: Optimized path planning for robots with motion uncertainty and imperfect state information

In this paper we present LQG-MP (linear-quadratic Gaussian motion planning), a new approach to robot motion planning that takes into account the sensors and the controller that will be used during the execution of the robot’s path. LQG-MP is based on the linear-quadratic controller with Gaussian models of uncertainty, and explicitly characterizes in advance (i.e. before execution) the a priori probability distributions of the state of the robot along its path. These distributions can be used to assess the quality of the path, for instance by computing the probability of avoiding collisions. Many methods can be used to generate the required ensemble of candidate paths from which the best path is selected; in this paper we report results using rapidly exploring random trees (RRT). We study the performance of LQG-MP with simulation experiments in three scenarios: (A) a kinodynamic car-like robot, (B) multi-robot planning with differential-drive robots, and (C) a 6-DOF serial manipulator. We also present a method that applies Kalman smoothing to make paths Ck-continuous and apply LQG-MP to precomputed roadmaps using a variant of Dijkstra’s algorithm to efficiently find high-quality paths.

[1]  C. Striebel,et al.  On the maximum likelihood estimates for linear dynamic systems , 1965 .

[2]  John N. Tsitsiklis,et al.  The Complexity of Markov Decision Processes , 1987, Math. Oper. Res..

[3]  Greg Welch,et al.  An Introduction to Kalman Filter , 1995, SIGGRAPH 2001.

[4]  Jean-Claude Latombe,et al.  Motion Planning with Uncertainty: A Landmark Approach , 1995, Artif. Intell..

[5]  Dimitri P. Bertsekas,et al.  Dynamic Programming and Optimal Control, Two Volume Set , 1995 .

[6]  Rachid Alami,et al.  A numerical technique for planning motion strategies of a mobile robot in presence of uncertainty , 1995, Proceedings of 1995 IEEE International Conference on Robotics and Automation.

[7]  Lydia E. Kavraki,et al.  Probabilistic roadmaps for path planning in high-dimensional configuration spaces , 1996, IEEE Trans. Robotics Autom..

[8]  Leslie Pack Kaelbling,et al.  Planning and Acting in Partially Observable Stochastic Domains , 1998, Artif. Intell..

[9]  Thierry Fraichard,et al.  Path planning with uncertainty for car-like robots , 1998, Proceedings. 1998 IEEE International Conference on Robotics and Automation (Cat. No.98CH36146).

[10]  Steven M. LaValle,et al.  An Objective-Based Framework for Motion Planning under Sensing and Control Uncertainties , 1998, Int. J. Robotics Res..

[11]  Wolfram Burgard,et al.  Coastal navigation-mobile robot navigation with uncertainty in dynamic environments , 1999, Proceedings 1999 IEEE International Conference on Robotics and Automation (Cat. No.99CH36288C).

[12]  S. LaValle,et al.  Randomized Kinodynamic Planning , 2001 .

[13]  Sebastian Thrun,et al.  Probabilistic robotics , 2002, CACM.

[14]  Gino van den Bergen,et al.  Collision Detection , 2003, Real-Time Rendering.

[15]  Gino van den Bergen.  Collision Detection in Interactive 3D Environments , 2003 .

[16]  Mark H. Overmars,et al.  Prioritized motion planning for multiple robots , 2005, 2005 IEEE/RSJ International Conference on Intelligent Robots and Systems.

[17]  Nicholas Roy,et al.  Adapting probabilistic roadmaps to handle uncertain maps , 2006, Proceedings 2006 IEEE International Conference on Robotics and Automation, 2006. ICRA 2006..

[18]  Pascal Poupart,et al.  Point-Based Value Iteration for Continuous POMDPs , 2006, J. Mach. Learn. Res..

[19]  Steven M. LaValle,et al.  Planning algorithms , 2006 .

[20]  Alain Lambert,et al.  Safe Path Planning in an Uncertain-Configuration Space using RRT , 2006, 2006 IEEE/RSJ International Conference on Intelligent Robots and Systems.

[21]  Oliver Brock,et al.  Sampling-Based Motion Planning With Sensing Uncertainty , 2007, Proceedings 2007 IEEE International Conference on Robotics and Automation.

[22]  Nicholas Roy,et al.  The Belief Roadmap: Efficient Planning in Linear POMDPs by Factoring the Covariance , 2007, ISRR.

[23]  Reid G. Simmons,et al.  Particle RRT for Path Planning with Uncertainty , 2007, Proceedings 2007 IEEE International Conference on Robotics and Automation.

[24]  Mark H. Overmars,et al.  Creating High-quality Paths for Motion Planning , 2007, Int. J. Robotics Res..

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

[26]  Jonathan P. How,et al.  Motion Planning in Complex Environments using Closed-loop Prediction , 2008 .

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

[28]  Florent Lamiraux,et al.  A framework for planning motions in stochastic maps , 2008, 2008 10th International Conference on Control, Automation, Robotics and Vision.

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

[30]  Anthony Stentz,et al.  Using linear landmarks for path planning with uncertainty in outdoor environments , 2009, 2009 IEEE/RSJ International Conference on Intelligent Robots and Systems.

[31]  Russ Tedrake,et al.  LQR-trees: Feedback motion planning on sparse randomized trees , 2009, Robotics: Science and Systems.

[32]  Karl Iagnemma,et al.  Stochastic mobility-based path planning in uncertain environments , 2009, 2009 IEEE/RSJ International Conference on Intelligent Robots and Systems.

[33]  Nicholas Roy,et al.  icLQG: Combining local and global optimization for control in information space , 2009, 2009 IEEE International Conference on Robotics and Automation.

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

[35]  Kamal K. Gupta,et al.  Collision-probability constrained PRM for a manipulator with base pose uncertainty , 2009, 2009 IEEE/RSJ International Conference on Intelligent Robots and Systems.

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