Path planning in graph SLAM using Expected uncertainty

In this work we address the problem of trajectory planning in Graph SLAM. We propose the use of Expected Value of the Final Uncertainty, which summarizes all the possible uncertainties that should be considered. In fully explored environments, this is used to determine the most reliable path to the final position. In partially explored environments, this criteria quantifies the reliability of the path planned in the free space. Tests demonstrate its ability to avoid unreliable paths in fully explored environments as compared to other uncertainty based criteria. In the exploration scenario, potential paths not present in the original graph are proposed using Voronoi Diagram of the space ahead observed by the sensors. A cost function is proposed considering the length of the path as well as the expected final uncertainty, thus including potentially shorter, but still reliable paths. Tests demonstrate the shortest path is preferred as long as it contains loop closures with low uncertainty.