Unique 4-DOF Relative Pose Estimation with Six Distances for UWB/V-SLAM-Based Devices

Francisco Molina Martel, Juri Sidorenko, Christoph Bodensteiner, Michael Arens, Urs HugentoblerSensors 2019 · DOI · OpenReview

TL;DR

This paper introduces a novel linear algorithm for estimating the relative 4-DOF pose between two devices using only distance measurements, significantly reducing the required number of measurements compared to existing 6-DOF methods. This advancement is crucial for cooperative SLAM and augmented reality applications, enabling more efficient and accurate map merging and localization in GPS-denied environments.

What Problem Does This Paper Address?

Accurate localization is fundamental for many applications, especially in robotics and augmented reality (AR). Simultaneous Localization and Mapping (SLAM) allows devices to build maps of their surroundings while simultaneously tracking their own position within those maps. Cooperative SLAM (C-SLAM) extends this by enabling multiple devices to share information and build a common map, which is particularly useful in scenarios where individual devices might struggle (e.g., GPS-denied environments, harsh conditions). A key challenge in C-SLAM is accurately estimating the relative pose (position and orientation) between devices. Traditional methods often rely on common map features or direct line-of-sight (LOS) measurements, which can be time-consuming or impossible. This paper addresses the problem of estimating this relative pose using only distance measurements between devices, specifically focusing on reducing the number of measurements needed and improving accuracy, thereby facilitating more proactive and efficient cooperative localization.

Methods

The core contribution of this paper is a novel linear algorithm for estimating the relative pose between two devices. Instead of tackling the full 6-DOF pose (3D position and 3D orientation), the authors constrain the problem to 4-DOF by leveraging inertial measurement unit (IMU) data to estimate the gravity vector and assume a common vertical axis. This simplification allows for a unique solution with fewer distance measurements.

The method formulates the problem as a system of linear equations derived from the squared Euclidean distance between corresponding points measured by each device. For the 2D case, this requires a minimum of five distance measurements for a unique solution, and for the 3D case, it requires six. The algorithm is designed to be robust and can handle more than the minimum required measurements by using an overdetermined system. It also serves as an excellent initial estimate for iterative least-squares refinement, which helps in avoiding local minima and reducing computation time. The authors also highlight that their method is decoupled from assumptions about the first distance measurement's norm, which can improve robustness.

Datasets

Not discussed in the paper. The paper focuses on a novel algorithm and its validation through simulations and an experimental setup.

Key Insights and Ideas

  • 4-DOF Simplification is Key: By leveraging IMU data to fix the vertical axis and estimate the gravity vector, the problem is reduced from 6-DOF to 4-DOF. This simplification is crucial for reducing the number of required distance measurements and improving accuracy.
  • Linear Solution Efficiency: The proposed linear algorithm offers a significant speed advantage over non-linear methods, making it suitable for real-time applications.
  • Fewer Measurements, Better Results: The paper demonstrates that their 4-DOF method requires fewer distance measurements (six for 3D) than existing 6-DOF methods (ten for 3D) while achieving superior accuracy.
  • Decoupling from First Distance: Not fixing the translation norm to the first distance measurement improves robustness and allows for better estimation of the relative frame transformation to the center of mass of the second coordinate system.
  • Foundation for Cooperative AR/SLAM: This method provides a robust and efficient way to establish relative poses between devices, which is a critical component for enabling seamless cooperative localization in AR and SLAM systems, especially in challenging environments.

Key Results

  • Reduced Measurement Requirements: The proposed 4-DOF method requires a minimum of five distances for 2D and six for 3D to estimate the relative pose uniquely, compared to ten distances for a 6-DOF method in 3D.
  • Improved Accuracy:
    • In 2D, the method reduces translation error by approximately 10% compared to a related linear algorithm.
  • In 3D, compared to a 6-DOF linear algorithm using the same number of measurements (ten), the proposed 4-DOF method reduces rotation error by a factor of 17 and translation error by a factor of seven.
  • Even when using the minimum required distances (six for 4-DOF vs. ten for 6-DOF), the 4-DOF method shows significant improvements, reducing orientation error by a factor of 4 and translation error by approximately a factor of 10.
  • Experimental Validation: The method was validated on an experimental setup using UWB transceivers integrated into SLAM-based AR devices, demonstrating its practical applicability and effectiveness in real-world conditions. The experimental results show a significant improvement in accuracy over the 6-DOF method, with relative pose estimation improved by approximately a factor of ten when using the same number of distances.

Limitations and Future Work

  • Gravity Vector Accuracy: The performance of the 4-DOF method is dependent on the accuracy of the estimated gravity vector. While IMUs are generally good, tilt errors can degrade performance. The paper notes that for precise range measurements and an inaccurate gravity vector estimate, a full 6-DOF pose estimation might be preferable. However, for UWB's typical centimeter-level range precision, the 4-DOF approach is still advantageous.
  • Future Work: The authors suggest that the proposed method is intended for use in augmented reality applications for cooperative localization with head-mounted displays and foresees practical use cases in cooperative SLAM for proactive map merging. Further exploration into integrating this method into more complex C-SLAM systems and AR applications would be a natural next step. The paper also mentions that the method can be used with any number of measurements larger than the minimum, providing a very accurate estimate that can be further refined with iterative least squares, suggesting potential for further optimization.

Summary updated

Abstract

In this work we introduce a relative localization method that estimates the coordinate frame transformation between two devices based on distance measurements. We present a linear algorithm that calculates the relative pose in 2D or 3D with four degrees of freedom (4-DOF). This algorithm needs a minimum of five or six distance measurements, respectively, to estimate the relative pose uniquely. We use the linear algorithm in conjunction with outlier detection algorithms and as a good initial estimate for iterative least squares refinement. The proposed method outperforms other related linear methods in terms of distance measurements needed and in terms of accuracy. In comparison with a related linear algorithm in 2D, we can reduce 10% of the translation error. In contrast to the more general 6-DOF linear algorithm, our 4-DOF method reduces the minimum distances needed from ten to six and the rotation error by a factor of four at the standard deviation of our ultra-wideband (UWB) transponders. When using the same amount of measurements the orientation error and translation error are approximately reduced to a factor of ten. We validate our method with simulations and an experimental setup, where we integrate ultra-wideband (UWB) technology into simultaneous localization and mapping (SLAM)-based devices. The presented relative pose estimation method is intended for use in augmented reality applications for cooperative localization with head-mounted displays. We foresee practical use cases of this method in cooperative SLAM, where map merging is performed in the most proactive manner.
BibTeX
@article{martel2019unique,
  author = {Francisco Molina Martel and Juri Sidorenko and Christoph Bodensteiner and Michael Arens and Urs Hugentobler},
  title = {{Unique 4-DOF Relative Pose Estimation with Six Distances for UWB/V-SLAM-Based Devices}},
  journal = {Sensors},
  year = {2019},
  doi = {10.3390/s19204366},
}
Figure