Certifiably Optimal Mutual Localization with Anonymous Bearing Measurements

Yingjian Wang, Xiangyong Wen, Longji Yin, Chao Xu, Yanjun Cao, Fei Gao

Abstract

Mutual localization is essential for coordination and cooperation in multi-robot systems. Previous works have tackled this problem by assuming available correspondences between measurements and received odometry estimations, which are difficult to acquire, especially for unified robot teams. Furthermore, most local optimization methods ask for initial guesses and are sensitive to their quality. In this paper, we present a certifiably optimal algorithm that uses only anonymous bearing measurements to formulate a novel mixed-integer quadratically constrained quadratic problem (MIQCQP). Then, we relax the original nonconvex problem into a semidefinite programming (SDP) problem and obtain a certifiably global optimum using with off-the-shelf solvers. As a result, our method can determine bearing-pose correspondences and furthermore recover the initial relative poses between robots under a certain condition. We compare the performance with local optimization methods on extensive simulations under different noise levels to show our advantage in global optimality and robustness. Real-world experiments are conducted to show the practicality and robustness.

Introduction

For a swarm of robots to collaborate effectively, they must establish a shared coordinate system. While global positioning systems (GPS) or pre-mapped environments are standard solutions, they are often unavailable in challenging environments like underground caves, dense forests, or disaster zones. In these scenarios, "mutual localization"—where robots determine their relative positions using onboard sensors—becomes critical.

However, two major hurdles have historically limited the reliability of mutual localization: anonymity and non-convexity. In unified teams where robots are visually identical, a sensor may detect other robots without knowing their specific identities, leading to a complex combinatorial data association problem. Simultaneously, the geometric nature of bearing measurements (angles) is inherently non-linear. Traditional local optimization methods, such as Levenberg-Marquardt, are prone to getting trapped in local minima—mathematically valid but physically incorrect solutions—unless provided with a near-perfect initial guess.

Methodology

This research proposes a pipeline that transforms the messy combinatorial search of anonymous localization into a clean, convex optimization problem. The approach is built on three pillars:

1. MIQCQP Formulation

The problem of data association (identifying which robot corresponds to which measurement) is represented using binary variables, while relative poses are represented using rotation matrices. This formulation results in a Mixed-Integer Quadratically Constrained Quadratic Problem (MIQCQP). To ensure the system remains computationally efficient as the number of measurements grows, the authors employ a marginalization trick to remove distance variables. This ensures that the optimization complexity scales primarily with the number of robots ($N$) rather than the total number of measurements ($n$).

2. Semidefinite Relaxation (SDR)

To solve the non-convex MIQCQP, the hard binary constraints and rotation constraints are relaxed into a Semidefinite Program (SDP). This relaxation makes the problem convex, allowing it to be solved using standard global solvers like Mosek or SDPT3. By adding redundant rotation constraints, the researchers significantly improved the "tightness" of the relaxation, ensuring the solver finds the correct solution even in the presence of noise.

3. Certifiable Optimality

A key contribution of this work is the proof of a "corank-one" condition ($n \ge 18N + 2$). When this condition is met and the data is noise-free, the relaxation is guaranteed to be "tight." This means the solution to the simplified convex problem is mathematically identical to the solution of the original, much harder problem, providing a certificate of global optimality.

Experimental Evaluation

The algorithm was evaluated using both synthetic and real-world datasets:

  • Synthetic Data: Randomly generated 3D trajectories with varying levels of Gaussian noise applied to bearing measurements to test the limits of the SDR approach.
  • Real-World Data: The team used a high-precision Vicon motion capture system with AprilTags to provide ground truth. They also tested the system using VINS-Mono (visual-inertial odometry) to ensure the algorithm could handle the natural "drift" found in real-world robot movement.

Key Results and Discussion

The proposed method demonstrated significant advantages over traditional local solvers:

  • Global Optimality: In simulations, the SDR method achieved a 100% success rate in finding the global optimum. In contrast, local methods failed nearly 80% of the time when given a random initial guess.
  • Computational Efficiency: Despite the complexity of SDP, the marginalization of distance variables allowed the method to solve problems with 200 measurements in milliseconds, whereas local methods often took over a second to converge.
  • Robustness: The algorithm maintained a successful "rank-one" solution up to a noise standard deviation of approximately 0.4 radians. In real-world 3-robot tests using noisy odometry, the method recovered relative translations with an error of only 0.77 meters, while local optimization produced errors exceeding 7 meters.

Conclusion and Future Work

This paper provides a robust framework for solving the anonymous mutual localization problem with guarantees of optimality. By treating data association and pose estimation as a joint optimization problem, the method avoids the pitfalls of iterative local solvers.

Future research will focus on improving scalability, as the current SDP formulation grows with $N^2$, which may limit its use in massive swarms of 50 or more robots. Additionally, the team aims to integrate outlier rejection to handle "false positive" detections, such as environmental objects being mistaken for robots, and to further explore theoretical noise tolerance thresholds.

Summary updated

Abstract

Mutual localization is essential for coordination and cooperation in multi-robot systems. Previous works have tackled this problem by assuming available correspondences between measurements and received odometry estimations, which are difficult to acquire, especially for unified robot teams. Furthermore, most local optimization methods ask for initial guesses and are sensitive to their quality. In this paper, we present a certifiably optimal algorithm that uses only anonymous bearing measurements to formulate a novel mixed-integer quadratically constrained quadratic problem (MIQCQP). Then, we relax the original nonconvex problem into a semidefinite programming (SDP) problem and obtain a certifiably global optimum using with off-the-shelf solvers. As a result, our method can determine bearing-pose correspondences and furthermore recover the initial relative poses between robots under a certain condition. We compare the performance with local optimization methods on extensive simulations under different noise levels to show our advantage in global optimality and robustness. Real-world experiments are conducted to show the practicality and robustness.
BibTeX
@article{wang2022certifiably,
  author = {Yingjian Wang and Xiangyong Wen and Longji Yin and Chao Xu and Yanjun Cao and Fei Gao},
  title = {{Certifiably Optimal Mutual Localization with Anonymous Bearing Measurements}},
  journal = {IEEE Robotics Autom. Lett.},
  year = {2022},
  url = {https://openreview.net/forum?id=Y2fhe1E7yR},
}
Figure