Efficient nearest-neighbor search algorithms for sub-Riemannian geometries

Abstract

The Motion Planning problem has been at the core of a significant amount of research in the past decades and it has recently gained traction outside academia with the rise of commercial interest in self-driving cars and autonomous aerial vehicles. Among the leading algorithms to tackle the problem are sampling-based planners, such as Probabilistic Road Maps (PRMs), Rapidly-exploring Random Trees (RRTs) and a large number of variants thereof. In this thesis, we focus on a crucial building block shared by these algorithms: nearest-neighbor search. While nearest-neighbor search is known as the asymptotically dominant bottleneck of sampling-based planners, popular algorithms to efficiently identify neighbors are limited to robots capable of unconstrained motions, commonly referred to as holonomic.

Nevertheless, this is rarely the case in the vast majority of practical applications, where the dynamical system at hand is often subject to a class of differential constraints called nonholonomic. We tackle the problem with sub-Riemannian geometries, a mathematical tool to study manifolds that can be traversed under local constraints. After drawing the parallel with nonholonomic mechanical systems, we exploit peculiar properties of these geometries and their natural notion of distance to devise specialized, efficient nearest-neighbor search algorithms. Our contributions are two-fold: First, we generalize existing space-partitioning techniques (k-d trees) to sub-Riemannian metrics. This is achieved by introducing i) a criterion - the outer Box Bound - that discards halfspaces consistently with the metric and ii) a space-partitioning technique - the Lie splitting strategy - that organizes the dataset for optimal asymptotic performance.

Second, we propose pruning techniques to further improve the query runtime. This is achieved by reducing the number of distance evaluations required to discern the nearest neighbors and exploiting heuristics that provably approximate a sub-Riemannian metric up to a constant factor, asymptotically.