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

TL;DR

The paper tackles GPS-denied MAV localization by fusing range measurements with visual-inertial odometry using a dual fixed-lag smoother framework. It introduces UFLS to jointly estimate trajectory and anchor biases from UWB range and VIO, and WFLS with a white-noise-on-acceleration motion model to produce smooth, high-rate frame offsets for control. The approach yields decimeter-to-sub-decimeter localization accuracy and decimeter-level tracking in real indoor environments, while remaining lightweight enough for onboard computation and online operation; code and datasets are released. This work enables reliable, drift-free navigation for constrained MAVs operating in cluttered, indoor, and GPS-denied settings, with practical impact for autonomous flight in complex environments.

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.

Figures (13)

• Figure 1: Overlay of trajectories from closed-loop flight experiments using our proposed method for localization. The proposed method uses a dual-rate fixed-lag smoother architecture to combine range measurements from ultrawideband radios and visual inertial odometry for localization. The UTIAS testbed (left top and left bottom) and the Myhal testbed (right bottom) present challenging scenarios in terms of poor geometry of anchors for range-based positioning, and sparse features for visual inertial odometry, respectively. The UTIAS cafeteria (right top) is challenging as ultrwideband signals are affected by reflections from the obstacles. A video of our experiments is available at: http://tiny.cc/uwb_vio.
• Figure 2: The problem of sensor fusion of UWB range measurements and VIO odometry is modeled as a frame alignment problem between the world frame $\{W\}$ and the odometry frame $\{o\}$. The absolute but non-smooth estimate $\mathbf{T}^W_i$ obtained from fusing UWB and VIO measurements is combined with the relative but smooth estimate ${\mathbf{T}}^o_i$ from VIO to estimate the offset $\mathbf{T}^W_o$ between the two frames.
• Figure 3: Overview of the proposed architecture. Range measurements from UWB radios and odometry from VIO are combined in the first fixed-lag smoother (UFLS) to estimate the robot pose ${\mathbf{T}}^W_i$ and the systematic biases in range measurements. The estimated robot pose is then combined with the latest estimate from VIO $\mathbf{T}^o_i$ to estimate the frame offset ${\mathbf{T}}^W_o$ between the world frame and the local frame. A white-noise-on-acceleration (WNOA) motion model is used to generate a smooth frame offset, $\hat{\mathbf{T}}^W_o$, using the second fixed-lag smoother (WFLS). The smoothed frame offset is then combined with VIO-baed odometry to obtain smooth, high-rate, and drift-free robot pose $\hat{\mathbf{T}}^W_i$ which is sent to the controller.
• Figure 4: Factor graph for the UWB-aided fixed-lag smoother (UFLS). Preintegrated odometry is added as a binary factor $\phi_{\mathbf{o}_t}$ to constrain consecutive robot poses. Each range measurement adds a binary factor $\phi_{\mathbf{r}_{lt}}$ between the robot pose $\mathbf{T}^W_{i_t}$ and the node $b_{a_l}$ representing the bias associated with anchor $a_l$.
• Figure 5: Factor graph for the white-noise-on-acceleration (WNOA) fixed-lag smoother (WFLS). Motion priors obtained using the WNOA motion model are added as binary factors $\phi_{\mathbf{w}_t}$ to constrain the position component of the frame offset, $\mathbf{p}^W_{o_t}$. Frame offsets computed by UFLS are added as unary measurement factors $\phi_{\mathbf{y}_t}$.