Simultaneous Localization and Planning for Physical Mobile Robots via Enabling Dynamic Replanning in Belief Space

Simultaneous planning while localizing is a crucial ability for an autonomous robot operating under uncertainty. This paper addresses this problem by designing methods to dynamically replan while the localization uncertainty or environment map is updated. In particular, relying on sampling-based methods, the proposed online planning scheme can cope with challenging situations, including when the localization update or changes in the environment alter the homotopy class of trajectories, in which the optimal plan resides. The proposed algorithm eliminates the need for stabilization in the state-of-the-art FIRM (Feedback-based Information RoadMap) method, and outperforms its performance and success probability. Applying belief space planning to physical systems brings with it a plethora of challenges. Thus, designing computationally tractable algorithms, a key focus and contribution of this paper is to implement the proposed planner on a physical robot and show the SLAP (simultaneous localization and planning) performance under uncertainty, in changing environments, and in the presence of large disturbances such as kidnapped robot situation.

[1]  Brahim Chaib-draa,et al.  AEMS: An Anytime Online Search Algorithm for Approximate Policy Refinement in Large POMDPs , 2007, IJCAI.

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

[3]  Guy Shani,et al.  Noname manuscript No. (will be inserted by the editor) A Survey of Point-Based POMDP Solvers , 2022 .

[4]  Nancy M. Amato,et al.  FIRM: Sampling-based feedback motion-planning under motion uncertainty and imperfect measurements , 2014, Int. J. Robotics Res..

[5]  Ryan M. Eustice,et al.  Perception-driven navigation: Active visual SLAM for robotic area coverage , 2013, 2013 IEEE International Conference on Robotics and Automation.

[6]  Robert Platt,et al.  Convex Receding Horizon Control in Non-Gaussian Belief Space , 2012, WAFR.

[7]  Joel Veness,et al.  Monte-Carlo Planning in Large POMDPs , 2010, NIPS.

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

[9]  Nancy M. Amato,et al.  FIRM: Feedback controller-based information-state roadmap - A framework for motion planning under uncertainty , 2011, 2011 IEEE/RSJ International Conference on Intelligent Robots and Systems.

[10]  David Hsu,et al.  Intention-aware online POMDP planning for autonomous driving in a crowd , 2015, 2015 IEEE International Conference on Robotics and Automation (ICRA).

[11]  Hugh F. Durrant-Whyte,et al.  A solution to the simultaneous localization and map building (SLAM) problem , 2001, IEEE Trans. Robotics Autom..

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

[13]  David Hsu,et al.  DESPOT: Online POMDP Planning with Regularization , 2013, NIPS.

[14]  A. Laub,et al.  Generalized eigenproblem algorithms and software for algebraic Riccati equations , 1984, Proceedings of the IEEE.

[15]  Lydia E. Kavraki,et al.  Automated model approximation for robotic navigation with POMDPs , 2013, 2013 IEEE International Conference on Robotics and Automation.

[16]  E. Todorov,et al.  A generalized iterative LQG method for locally-optimal feedback control of constrained nonlinear stochastic systems , 2005, Proceedings of the 2005, American Control Conference, 2005..

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

[18]  Reid G. Simmons,et al.  Point-Based POMDP Algorithms: Improved Analysis and Implementation , 2005, UAI.

[19]  Kostas E. Bekris,et al.  The Importance of a Suitable Distance Function in Belief-Space Planning , 2015, ISRR.

[20]  Nancy M. Amato,et al.  Robust online belief space planning in changing environments: Application to physical mobile robots , 2014, 2014 IEEE International Conference on Robotics and Automation (ICRA).

[21]  David Hsu,et al.  Motion planning under uncertainty for robotic tasks with long time horizons , 2011, Int. J. Robotics Res..

[22]  Nikos A. Vlassis,et al.  Perseus: Randomized Point-based Value Iteration for POMDPs , 2005, J. Artif. Intell. Res..

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

[24]  Oliver Brock,et al.  Sensor-based, task-constrained motion generation under uncertainty , 2014, 2014 IEEE International Conference on Robotics and Automation (ICRA).

[25]  Ron Alterovitz,et al.  Efficient Approximate Value Iteration for Continuous Gaussian POMDPs , 2012, AAAI.

[26]  Ian D. Reid,et al.  On the comparison of uncertainty criteria for active SLAM , 2012, 2012 IEEE International Conference on Robotics and Automation.

[27]  Herbert G. Tanner,et al.  Stochastic receding horizon control for robots with probabilistic state constraints , 2012, 2012 IEEE International Conference on Robotics and Automation.

[28]  Nicholas M. Patrikalakis,et al.  Global motion planning under uncertain motion, sensing, and environment map , 2011, Auton. Robots.

[29]  O. Bosgra,et al.  A full solution to the constrained stochastic closed-loop MPC problem via state and innovations feedback and its receding horizon implementation , 2003, 42nd IEEE International Conference on Decision and Control (IEEE Cat. No.03CH37475).

[30]  Reid G. Simmons,et al.  Heuristic Search Value Iteration for POMDPs , 2004, UAI.

[31]  Vijay Kumar,et al.  Identification and Representation of Homotopy Classes of Trajectories for Search-based Path Planning in 3D , 2011, Robotics: Science and Systems.

[32]  Leslie Pack Kaelbling,et al.  Integrated task and motion planning in belief space , 2013, Int. J. Robotics Res..

[33]  Daniel Lyons,et al.  Uncertainty-constrained robot exploration: A mixed-integer linear programming approach , 2014, 2014 IEEE International Conference on Robotics and Automation (ICRA).

