Belief-Space Planning for Real-World Systems: Efficient SLAM-Based Belief Propagation and Continuous-Time Safety

Abstract

Uncertainty-aware planning has long been a recurring goal in robotics. By enabling autonomous systems to explicitly reason about their own uncertainty, desirable behaviors that increase observability and ensure robust constraint satisfaction arise naturally from high-level optimization specifications. For partially-observable and under-sensed systems in particular, belief-space planning (BSP) provides a natural probabilistic formulation. Despite significant research attention over the years, a few key challenges have prevented the application of BSP to the real-world systems that would stand to benefit the most, such as SLAM-reliant Micro-Aerial Vehicles (MAVs).

The most fundamental of these challenges is that of efficiently propagating the state belief, particularly under SLAM-based estimation schemes like Visual-Inertial Odometry (VIO). This thesis describes a structureless and consistent approximation for belief propagation under SLAM, the efficacy of which is demonstrated in the challenging setting of observability-aware planning for VIO.

A key attraction of BSP is the ability to specify constraints on the total probability of failure – however, actually encoding these constraints within practical optimization schemes remains a challenge, particularly for physical systems, which evolve continuously in time. General-purpose Monte-Carlo methods can be used to accurately assess failure rates, but these are cumbersome to optimize against, while more convenient “direct” estimates are based on discrete-time simplifications and fail to meaningfully constrain the full continuous-time risk. To address this gap, a novel risk estimate is derived directly in continuous-time, providing a principled, lightweight, and convenient means of ensuring probabilistic safety for real-world systems. Together, these contributions enable online, risk-constrained BSP for a large class of systems of widespread practical interest.