Radio-based Multi-Robot Odometry and Relative Localization

This paper presents a robust, open-source multi-robot UGV-UAV localization system that fuses UWB and radar data with standard odometry sensors within a pose-graph optimization framework to accurately estimate relative positions in challenging environments, outperforming state-of-the-art methods and offering extensibility to full SLAM.

The Gist

Imagine you are in a thick fog inside a massive, empty warehouse. You can't see more than a few feet in front of you, and your GPS is broken because the roof is too high. You are trying to find your way, but you are also part of a team: you are a small flying drone, and your partner is a large, heavy robot on wheels.

Your goal? To stay together and know exactly where you both are relative to each other, even though you can't see each other and the environment is confusing.

This paper describes a clever new "teamwork system" that helps robots do exactly that. Instead of relying on cameras or lasers (which get blinded by fog, dust, or darkness), the robots use radio waves and sound-like pulses to "feel" their way around.

Here is how the system works, broken down into simple parts:

1. The Two Superpowers: UWB and Radar

The robots are equipped with two special tools:

• The "Radio Ruler" (UWB): Think of this like a high-tech tape measure that uses radio signals. The flying robot has "tags" (like little name tags), and the ground robot has "anchors" (like fixed rulers). They constantly shout out, "How far apart are we?" The system uses these distance measurements to figure out where the drone is relative to the ground robot. It's like two people in a dark room holding a rope; even if they can't see, they know exactly how far apart they are by how much rope is between them.
• The "Echolocation Eye" (Radar): This is like a bat's sonar. The radar sends out radio pulses and listens for the echoes bouncing off walls or the ground. It can tell the robot how fast it's moving and in what direction, even in total darkness or heavy rain. It's the robot's way of saying, "I'm moving forward at 2 meters per second," even if its wheels are slipping.

2. The Problem: Drifting Off Course

Every robot has a "memory" of where it thinks it is (called odometry). But this memory is flawed.

• If the ground robot's wheels slip on a patch of mud, it thinks it moved 10 meters, but it only moved 8.
• If the drone gets hit by a gust of wind, it thinks it's in one spot, but it's actually somewhere else.

Over time, this "drift" makes the robots lose their way. If they rely only on their own memory, they will eventually think they are in a completely different part of the warehouse than they actually are.

3. The Solution: The "Group Hug" (Pose-Graph Optimization)

This is the brain of the operation. The researchers built a mathematical system that acts like a group hug for the robots' data.

Instead of each robot trying to solve the puzzle alone, they share their information:

1. The Radio Ruler tells them, "Hey, we are exactly 5 meters apart right now."
2. The Echolocation Eye tells them, "I'm moving forward, and I'm not slipping."
3. The Group Hug (Optimizer): This is a smart computer algorithm (using something called Ceres) that takes all these conflicting clues and smooths them out. It says, "Okay, the ground robot thinks it moved 10 meters, but the radio says it's only 8 meters away from the drone. Let's correct the ground robot's memory."

It constantly adjusts their positions, pulling them back to the truth, ensuring that even if one robot slips or gets confused, the other one helps correct it.

4. The "Magic" Simulation

To test this, the researchers didn't just build robots; they built a virtual reality video game (using a tool called Gazebo) that mimics real life perfectly.

• They created a fake radio system that behaves exactly like real radios (including the static and errors).
• They tested it in this virtual world first, then tried it with real robots in a real warehouse.

The Results: A Winning Team

The system worked incredibly well.

• In the fog: While cameras would have failed, the radio and radar kept the robots on track.
• In the dark: The system didn't need a single light source.
• The Accuracy: Even when the robots were moving fast or far apart, the system kept their relative position accurate to within about a meter (or roughly 3 feet), which is amazing for robots moving in 3D space.

Why This Matters

Think of this as the foundation for a future where robots can work together in disaster zones (like collapsed buildings or nuclear plants) where humans can't go, and where GPS doesn't work.

• Old Way: Robots get lost because they can't see and their wheels slip.
• New Way: They hold hands (via radio) and listen to the wind (via radar), constantly correcting each other so they never get lost.

The researchers even made all their code and data public, like sharing a recipe, so other scientists can use it to build even better robot teams. It's a big step toward making robots that are truly reliable partners in the real world.

Technical Summary

1. Problem Statement

The paper addresses the challenge of relative localization in heterogeneous multi-robot systems (specifically Unmanned Ground Vehicles [UGVs] and Unmanned Aerial Vehicles [UAVs]) operating in GPS-denied environments.

• Limitations of Current Methods: Traditional vision and LiDAR-based localization fail in unstructured environments with poor lighting, fog, dust, or clutter.
• Radio Challenges: While Ultra-Wideband (UWB) and Radar offer robustness to weather and lighting, existing radio-based methods often lack:
  • Redundancy (using multiple onboard transceivers).
  • Fusion with radar odometry for 3D localization.
  • Real-time capability in heterogeneous teams.
  • Robustness to noise and outliers compared to closed-form algebraic solutions.

2. Methodology

The proposed system is a radio-based localization framework that fuses UWB range data, radar odometry, and proprioceptive sensors (IMUs, wheel encoders) within a Pose-Graph Optimization structure. The pipeline consists of three main modules:

A. Nonlinear Relative Transformation Estimation (UWB)

