Fusion of Visual-Inertial Odometry with LiDAR Relative Localization for Cooperative Guidance of a Micro-Scale Aerial Vehicle

This paper proposes a cooperative navigation framework that fuses LiDAR-based relative localization from a primary UAV with Visual-Inertial Odometry from a secondary micro-scale UAV to achieve accurate, drift-free guidance and enable large-area exploration with high precision.

The Gist

Here is an explanation of the paper using simple language and creative analogies.

The Big Idea: The "Big Brother" and the "Tiny Scout"

Imagine you are leading a search-and-rescue mission in a dark, confusing maze (like a collapsed building or a dense forest). You have two types of drones:

1. The Big Brother (Primary UAV): This is a large, heavy drone carrying a powerful 3D LiDAR (a laser scanner that sees the world in perfect 3D, like a bat using sonar). It is incredibly accurate and doesn't get lost easily, but it's too big to squeeze through tiny cracks or narrow hallways.
2. The Tiny Scout (Secondary UAV): This is a tiny, lightweight drone with just a camera. It can fly through the smallest holes and reach places the Big Brother can't. However, its camera-based navigation is a bit "drifty." Over time, it starts to think it's in a different spot than it actually is, like a person walking in a circle in the dark without a compass.

The Problem: If you send the Tiny Scout alone, it will eventually get lost and crash. If you send the Big Brother, it can't fit in the tight spots.

The Solution: This paper describes a system where the Big Brother acts as a GPS for the Tiny Scout, even when there is no satellite signal (GPS) available. The Big Brother uses its laser scanner to constantly check where the Tiny Scout is, corrects its "drift," and guides it safely through the maze.

---

How It Works: The "Dance Partner" Analogy

Think of the two drones as dance partners.

• The Tiny Scout is trying to dance a specific routine (a pre-planned path), but it's getting dizzy and losing its rhythm (drift). It thinks it's moving straight, but it's actually drifting off course.
• The Big Brother is the experienced dance instructor standing right next to the Tiny Scout. The instructor has perfect vision (LiDAR).

The Process:

1. The Look: The Big Brother's laser scanner constantly "looks" at the Tiny Scout. It sees exactly where the scout is in 3D space, ignoring the darkness or the lack of texture on the walls.
2. The Correction: The Big Brother compares where the Tiny Scout thinks it is (based on its camera) with where it actually is (based on the laser).
3. The Whisper: The Big Brother sends a quick message over the radio: "Hey, you think you're at point A, but you're actually at point B. Turn left a bit."
4. The Fusion: The Tiny Scout doesn't just blindly follow the radio; it mixes its own camera data with the Big Brother's correction. This creates a "super-accurate" position that is much better than the camera alone.

Why This is a Big Deal (The "Metaphor" of the Heavy Backpack)

Usually, to get high-precision navigation, a drone needs to carry a heavy backpack full of expensive lasers and computers. This makes the drone heavy, shortening its battery life and making it unable to fly into small spaces.

This paper proposes a teamwork strategy:

• The Heavy Backpack (the LiDAR) stays on the Big Brother.
• The Tiny Scout carries only a lightweight camera (a small notebook).
• Because they are working together, the Tiny Scout gets the benefits of the heavy backpack without actually having to carry it.

What Happens When Things Go Wrong?

The researchers tested this in simulations and real life, including tricky situations:

• The "Wall" Test: What if the Tiny Scout flies behind a wall and the Big Brother can't see it for a moment?
  • Result: The system is smart. It uses the Tiny Scout's camera data to guess where it is, but the moment the Big Brother sees it again (like peeking around a corner), it instantly snaps the Tiny Scout's position back to reality, correcting any errors that happened while it was hidden.
• The "Drunk" Test: They simulated the Tiny Scout having a very bad camera (lots of drift).
  • Result: Even when the camera was "drunk" and trying to fly in circles, the Big Brother's laser guidance kept the Tiny Scout on the straight and narrow path.

The Real-World Result

In a real-world test inside a large industrial warehouse:

• The Big Brother flew in the main hallway.
• The Tiny Scout flew into narrow side corridors to map them out.
• The Big Brother guided the Tiny Scout, corrected its position, and helped merge their maps into one perfect picture of the warehouse.

The Bottom Line:
This paper proves that you don't need every drone to be a heavy, expensive super-computer. By having one "smart" drone guide a swarm of "dumb" but agile drones, you can explore dangerous, tight, and complex environments with high precision and safety. It's like having a lighthouse guiding a small boat through a foggy harbor.

Technical Summary

Here is a detailed technical summary of the paper "Fusion of Visual-Inertial Odometry with LiDAR Relative Localization for Cooperative Guidance of a Micro-Scale Aerial Vehicle."

1. Problem Statement

The paper addresses the challenge of guiding micro-scale Unmanned Aerial Vehicles (UAVs) in Global Navigation Satellite System (GNSS)-denied environments (e.g., indoor or subterranean spaces).

• The Dilemma:
  • LiDAR-equipped UAVs: Offer high-precision localization and robustness to lighting conditions but are heavy, power-hungry, and too bulky to navigate narrow, confined spaces.
  • Camera-equipped (Micro) UAVs: Are lightweight and can access tight spaces but rely on Visual-Inertial Odometry (VIO). VIO suffers from significant long-term drift, scale unobservability, and sensitivity to lighting and texture, making precise trajectory tracking difficult over time.
