Robo-Centric ESDF for Collision Avoidance

• Robo-Centric ESDF is a representation that computes collision distances in the robot’s body frame, ensuring shape-fidelity-aware evaluation for any-shape mobile robots.
• It employs a grid-based Euclidean distance transform and lazy collision checking to dramatically reduce computational complexity and memory usage compared to traditional methods.
• RC-ESDF integrates analytic gradient computations for efficient trajectory optimization and control barrier function approaches in safety-critical robotic planning.

The Robo-Centric Euclidean Signed Distance Field (RC-ESDF) is a representation and computational mechanism for whole-body collision evaluation in robotic planning, in which an ESDF is constructed in the robot’s own body frame—rather than in the world or environment frame—to provide efficient, accurate, and shape-fidelity-aware collision checking for any-shape mobile robots, including those with non-convex geometries. By storing the signed distance to the robot’s boundary in a body-fixed grid and evaluating obstacle proximity by transforming workspace obstacles into the robot’s frame, the RC-ESDF supports “lazy” collision checking during trajectory optimization and real-time safety-critical control, while dramatically reducing both computational complexity and memory requirements compared to classical, environment-centric ESDF methods (Geng et al., 2023, Huang et al., 2023).

1. Definition and Mathematical Formulation

RC-ESDF operates on two coordinate frames: the robot’s body frame {B}, with origin at its center of rotation and static shape, and the world frame {W}, in which the robot moves and obstacles are represented. For any query point $q \in \mathbb{R}^2$ or $\mathbb{R}^3$, its body-frame coordinates relate to world-frame coordinates $x_W$ by $x_B = R(\psi)^T (x_W - p)$, with $R(\psi) \in SO(2)$ representing planar robot yaw and $p$ the robot’s position in {W}.

The RC-ESDF $d_B: \mathbb{R}^2 \rightarrow \mathbb{R}$ is defined as: $$d_B(q) = \begin{cases} - \|q - q_{\text{proj}}(q)\|_2 & \text{if } q \in \text{Robot body} \ 0 & \text{otherwise} \end{cases}$$ where $q_{\text{proj}}(q)$ is the closest point on the robot’s boundary to $q$. This definition ensures negative values inside the robot (indicating penetration by obstacles), and zero outside.

This signed distance is discretized on a regular 2D or 3D grid covering a tight axis-aligned bounding box of the robot in its body frame. For each grid vertex $v_{i,j}$ in 2D: $$v_{i,j} = \left(i \cdot r + x_{\min},\; j \cdot r + y_{\min}\right)^T$$ with grid resolution $r$. Signed distances are stored in an array $D[i, j]$ for query and interpolation.

2. Construction Pipeline and Data Structures

The RC-ESDF precomputation proceeds as follows (Geng et al., 2023):

• Mesh Acquisition: Extract a dense boundary set from the robot’s CAD or mesh model in {B}.
• Grid Generation: Compute the axis-aligned bounding box $[x_{\min}, x_{\max}] \times [y_{\min}, y_{\max}]$ and choose grid resolution $r$, yielding $M_x \times M_y$ grid cells (e.g., for a $1.2\,\text{m} \times 1.2\,\text{m}$ robot with $r = 0.05\,\text{m}$, $M \approx 25 \times 25$).
• Interior Labeling: Use a point-in-polygon or signed distance transform to mark cells as interior ($D[i,j]<0$) or exterior ($D[i,j]=0$).
• Euclidean Distance Transform: Apply an algorithm (Felzenszwalb & Huttenlocher, 2012) in $O(M_x M_y)$ to assign each interior cell the negative Euclidean distance to the boundary, and exterior cells to zero.
• Gradient Representation: Optionally precompute $\partial D/\partial x$, $\partial D/\partial y$ by central differences, or calculate gradients online using analytic differentiation of trilinear interpolation at query time.

The field is stored as a 2D (or 3D, for manipulators) contiguous array for rapid indexed and interpolated access.

3. Lazy Collision Evaluation

Traditionally, environment-centric ESDF approaches involve sampling the robot surface and looking up each sample in the environment’s ESDF, resulting in high computational overhead due to the large grid and numerous queries. RC-ESDF reverses this process (Geng et al., 2023):

1. Given robot pose $(p, \psi)$, compute the body-frame bounding box corners and transform to form the AABB in world space.
2. Retrieve only world obstacle points inside this AABB (via octree or voxel hash), yielding $M_d$ obstacle points.
3. For each obstacle point $q_{W,i}$, transform into body frame: $q_{B,i} = R(\psi)^T (q_{W,i} - p)$.
4. For each $q_{B,i}$, evaluate $d_i = d_B(q_{B,i})$ and gradient $g_i = \nabla d_B(q_{B,i})$.
5. The instantaneous collision penalty is

$F_c(p,\psi) = \left(\sum_{i=1}^{M_d} d_i\right)^2$

Only the obstacles invading the robot’s footprint are checked; all others can be ignored since their corresponding distances are by definition zero in the RC-ESDF field.

4. Analytical Gradient Computation