• Goal: Estimate the rigid transformation ($O_1T_{O2}$) between the local odometry frames of the UAV and UGV.
• Setup: The UAV carries 2 UWB tags; the UGV carries 4 UWB anchors.
• Approach: Instead of closed-form algebraic solutions (which are sensitive to noise), the authors use a Nonlinear Least Squares (NLS) optimization.
  • Cost Function: Minimizes the difference between predicted and measured distances between all anchor-tag pairs, weighted by measurement noise ($\sigma$).
  • 4-DOF Constraint: To simplify the problem, Roll and Pitch are assumed to be accurately estimated by onboard IMUs. The optimization solves only for 3 translations ($x, y, z$) and 1 rotation (Yaw).
  • Sliding Window: Uses a distance-based sliding window (2–10 meters) to process anchor-tag pairs, generating hundreds of residuals per optimization step.

B. Radar Odometry

• Goal: Provide ego-motion estimation (velocity and pose change) robust to environmental conditions.
• Approach: Extends a loosely coupled radar-inertial odometry method for multi-robot scenarios.
  • Pre-processing: Filters false positives and dynamic objects.
  • Ego-Motion Estimation: Uses the Doppler effect. Radial velocities ($v_D$) are projected onto the sensor's Line of Sight (LoS) to solve for the robot's linear velocity vector ($v$) via a linear least-squares system.
    • UGV: Uses IMU roll/pitch to reduce degrees of freedom (holonomic assumption).
    • UAV: Estimates all three linear velocity components independently due to 3D motion capabilities.
  • Scan Matching: The estimated velocity initializes a Generalized ICP (GICP) scan-matching process on filtered point clouds to generate relative pose constraints.

C. Multi-Robot Pose-Graph Optimization

• Goal: Fuse all sensor data to estimate the global poses of both robots simultaneously.
• Framework: Implemented using Ceres Solver in ROS 2.
• Graph Structure:
  • Nodes: Represent robot poses ($x, y, z, \theta$) at keyframes.
  • Edges (Factors):
    1. Odometry: Constraints from wheel encoders (UGV), IMU integration (UAV), and Radar scan-matching.
    2. Inter-Robot Constraints: Virtual observations derived from the UWB relative transformation estimation (Section III-A).
    3. Priors: Anchor nodes fix the global frame (gauge freedom) and anchor trajectories to start points.
• Optimization: Solves for the maximum a posteriori (MAP) estimate of the states by minimizing the sum of squared errors across all factors.

3. Key Contributions

1. Integrated Localization System: A novel framework fusing UWB inter-robot ranges and Radar odometry for heterogeneous UGV-UAV teams in 3D space.
2. Robust Optimization: Replaces fragile closed-form relative transformation methods with a Nonlinear Least Squares (NLS) approach that leverages redundancy from multiple onboard transceivers (2 tags, 4 anchors).
3. Real-Time Multi-Robot Pose Graph: A factor-graph formulation that simultaneously optimizes poses for both robots, handling inter-robot constraints and odometry in real-time.
4. Realistic Simulation Plugin: A custom Gazebo Harmonic plugin that models UWB range measurements based on real-world data (including noise and dropouts), bridging the gap between Simulation (SITL) and reality.
5. Open Source: Full code and a real-world dataset are publicly released to support reproducibility and benchmarking.

4. Experimental Results

The system was validated in Software-in-the-Loop (SITL) simulations and a real-world dataset involving a DJI M210 UAV and a custom holonomic UGV.

• Relative Transformation Accuracy:
  • The proposed NLS method outperformed state-of-the-art closed-form linear methods.
  • Results: Achieved a mean translational RMSE of 0.57 m and rotational RMSE of 22.9° (vs. 0.71 m and 35.5° for the linear baseline) in simulation.
• Global Pose Estimation:
  • Simulation: Successfully estimated global poses without prior knowledge of the initial transformation.
  • Real-World Dataset:
    • UGV: Absolute Trajectory Error (ATE) of 0.94 m.
    • UAV: ATE of 1.44 m.
    • Relative Error: Converged to approximately 1.1 m translation and 5° rotation error after 40 seconds of operation.
• Radar Performance: The radar ego-motion algorithm provided consistent velocity estimates for both the ground robot (forward motion) and the aerial robot (3D motion), aiding the initialization of scan matching.
• Computational Efficiency:
  • The system runs in real-time (>5 Hz).
  • Relative transformation estimation takes ~140 ms.
  • Pose-graph optimization takes ~118 ms (average), scaling efficiently for the tested 2-robot setup.

5. Significance and Future Work

• Significance: This work demonstrates that radio-based sensors (UWB + Radar) can provide robust, real-time localization for heterogeneous robot teams in environments where vision/LiDAR fail (e.g., fog, darkness). The factor-graph approach allows for extensibility to full Simultaneous Localization and Mapping (SLAM).
• Limitations & Future Work:
  • NLOS: The current UWB model does not fully account for Non-Line-of-Sight (NLOS) bias; future work will analyze this.
  • Place Recognition: The system lacks explicit loop closure (place recognition), leading to growing uncertainty over long durations.
  • Scalability: While the current cost is near-linear for disjoint pairs, future work will validate the system on larger teams (more than 2 robots) and explore learning-based feature extraction to improve radar place recognition.

In conclusion, the paper presents a robust, open-source solution for cooperative localization that effectively leverages the complementary strengths of UWB (range) and Radar (velocity/structure) to overcome the limitations of traditional sensors in challenging environments.