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

Abstract— 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 algebraicnumeric technique, we are able to solve the corresponding firstorder 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 ...

• Type: Journal
• Year: 2010
• Where: ICRA
• Authors: Nikolas Trawny, Stergios I. Roumeliotis
• Keywords: Distance Measurements, ICRA 2010, Optimality Conditions, Robotics, WLS Problem