Prioritized Planning for Cooperative Range-Only Localization in Multi-Robot Networks

Abstract

We present a novel path-planning algorithm to reduce localization error for a network of robots cooperatively localizing via inter-robot range measurements. The quality of localization with range measurements depends on the configuration of the network, and poor configurations can cause substantial localization errors. To reduce the effect of network configuration on localization error for moving networks we consider various optimality measures of the Fisher information matrix (FIM), which have well-studied relationships with the localization error. In particular, we pose a trajectory planning problem with constraints on the FIM optimality measures. By constraining these optimality measures we can control the statistical properties of the localization error. To efficiently generate trajectories which satisfy these FIM constraints we present a prioritized planner which leverages graph-based planning and unique properties of the range-only FIM. We show results in simulated experiments that demonstrate the trajectories generated by our algorithm reduce worst-case localization error by up to 42\% in comparison to existing planning approaches and can scalably plan distance-efficient trajectories in complicated environments for large numbers of robots.