PaperlayerView exact revision on arXiv

Source-linked AI summary

FIESTA: Fast Incremental Euclidean Distance Fields for Online Motion Planning of Aerial Robots

Luxin Han, Fei Gao, Boyu Zhou, Shaojie Shen

arXiv:1903.02144v3cs.RO

TL;DR

Real-time aerial-robot planning needs accurate ESDF maps, but incrementally maintaining a global map is computationally demanding. FIESTA uses separate insertion and deletion queues with efficient voxel-maintenance structures and BFS updates to change few nodes. It reports order-of-magnitude gains over Voxblox in performance and accuracy, with simulation and onboard quadrotor validation.

  • Problem

    Incrementally maintaining an accurate global ESDF map is a bottleneck for real-time onboard motion planning, especially with limited sensing range and computing resources.

  • Method

    FIESTA incrementally updates a global ESDF using separate insertion and deletion queues, indexing structures, doubly linked lists, and a BFS-based algorithm.

  • Results

    FIESTA beats Voxblox by an order of magnitude in accuracy and time complexity using a hash table with block size = 8 and 24-Connectivity.

  • Takeaways & Limitations

    FIESTA provides a lightweight, flexible global ESDF mapping framework validated through simulation and onboard quadrotor experiments.

Abstract

from arXiv · show

Euclidean Signed Distance Field (ESDF) is useful for online motion planning of aerial robots since it can easily query the distance and gradient information against obstacles. Fast incrementally built ESDF map is the bottleneck for conducting real-time motion planning. In this paper, we investigate this problem and propose a mapping system called FIESTA to build global ESDF map incrementally. By introducing two independent updating queues for inserting and deleting obstacles separately, and using Indexing Data Structures and Doubly Linked Lists for map maintenance, our algorithm updates as few as possible nodes using a BFS framework. Our ESDF map has high computational performance and produces near-optimal results. We show our method outperforms other up-to-date methods in term of performance and accuracy by both theory and experiments. We integrate FIESTA into a completed quadrotor system and validate it by both simulation and onboard experiments. We release our method as open-source software for the community.

I. INTRODUCTION

Online quadrotor planning depends on accurate, rapidly updated ESDF maps, but global incremental maintenance remains computationally demanding. FIESTA addresses this bottleneck with a lightweight, flexible framework designed to update few nodes while supporting accurate global maps.

  • Motivation: ESDF maps provide obstacle-distance and gradient information needed by gradient-based quadrotor motion planning.These queries help planners push trajectories away from obstacles and improve path clearance.
  • Motivation: Global incremental ESDF updates are a bottleneck for real-time onboard planning under limited sensing range and computing resources.Local sliding maps can support replanning but discard past information needed for global or repeatable planning.
  • FIESTA: FIESTA builds ESDF maps incrementally from pose and depth measurements while supporting general mapping frameworks, including occupancy grids and TSDF maps.Its data structures provide a foundation for efficient updates and allow time–space complexity trade-offs.
  • FIESTA: FIESTA uses a BFS-based updating algorithm that expands as few nodes as possible and produces more accurate ESDF values.The framework introduces separate handling for inserting and deleting obstacles and analyzes time, space, accuracy, and optimality.
  • Validation and release: The method is integrated into a completed quadrotor system, demonstrated in onboard motion-planning experiments, and released as open-source software.The contributions include theoretical and practical analyses alongside system-level validation.

II. RELATED WORK

Prior work uses diverse maps and ESDF construction strategies for quadrotor planning, but incremental methods face accuracy and update limitations. FIESTA instead incrementally computes ESDF directly from an occupancy grid while retaining a BFS-based framework.

  • Mapping and planning frameworks: Quadrotor planners use grid maps, Octomaps, point clouds, convex safe-space clusters, and topological maps for collision checking, search, and trajectory generation.These approaches support different representations of free space and environmental structure.
  • ESDF computation: Batch Euclidean Distance Transform computation is efficient but repeated ESDF recalculation limits the combination of map accuracy and update frequency.This limitation affects motion-planning performance when ESDF values must be recalculated repeatedly.
  • Incremental ESDF methods: Voxblox incrementally builds ESDF maps from TSDF maps using BFS and supports real-time CPU operation with dynamically growing map sizes.Its ESDF values can deviate from actual Euclidean distances because of quasi-Euclidean expansion and TSDF projective-distance overestimation.
  • FIESTA: FIESTA separates obstacle insertions and deletions into updating queues, providing distinct starting points for BFS updates.This design makes the update process easier to analyze and optimize.

