IBBT: Informed Batch Belief Trees for Motion Planning Under Uncertainty

In this work, we propose the Informed Batch Belief Trees (IBBT) algorithm for motion planning under motion and sensing uncertainties. The original stochastic motion planning problem is divided into a deterministic motion planning problem and a graph search problem. First, we solve the deterministic planning problem using Rapidly-exploring Random Graph (RRG) to construct a nominal trajectory graph. Then, an informed cost-to-go heuristic for the original problem is computed based on the nominal trajectory graph. Finally, we grow a belief tree by searching the graph using the proposed heuristic. IBBT interleaves batch state sampling, nominal trajectory graph construction, heuristic computing, and searching over the graph to find belief space motion plans. IBBT is an anytime, incremental algorithm. With an increasing number of batches of samples added to the graph, the algorithm finds improved plans. IBBT is efficient by reusing results between sequential iterations. The belief tree search is an ordered search guided by an informed heuristic. We test IBBT in different planning environments. Our numerical investigation confirms that IBBT finds non-trivial motion plans and is faster compared with previous similar methods.