Range-Visual-Inertial Sensor Fusion for Micro Aerial Vehicle Localization and Navigation

Abhishek Goudar, Wenda Zhao, Angela P. Schoellig

TL;DR

Researchers have developed a dual-smoother sensor fusion architecture that combines the drift-free nature of Ultra-Wideband (UWB) range sensors with the high-rate smoothness of Visual-Inertial Odometry (VIO). By decoupling trajectory estimation from pose generation for control, the system achieves decimeter-level positioning accuracy and stable flight for micro aerial vehicles (MAVs) in cluttered indoor environments.

What Problem Does This Paper Address?

Navigating a small drone (MAV) indoors is a balancing act between two sensing worlds. Visual-Inertial Odometry (VIO) provides smooth, high-frequency data perfect for keeping a drone stable, but it "drifts" over time—the drone's perceived position slowly deviates from reality. Range-based sensors (like UWB) are "drift-free" because they measure distance to fixed anchors, but they are low-dimensional, noisy, and prone to "jumps" caused by signal interference (Non-Line-of-Sight or NLOS).

The challenge is that simply "plugging in" UWB data to fix VIO drift often results in non-smooth position estimates. For a flight controller, these sudden jumps are disastrous, leading to oscillatory behavior or crashes. This paper tackles how to fuse these sensors to get the best of both: the absolute accuracy of UWB and the control-ready smoothness of VIO.

Methods

The authors propose a dual fixed-lag smoother (FLS) architecture:

  1. UWB-aided FLS (UFLS): This is the "accuracy engine." It runs at a lower rate and fuses UWB ranges with VIO data to estimate the robot's trajectory and—crucially—the systematic biases of the UWB anchors. To keep things computationally light, they introduce Odometry Preintegration, a technique that bunches multiple high-rate VIO measurements into a single "delta" for the smoother.
  2. WNOA-based FLS (WFLS): This is the "smoothness engine." It takes the (potentially jumpy) output from the first smoother and applies a White-Noise-on-Acceleration (WNOA) motion model. This acts as a sophisticated filter that generates a smooth "frame offset" between the VIO's local coordinates and the world's global coordinates.

By separating these two tasks, the drone can calculate its "true" position carefully while still receiving a buttery-smooth signal for its flight motors.

Datasets

The approach was validated using:

  • Simulation: Gazebo/Rotors simulator for initial qualitative testing.
  • Real-World Experiments: Conducted in three distinct indoor environments:
  • UTIAS Testbed: A controlled flight arena with Vicon motion capture for ground truth.
  • UTIAS Cafeteria: A challenging environment with obstacles causing signal reflections.
  • Myhal Testbed: An area with sparse visual features, challenging for VIO.
  • Open Source: The authors released their source code and the dataset collected during these experiments to the community.

Key Insights and Ideas

  • Bias is the Enemy: In cluttered indoor spaces, UWB signals don't travel in straight lines. The authors found that modeling and estimating these anchor-specific biases online reduced trajectory error by 50% compared to ignoring them.
  • Decoupling is Key: You shouldn't use the same estimate for "mapping" that you use for "steering." By using two smoothers, the system can "think" about the best global position in the background while providing the controller with a jitter-free local estimate.
  • Odometry Preintegration: Borrowing a concept from IMU processing, the authors showed that you don't need to add a node to your factor graph for every single camera frame. Pre-integrating odometry keeps the "math" small enough to run on a constrained onboard computer like an Nvidia Jetson TX1.

Key Results

  • Accuracy: The system achieved decimeter-to-sub-decimeter level positioning accuracy (often below 10 cm RMSE) across various flight paths (squares, slaloms, butterflies).
  • Tracking: Even with minimally tuned, off-the-shelf controllers, the MAV maintained decimeter-level tracking of commanded trajectories at speeds up to 2 m/s.
  • Robustness: The system remained stable in the UTIAS cafeteria, where metallic objects and walls created significant NLOS issues for the UWB signals.
  • Efficiency: The entire pipeline runs online and onboard a computationally constrained MAV.

Limitations and Future Work

  • Anchor Geometry: The authors noted that accuracy drops at the extremities of the test arena due to "dilution of precision"—essentially, when the angles to the UWB anchors become too shallow, the math becomes less certain.
  • Manual Calibration: While biases are estimated online, the system currently requires a "calibration flight" to set initial priors for those biases.
  • Known Anchor Positions: The system assumes the 3D locations of the UWB anchors are known beforehand. Future work could involve "Auto-Surveying," where the drone maps the anchor locations itself while flying.

Summary updated

Abstract

We propose a fixed-lag smoother-based sensor fusion architecture to leverage the complementary benefits of range-based sensors and visual-inertial odometry (VIO) for localization. We use two fixed-lag smoothers (FLS) to decouple accurate state estimation and high-rate pose generation for closed-loop control. The first FLS combines ultrawideband (UWB)-based range measurements and VIO to estimate the robot trajectory and any systematic biases that affect the range measurements in cluttered environments. The second FLS estimates smooth corrections to VIO to generate pose estimates at a high rate for online control. The proposed method is lightweight and can run on a computationally constrained micro-aerial vehicle (MAV). We validate our approach through closed-loop flight tests involving dynamic trajectories in multiple real-world cluttered indoor environments. Our method achieves decimeter-to-sub-decimeter-level positioning accuracy using off-the-shelf sensors and decimeter-level tracking accuracy with minimally-tuned open-source controllers.
BibTeX
@article{goudar2024range,
  author = {Abhishek Goudar and Wenda Zhao and Angela P. Schoellig},
  title = {{Range-Visual-Inertial Sensor Fusion for Micro Aerial Vehicle Localization and Navigation}},
  journal = {IEEE Robotics Autom. Lett.},
  year = {2024},
  doi = {10.1109/lra.2023.3335772},
}

Loading related content...

Figure