Multi-Robot Bearing-only Pose Estimation via Angle Rigidity

Abstract: This letter proposes a novel distributed bearing-based pose estimator for time-varying multi-robot systems. The method uses angles computed from body-frame bearings to estimate the robots' positions in $\mathbb{R}^3$ without knowledge of their orientations. The orientations in $\mathrm{SO}(3)$ are recovered from the estimated positions, the bearings, and the bearing derivatives. The proposed observer only requires the (directed) sensing topology to be \textit{angle-rigid}, a weaker condition than the commonly used ones like bearing rigidity. Local uniform exponential stability of the proposed observer is established under the assumption of persistently exciting motions for a subset of robots. Simulations are presented and discussed to evaluate the scheme's effectiveness and practicality.

• The paper introduces a novel observer for distributed multi-robot pose estimation using only body-frame bearing measurements and angle rigidity.
• It utilizes gradient descent on a cost function of angle errors to ensure exponential convergence in both position and orientation estimation.
• Simulation studies validate the approach's rapid error decay, highlighting its practical benefits for flexible and resource-efficient multi-robot networks.

Multi-Robot Bearing-Only Pose Estimation via Angle Rigidity: Technical Overview

Problem Formulation and Motivation

The paper addresses distributed pose estimation in time-varying multi-robot networks using only body-frame bearing measurements, circumventing traditional reliance on reciprocal bearing measurements, minimum bearing-per-robot constraints, and relative rotations. The approach leverages angle rigidity to provide localization in $R^3$, obviating the need for orientation knowledge to compute agent positions. Angle rigidity imposes weaker requirements on sensing topology compared to bearing rigidity, enabling more practical deployment, especially in networks where reciprocal measurement or fixed topology is infeasible.

Theoretical Foundations: Rigidity Theory

Angle rigidity theory extends classical rigidity concepts to network localization based on angle measurements, which are computed as the inner products of measured bearings, and are invariant to robot orientations. The framework $(G,\bm{q})$, with $G$ denoting the directed sensing graph and $\bm{q}$ the relative positions in anchor’s reference frame, is infinitesimally angle-rigid if the null space of the angle rigidity matrix consists only of similarity transformations. This property is guaranteed if the minimum angle rigidity eigenvalue $\lambda_8$ of the matrix $\bm{L}_G(\bm{q})=A_G^\top(\bm{q})A_G(\bm{q})$ satisfies $\lambda_8>0$.

The anchor robot measures the bearings and distances to at least two agents to resolve similarity ambiguities (translation, rotation, scaling), ensuring unique pose recovery modulo transformations.

Observer Architecture and Estimation Process

Structure

Robots are partitioned into angle-sensing ($|\mathcal{O}_i|\geq2$) and angle-free robots ($|\mathcal{O}_i|<2$), allowing flexible sensing graph designs.

The observer comprises two subsystems:

• Position Estimator: Recovers positions via gradient descent on a cost function composed of angle errors and anchor constraints (see below).
• Orientation Estimator: For angle-sensing robots, orientation is determined by matching measured bearings to estimated relative positions; for angle-free robots, orientation is recovered by persistent excitation and comparison of control inputs to estimated velocities.

Position Estimation

The estimator minimizes the functional:

$$\mathcal{L}(\hat{\bm{q}}) = \frac{1}{2} \sum_{(i,j,k)\in A}\left(\alpha_{ijk}(\hat{\bm{q}})-\alpha_{ijk}(\bm{q})\right)^2 + \frac{\kappa_s}{2}\sum_{r\in\{a,b,c\}}\|\hat{q}_r-q_r\|^2$$

which has an isolated minimum at the true pose. The cost gradient is computed for each robot, yielding distributed update laws that only require neighbor angle exchanges.

Orientation Estimation

• Angle-sensing robots: A cost function, $(G,\bm{q})$0, penalizes deviation between predicted and measured bearings. The gradient flow on $(G,\bm{q})$1 provides correction terms using only bearing measurements and local estimated positions.
• Angle-free robots: Orientation is inferred from control input–velocity alignment, contingent upon persistently exciting motions (per Assumption 6). The method aligns the estimated body velocity with the measured velocity in the global frame, using a cost functional $(G,\bm{q})$2, whose gradient produces suitable correction.

Stability Analysis

Local uniform exponential stability (LUES) is established for the full observer under reasonable assumptions:

• Bounded inter-agent distances (collision avoidance)
• Uniform infinitesimal angle rigidity (rigidity maintenance)
• Strict non-collinearity in anchor and outgoing bearings (almost always achievable)
• Persistent excitation for free robots’ linear velocities

The observer’s cascade architecture allows rigorous Lyapunov analysis; sufficiently large position correction gain ensures exponential error decay in both position and orientation estimation subsystems. Notably, orientation errors for sensing robots converge rapidly due to direct bearing-position coupling, while angle-free robots require temporal excitation.

Simulation Studies

The methodology is substantiated by simulation experiments with $(G,\bm{q})$3 robots in $(G,\bm{q})$4 (Figure 1). Sparse directed sensing graphs are employed, notably not bearing-rigid nor compatible with structures assumed in prior works. Results demonstrate rapid error decay in both pose and orientation (Figure 2), with sensing robots achieving faster convergence than angle-free robots, whose estimation depends critically on motion excitation.

Figure 1: Robot trajectories computed for a sparse angle-rigid sensing topology with directed bearings, visually representing anchor, sensing, and free robots.

Figure 2: Pose estimation errors across robots, showing rapid convergence of position and orientation errors under the proposed observer.

In the secondary scenario, static robots and variable topology illustrate observer versatility and the necessity for persistent excitation in orientation estimation for angle-free robots.

Practical Implications and Future Directions

This approach significantly expands applicability for distributed pose estimation in robotics:

• Enables operation with minimal, sparse, and non-reciprocal sensing connectivity
• Reduces sensor and computation burden by not requiring relative rotations or minimum bearing constraints
• Allows visual sensors to be repurposed for auxiliary tasks (mapping, tracking) due to less stringent bearing requirements
• Robustness to noise is improved, as position estimation is only upstream of orientation correction

Theoretically, the observer enriches rigidity-based localization, connecting angle rigidity with practical estimation protocols under time-varying networks.

Future investigations include quantifying the region of attraction for orientation errors, analyzing robustness under noisy bearings, and integrating active rigidity maintenance for long-horizon deployments. The architecture opens avenues for flexible, resilient, and scalable localization in heterogeneous robot collectives.

Conclusion

The paper establishes a distributed, angle-rigidity-based observer for robot pose estimation using only body-frame bearing measurements. The methodology relaxes classical rigidity and sensing requirements, supporting sparse and directed topologies. Exponential convergence is theoretically guaranteed and empirically validated, with practical implications for sensor utilization and network design. The observer architecture provides a foundation for further research in robust, scalable, and flexible multi-robot localization under realistic operational constraints.