Heuristic search in belief space for motion planning under uncertainties

In order to fully exploit the capabilities of a robotic systems, it is necessary to consider the limitations and errors of actuators and sensors already during the motion planning phase. In this paper, a framework for path planning is introduced, that uses heuristic search to build up a search graph in belief space, an extension to the deterministic state space considering the uncertainty associated with this space. As sources of uncertainty actuator errors and map uncertainties are considered. We apply this framework to various scenarios for a non-holonomic vehicle and compare the resulting paths to heuristic state space planners and LQG-MP[1] with the help of simulations. As a result, paths generated with this framework could either not be found with worst-case assumptions or have a higher probability of being successfully executed compared to planners with more relaxed constraints.