Distance and Collision Probability Estimation from Gaussian Surface Models

Kshitij Goel and Wennie Tabib

IEEE International Conference on Robotics and Automation (ICRA), 2025 (Under Review)

teaser
This work contributes methods to estimate continuous-space collision probability, Euclidean distance and gradient of an ellipsoidal robot body model from a Gaussian surface model (GSM) of the surface. A 3D point cloud is approximated with a GMM, shown as a set of ellipsoids. The Euclidean distance over a 2D slice predicted by the proposed approach is shown as a heatmap (increasing distances from blue to red). The collision probability values (decreasing from red to black, 1.0 in white regions) over the same 2D slice when the robot position is uncertain.

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

figure_description
Illustration of various quantities involved in the proposed methods for Euclidean distance, gradient, and collision probability estimation.

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}
}