Certifiable Mutual Localization and Trajectory Planning for Bearing-Based Robot Swarm

TL;DR

This work introduces a certifiable, bearing-based approach for mutual localization and trajectory planning in robot swarms operating without external positioning. By formulating relative pose estimation as a maximum-likelihood problem and solving it with a lossless semidefinite relaxation, the method achieves global optimality in noise-limited regimes and provides a verifiable certificate of optimality. The authors further develop a theory of a certificate matrix and its eigenvalues to connect swarm motion with estimation robustness, deriving a computable bound that guarantees estimation performance under bounded noise and degeneracy conditions. Building on these insights, a two-stage planner (front-end visibility-guaranteed path searching and back-end trajectory optimization) produces certifiable swarm trajectories that maintain inter-robot visibility and maximize estimation robustness, demonstrated in extensive simulations and real-world experiments. Overall, the framework enables robust, certifiable closed-loop swarm intelligence for bearing-based multi-robot systems, with potential applications in large-scale mapping and long-range autonomous navigation.

Abstract

Bearing measurements,as the most common modality in nature, have recently gained traction in multi-robot systems to enhance mutual localization and swarm collaboration. Despite their advantages, challenges such as sensory noise, obstacle occlusion, and uncoordinated swarm motion persist in real-world scenarios, potentially leading to erroneous state estimation and undermining the system's flexibility, practicality, and robustness.In response to these challenges, in this paper we address theoretical and practical problem related to both mutual localization and swarm planning.Firstly, we propose a certifiable mutual localization algorithm.It features a concise problem formulation coupled with lossless convex relaxation, enabling independence from initial values and globally optimal relative pose recovery.Then, to explore how detection noise and swarm motion influence estimation optimality, we conduct a comprehensive analysis on the interplay between robots' mutual spatial relationship and mutual localization. We develop a differentiable metric correlated with swarm trajectories to explicitly evaluate the noise resistance of optimal estimation.By establishing a finite and pre-computable threshold for this metric and accordingly generating swarm trajectories, the estimation optimality can be strictly guaranteed under arbitrary noise. Based on these findings, an optimization-based swarm planner is proposed to generate safe and smooth trajectories, with consideration of both inter-robot visibility and estimation optimality.Through numerical simulations, we evaluate the optimality and certifiablity of our estimator, and underscore the significance of our planner in enhancing estimation performance.The results exhibit considerable potential of our methods to pave the way for advanced closed-loop intelligence in swarm systems.

Figures (23)

• Figure 1: A snapshot of a bearing-based robot swarm. Robots use active light emitter and omnidirectional sensor to obtain inter-robot bearings with pixel-level precision.
• Figure 2: Proposed framework of our bearing-based robot swarm. Team robots estimate repective states, including rotation and translation and obtain inter-robot bearings by mutual detection. These data are communicated to a host robot, and stored in the bearing buffer and local odometry buffer. The certifiable mutual localization module recover relative poses using these data and align robots' reference frames. The certifiable swarm planning module generate swarm trajectories in the common frame, which are sent back robots for tracking.
• Figure 3: Demonstration of initial poses in the world ($\mathbf{T}_x = \{\mathbf{R}_x, t_x\}$) and local odometry ($\mathbf{T}_{x_\tau} = \{\mathbf{R}_{x_\tau},t_{x_\tau}\}$) for the robot $x$, $x\in\{i,j\}$. The ominidirectional bearing observations ($b_{ij}^\tau$ and $b_{ij}^\tau$) and the distance ($d_\tau$) between robots at time $\tau$ are represented as colored arrows and doted lines.
• Figure 4: Demonstration of the dual description of the certificate matix $\mathbf{K}$. On the left, discrete poses and bearing measurements represent the estimation process, with $\mathbf{K}$ being constructed from the actual measurement data. On the right, continuous swarm trajectories and triangle sections symbolize the swarm motion and sampling process, respectively, where $\mathbf{K}$ is formulated by sampling along these trajectories.
• Figure 5: Demonstration of the impact of noise on estimation optimality. Figures $\normalsize{\textcircled{\scriptsize{1}}}\normalsize$-$\normalsize{\textcircled{\scriptsize{3}}}\normalsize$ illustrate different swarm trajectories for three robots, while figures $\normalsize{\textcircled{\scriptsize{4}}}\normalsize$-$\normalsize{\textcircled{\scriptsize{6}}}\normalsize$ illustrate scenarios involving four robots. The corresponding right figures show the variation of the certificate eigenvalue under different levels of noise.

Theorems & Definitions (10)

• Theorem 1
• Theorem 2
• Theorem 3
• Corollary 1
• Theorem 4
• Theorem 5
• Definition 1
• Definition 2
• Lemma 1
• Lemma 2