• The Goal: To enable a heterogeneous team where a primary, LiDAR-equipped UAV guides a secondary, lightweight camera-equipped UAV. The system must fuse LiDAR relative localization data with the secondary UAV's VIO output to correct drift and guide the micro-UAV along precise trajectories defined in the primary UAV's reference frame, all while operating fully onboard without ground stations.

2. Methodology

The proposed system consists of a Primary UAV (carrying a 3D LiDAR) and a Secondary UAV (carrying a monocular camera and IMU). The core architecture involves three main modules running on the Primary UAV:

A. LiDAR-Based Detector

• Function: Detects and tracks the Secondary UAV within the Primary UAV's 3D LiDAR point cloud.
• Mechanism: Uses a voxel-based occupancy map and a Kalman filter-based point cluster tracking approach.
• Robustness: Can operate in marker-less mode or use passive reflective markers to distinguish the UAV from clutter (e.g., hanging lights). It handles false detections and variable processing delays.

B. UAV Guider (Fusion & Estimation)

This is the core innovation, fusing LiDAR detections ($L_d$) with the Secondary UAV's VIO pose ($V_p$) to estimate the relative transformation between the two vehicles. It operates in two loosely-coupled steps:

1. NLS-based Estimator (Non-Linear Least Squares):
   • Aligns LiDAR detections and VIO poses over a sliding window to estimate the 4-DOF transformation (3D translation + relative heading) between the LiDAR frame ($L$) and the VIO frame ($V$).
   • Uses a robust loss function (Soft L1) to minimize the impact of outliers.
   • Optimizes for the transformation $^V_L T$.
2. LKF-based Tracker (Linear Kalman Filter):
   • Takes the output from the NLS estimator and fuses it with real-time LiDAR detections and VIO data.
   • Uses a 4-DOF constant-velocity state model (position, velocity, heading, heading rate).
   • Key Feature: Utilizes a recalculating history buffer to handle asynchronous measurements and communication delays. It predicts the state to the time of the latest measurement, allowing it to fuse data even if VIO or LiDAR updates arrive with different latencies.
   • Outputs a drift-compensated estimate of the Secondary UAV's pose in the Primary UAV's frame ($^L_S T$).

C. Reference Transformer

• Transforms the desired trajectory (defined in the Primary UAV's LiDAR frame) into the Secondary UAV's local frame.
• Transmits these transformed references to the Secondary UAV, which tracks them using its onboard VIO controller.

3. Key Contributions

1. Novel Fusion Approach: The first solution to fuse LiDAR relative localization with VIO ego-motion for heterogeneous UAV teams. It provides a common reference frame without requiring prior knowledge of UAV sizes or initial poses.
2. Drift Mitigation Strategy: Successfully mitigates VIO drift by using high-precision LiDAR relative measurements to correct the micro-UAV's trajectory in real-time.
3. Robustness to Degenerate Cases: The system handles:
   • Loss of Line-of-Sight (NLOS): Continues tracking via VIO until LiDAR contact is re-established, at which point the estimate is corrected.
   • Communication Delays: Uses a history buffer to fuse delayed measurements correctly.
   • False Detections: Uses Mahalanobis distance for data association to reject outliers.
4. Open Dataset: The authors released raw data from real-world experiments to facilitate future research in cooperative relative localization.

4. Experimental Results

The method was evaluated through simulations and real-world flights in an industrial warehouse.

• Simulation (Drift Compensation):
  • Tested with artificial constant-velocity drift up to $1.0 , m/s$.
  • The system successfully guided the UAV with zero failures up to $0.7 , m/s$ drift.
  • Mean path deviation remained low (0.14m to 0.67m) even with significant drift.
• Real-World Localization Accuracy:
  • Metrics: Absolute Trajectory Error (ATE) compared against RTK ground truth.
  • Results:
    • LiDAR + VIO Fusion: Average 3D ATE of 0.28 m.
    • Raw VIO (VINS-Mono): Average 3D ATE of 2.38 m.
    • Raw VIO (OpenVINS): Average 3D ATE of 0.58 m.
  • The fusion method reduced the 3D error by approximately 88% compared to raw VINS-Mono.
• Cooperative Mapping Scenario:
  • A Primary UAV stayed in a main corridor while guiding a Secondary UAV into narrow side corridors to map an industrial warehouse.
  • The system successfully merged local maps from both UAVs into a consistent global occupancy map, demonstrating the method's utility in complex, cluttered environments where the Primary UAV could not physically enter.

5. Significance

This work demonstrates a practical pathway for heterogeneous multi-robot systems in emergency response and Search and Rescue (SAR) missions.

• Operational Efficiency: It allows large, robust robots to "herd" or guide smaller, agile robots into dangerous or inaccessible areas, correcting the smaller robots' navigation errors without requiring them to carry heavy sensors.
• Scalability: The communication analysis suggests the system can scale to large swarms (theoretically supporting hundreds of secondary UAVs) as bandwidth requirements are low.
• Autonomy: By running entirely onboard, the system is resilient to communication blackouts and does not rely on external infrastructure, making it ideal for subterranean or disaster zones.

In summary, the paper presents a robust, computationally efficient framework that leverages the strengths of both LiDAR (accuracy, robustness) and cameras (lightweight, agility) to enable precise cooperative navigation in GPS-denied environments.