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

This paper presents LQG-MP (linear-quadratic Gaussian motion planning), a new approach to robot motion planning that takes into account the sensors and the control ler that will be used during execution of the robot’s path. LQGMP 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 needed ensemble of candidate path s from which the best path is selected; in this paper we report results using the RRT-algorithm. We study the performance of LQG-MP with simulation experiments in three scenarios involving a kinodynamic car-like robot, multi-robot plann ing with differential-drive robots, and a 6-DOF manipulator. I. I NTRODUCTION Motion uncertainty, i.e. the fact that the motion of the robot unpredictably deviates from what a dynamics model predicts, and imperfect state information due to partial or noisy measurements of the robot’s state, arise in many realworld robotic tasks ranging from guiding mobile robots over uneven terrain to performing robotic surgery with high-DOF manipulators. The amount of motion and sensing uncertainty may depend on the particular motion that is executed and the state the robot is in, so different paths for the robot will ha ve different uncertainties associated with them. Because saf ty and accuracy are of critical importance for many robotic tasks, these uncertainties will have significant influence o n which path is best for the task at hand. The challenge we discuss in this paper is to precisely quantify these uncerta inties in advance, such that the best path can be selected for the robot. Many traditional path planners assume deterministic motion and full knowledge of the state [18], [13], and leave issues of uncertainty to thecontrol phase in which the path may be executed using a feedback controller [15]. Planning and control are related but distinct fields. While recent work on path planning has addressed motion and/or sensing uncertainty (see Section II), most planning method s do not account for control during execution and most control methods take the path as given. LQG-MP builds a bridge between these disciplines and draws from results in both. The key insight of LQG-MP is that the a-priori knowledge of the sensors and controller that will be used during the execution of the path can be used to optimize the path in the This work was supported in part by NSF Award 0905344 and NIH Aw ard 1R01EB-006435-01A1. The authors are with the University of California at Berkele y, Berkeley, CA, USA. E-mail:{berg, pabbeel, goldberg }@berkeley.edu. ct

[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]  Jean-Claude Latombe,et al.  Motion Planning with Uncertainty: A Landmark Approach , 1995, Artif. Intell..

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

[5]  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.

[6]  B. Faverjon,et al.  Probabilistic Roadmaps for Path Planning in High-Dimensional Con(cid:12)guration Spaces , 1996 .

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

[8]  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).

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

[10]  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).

[11]  Steven M. LaValle,et al.  Randomized Kinodynamic Planning , 1999, Proceedings 1999 IEEE International Conference on Robotics and Automation (Cat. No.99CH36288C).

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

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

[14]  J. L. Roux.  An Introduction to the Kalman Filter , 2003 .

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

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

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

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

[19]  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.

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

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

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

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

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

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

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

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

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

[29]  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.

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

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

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

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

[34]  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.

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

[36]  Miljenko Huzak,et al.  Chi-Square Distribution , 2011, International Encyclopedia of Statistical Science.