SQ-CBF: Signed Distance Functions for Numerically Stable Superquadric-Based Safety Filtering

TL;DR

This work addresses safe real-time robot operation in cluttered environments by identifying ill-conditioned gradients when using implicit SQ functions as CBFs. It proposes an SDF-based CBF framework for SQs, computing distances with the GJK/EPA pipeline and obtaining robust gradients via randomized smoothing, preserving a consistent gradient magnitude $\ abla d$. The approach is integrated into a CBF-QP safety filter that jointly enforces collision avoidance and, optionally, manipulability constraints at high frequency. Extensive simulations and real-world experiments demonstrate collision-free performance in challenging geometries and noise, with notable improvements in teleoperation efficiency, highlighting a practical path to reliable safety filters in complex environments.

Abstract

Ensuring safe robot operation in cluttered and dynamic environments remains a fundamental challenge. While control barrier functions provide an effective framework for real-time safety filtering, their performance critically depends on the underlying geometric representation, which is often simplified, leading to either overly conservative behavior or insufficient collision coverage. Superquadrics offer an expressive way to model complex shapes using a few primitives and are increasingly used for robot safety. To integrate this representation into collision avoidance, most existing approaches directly use their implicit functions as barrier candidates. However, we identify a critical but overlooked issue in this practice: the gradients of the implicit SQ function can become severely ill-conditioned, potentially rendering the optimization infeasible and undermining reliable real-time safety filtering. To address this issue, we formulate an SQ-based safety filtering framework that uses signed distance functions as barrier candidates. Since analytical SDFs are unavailable for general SQs, we compute distances using the efficient Gilbert-Johnson-Keerthi algorithm and obtain gradients via randomized smoothing. Extensive simulation and real-world experiments demonstrate consistent collision-free manipulation in cluttered and unstructured scenes, showing robustness to challenging geometries, sensing noise, and dynamic disturbances, while improving task efficiency in teleoperation tasks. These results highlight a pathway toward safety filters that remain precise and reliable under the geometric complexity of real-world environments.

Figures (7)

• Figure 1: Overview of the proposed SQ-CBF safety filter. The framework represents both the robot (blue) and the obstacles (red) using compact superquadrics and enforces collision avoidance through an SDF-based CBF. SDFs are computed from superquadric geometries using GJK/EPA, while well-behaved distance gradients are estimated online and incorporated into the safety filter. Given an unverified control command (e.g., from teleoperation or a planner), the safety filter modifies the command online by solving a CBF-QP, intervening only when safety is at risk. A video demonstrating the safety filter's performance can be found here: http://tiny.cc/sq-cbf.
• Figure 2: Evaluating the implicit function $f_{\text{sq}}^*(x) = \min_{\bm{p} \in \mathbb{R}^3} f_{\text{sq}}(\bm{R}_z(\pi / 3)^\top (\bm{p} - \bar{\bm{p}}_1), \bm{s}_1) ~ \text{s.t.} ~f_{\text{sq}}(\bm{R}_z(-\pi / 4)^\top(\bm{p} - \bar{\bm{p}}_2(x)), \bm{s}_2) \leq 1$ from Dai2023RAL ($\bm{R}_z(\varphi)$ is a rotation around the $z$-axis by $\varphi$) and the SDF $d(x)$ and their gradients for two superquadrics with parameters $\bm{s }_1 = [0.5, 1.5, 1.0, 0.2, 0.2]^\top$ and $\bm{s }_2 = [1.0, 0.5, 1.0, 0.2, 0.2]^\top$ while varying their relative $x$ position with $\bar{\bm{p}}_1 - \bar{\bm{p}}_2(x) = [0, 0, 0]^\top - [x, 3, 0]^\top$. The implicit function and its gradient are large (greater than $10^4$), leading to ill-conditioned matrices in the CBF-QP. In practice, this may render the optimization infeasible or lead to oscillatory control behavior.
• Figure 3: Example of a real-world obstacle and its superquadric-based collision model used in our proposed safety filter framework.
• Figure 4: Mean cycle time (error bars represent three standard deviations) of the proposed SDF and gradient evaluation pipeline as a function of the number of CBF constraints under different CPU parallelization levels. Multi-core execution significantly improves scalability, enabling real-time ($100Hz$) velocity control with hundreds of collision constraints.
• Figure 5: Accuracy of the gradient estimation for different centroid distances $d_{\text{c}}$ and temperature values $\varepsilon$. The relative error (logarithmic scale) of the estimated gradient ($x$-component) is evaluated against ground-truth gradients for two representative SQ pairs: (a) sphere-sphere and (b) cube-cube. Results are reported for different relative orientations (face-face and vertex-vertex). The accuracy is sensitive to temperature $\varepsilon$ in all cases except face-face cubes. This highlights the importance of selecting an appropriate temperature $\varepsilon$ to achieve sufficiently accurate gradient estimates.