Methods and systems are provided for belief space planning for multiple robots at an initial belief state, for reducing belief space uncertainty, including receiving multiple, candidate, multi-robot paths, each including one or more pose constraints; generating a candidate factor graph for each candidate multi-robot path, and generating from each candidate factor graph a topology graph; calculating a signature metric from each topology graph, wherein the signature metric is one of a Von Neumann (VN) metric and a spanning tree (ST) metric; selecting a multi-robot path having the greatest signature metric from among the candidate, multi-robot paths; and instructing the multiple robots to proceed according to the selected multi-robot path.
TOPOLOGICAL BELIEF SPACE PLANNING