III. SYSTEM FRAMEWORK

FIESTA integrates sensor and pose measurements into an occupancy grid, tracks voxel information, and incrementally updates a global ESDF through coordinated data structures and queues.

  • FIESTA raycasts depth and pose measurements into an occupancy grid map, placing changed voxels in insertion and deletion queues.
  • FIESTA merges occupancy changes into an update queue and applies a BFS-based ESDF update to potentially changed voxels.
  • The Indexing Data Structure maps voxel coordinates to their corresponding Voxel Information Structures.
  • Voxel Information Structures store each voxel’s probabilistic occupancy information and additional update-related members.

AND ABBREVIATION IN PSEUDOCODE

FIESTA uses coordinate indexing and doubly linked lists to maintain voxel relationships efficiently, while supporting different memory–time trade-offs for map lookup.

  • Coordinate lookup uses either an array for known bounded regions with sufficient memory or a hash table when memory is limited or bounds are unknown.
  • Block-based indexing combines a hash table for blocks with per-block arrays, providing an intermediate memory–performance trade-off.
  • All supported indexing choices provide average Θ(1) lookup time, which matters for real-time operation on limited resources.
  • Doubly linked lists represent voxels sharing a closest obstacle and support efficient ESDF maintenance when occupied voxels become free.
  • With indexing and doubly linked lists, inserting or deleting a DLL node takes Θ(1).

V. ALGORITHMS

FIESTA integrates occupancy changes from depth data and updates the ESDF incrementally using separate change queues, initialization, and BFS propagation.

  • The ESDF value can be obtained from EDF values for the occupancy grid and its logical complement within twice the EDF calculation time.
  • Raycasting integrates aligned pose and depth measurements into the occupancy grid, and occupancy-state changes enter separate insertion and deletion queues.
  • Algorithm 1 repeatedly removes a voxel from the update queue and propagates a shorter Euclidean distance estimate to its neighbors.
  • The updating procedure maintains each voxel’s closest obstacle and reorganizes the corresponding doubly linked lists when a shorter estimate is found.

B. ESDF Updating Algorithm for Insert-only Case

FIESTA handles obstacle insertions and deletions through queue initialization, doubly linked-list maintenance, and BFS-based propagation under a bounded-influence assumption.

  • B. ESDF Updating Algorithm for Insert-only Case: 18 BFS updates neighbors after newly inserted obstacles are initialized, assuming each obstacle affects a continuous bounded region of voxels.
  • B. ESDF Updating Algorithm for Insert-only Case: The insert-only case leaves deleteQueue empty, adds insertQueue contents to updateQueue, and then runs the complete ESDF update algorithm.
  • B. ESDF Updating Algorithm for Insert-only Case: Insertions set each changed voxel as its own closest obstacle with distance 0, then push it into the update queue.
  • B. ESDF Updating Algorithm for Insert-only Case: Deletions iterate through voxels linked to the removed obstacle, clear their closest-obstacle assignments, and reset distances to ∞.
  • B. ESDF Updating Algorithm for Insert-only Case: After deletion, neighboring surviving obstacles provide replacement distances; unresolved voxels remain linked to the Ideal Point.

D. ESDF Updating Algorithm for Limited Observations

Limited observations can leave previously observed voxels inconsistent when updates propagate only from newly observed neighbors. A patch extends the update process to restore accurate whole-map ESDF values.

  • D. ESDF Updating Algorithm for Limited Observations: Limited observations can prevent existing voxels from updating through newly observed neighbors, producing inconsistent ESDF values.The example updates voxel (1) to 2 although its correct distance to the earlier obstacle at (0) is 1.
  • D. ESDF Updating Algorithm for Limited Observations: Algorithm 3 patches Algorithm 1 so the whole map remains accurate despite limited observations.The patch is inserted between Lines 3–4 of Algorithm 1.
  • D. ESDF Updating Algorithm for Limited Observations: For each current voxel, neighboring closest-obstacle distances can replace its current distance when smaller, updating obstacle ownership and linked-list membership.A changed voxel is pushed back into updateQueue for continued propagation.

E. Theoretical Analysis

