Implicit Swept Volume SDF: Enabling Continuous Collision-Free Trajectory Generation for Arbitrary Shapes

TL;DR

This work tackles the challenge of continuous collision-free trajectory generation for arbitrarily shaped objects in complex environments. It introduces an implicit Swept Volume Signed Distance Field ($SVSDF$) computed via a Generalized Semi-Infinite Programming ($GSIP$) formulation, eliminating the need for explicit surface reconstruction. A three-stage hierarchical planner—front-end asymmetric $SE(3)$ A*, mid-end MINCO-based initialization, and back-end SVSDF-driven optimization—achieves state-of-the-art continuous collision avoidance for both rigid and deformable shapes. Empirical results demonstrate accurate SVSDF queries inside the swept volume, robust obstacle gradients for optimization, and superior CCA performance compared with baselines, with code released to the community. The approach integrates graphics and robotics techniques to enable reliable, geometry-aware motion planning applicable to animation, CAD, and robotics workflows.

Abstract

In the field of trajectory generation for objects, ensuring continuous collision-free motion remains a huge challenge, especially for non-convex geometries and complex environments. Previous methods either oversimplify object shapes, which results in a sacrifice of feasible space or rely on discrete sampling, which suffers from the "tunnel effect". To address these limitations, we propose a novel hierarchical trajectory generation pipeline, which utilizes the Swept Volume Signed Distance Field (SVSDF) to guide trajectory optimization for Continuous Collision Avoidance (CCA). Our interdisciplinary approach, blending techniques from graphics and robotics, exhibits outstanding effectiveness in solving this problem. We formulate the computation of the SVSDF as a Generalized Semi-Infinite Programming model, and we solve for the numerical solutions at query points implicitly, thereby eliminating the need for explicit reconstruction of the surface. Our algorithm has been validated in a variety of complex scenarios and applies to robots of various dynamics, including both rigid and deformable shapes. It demonstrates exceptional universality and superior CCA performance compared to typical algorithms. The code will be released at https://github.com/ZJU-FAST-Lab/Implicit-SVSDF-Planner for the benefit of the community.

Figures (16)

• Figure 1: Our approach facilitates the generation of continuous, collision-free animated trajectories for the X-wing. The left figure shows the swept volume of the X-wing during flight. The four figures on the right show the trajectories before and after optimization from two different perspectives. The motions are visualized using swept volumes in the figure, providing an intuitive representation of continuous collision scenarios.
• Figure 2: An L-shaped object follows a path that combines translation and rotation, creating the light blue SV. Points $\bm{p}_A$ and $\bm{p}_B$ are outside the SV, marked by yellow circles indicating the smallest tangent circles with radii equal to the SVSDF values at those points. In contrast, points $\bm{p}_C$ and $\bm{p}_D$ are inside the SV, with their SVSDF values corresponding to the negative radii value of the centered green circles.
• Figure 3: The figure shows a simplified iterative method for computing the internal SVSDF in 2D using the GSIP, a process that is also applicable in 3D. In the discretized data, green points outside SV use gradient descent (as described in Section \ref{['acceleration strategy']}) to find their metric value $g$, which is the radius of the tangent circle. The largest $g^{*}$ among these values indicates the largest current constraint violation. The radius of the next iteration is reduced by $g^{*}$. Increasing the sampling density and iterations quickly yields accurate SDF values within SV.
• Figure 4: The hierarchical trajectory generation framework consists of three stages: the front-end, the mid-end, and the back-end. The front-end generates a discrete sequence of high-dimensional position-pose states, the mid-end facilitates the generation of initial values for the optimized trajectory, and the back-end uses the exact SVSDF to formulate a continuous collision-avoidance trajectory.
• Figure 5: Here is a 2D example for visual clarity: An L-shaped robot performs collision detection with the environment, where the pose dimension is discretized by the yaw.