TL;DR
This paper introduces CREPES, a novel system for robust 6-DOF relative pose estimation between multi-robot systems. It combines active IR LEDs, a fisheye camera, UWB, and IMU for accurate, instantaneous pose estimation, even in challenging conditions like low light and occlusion, and leverages Pose Graph Optimization for enhanced accuracy in multi-robot scenarios.
What Problem Does This Paper Address?
Accurate and stable relative pose estimation is fundamental for effective multi-robot cooperation. Existing solutions often rely on external infrastructure (like GPS or motion capture systems), require extensive calibration, or are computationally expensive. Furthermore, many methods struggle in environments with limited features, poor lighting, or when robots are occluded from each other. This paper aims to address these limitations by proposing a self-contained, robust, and accurate system for estimating the 6-DOF relative pose between robots in real-time, without relying on external infrastructure.
Methods
CREPES employs a multi-sensor fusion approach with a novel hardware design and a sophisticated software pipeline.
- Hardware: The system integrates active infrared (IR) LEDs for communication and identification, an IR fisheye camera for visual detection and directional measurements, an ultra-wideband (UWB) module for ranging, and an inertial measurement unit (IMU) for motion sensing.
- ID Extraction: This module uses the IR camera to detect and decode unique IDs from the IR LEDs of neighboring robots. It also extracts directional information from the detected IR spots.
- Raw Relative Pose Estimation: This core component fuses data from the camera (directional information), UWB (distance), and IMU (gravity constraints) to estimate an initial 3-DOF relative position and then extends it to 6-DOF relative pose. It leverages gravity information from the IMU to constrain roll and pitch angles, and then calculates the yaw angle based on projected directional vectors.
- Error-State Kalman Filter (ESKF): To improve accuracy and robustness, an ESKF is used to filter the raw pose estimations. This ESKF is adapted to handle the motion of the reference frame, making it suitable for dynamic multi-robot environments.
- Pose Graph Optimization (PGO): For scenarios with multiple neighboring robots, a PGO algorithm is applied. This treats each robot as a node in a graph and the estimated relative poses as edges, allowing for a globally optimized and more accurate estimation of relative poses across the team.
Datasets
Not discussed in the paper. The experiments were conducted using custom setups with UAVs and UGVs, and ground truth was obtained using Motion Capture Systems (MCS) and RTK GPS.
Key Insights and Ideas
- IR-based Data Association: The clever use of IR LEDs and a fisheye camera allows for robust data association between visual detections and UWB ranging, even in challenging lighting conditions.
- Gravity-Constrained 6-DOF Estimation: By incorporating IMU data, the system effectively uses gravity to constrain roll and pitch, enabling the estimation of the full 6-DOF relative pose from a single frame of sensor measurements.
- ESKF for Dynamic Reference Frames: Adapting the ESKF to account for the motion of the reference frame is crucial for maintaining accuracy in a multi-robot system where relative poses are constantly changing.
- PGO for Multi-Robot Synergy: The application of PGO in a single-frame context for multi-robot scenarios demonstrates its effectiveness in improving overall localization accuracy by leveraging the collective information from all robots.
- Robustness to Challenging Conditions: The system shows promising performance in dark environments, under occlusion, and during aggressive motion, highlighting its practical applicability.
Key Results
- Accuracy: In clear conditions, CREPES achieves a median position accuracy of 0.13 meters and a median orientation accuracy of 1.16 degrees.
- Performance: The system demonstrates robustness in various scenarios, including:
- Two-Robot Localization: Median position errors between 0.102 to 0.161 meters and median orientation errors between 0.733 to 1.517 degrees.
- Multi-Robot Localization: PGO improves accuracy, reducing average Absolute Trajectory Error (ATE).
- Dark Scenario: Consistently estimates relative poses in low-light conditions.
- Long-Range Scenario: Stably estimates poses up to 27.5 meters.
- Aggressive Motion: Maintains position error below 0.4 meters and angle error below 6 degrees during significant roll/pitch/yaw changes.
- Occlusion: Shows acceptable accuracy even when robots are occluded, with slightly larger errors and more outliers, demonstrating the PGO's ability to recover from temporary measurement loss.
- Computational Efficiency: ID extraction takes <2 ms, ESKF iteration <1 ms, and PGO ~3 ms per frame, making it suitable for real-time applications on platforms like an Intel NUC-i5.
Limitations and Future Work
- Occlusion Handling: While the system performs acceptably under occlusion, the authors note an increase in outliers. Future work will focus on improving this through multi-robot cooperative optimization in a continuous-time domain.
- Hardware Iteration: The authors plan to iterate on the hardware to make the system smaller and more suitable for a wider range of robotic applications.
- Continuous-Time Optimization: The current PGO is formulated for single frames. Extending this to a continuous-time domain is a potential future direction for even more robust cooperative localization.
Summary updated