Rapidly-exploring Random Belief Trees for motion planning under uncertainty

TLDR

Planning domains with nontrivial dynamics, spatially varying measurement properties, and obstacle constraints motivate the study. The paper tackles motion planning under state uncertainty, i.e., planning in belief space. By constraining motion to a nominal trajectory stabilized with a linear estimator and controller, the authors predict future state distributions, use them to enforce bounded collision probability, and incrementally build a graph of trajectories while efficiently searching candidate paths; theoretical analysis and simulations validate the approach. The resulting belief‑space search tree provably converges to the optimal path and demonstrates that balancing information gathering reduces uncertainty while yielding low‑cost trajectories.

Abstract

In this paper we address the problem of motion planning in the presence of state uncertainty, also known as planning in belief space. The work is motivated by planning domains involving nontrivial dynamics, spatially varying measurement properties, and obstacle constraints. To make the problem tractable, we restrict the motion plan to a nominal trajectory stabilized with a linear estimator and controller. This allows us to predict distributions over future states given a candidate nominal trajectory. Using these distributions to ensure a bounded probability of collision, the algorithm incrementally constructs a graph of trajectories through state space, while efficiently searching over candidate paths through the graph at each iteration. This process results in a search tree in belief space that provably converges to the optimal path. We analyze the algorithm theoretically and also provide simulation results demonstrating its utility for balancing information gathering to reduce uncertainty and finding low cost paths.