Introduction
In multi-robot missions, such as search and rescue or subterranean exploration, robots must navigate unknown environments while maintaining accurate maps. Typically, robots send data to a centralized server that performs global optimization and loop closure. However, a significant challenge remains: providing real-time feedback to the robots. Without corrections from the server, individual robots continue to operate with drifted odometry, leading to inconsistent local paths and navigation failures.
This paper introduces a novel feedback mechanism that utilizes spectral graph wavelets to identify and correct odometry drift in real-time. By analyzing structural differences between a robot's local estimate and the server's optimized global map at multiple spatial scales, the system can pinpoint error origins and transmit sparse, high-impact constraints back to the robots, improving onboard localization accuracy by up to 90%.
Methods
The proposed framework treats a robot's trajectory as a pose graph where nodes represent poses and edges represent spatial relationships. The core of the approach lies in Spectral Graph Theory.
- Graph Signal Processing: A signal is defined on the pose graph based on $SE(3)$ distances, capturing both position and orientation.
- Spectral Graph Wavelets: The graph signal is transformed into the spectral domain using the Graph Laplacian. Wavelets are applied at different scales: small scales detect local jitter, while large scales identify structural, long-term drift.
- Kron Reduction: To minimize bandwidth, the server uses "Kron reduction" to create a sparse abstraction of the global graph that preserves its essential structural properties.
- Constraint Generation: The robot compares its local graph wavelets to the server’s reduced "fingerprints." Differences guide the automatic generation of new constraints (adjacent, n-hop, or submap-level) for the robot's local optimizer (like iSAM2).
Experimental Evaluation
The framework was validated across diverse datasets and real-world deployments:
- EuRoC Machine Hall: Standard visual-inertial odometry benchmarks using aerial drones.
- Indoor/Outdoor ANYmal: Legged robot data featuring transitions designed to induce localization failure.
- Hagerbach Underground Tunnel: A 1.2 km autonomous subterranean mission.
- DARPA Subterranean (SubT) Challenge: Data from Team CERBERUS’s prize-winning run in complex environments.
Key Insights
- Root Cause Identification: Multi-scale wavelets allow the system to identify where drift originated, enabling effective corrections with minimal constraints.
- Modality Independence: By operating on abstract pose graphs rather than raw sensor data, the system supports heterogeneous robot teams (e.g., drones and legged robots).
- The Role of $SE(3)$: Using full $SE(3)$ geometry for the graph signal is significantly more effective than simple Euclidean distance ($R^3$), as catching rotation errors early prevents massive downstream positional drift.
- Bandwidth Efficiency: Kron reduction can reduce graph size by over 60% while maintaining high correction accuracy.
Results
The proposed method demonstrated significant improvements in localization accuracy. In the EuRoC datasets, it achieved the lowest collaborative error, often outperforming state-of-the-art centralized SLAM systems.
In the "Rooftop" experiment, where LiDAR-based localization failed, server feedback recovered the trajectory, reducing RMSE from 2.22m to 0.30m. During the DARPA SubT Final, the system corrected drift that would have caused a scoring failure for a detected artifact, bringing its reported position back within the required 5m threshold.
Conclusion
This framework provides a robust, bandwidth-efficient method for real-time collaborative mapping. Future work will focus on reducing the computational overhead of local graph optimization for long-range missions and exploring hierarchical graph structures to further optimize the trade-off between map reduction and accuracy.
Summary updated