[论文解读] FIESTA: Fast Incremental Euclidean Distance Fields for Online Motion Planning of Aerial Robots

Luxin Han, Fei Gao+2 more|arXiv (Cornell University)|Mar 6, 2019

Robotic Path Planning Algorithms被引用 6

一句话总结

FIESTA 提出了一种快速、增量式的欧几里得符号距离场（ESDF）映射系统，适用于空中机器人的实时运动规划。通过使用独立的插入/删除队列、优化的索引数据结构以及最小节点BFS更新策略，FIESTA 在保持近似最优ESDF精度的同时，实现了比以往方法（如Voxblox）快几个数量级的性能提升和更高的精度，从而在四旋翼无人机上实现了高效的机载规划。

ABSTRACT

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.

研究动机与目标

• 解决自主空中机器人在线运动规划中实时ESDF地图维护的瓶颈问题。
• 克服现有方法（如Voxblox）依赖TSDF所导致的准欧几里得近似误差和高计算成本的局限性。
• 开发一种轻量化、灵活且全局一致的ESDF映射框架，支持增量更新并保持极低的计算开销。
• 实现适用于资源受限四旋翼无人机机载部署的高精度、实时ESDF更新。
• 将映射系统集成到完整的自主四旋翼系统中，并通过仿真和真实世界实验进行验证。

提出的方法

• 引入两个独立的更新队列——一个用于障碍物插入，一个用于删除——以解耦并优化更新过程。
• 采用索引数据结构（如可配置块大小的哈希表）和双向链表，高效管理体素状态及更新传播。
• 设计一种基于BFS的更新算法，仅扩展必要的最小节点数，减少冗余计算，同时保持高精度。
• 使用24-连通性（面、边及两步面）在精度与性能之间取得平衡，相比低连通性方案显著降低误差。
• 将ESDF计算与任何特定底层地图类型（如TSDF或占用网格）解耦，支持与多种映射框架的兼容性。
• 通过利用空间局部性和BFS遍历中的早期终止机制优化更新过程，最小化冗余的距离重新计算。

实验结果

研究问题

• RQ1 如何在保持高精度的同时，以极低计算成本实现ESDF地图的增量更新，以支持实时运动规划？
• RQ2 何种数据结构与更新策略可最小化增量ESDF更新过程中冗余的节点扩展？
• RQ3 连通性选择（如6-、18-、24-、26-、32-连通性）如何影响精度与计算性能之间的权衡？
• RQ4 能否设计一种与底层地图表示形式（如TSDF或占用网格）无关的映射系统，同时仍保持高精度？
• RQ5 在真实环境与仿真环境中，所提出方法在速度与精度方面相较于SOTA系统（如Voxblox）的优越程度如何？

主要发现

• 与Voxblox相比，FIESTA在更新速度和精度上均实现了数量级的提升，测试数据集上的RMS误差最高降低了10倍。
• 使用块大小为8的哈希表在时间和空间复杂度之间提供了最佳平衡，优于基于数组的实现及其他块大小配置。
• 24-连通性配置在所有测试连通性中实现了精度（最低RMS误差）与计算成本的最佳权衡。
• 在双核3.00GHz Intel i7处理器上，FIESTA的全局ESDF地图更新时间低于20ms，支持20Hz的更新频率，适用于实时机载规划。
• 搭载VLP-16 LiDAR的四旋翼无人机机载实验表明，FIESTA能够在未知且杂乱的环境中实现可靠、实时的运动规划。
• 该系统已成功集成到完整的自主四旋翼飞行系统中，并通过仿真与真实飞行实验得到验证，展现出良好的鲁棒性与可扩展性。