The BFS update rule uses neighbor obstacle information to assign Euclidean distances, but finite connectivity cannot guarantee exact global accuracy. Its worst-case error decreases as obstacle distance increases and is acceptable in practice under the stated conditions.

  • 1) Optimality: FIESTA updates each voxel using the shortest Euclidean distance to closest obstacles associated with its neighbors.This produces Euclidean rather than quasi-Euclidean distance values.
  • 1) Optimality: Finite connectivity leaves remaining regions where the origin’s true obstacle distance is smaller than the BFS-computed value.The 4-connectivity example computes the origin’s distance as 5 while uncovered points inside the red circle are closer.
  • 1) Optimality: The illustrated worst case shrinks when a neighboring obstacle moves inward or outward relative to its corresponding circle.Either movement reduces the remaining space contributing to the error.
  • 1) Optimality: RMS Error is acceptable in practice because worst cases occur infrequently and only integer points in the remaining space affect the algorithm.The paper states that error decreases as the radius increases.
  • 2) Time Complexity: Using a FIFO queue gives Algorithm 2 time complexity Θ(k), while priority-queue BFS for Algorithm 1 gives Θ(n log n).k counts necessary voxels handled by Algorithm 2; n counts voxels updated by Algorithm 1.

3) Space Complexity:

FIESTA’s storage overhead is controlled through customizable indexing structures, with performance and memory trade-offs evaluated across connectivity and data-structure choices.

  • 3) Space Complexity: Using block size = 1 can make FIESTA’s space complexity Θ(m), where m is the number of ever-observed voxels.This choice is described as not highly efficient, motivating customizable indexing structures.
  • 3) Space Complexity: Connectivity and indexing choices expose a trade-off between computational performance, accuracy, and memory consumption.The system provides multiple indexing options to match real-system requirements.
  • 3) Space Complexity: The indexing-structure evaluation measures average updating time and memory consumption by the number of VIS.The experiment compares alternatives including hash tables and arrays across block sizes.

1) Parameters Tuning:

Parameter tuning evaluates connectivity and indexing choices using RMS Error, updating time, and VIS memory, then compares FIESTA with Voxblox across voxel sizes and datasets.

  • 1) Parameters Tuning: The parameter experiments update ESDF every 0.5s and measure performance by average updating time, RMS Error in voxels, and VIS count.Cow and Lady data from an RGB-D camera are used for tuning.
  • 1) Parameters Tuning: 24-connectivity is reported as one of the best choices for jointly balancing RMS Error and performance.The tested alternatives include 6-, 18-, 26-, 24-, and 32-connectivity.
  • 1) Parameters Tuning: With unknown map boundaries, a hash table with block size = 8 is one of the best choices for time and space; otherwise, the array has the best performance.This comparison fixes connectivity at 24.
  • 2) Comparison with Voxblox: FIESTA and Voxblox are compared across voxel sizes using Cow and Lady plus EuRoC datasets, with 24-connectivity and hash-table block size = 8.The reported comparison evaluates average updating time and RMS Error in voxels.
  • 2) Comparison with Voxblox: FIESTA outperforms Voxblox by an order of magnitude in both performance and accuracy in the reported comparison.A visualization uses voxel size = 0.05 on the Cow and Lady dataset.

B. Tests of Quadrotor Motion Planning in Simulation

FIESTA is evaluated in simulated and onboard quadrotor motion-planning experiments, where it incrementally builds a global ESDF map. The experiments validate efficient integration of this mapping system into quadrotor planning.

  • Simulation: FIESTA incrementally builds a global ESDF map from locally sensed obstacles to support efficient quadrotor motion planning in simulation.Simulation uses randomly generated maps and start/destination points, with sensing limited to obstacles within a radius around the MAV.
  • Onboard experiments: Onboard experiments run mapping, estimation, planning, and control on a quadrotor equipped with a Velodyne VLP-16 lidar.The onboard system uses a dual-core 3.00GHz Intel i7-5500U processor in unknown cluttered environments.
  • Onboard experiments: 20 ms update time supports a 20 Hz ESDF update rate during onboard experiments.The reported update time is below the experiment’s mapping update period.
  • Evaluation: FIESTA’s updating rule is reported to outperform Voxblox’s quasi-Euclidean rule, while experiments validate high-efficiency global ESDF construction.The paper reports theoretical and practical analysis of complexity, accuracy, and optimality, alongside simulation and onboard validation.
Loading 1903.02144v3…