Aligning coordinate frames in multi-robot systems with relative sensing information

In this paper, we present both centralized and distributed algorithms for aligning coordinate frames in multi-robot systems based on inter-robot relative position measurements. Robot orientations are not measured, but are computed by our algorithms. Our algorithms are robust to measurement error and are useful in applications where a group of robots need to establish a common coordinate frame based on relative sensing information. The problem of establishing a common coordinate frame is formulated in a least squares error framework minimizing the total inconsistency of the measurements. We assume that robots that can sense each other can also communicate with each other. In this paper, our key contribution is a novel asynchronous distributed algorithm for multi-robot coordinate frame alignment that does not make any assumptions about the sensor noise model. After minimizing the least squares error (LSE) objective for coordinate frame alignment of two robots, we develop a novel algorithm that out-performs state-of-the-art centralized optimization algorithms for minimizing the LSE objective. Furthermore, we prove that for multi-robot systems (a) with redundant noiseless relative sensing information, we will achieve the globally optimal solution (this is non-trivial because the LSE objective is non-convex for our problem), (b) with noisy information but no redundant sensing (e.g. sensing graph has a tree topology), our algorithm will optimally minimize the LSE objective. We also present preliminary results of the real-world performance of our algorithm on TurtleBots equipped with Kinect sensors.