CCO-VOXEL: Chance Constrained Optimization over Uncertain Voxel-Grid Representation for Safe Trajectory Planning

TL;DR

CCO-VOXEL introduces a real-time, probabilistic trajectory planning algorithm that directly operates on voxel-grid representations, providing safety guarantees without assuming specific sensor noise models.

Contribution

It is the first chance-constrained optimization method for voxel-grid based planning that uses Hilbert Space embeddings and MMD for nonparametric uncertainty handling.

Findings

Improves collision avoidance and trajectory smoothness over existing methods.

Achieves real-time performance on onboard hardware.

Demonstrates effectiveness in simulation and real quadrotor flights.

Abstract

We present CCO-VOXEL: the very first chance-constrained optimization (CCO) algorithm that can compute trajectory plans with probabilistic safety guarantees in real-time directly on the voxel-grid representation of the world. CCO-VOXEL maps the distribution over the distance to the closest obstacle to a distribution over collision-constraint violation and computes an optimal trajectory that minimizes the violation probability. Importantly, unlike existing works, we never assume the nature of the sensor uncertainty or the probability distribution of the resulting collision-constraint violations. We leverage the notion of Hilbert Space embedding of distributions and Maximum Mean Discrepancy (MMD) to compute a tractable surrogate for the original chance-constrained optimization problem and employ a combination of A* based graph-search and Cross-Entropy Method for obtaining its minimum. We show tangible performance gain in terms of collision avoidance and trajectory smoothness as a consequence of our probabilistic formulation vis a vis state-of-the-art planning methods that do not account for such nonparametric noise. Finally, we also show how a combination of low-dimensional feature embedding and pre-caching of Kernel Matrices of MMD allows us to achieve real-time performance in simulations as well as in implementations on on-board commodity hardware that controls the quadrotor flight