## Abstract

This paper describes methodologies to estimate the collision probability, Euclidean distance and gradient between a robot and a surface, without explicitly constructing a free space representation. The robot is assumed to be an ellipsoid as opposed to the commonly used spherical model, thus providing a tighter approximation for navigation in cluttered and narrow spaces. Instead of computing distances over raw point clouds or large high-resolution occupancy grids, the environment is modeled using Gaussian mixture models and approximated via a set of ellipsoids. A parallelizable strategy to accelerate an existing ellipsoid-ellipsoid distance computation method is presented. Evaluation in 3D environments demonstrates improved performance over a state-of-the-art Gaussian Process-based method. Execution times for the approach are within a few microseconds per ellipsoid pair using a single-thread on low-power embedded computers.

## Idea

Many optimization-based approaches have been proposed for this purpose. In this work, the method by Rimon and Boyd is leveraged because it formulates the optimization as an eigenvalue problem. However, the expres- sions stated in their work require several matrix inversions and square roots. It is not obvious how to avoid these calculations and leverage numerically stable and faster alternatives (e.g. linear system solvers, decompositions) without introducing approximations. Proposition 1 in the paper restates the result from Rimon and Boyd but removes the need to explicitly calculate matrix inversions and square roots. The distance calculation is highly efficient and parallelizable.

The gradient of distance is approximated using the direction vector to the nearest ellipsoid.

The collision probabilities of the K nearest surface ellipsoids from the robot ellipsoid are blended together to provide smooth estimates.

## Video

## Citation

```
@article{goel2024distance,
title={Distance and Collision Probability Estimation from Gaussian Surface Models},
author={Goel, Kshitij and Tabib, Wennie},
journal={arXiv preprint arXiv:2402.00186},
year={2024}
}
```