Robot-to-Robot Relative Pose Estimation based on Semidefinite Relaxation Optimization

Abstract

In this paper, the 2D robot-to-robot relative pose (position and orientation) estimation problem based on ego-motion and noisy distance measurements is considered. We address this problem using an optimization-based method, which does not require complicated numerical analysis while yields no inferior relative localization (RL) results compared to existing approaches. In particular, we start from a state-of-the-art method named square distances weighted least square (SD-WLS), and reformulate it as a non-convex quadratically constrained quadratic programming (QCQP) problem. To handle its non-convex nature, a semidefinite programming (SDP) relaxation optimization-based method is proposed, and we prove that the relaxation is tight when measurements are free from noise or just corrupted by small noise. Further, to obtain the optimal solution of the relative pose estimation problem in the sense of maximum likelihood estimation (MLE), a theoretically optimal WLS method is developed to refine the estimate from the SDP optimization. Comprehensive simulations and well-designed experiments are presented for validating the tightness of the SDP relaxation, and the effectiveness of the proposed algorithm is highlighted by comparing it to the existing approaches.