怕什么真理无穷，进一寸有一寸的欢喜。——读《胡适谈读书》
“FIESTA: Fast Incremental Euclidean Distance Fields for Online Motion Planning of Aerial Robots”由香港科技大学的Luxin Han、Fei Gao、Boyu Zhou和Shaojie Shen撰写。该论文提出了一种名为FIESTA的建图系统，用于增量式构建全局ESDF地图，以解决空中机器人在线运动规划中ESDF地图更新的瓶颈问题。
1. 研究背景
- 无人机自主系统的关键环节：对于完全自主的微型无人机（MAV），感知 - 规划 - 控制流程至关重要，其中建图模块为机载运动规划提供基础，是系统的关键组成部分。
- 现有建图方法的不足：尽管存在多种建图数据结构和算法，但对于自主四旋翼导航而言，多数方法存在问题。如Octomap和TSDF等框架在平衡深度测量融合精度与环境精细表示存储开销方面存在不足，且真正对规划有用的是自由空间信息而非障碍物信息，而像基于栅格地图的方法在实时性和准确性上难以同时满足要求。
- ESDF地图的重要性与更新瓶颈：ESDF地图能评估与障碍物的距离和梯度信息，对基于梯度的规划方法（如CHOMP）必不可少，但实时维护和更新ESDF地图的效率与准确性一直是瓶颈。例如，一些全局计算ESDF的方法在无人机平台上不实用，而仅维护局部地图的方法又无法用于全局或重复规划场景。Voxblox虽能增量构建ESDF地图，但存在计算距离不准确（基于准欧几里得距离更新）以及依赖TSDF映射导致误差（TSDF投影距离可能高估实际距离）等问题。
2. 系统框架 - 数据获取与融合：系统从立体、RGB - D传感器或单目深度估计获取深度测量数据，从GPS、Vicon等外部设备或内部视觉惯性里程计（VIO）获取位姿测量数据，然后通过光线投射将其整合到占用栅格地图中，整合过程中，占用状态改变的体素分别被添加到插入队列（insertQueue）和删除队列（deleteQueue）。
- ESDF地图更新：通过ESDF更新初始化过程将两个队列合并为更新队列（updateQueue），再基于广度优先搜索（BFS）的ESDF更新算法更新可能改变的体素，以最小计算开销全局更新ESDF地图。
3. 数据结构 - 占用栅格地图与体素信息结构：占用栅格地图存储体素占用概率，体素信息结构（VIS）存储体素的各种信息，包括位置、占用概率、到最近障碍物的欧几里得距离（ESDF）、最近障碍物体素坐标、是否被观测等，VIS还包含用于双向链表（DLLs）操作和获取邻居体素的方法。
- 索引数据结构：根据规划区域边界框是否已知及内存情况，选择数组或哈希表进行索引，将体素坐标映射到对应的VIS指针，以实现高效查询，时间复杂度平均为Θ(1)。例如，当边界框已知且内存充足时用数组，否则用哈希表管理块（block），通过优化可在一定程度上平衡性能和内存消耗。
- 双向链表：用于高效处理占用体素变为空闲的情况。每个体素的VIS中有一个dll方法，用于表示以该体素为最近障碍物的所有体素构成的双向链表，链表头指针由VIS的head成员获取。初始化时，所有体素的最近障碍物设为理想点（无穷远点IP），新观测到的体素加入IP的dll，借助索引数据结构，可在Θ(1)时间内完成双向链表的插入和删除操作。
4. 算法 - 占用整合算法：每次获取位姿和深度估计后，通过光线投射将新的占用信息整合到占用栅格地图，新观测到的体素插入IP的dll并标记为已观测，占用状态改变的体素分别加入插入队列或删除队列。
- 仅插入情况的ESDF更新算法：基于BFS，先假设新障碍物仅影响连续有界区域内体素的ESDF和最近障碍物，将插入队列中的体素经简单初始化后加入更新队列，然后用BFS更新邻居体素，更新方式为若邻居体素到当前体素最近障碍物的距离小于其自身ESDF值，则更新邻居体素的ESDF值、最近障碍物坐标，并在双向链表中进行相应操作，若更新则将邻居体素加入更新队列。
- 全动态情况的ESDF更新算法：对于插入队列的处理与仅插入情况类似，对于删除队列中的每个体素，遍历其dll中的所有体素，将状态重置为观测但未更新（最近障碍物设为IP，ESDF设为∞），然后尝试根据邻居体素的现有最近障碍物更新其ESDF，若更新则将该体素插入其最近障碍物的dll并加入更新队列，最后执行ESDF更新算法。
- 有限观测情况的ESDF更新算法：针对之前算法在有限观测下可能导致系统不一致的问题（如新观测体素未被之前体素更新），在ESDF更新算法中添加补丁代码，在更新邻居体素前检查是否应从其他邻居体素更新当前体素的ESDF，若更新则调整相关参数并将当前体素加入更新队列。
- 理论分析
- 最优性：算法通过体素与其邻居最近障碍物的最短欧几里得距离更新ESDF值，比准欧几里得方法更准确，但基于BFS的ESDF更新算法无论选择何种连通性都无法完全准确。以2D情况为例，由于用有限小圆圈覆盖大圆圈存在剩余空间，会导致算法计算距离与实际距离存在误差，不过考虑到最差情况较少且只有剩余空间内的整数点影响算法，实际均方根误差（RMS Error）可接受。
- 时间复杂度：ESDF更新初始化算法（Alg. 2）处理插入队列或其最近障碍物在删除队列中的体素，时间复杂度为Θ(k)（k为需处理的体素数量）；ESDF更新算法（Alg. 1）若用优先队列进行BFS，时间复杂度为Θ(n log n)（n为需更新的体素数量）。
- 空间复杂度：与基于哈希表的纯占用栅格地图相比，系统仅VIS有修改，空间复杂度仅比纯占用栅格地图高常数级别，通过选择block_size = 1，可使空间复杂度等于Θ(m)（m为所有观测到的体素数量），但效率较低，因此高度可定制的索引数据结构对实际系统很重要。
5. 实验结果 - 真实世界数据集实验
- 参数调优：对系统内的连通性和索引数据结构等参数进行测试，以找到准确性、性能和内存消耗的最佳权衡。例如，固定索引数据结构为block_size = 8的哈希表，测试不同连通性（6 - 、18 - 、26 - 、24 - 、32 - 连通性）对准确性和性能的影响，发现24 - 连通性在均方根误差和性能方面是较好选择；然后固定连通性为24，比较不同block_size（1、2、4、8、16）的数组和哈希表实现的索引数据结构，结果表明若地图边界未知，block_size = 8的哈希表在时间和空间复杂度上是较好选择，否则数组性能最佳。
- 与Voxblox比较：在不同体素大小下，使用Cow和Lady数据集（RGB - D相机）和EuRoC数据集（立体相机）比较FIESTA和Voxblox的性能和准确性，结果显示FIESTA在性能和准确性上均优于Voxblox一个数量级，同时给出了使用FIESTA系统（voxel_size = 0.05）运行Cow和Lady数据集时占用栅格地图和ESDF地图切片的可视化结果。
- 四旋翼运动规划模拟测试：通过模拟四旋翼飞行证明所提出的地图可用于运动规划，采用[8]中的运动规划方法，在模拟中随机生成地图、起始点和目标点，FIESTA系统增量构建全局ESDF地图，帮助运动规划算法高效运行，展示了样本结果。
- 自主四旋翼机载实验：在未知杂乱环境中进行机载实验，平台为配备Velodyne VLP - 16 3D激光雷达的四旋翼，用于位姿估计和深度测量，所有模块在双核3.00GHz Intel i7 - 5500U处理器上运行。尽管ESDF地图可在20ms内完成更新，但由于实验对飞行速度要求低，为节省资源，以20Hz的频率更新ESDF地图，给出了实验快照以及增量建图和规划结果。
