Relative Transformation Estimation Based on Fusion of Odometry and UWB Ranging Data

Abstract

In this article, we study the problem of estimating the four-degree-of-freedom (3-D position and heading) robot-to-robot relative frame transformation using onboard odometry and interrobot distance measurements. First, we present a theoretical analysis of the problem, namely, the derivation and interpretation of the Cramèr–Rao lower bound, the Fisher information matrix, and its determinant. Second, we propose optimization-based solutions, including a quadratically constrained quadratic programming (QCQP) formulation and its semidefinite programming (SDP) relaxation. Third, based on the theoretical results, we can detect singular configurations as well as measure the uncertainty of each individual parameter. We perform extensive simulations and real-life experiments with aerial robots to show that the proposed QCQP and SDP methods can outperform state-of-the-art approaches, especially in geometrically poor or large measurement noise conditions. In general, the QCQP method provides the best results at the expense of computational time, while the SDP method runs much faster and is sufficiently accurate in most cases.