Abstract
In this work, the problem of 4 degree-of-freedom (3D position and heading) robot-to-robot relative frame transformation estimation using onboard odometry and inter-robot distance measurements is studied. Firstly, we present a theoretical analysis of the problem, namely the derivation and interpretation of the Cramer-Rao Lower Bound (CRLB), the Fisher Information Matrix (FIM) and its determinant. Secondly, we propose optimization-based methods to solve the problem, including a quadratically constrained quadratic programming (QCQP) and the corresponding semidefinite programming (SDP) relaxation. Moreover, we address practical issues that are ignored in previous works, such as accounting for spatial-temporal offsets between the ultra-wideband (UWB) and odometry sensors, rejecting UWB outliers and checking for singular configurations before commencing operation. Lastly, extensive simulations and real-life experiments with aerial robots show that the proposed QCQP and SDP methods outperform state-of-the-art methods, 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.
Introduction
Effective collaboration in multi-robot systems, such as swarm formations or search and rescue operations, requires robots to operate within a common reference frame. In environments where external positioning systems like GPS or pre-installed Ultra-Wideband (UWB) anchors are unavailable, robots must determine their relative poses using only onboard sensors.
While visual or LiDAR-based methods are common, they are limited by line-of-sight requirements and high computational costs. Fusing local odometry (e.g., Visual-Inertial Odometry) with inter-robot UWB distance measurements offers a lightweight, omnidirectional alternative. However, a single distance measurement is insufficient to resolve a relative pose; data must be fused over time as robots move. This paper addresses the 4-DoF Relative Transformation Estimation (RTE) problem—comprising 3D translation and 1D yaw—proposing robust, non-iterative estimators that overcome the limitations of existing methods under high noise or poor geometric configurations.
Methodology
The proposed approach combines theoretical rigor with practical optimization:
- Theoretical Analysis: The authors derive the Fisher Information Matrix (FIM) and Cramer-Rao Lower Bound (CRLB) for the 4-DoF RTE problem. Analysis of the FIM determinant provides a mathematical foundation for identifying unobservable trajectory configurations.
- Optimization Solvers: The estimation is formulated as a Squared Distances Weighted Least Squares (SD-WLS) problem, converted into a non-convex Quadratically Constrained Quadratic Programming (QCQP) problem. To ensure real-time feasibility, a Semidefinite Programming (SDP) relaxation is also derived.
- Practical Pipeline: The system incorporates real-world necessities such as UWB outlier rejection, compensation for spatial-temporal offsets between sensors, and real-time monitoring of the FIM's condition number to detect unobservable states.
The Geometry of Observability
A key insight of this work is the relationship between robot trajectories and the ability to resolve relative poses. The determinant of the FIM corresponds to the volume of parallelepipeds formed by the robots' relative position vectors. If this volume collapses to zero—for instance, if robots move in parallel or remain on the same 2D plane—the relative pose becomes unobservable.
The proposed QCQP and SDP methods are particularly effective in "hard" scenarios, such as when the initial distance between robots is large relative to their movement radius, where standard algebraic methods typically fail.
Experimental Results
The methods were validated through extensive Monte-Carlo simulations and real-world flight tests using quadrotors equipped with VIO and UWB sensors.
- Accuracy: In high-noise simulations, the QCQP and SDP methods significantly outperformed state-of-the-art linear and algebraic solvers. In flight tests, QCQP achieved the lowest translation error (RMSE 0.108m–0.296m).
- Efficiency: The SDP relaxation provides a significant speed advantage, running approximately 50% faster than the full QCQP solver while maintaining sufficient accuracy for most real-time applications.
- Robustness: By monitoring the FIM condition number, the system can identify when it lacks sufficient information, reporting uncertainty rather than incorrect estimates.
Conclusion and Future Work
This work provides a robust framework for 4-DoF relative transformation estimation. While the QCQP solver offers peak accuracy, its computational cost can be high in challenging scenarios. Future research will focus on extending this framework to multi-robot swarms using distributed optimization and employing the FIM determinant for active trajectory planning to maximize observability autonomously.
Summary updated