Collision-Affording Point Trees: SIMD-Amenable Nearest Neighbors for Fast Collision Checking

TL;DR

This paper introduces the collision-affording point tree (CAPT), a novel data structure that significantly accelerates collision checking in motion planning, enabling real-time robot path planning in complex environments.

Contribution

The paper presents CAPT, an exact point cloud representation that boosts collision query speed by an order of magnitude and introduces a point cloud filtering method for efficient planning.

Findings

Collision checks are accelerated by an order of magnitude.

Sampling-based planners achieve path planning in under a millisecond.

End-to-end planning runs faster than 60 FPS on a standard CPU.

Abstract

Motion planning against sensor data is often a critical bottleneck in real-time robot control. For sampling-based motion planners, which are effective for high-dimensional systems such as manipulators, the most time-intensive component is collision checking. We present a novel spatial data structure, the collision-affording point tree (CAPT): an exact representation of point clouds that accelerates collision-checking queries between robots and point clouds by an order of magnitude, with an average query time of less than 10 nanoseconds on 3D scenes comprising thousands of points. With the CAPT, sampling-based planners can generate valid, high-quality paths in under a millisecond, with total end-to-end computation time faster than 60 FPS, on a single thread of a consumer-grade CPU. We also present a point cloud filtering algorithm, based on space-filling curves, which reduces the number of points in a point cloud while preserving structure. Our approach enables robots to plan at real-time speeds in sensed environments, opening up potential uses of planning for high-dimensional systems in dynamic, changing, and unmodeled environments.