Collaborative 3D localization of robots from relative pose measurements using gradient descent on manifolds

We propose a distributed algorithm for estimating the full 3-D pose (position and orientation) of multiple autonomous vehicles with respect to a common reference frame when GPS is not available. This algorithm does not rely on the use of any maps, or the ability to recognize landmarks in the environment. Instead we assume that noisy measurements of the relative pose between pairs of robots are intermittently available. We utilize the additional information about each robot´s pose provided by these measurements to improve over self-localization estimates. The proposed method is based on solving an optimization problem in an underlying product manifold (SO(3)×R³)^n(k). A provably correct explicit gradient descent law is provided. Unlike many previous approaches, the proposed algorithm is applicable to the 3-D case. The method is also capable of handling a fully dynamic scenario where the neighbor relationships are time-varying. Simulations show that the errors in the localization estimates obtained using this algorithm are significantly lower then what is achieved when robots estimate their pose without cooperation. Results from experiments with a pair of ground robots with vision-based sensors reinforce these findings.