CREPES-X: Hierarchical Bearing-Distance-Inertial Direct Cooperative Relative Pose Estimation System

TL;DR

CREPES-X introduces a robust, robocentric relative localization framework for multi-robot swarms that operates without global infrastructure and remains effective under non-ideal sensing (NLOS, outliers, non-inertial environments). It combines a compact hardware payload with a two-stage estimator: a fast single-frame closed-form solver and a multi-frame IMU-informed refinement that leverages robocentric relative kinematics and IMU preintegration. The system yields four output streams (SFC, SFO, MFLO, MFTO) to suit varying latency-accuracy needs and demonstrates strong resilience to high bearing outlier rates, achieving real-world RMSE around 0.073 m and 1.817° in challenging scenarios. Extensive benchmarks and real-world experiments confirm real-time performance on portable hardware and scalability to swarm sizes, highlighting CREPES-X as a practical solution for cooperative navigation, map merging, and distributed swarm control.

Abstract

Relative localization is critical for cooperation in autonomous multi-robot systems. Existing approaches either rely on shared environmental features or inertial assumptions or suffer from non-line-of-sight degradation and outliers in complex environments. Robust and efficient fusion of inter-robot measurements such as bearings, distances, and inertials for tens of robots remains challenging. We present CREPES-X (Cooperative RElative Pose Estimation System with multiple eXtended features), a hierarchical relative localization framework that enhances speed, accuracy, and robustness under challenging conditions, without requiring any global information. CREPES-X starts with a compact hardware design: InfraRed (IR) LEDs, an IR camera, an ultra-wideband module, and an IMU housed in a cube no larger than 6cm on each side. Then CREPES-X implements a two-stage hierarchical estimator to meet different requirements, considering speed, accuracy, and robustness. First, we propose a single-frame relative estimator that provides instant relative poses for multi-robot setups through a closed-form solution and robust bearing outlier rejection. Then a multi-frame relative estimator is designed to offer accurate and robust relative states by exploring IMU pre-integration via robocentric relative kinematics with loosely- and tightly-coupled optimization. Extensive simulations and real-world experiments validate the effectiveness of CREPES-X, showing robustness to up to 90% bearing outliers, proving resilience in challenging conditions, and achieving RMSE of 0.073m and 1.817° in real-world datasets.

Figures (23)

• Figure 1: CREPES-X works in a robocentric frame, independent of the environment, and provides accurate and robust relative state estimation in real-time. (a) The compact hardware design of CREPES-X. (b) IR LEDs and an IR camera work as light-coded communication, providing bearings with ID. (c) Multiple CREPES-X can overcome challenges in non-line-of-sight scenario. (d) CREPES-X can be used in in ① map merging, ② relative motion control zhang2023coni, and ③ cooperative navigation li2024colagchen2024cost.
• Figure 2: CREPES-X estimates relative states using distance, bearing, inertial, and optionally gravity measurements. The proposed two-stage hierarchical estimator is designed to satisfy different accuracy and latency requirements: (a) The single-frame estimator delivers instantaneous relative poses through multi-stage closed-form solutions followed by optimization. (b) The multi-frame estimator refines relative states over a time window using loosely- and tightly-coupled optimizations for improved accuracy and robustness.
• Figure 3: System architecture of CREPES-X. Time synchronization between different robots is provided by UWB. The IR-camera and IR-LEDs are alternately triggered by the synchronized clock. The camera captures images for ID extraction to obtain bearing-ID pairs, which, along with distance, inertial, and gravity data, are broadcast to neighbors. Received data are used in decentralized estimation. The Single-Frame Relative Estimator (SFRE) computes relative poses from a single time frame. It first applies the Single-Frame Closed-form solver (SFC) with outlier rejection, and then refines the solution using Single-Frame Optimization (SFO). The Multi-Frame Relative Estimator (MFRE) extends SFRE by fusing temporal information within a sliding window. It performs Multi-Frame Loosely-coupled Optimization (MFLO) to generate robust initial guesses, followed by Multi-Frame Tightly-coupled Optimization (MFTO).
• Figure 4: Hardware Comparison
• Figure 5: Synchronized LED-camera system with global clock triggering. Red arrows: LED on, black arrows: LED off, yellow arrows: camera capture. Sequential frames capture LED binary codes for ID extraction.