Batch Belief Trees for Motion Planning Under Uncertainty

In this work, we develop the Batch Belief Trees (BBT) algorithm for motion planning under motion and sensing uncertainties. The algorithm interleaves between batch sampling, building a graph of nominal trajectories in the state space, and searching over the graph to find belief space motion plans. By searching over the graph, BBT finds sophisticated plans that will visit (and revisit) information-rich regions to reduce uncertainty. One of the key benefits of this algorithm is the modified interplay between exploration and exploitation. Instead of an exhaustive search (exploitation) after one exploration step, the proposed algorithm uses batch samples to explore the state space and also does not require exhaustive search before the next iteration of batch sampling, which adds flexibility. The algorithm finds motion plans that converge to the optimal one as more samples are added to the graph. We test BBT in different planning environments. Our numerical investigation confirms that BBT finds non-trivial motion plans and is faster compared with previous similar methods.

[1]  Steven L. Waslander,et al.  Uncertainty-Constrained Differential Dynamic Programming in Belief Space for Vision Based Robots , 2020, IEEE Robotics and Automation Letters.

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

[3]  Emilio Frazzoli,et al.  Sampling-based algorithms for optimal motion planning , 2011, Int. J. Robotics Res..

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

[5]  Tyler H. Summers,et al.  Risk-Averse RRT* Planning with Nonlinear Steering and Tracking Controllers for Nonlinear Robotic Systems Under Uncertainty , 2021, 2021 IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS).

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

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

[8]  Brian C. Williams,et al.  Non-Gaussian Chance-Constrained Trajectory Planning for Autonomous Vehicles Under Agent Uncertainty , 2020, IEEE Robotics and Automation Letters.

[9]  Marcelo H. Ang,et al.  Incremental sampling-based algorithm for risk-aware planning under motion uncertainty , 2014, 2014 IEEE International Conference on Robotics and Automation (ICRA).

[10]  Jonathan P. How,et al.  Robust Sampling-based Motion Planning with Asymptotic Optimality Guarantees , 2013 .

[11]  J. How,et al.  Chance Constrained RRT for Probabilistic Robustness to Environmental Uncertainty , 2010 .

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

[13]  Marco Pavone,et al.  Real-time stochastic kinodynamic motion planning via multiobjective search on GPUs , 2016, 2017 IEEE International Conference on Robotics and Automation (ICRA).

[14]  Vijay Kumar,et al.  Belief Space Planning for Mobile Robots With Range Sensors Using iLQG , 2021, IEEE Robotics and Automation Letters.

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

[16]  Jonathan P. How,et al.  Probabilistically safe motion planning to avoid dynamic obstacles with uncertain motion patterns , 2013, Auton. Robots.

[17]  Tyler Summers,et al.  Distributionally Robust Sampling-Based Motion Planning Under Uncertainty , 2018, 2018 IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS).

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

[19]  Brendan Englot,et al.  Belief roadmap search: Advances in optimal and efficient planning under uncertainty , 2017, 2017 IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS).

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

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

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

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

[24]  Marco Pavone,et al.  Monte Carlo Motion Planning for Robot Trajectory Optimization Under Uncertainty , 2015, ISRR.

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

[26]  Masahiro Ono,et al.  Chance-Constrained Optimal Path Planning With Obstacles , 2011, IEEE Transactions on Robotics.

[27]  Allen Wang,et al.  Moment State Dynamical Systems for Nonlinear Chance-Constrained Motion Planning , 2020, ArXiv.

[28]  Ron Alterovitz,et al.  High-Frequency Replanning Under Uncertainty Using Parallel Sampling-Based Motion Planning , 2015, IEEE Transactions on Robotics.