A Vision-aided Inertial Navigation System for Agile High-speed Flight in Unmapped Environments

Small, lightweight flight vehicles, such as consumer-grade quadrotors, are becoming increasingly common. These vehicles' on-board state estimators are typically reliant upon frequent and accurate updates from external systems such as the Global Positioning System (GPS) to provide state estimates required for stable flight. However, in many cases GPS signals may be unavailable or unreliable, and loss of GPS can cause these vehicles to go unstable or crash, potentially putting operators, bystanders, and property in danger. Thus reliance on GPS severely limits the robustness and operational capabilities of lightweight flight vehicles. This paper introduces the Smoothing And Mapping With Inertial State Estimation (SAMWISE) navigation system. SAMWISE is a vision-aided inertial navigation system capable of providing high-rate, low-latency state estimates to enable high-dynamic flight through obstacle-laden unmapped indoor and outdoor environments. SAMWISE offers a flexible framework for inertial navigation with nonlinear measurements, such as those produced by visual feature trackers, by utilizing an incremental smoother to efficiently optimize a set of nonlinear measurement constraints, estimating the vehicle trajectory in a sliding window in real-time with a slight processing delay. To overcome this delay and consistently produce state estimates at the high rates necessary for agile flight, we propose a novel formulation in which the smoother runs in a background thread while a low-latency inertial strapdown propagator outputs position, attitude, and velocity estimates at high-rate. We additionally propose a novel measurement buffering approach to seamlessly handle delayed measurements, measurements produced at inconsistent rates, and sensor data requiring significant processing time, such as camera imagery. We present experimental results high-speed flight with a fully autonomous quadrotor using SAMWISE for closed-loop state estimation from flight demonstrations during the DARPA Fast Lightweight Autonomy (FLA) program in April and November of 2016. SAMWISE achieved less than 1% position error and up to 5.5 m/s (12 mph) flight in a simulated indoor warehouse environment using a scanning-lidar, inertial measurement unit, and laser altimeter during the first FLA milestone event in April 2016. In November 2016, SAMWISE achieved approximately 3% error and up to 20 m/s (45 mph) flight in an open outdoor environment with large obstacles during the second FLA milestone event. The results of these flight tests demonstrate that our navigation system works robustly at high speed across multiple distinct environments.