BFS-based ESDF Update for Aerial Robotics

• BFS-based ESDF Update Mechanism is a framework that incrementally updates a 3D grid of Euclidean distances to obstacles, enabling real-time collision avoidance in aerial robotics.
• It leverages efficient data structures like O(1) indexing and doubly linked lists combined with a BFS propagation strategy to minimize redundant computations.
• The mechanism achieves near-optimal performance using 24-neighbor connectivity and careful update scheduling, ensuring fast, reliable map maintenance under stringent CPU constraints.

A BFS-based ESDF (Euclidean Signed Distance Field) update mechanism is a framework for maintaining a discrete grid of Euclidean distances from free space to the nearest obstacles, incrementally and in real time, as new occupancy information arrives from sensors. This methodology is fundamental for online motion planning in aerial robotics, where planners require fast, near-optimal queries of distance and gradient to obstacles and must operate under stringent CPU constraints. The BFS-based approach, exemplified by the FIESTA system, minimizes redundant computation by updating only the affected voxels in response to dynamic map changes, leveraging efficient data structures and carefully designed propagation rules (Han et al., 2019).

1. Problem Definition and Motivation

A Euclidean Signed Distance Field assigns every voxel $x$ in a 3D grid the value $D(x)$, representing the Euclidean distance to the closest obstacle. In the context of micro aerial vehicle (MAV) planning, maintaining the ESDF incrementally is essential, since new sensor data can cause both insertions (free voxels becoming occupied) and deletions (occupied voxels becoming free). The primary challenge is to update only the minimal subset of voxels whose distances or closest-obstacle assignments have truly changed, ensuring the map's accuracy while conserving computation.

This requirement emerges from the real-time needs of onboard motion planning, where ESDF queries for gradients and distances are central for collision-free trajectory optimization. A plausible implication is that failure to incrementally update only the necessary voxels—resulting in unnecessary global recomputation—would render real-time replanning intractable for embedded platforms.

2. Data Structures for Efficient State Maintenance

The BFS-based ESDF update utilizes several specialized data structures:

• Indexing for O(1) Access: If the map bounding box and sufficient memory are available, a global 3D array of pointers to "Voxel Information Structures" (VIS) is maintained. For larger or unbounded maps, a hash-map of fixed-size blocks (e.g., $8 \times 8 \times 8$ voxels per block) is used, with voxel coordinates mapped to block indices using $O(1)$ average hash and intra-block contiguous arrays for cache efficiency.
• Doubly Linked Lists (DLL) of Dependents: Each VIS stores occupancy probability (occ), current ESDF distance (dis), coordinates of the closest-obstacle voxel (coc), and doubly-linked pointers. For every obstacle voxel $o$, the system maintains a DLL of all voxels with $coc = o$. This enables $O(1)$ reassignment or removal of dependents when $o$ is inserted or removed.

This hybrid arrangement allows trading memory for speed and supports efficient per-voxel operations, critical for high-frequency map updates (Han et al., 2019).

3. Update Scheduling: Queues and Initialization

Three distinct queues orchestrate the update workflow:

• insertQueue: Holds newly occupied voxels.
• deleteQueue: Contains newly freed voxels.
• updateQueue: Tracks voxels requiring ESDF recalculation.

The update sequence proceeds as follows:

• For every voxel in insertQueue, its distance is set to zero, its coc is set to its position, its old dependent-list membership is updated, and it is pushed to updateQueue.
• For every voxel in deleteQueue, all dependents in its DLL are:
  • Removed from their existing DLLs,
  • Reset to infinite distance and coc set to a "point at infinity" (IP),
  • Scanned over neighbors; if a neighbor has a valid coc and yields a closer path, the dependent inherits this,
  • If a valid new coc is found, reinserted into the corresponding DLL and pushed to updateQueue, else assigned to the DLL for IP.

This two-phase initialization ensures that only those voxels whose coc has truly changed are queued for recomputation, preventing unnecessary propagation.

4. BFS Relaxation and Distance Field Propagation

After initialization, the BFS-based update propagates ESDF changes as follows:

• For voxel $x$, the update rule is

$$D(x) = \min \left\{ D(y) + \|x - y\| \mid y \in N(x) \right\}$$

where $D(x)$0 denotes observed neighbors under a chosen connectivity (typically 24).

• Pseudocode for the BFS iteration: $8 \times 8 \times 8$3
• FIFO order is used for updateQueue, as experiments indicate this yields the best real-time throughput. A priority queue sorted by dis would reduce to $D(x)$1 complexity per update, but pure BFS is favored for efficiency.

Because only voxels whose distances may decrease are enqueued, and DLLs permit efficient dependent tracking, unnecessary updates are avoided (Han et al., 2019).

5. Complexity Analysis and Runtime Behavior

Empirical and theoretical analysis yields the following results:

Operation	Per-Voxel Cost	Overall Complexity
VIS coordinate index	$D(x)$2	—
ESDF update (BFS)	$D(x)$3	$D(x)$4 (BFS);
		$D(x)$5 (PQ)
DLL insert/delete	$D(x)$6	—

• Initialization requires time $D(x)$7, where $D(x)$8 is the sum of the number of voxels in insertQueue and the dependents of deleted obstacles.
• BFS propagation requires $D(x)$9 time, $8 \times 8 \times 8$0 being the number of voxels whose ESDF values actually change.

In typical aerial robot workloads, FIESTA updates tens to a few hundred voxels per sensor frame, attaining update times of approximately 10–20 ms at 20 Hz on a dual-core i7 processor.

Near-optimality is established as follows: using true Euclidean distances in every relaxation step, the dominant source of error is due to finite connectivity—empirically, using 24-connectivity produces RMS error below 0.5 voxel, which is markedly better than quasi-Euclidean BFS (Han et al., 2019).

6. Implementation Considerations for Real-Time Systems

Several implementation-specific guidelines enhance practical utility on aerial robot platforms:

• 24-neighbor connectivity is preferred, encompassing faces, edges, and two-step face neighbors, with a practical per-voxel neighbor count near 24, yielding a favorable trade-off between completeness and update cost.
• Block-hash indexing with block size 8 and bit-shifting for indexing achieves a balance between memory efficiency and fast access.
• All neighbor offsets and associated Euclidean distances are precomputed at startup for runtime efficiency.
• Single-threaded FIFO implementation for updateQueue avoids synchronization overhead.
• Storing VIS pointers in contiguous arrays within each block improves cache hit rates.
• Full ESDF map update can be delayed to a lower rate (e.g., 20 Hz), with occupancy integration processed at a higher frequency to prioritize planner CPU resources.

7. Relevance and Impact in Online Motion Planning

By integrating $8 \times 8 \times 8$1 grid lookup, $8 \times 8 \times 8$2 DLL operations, and BFS schedule with true Euclidean relaxation, the BFS-based update mechanism enables recurrent, near-globally optimal ESDF maintenance under demanding constraints. This supports high-frequency planning for collision avoidance and trajectory optimization in MAVs, validated both in theory and through simulation and onboard experiments. The method, realized in FIESTA and released as open-source software, establishes a practical algorithmic foundation for real-time robotic systems requiring fast, reliable proximity map maintenance (Han et al., 2019).