FOCI: Trajectory Optimization on Gaussian Splats

TL;DR

This paper introduces FOCI, a novel trajectory optimization algorithm on Gaussian Splats that enables fast, orientation-aware, collision-free path planning in complex 3D environments for legged robots.

Contribution

FOCI is the first method to optimize robot trajectories directly on Gaussian Splats using a new collision formulation, improving efficiency and environmental representation accuracy.

Findings

Collision-free trajectories computed in seconds.

Effective in both synthetic and real environments.

Supports orientation-aware planning through Gaussian representation.

Abstract

3D Gaussian Splatting (3DGS) has recently gained popularity as a faster alternative to Neural Radiance Fields (NeRFs) in 3D reconstruction and view synthesis methods. Leveraging the spatial information encoded in 3DGS, this work proposes FOCI (Field Overlap Collision Integral), an algorithm that is able to optimize trajectories directly on the Gaussians themselves. FOCI leverages a novel and interpretable collision formulation for 3DGS using the notion of the overlap integral between Gaussians. Contrary to other approaches, which represent the robot with conservative bounding boxes that underestimate the traversability of the environment, we propose to represent the environment and the robot as Gaussian Splats. This not only has desirable computational properties, but also allows for orientation-aware planning, allowing the robot to pass through very tight and narrow spaces. We extensively test our algorithm in both synthetic and real Gaussian Splats, showcasing that collision-free trajectories for the ANYmal legged robot that can be computed in a few seconds, even with hundreds of thousands of Gaussians making up the environment. The project page and code are available at https://rffr.leggedrobotics.com/works/foci/