On the global optimum of planar, range-based robot-to-robot relative pose estimation

In this paper, we address the problem of determining the relative position and orientation (pose) of two robots navigating in 2D, based on known egomotion and noisy robot-to-robot distance measurements. We formulate this as a weighted Least Squares (WLS) estimation problem, and determine the exact global optimum by directly solving the multivariate polynomial system resulting from the first-order optimality conditions. Given the poor scalability of the original WLS problem, we propose an alternative formulation of the WLS problem in terms of squared distance measurements (squared distances WLS or SD-WLS). Using a hybrid algebraic-numeric technique, we are able to solve the corresponding first-order optimality conditions of the SD-WLS in 125 ms in Matlab. Both methods solve the minimal (3 distance measurements) as well as the overdetermined problem (more than 3 measurements) in a unified fashion. Simulation and experimental results show that the SD-WLS achieves performance virtually indistinguishable from the maximum likelihood estimator, and significantly outperforms current algebraic methods.