Mutual localization plays a crucial role in multi-robot systems. In this work, we propose a novel system to estimate the 3D relative pose targeting real-world applications. We design and implement a compact hardware module using active infrared (IR) LEDs, an IR fish-eye camera, an ultra-wideband (UWB) module and an inertial measurement unit (IMU). By leveraging IR light communication, the system solves data association between visual detection and UWB ranging. Ranging measurements from the UWB and directional information from the camera offer relative 3D position estimation. Combining the mutual relative position with neighbors and the gravity constraints provided by IMUs, we can estimate the 3D relative pose from every single frame of sensor fusion. In addition, we design an estimator based on the error-state Kalman filter (ESKF) to enhance system accuracy and robustness. When multiple neighbors are available, a Pose Graph Optimization (PGO) algorithm is applied to further improve system accuracy. We conduct experiments in various environments, and the results show that our system outperforms state-of-the-art accuracy and robustness, especially in challenging environments.

翻译：相互定位在多机器人系统中扮演着关键角色。本文提出了一种面向实际应用的三维相对位姿估计新系统。我们设计并实现了一个紧凑型硬件模块，该模块集成了主动式红外LED、红外鱼眼相机、超宽带模块和惯性测量单元。通过利用红外光通信，系统解决了视觉检测与超宽带测距之间的数据关联问题。超宽带测距数据与相机提供的方向信息相结合，可实现相对三维位置估计。将相邻机器人的相互相对位置与IMU提供的重力约束相结合，我们能够通过每一帧传感器融合数据估计三维相对位姿。此外，我们设计了一种基于误差状态卡尔曼滤波（ESKF）的估计器以增强系统的精度和鲁棒性。当存在多个相邻机器人时，采用位姿图优化（PGO）算法进一步提升系统精度。我们在多种环境下进行了实验，结果表明，我们的系统在精度和鲁棒性方面均优于现有技术，尤其在具有挑战性的环境中表现更为突出。