Multi-Robot Bearing-based Pose Estimation via Angle Rigidity

Abstract

This letter proposes a novel distributed pose estimator for multi-robot systems evolving on $\mathrm{SE}(3)$. The robots' positions are estimated in $\mathbb{R}^3$ using angles computed from body-frame bearings, without requiring orientation knowledge. The robots' orientations are then recovered in $\mathrm{SO}(3)$ from the estimated positions, together with bearing and bearing-rate measurements. The estimator accommodates directed sensing topologies and requires only infinitesimal angle rigidity (IAR), thereby relaxing the requirement, common in bearing-based approaches, that every robot acquire at least two bearings. Unlike existing angle-based schemes, the proposed method also estimates the robots' orientations. We prove local uniform exponential stability of the observer, assuming that a subset of robots executes persistently exciting motions. These theoretical results are corroborated through numerical simulations.