Multi-Robot Relative Pose Estimation in SE(2) with Observability Analysis: A Comparison of Extended Kalman Filtering and Robust Pose Graph Optimization

TL;DR

This work tackles multi-robot localization in SE$(2)$ with observability analysis, focusing on cooperative localization, observability under different information structures, and praktisch implementations. It compares two back-ends—Extended Kalman Filtering (EKF) and Pose Graph Optimization (PGO)—across ROS/Gazebo simulations and real hardware, including scenarios with and without global odometry data. The study provides nonlinear observability insights for range-only, bearing-only, and orientation-only measurements, showing observability gains when both range and bearing are available, even without neighboring odometry. Robust M-estimation within the PGO framework demonstrates superior resilience to outliers, while hardware experiments with Turtlebot3s validate the practicality of range-bearing inter-robot localization and formation control implications. Overall, the work advances understanding of distributed data fusion, uncertainty propagation, and RA-SLAM-like approaches for reliable multi-robot relative pose estimation.

Abstract

In this study, we address multi-robot localization issues, with a specific focus on cooperative localization and observability analysis of relative pose estimation. Cooperative localization involves enhancing each robot's information through a communication network and message passing. If odometry data from a target robot can be transmitted to the ego robot, observability of their relative pose estimation can be achieved through range-only or bearing-only measurements, provided both robots have non-zero linear velocities. In cases where odometry data from a target robot are not directly transmitted but estimated by the ego robot, both range and bearing measurements are necessary to ensure observability of relative pose estimation. For ROS/Gazebo simulations, we explore four sensing and communication structures. We compare extended Kalman filtering (EKF) and pose graph optimization (PGO) estimation using different robust loss functions (filtering and smoothing with varying batch sizes of sliding windows) in terms of estimation accuracy. In hardware experiments, two Turtlebot3 equipped with UWB modules are used for real-world inter-robot relative pose estimation, applying both EKF and PGO and comparing their performance.

Figures (14)

• Figure 1: Schematic for multi-robot localization: (a) (Centralized) multi-robot localization is to estimate the set of absolute poses $\{\xi_{i}\}_{i \in {\mathcal{N}}}$ for multiple robots in the set ${\mathcal{N}}$; (b) Distributed multi-robot localization is to estimate the state $\{\xi_{i}, \{{}^{i}\xi_{j}\}_{j \in {\mathcal{N}}_i}\}$ concatenating the absolute pose of the ego-robot and the relative poses of the neighborhood-robots for each Robot ${\tt R}_{i}$, $i \in {\mathcal{N}}$ where the sets of robots ${\mathcal{N}}$ and ${\mathcal{N}}_{i}$ (for $i \in {\mathcal{N}}$) could be time-varying; and (c) Cooperative localization is to solve (a) or (b) by communicating extra information such as odometry data and state estimate.
• Figure 2: Factor graph representation of multi-robot absolute and relative pose estimation.
• Figure 4: ROS/Gazebo simulation environments for inter-robot relative pose estimation of two Turtlebot3 robots.
• Figure 5: EKF-based relative pose estimation results with different information structures (Cases 1$\sim$4).
• Figure 7: Diagram of NLS-based PGO for state estimation with different data processing strategies: Sliding Filtering, Sliding Batch, and Full Batch. The colored solid lines refer to the horizon of measurement data considered for optimization-based estimation, while the colored square boxes correspond to the state estimates resulting from the applied methods.

Theorems & Definitions (6)

• Remark 1
• Remark 2
• Remark 3: Velocity tracking
• Remark 4: Uncertainty propagation
• Remark 5: Symmetries and Perturbation Map
• Remark 6: KF-based probabilistic inference vs. NLP-based PGO