Multi-goal motion planning using traveling salesman problem in belief space

In this paper, the multi-goal motion planning problem of an environment with some background information about its map is addressed in detail. The motion planning goal is to find a policy in belief space for the robot to traverse through a number of goal points. This problem is modeled as an asymmetric traveling salesman problem (TSP) in the belief space using Partially Observable Markov Decision Process (POMDP) framework. Then, feedback-based information roadmap (FIRM) algorithm is utilized to reduce the computational burden and complexity. By generating a TSP-FIRM graph, the search policy is obtained and an algorithm is proposed for online execution of the policy. Moreover, approaches to cope with challenges such as map updating, large deviations and high uncertainty in localization, which are more troublesome in a real implementation, are carefully addressed. Finally, in order to evaluate applicability and performance of the proposed algorithms, it is implemented in a simulation environment as well as on a physical robot in which some challenges such as kidnapping and discrepancies between real and computational models and map are examined.