1Institute for Robotics and Intelligent Machines, Georgia Institute of Technology, USA 2Laboratory for Information and Decision Systems, Massachusetts Institute of Technology, USA 3US Army Research Laboratory, USA
Abstract
We propose a novel distributed algorithm to estimate the 3D
trajectories of multiple cooperative robots from relative
pose measurements. Our approach leverages recent results
which show that the maximum likelihood trajectory is well
approximated by a sequence of two quadratic subproblems.
The main contribution of the present work is to show that
these subproblems can be solved in a distributed manner,
using the distributed Jacobi (DJ) algorithm. Our approach
has several advantages. It requires minimal information
exchange, which is beneficial in presence of communication
and privacy constraints. It has an anytime flavor: after
few iterations the trajectory estimates are already
accurate, and they asymptotically convergence to the
centralized estimate. The DJ approach scales well to large
teams, and it has a straightforward implementation. We test
the approach in extensive simulations and field tests,
confirming its practicality and showing its advantages over
related techniques.