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

Abhishek Goudar, Wenda Zhao, Angela P. Schoellig

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.

Introduction

Navigating a micro aerial vehicle (MAV) in GPS-denied indoor environments requires a delicate balance between high-frequency stability and long-term global accuracy. Visual-Inertial Odometry (VIO) is a standard solution for local stability, providing smooth, high-rate pose estimates. However, VIO is inherently prone to drift over time. Conversely, range-based sensors, such as Ultra-Wideband (UWB), offer drift-free absolute positioning by measuring distances to fixed anchors.

The MAV navigating a cluttered cafeteria environment, demonstrating the system's ability to handle signal reflections and obstacles.

The primary challenge in fusing these modalities lies in their disparate characteristics: UWB data is often low-dimensional, noisy, and susceptible to non-line-of-sight (NLOS) errors in cluttered spaces. Directly incorporating jumpy UWB measurements into a flight controller can lead to instability or crashes. This work presents a dual-smoother architecture designed to provide both the absolute accuracy of UWB and the control-ready smoothness of VIO.

Proposed Architecture

The core of the proposed system is a dual fixed-lag smoother (FLS) architecture that separates the tasks of global trajectory estimation and high-rate pose generation:

  1. UWB-aided FLS (UFLS): This component serves as the "accuracy engine." It operates at a lower frequency, fusing UWB range measurements with VIO data. Crucially, it estimates not only the robot's trajectory but also the systematic biases of the UWB anchors, which are common in indoor environments. To maintain computational efficiency on constrained hardware, the system employs Odometry Preintegration, grouping high-rate VIO measurements into single relative constraints within the factor graph.
  2. WNOA-based FLS (WFLS): This "smoothness engine" generates high-rate pose estimates for closed-loop control. It applies a White-Noise-on-Acceleration (WNOA) motion model to the output of the UFLS, effectively filtering out jumps and providing a smooth "frame offset" between the VIO's local coordinates and the global world frame.

By decoupling these processes, the MAV can perform complex global optimization in the background while maintaining a jitter-free signal for its flight motors.

Experimental Validation

The system was validated through extensive closed-loop flight tests in three distinct real-world environments: a controlled motion-capture arena (UTIAS Testbed), a cluttered cafeteria with significant signal reflections, and a feature-sparse laboratory (Myhal Testbed).

Composite view of flight tests across the three experimental environments, showing the complex trajectories (squares, slaloms, butterflies) used for validation.

Key Results

  • Positioning Accuracy: The architecture achieved decimeter-to-sub-decimeter level positioning accuracy, frequently maintaining a Root Mean Square Error (RMSE) below 10 cm across various dynamic flight paths.
  • Tracking Performance: Even using minimally tuned, open-source controllers, the MAV successfully tracked commanded trajectories at speeds up to 2 m/s with decimeter-level precision.
  • Robustness to NLOS: Online estimation of anchor-specific biases proved vital; modeling these biases reduced trajectory error by approximately 50% in cluttered environments like the UTIAS cafeteria.
  • Computational Efficiency: The entire pipeline was demonstrated to run online and onboard a computationally constrained Nvidia Jetson TX1.

Discussion and Future Work

While the system demonstrates high performance, certain limitations remain. Accuracy is sensitive to anchor geometry; when the MAV moves to the extremities of the anchor constellation, "dilution of precision" can occur. Additionally, the system currently requires a brief calibration flight to initialize bias priors and assumes known anchor locations.

Future research will focus on "Auto-Surveying" techniques, allowing the MAV to map anchor locations autonomously during flight, and further refining the bias models to handle more extreme NLOS conditions. The authors have released the source code and datasets to support further community research in range-visual-inertial fusion.

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},
}
Figure