Position Tracking and Path Planning in Uncertain Maps for Robot Formations*

This paper presents a complete working system for robot formations, where path planning and localization tasks are integrated in such a way that environment uncertainty is considered in each of the tasks. Feature-based and grid-based mapping strategies are combined in a probabilistic way to compute an obstacle-free and of minimum-risk plan towards the goal. The formation benefits from the cooperative perception to obtain a joint vision of the environment, represented in a leadercentric way to minimize the effects of the uncertainty. The system has been tested and validated by means of a set of simulations as well as in real experiments.