Path Planning in Belief Space with Pose SLAM

The probabilistic belief networks that result from standard feature-based simultaneous localization and map building cannot be directly used to plan trajectories. The reason is that they produce a sparse graph of landmark estimates and their probabilistic relations, which is of little value to find collision free paths for navigation. In contrast, we argue in this paper that Pose SLAM graphs can be directly used as belief roadmaps. We present a method that devises optimal navigation strategies by searching for the path in the pose graph with lowest accumulated robot pose uncertainty, independently of the map reference frame. The method shows improved navigation results when compared to shortest paths both over synthetic data and real datasets.