[34]  Wolfram Burgard,et al.  Probabilistic Robotics (Intelligent Robotics and Autonomous Agents) , 2005 .

[35]  Jean-Claude Latombe,et al.  On the Probabilistic Foundations of Probabilistic Roadmap Planning , 2006, Int. J. Robotics Res..

[36]  Nancy M. Amato,et al.  On the probabilistic completeness of the sampling-based feedback motion planners in belief space , 2012, 2012 IEEE International Conference on Robotics and Automation.

[37]  Dimitri P. Bertsekas,et al.  Dynamic programming and optimal control, 3rd Edition , 2005 .

[38]  Daniel E. Koditschek,et al.  Sequential Composition of Dynamically Dexterous Robot Behaviors , 1999, Int. J. Robotics Res..

[39]  Nicholas Roy,et al.  Efficient Planning under Uncertainty with Macro-actions , 2014, J. Artif. Intell. Res..

[40]  Nancy M. Amato,et al.  MAPRM: a probabilistic roadmap planner with sampling on the medial axis of the free space , 1999, Proceedings 1999 IEEE International Conference on Robotics and Automation (Cat. No.99CH36288C).

[41]  Peilin Fu,et al.  Variance minimization approach for a class of dual control problems , 2002, IEEE Trans. Autom. Control..

[42]  Ron Alterovitz,et al.  Closed-loop global motion planning for reactive execution of learned tasks , 2014, 2014 IEEE/RSJ International Conference on Intelligent Robots and Systems.

[43]  Lydia E. Kavraki,et al.  Path planning using lazy PRM , 2000, Proceedings 2000 ICRA. Millennium Conference. IEEE International Conference on Robotics and Automation. Symposia Proceedings (Cat. No.00CH37065).

[44]  Steven M. LaValle,et al.  On the Relationship between Classical Grid Search and Probabilistic Roadmaps , 2004, Int. J. Robotics Res..

[45]  Richard T. Vaughan,et al.  The Player/Stage Project: Tools for Multi-Robot and Distributed Sensor Systems , 2003 .

[46]  Joel W. Burdick,et al.  Robotic motion planning in dynamic, cluttered, uncertain environments , 2010, 2010 IEEE International Conference on Robotics and Automation.

[47]  Claude Samson,et al.  Feedback control of a nonholonomic wheeled cart in Cartesian space , 1991, Proceedings. 1991 IEEE International Conference on Robotics and Automation.

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

[49]  Marilena Vendittelli,et al.  WMR control via dynamic feedback linearization: design, implementation, and experimental validation , 2002, IEEE Trans. Control. Syst. Technol..

[50]  Nancy M. Amato,et al.  Sampling-based nonholonomic motion planning in belief space via Dynamic Feedback Linearization-based FIRM , 2012, 2012 IEEE/RSJ International Conference on Intelligent Robots and Systems.

[51]  Hanna Kurniawati,et al.  An Online POMDP Solver for Uncertainty Planning in Dynamic Environment , 2013, ISRR.

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

[53]  Frank Dellaert,et al.  Planning in the continuous domain: A generalized belief space approach for autonomous navigation in unknown environments , 2015, Int. J. Robotics Res..

[54]  David Hsu,et al.  Monte Carlo Value Iteration for Continuous-State POMDPs , 2010, WAFR.

[55]  David Hsu,et al.  Planning under Uncertainty for Robotic Tasks with Mixed Observability , 2010, Int. J. Robotics Res..

[56]  S. Sastry,et al.  Nonholonomic motion planning: steering using sinusoids , 1993, IEEE Trans. Autom. Control..

[57]  Joelle Pineau,et al.  Online Planning Algorithms for POMDPs , 2008, J. Artif. Intell. Res..

[58]  Pieter Abbeel,et al.  Scaling up Gaussian Belief Space Planning Through Covariance-Free Trajectory Optimization and Automatic Differentiation , 2014, WAFR.

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

[60]  Marilena Vendittelli,et al.  Control of Wheeled Mobile Robots: An Experimental Overview , 2001 .

[61]  Howie Choset,et al.  Principles of Robot Motion: Theory, Algorithms, and Implementation ERRATA!!!! 1 , 2007 .

[62]  Pravin Varaiya,et al.  Stochastic Systems: Estimation, Identification, and Adaptive Control , 1986 .

[63]  Edward J. Sondik,et al.  The Optimal Control of Partially Observable Markov Processes over a Finite Horizon , 1973, Oper. Res..

[64]  Nicholas Roy,et al.  Robust Navigation Execution by Planning in Belief Space , 2013 .

[65]  Yasir Latif,et al.  On the monotonicity of optimality criteria during exploration in active SLAM , 2015, 2015 IEEE International Conference on Robotics and Automation (ICRA).

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

[67]  Suman Chakravorty,et al.  Information Space Receding Horizon Control , 2013, IEEE Transactions on Cybernetics.

[68]  Joelle Pineau,et al.  Point-based value iteration: An anytime algorithm for POMDPs , 2003, IJCAI.

[69]  Ron Alterovitz,et al.  Motion planning under uncertainty using iterative local optimization in belief space , 2012, Int. J. Robotics Res..

[70]  Rodney A. Brooks,et al.  A Robust Layered Control Syste For A Mobile Robot , 2022 .

[71]  Emilio Frazzoli,et al.  Sampling-based algorithms for continuous-time POMDPs , 2013, 2013 American Control Conference.

[72]  Manfred Morari,et al.  Model predictive control: Theory and practice - A survey , 1989, Autom..

[73]  Basilio Bona,et al.  Active SLAM and Exploration with Particle Filters Using Kullback-Leibler Divergence , 2014, J. Intell. Robotic Syst..