Analytic collision cost gradients with respect to robot position $p$ and yaw $\psi$ are critical for gradient-based optimization. The chain rule yields: $$\frac{\partial F_c}{\partial p} = 2S \sum_{i=1}^{M_d} \frac{\partial d_i}{\partial p}, \quad \frac{\partial F_c}{\partial \psi} = 2S \sum_{i=1}^{M_d} \frac{\partial d_i}{\partial \psi}$$ where $S = \sum_i d_i$.

For each $d_i = d_B(q_{B,i})$ with $q_{B,i} = R(\psi)^T (q_{W,i} - p)$:

• $\frac{\partial q_{B,i}}{\partial p} = -R(\psi)^T$
• $$\frac{\partial q_{B,i}}{\partial\psi} = \left( \frac{\partial R(\psi)^T}{\partial \psi} \right) (q_{W,i} - p)$$

Let $g_i = \nabla d_B(q_{B,i})$:

• $$\frac{\partial d_i}{\partial p} = - (R(\psi) g_i)^T$$
• $\frac{\partial d_i}{\partial \psi}$ involves matrix terms as above.

Closed-form expressions for these derivatives are provided for efficient back-propagation through the trajectory optimizer (Geng et al., 2023).

5. Integration into Planning and Control

Trajectory Optimization

RC-ESDF supports trajectory optimization frameworks by making its collision cost and gradients available in analytic form. A robot trajectory in SE(2) is parameterized by order-3 uniform B-splines for position and yaw, controlled by $N_c$ spline control points. The total trajectory cost $J$ consists of terms for smoothness, feasibility, and collision: $$J = \lambda_{\mathrm{ps}} J_{\mathrm{ps}} + \lambda_{\mathrm{pf}} J_{\mathrm{pf}} + \lambda_{\psi\mathrm{s}} J_{\psi\mathrm{s}} + \lambda_{\psi\mathrm{f}} J_{\psi\mathrm{f}} + \lambda_c J_c$$ where $J_c = \sum_s F_c(p(s), \psi(s))$. The collision and its gradients are evaluated at sample times along the spline, and the optimizer (e.g., L-BFGS with nonsmooth line search) adjusts control points for joint satisfaction of whole-body safety, dynamical limits, and smoothness (Geng et al., 2023).

Control Barrier Functions (CBF) and Quadratic Programming

RC-ESDF can be incorporated into safety-critical nominal control via time-varying Control Barrier Functions. For each obstacle discretized as points, $h_{i,j}$ is the signed distance from the obstacle point (in body frame) to the robot surface, as defined by the RC-ESDF. For general robot shapes, $h_{i,j}$ may be evaluated analytically or by finite-differences.

The barrier condition, enforcing $h_{i,j} \geq 0$ for all time, appears as an affine constraint in a quadratic program (QP) that also includes Control Lyapunov Function (CLF) constraints ensuring convergence to goal. This enables real-time, provably safe control for robots with complex, non-convex body shapes even in dynamic environments (Huang et al., 2023).

6. Computational Complexity and Runtime Analysis

The RC-ESDF scheme exhibits favorable computational properties:

Method	Build Complexity	Memory Usage	Per-Iteration Cost	Empirical Timings (per iter.)
RC-ESDF	$O(M_x M_y)$ (offline)	$O(M_x M_y)$ floats	$O(M_d)$ (few tens of points)	$<$0.02 s (build), 0.3 ms (online)
Env-ESDF	$O(M'_x M'_y)$ (online)	$O(M'_x M'_y)$	$O(N_{\text{samples}})$ (100–200)	10–25 ms per iter., 1–13 s opt.

For a $1.2\,\text{m} \times 1.2\,\text{m}$ robot at $r=0.05\,\text{m}$, the RC-ESDF fits in $\approx 2.5\,\text{KiB}$. Trajectory optimization completes in $0.1$–$0.5\,$s (RC-ESDF) vs. $1$–$13$ s (Env-ESDF), yielding $20$–$50\times$ speedup in collision evaluation and an order-of-magnitude faster optimization (Geng et al., 2023).

7. Applications and Limitations

RC-ESDF has demonstrated effectiveness in mobile robotic planning, whole-body collision avoidance, and real-time safety-critical control (Geng et al., 2023, Huang et al., 2023). It is directly applicable to any-shape robots, including non-convex and articulated bodies, and enables integration with both direct trajectory optimization and CBF-QP controllers.

In vision-guided manipulation, RC-ESDF can be constructed from learned volumetric density fields (e.g., from NeRF) by extracting object surfaces and precomputing a signed distance grid in the robot’s local frame. This enables planning and MPC in visually reconstructed tabletop scenes with high spatial accuracy (Tang et al., 2022).

Limitations include the static scene assumption for the precomputed RC-ESDF—dynamic changes in robot shape or configuration would require grid rebuilding—and the requirement for precise robot boundary modeling. For highly dynamic environments or manipulators, extensions with dynamic neural rendering or partial incremental grid update have been suggested (Tang et al., 2022).

• "Robo-centric ESDF: A Fast and Accurate Whole-body Collision Evaluation Tool for Any-shape Robotic Planning" (Geng et al., 2023)
• "Whole-body Dynamic Collision Avoidance with Time-varying Control Barrier Functions" (Huang et al., 2023)
• "RGB-Only Reconstruction of Tabletop Scenes for Collision-Free Manipulator Control" (Tang et al., 2022)