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

摘要

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-known relationships with localization error. 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 properties of the range-only FIM. We demonstrate in simulations that the trajectories generated by our algorithm reduce worst-case localization error by up to 42% in comparison to existing planners and can scalably plan distance-efficient trajectories in complicated environments for large numbers of robots.