Fast Planner的ESDF地图是怎么算出来的手把手拆解3D欧几里得距离转换EDT算法在机器人路径规划领域ESDF欧几里得符号距离场地图是许多先进规划器的核心组件。它不仅能告诉机器人哪里有障碍物还能精确量化离障碍物有多远。这种距离信息对于生成平滑、安全的轨迹至关重要。Fast Planner作为一款高效的规划算法其性能很大程度上依赖于ESDF地图的快速构建。本文将深入解析ESDF地图背后的3D欧几里得距离转换EDT算法带你从原理到实现全面理解这一关键技术。1. 欧几里得距离转换EDT基础概念EDT算法的核心任务是对于给定的三维空间中的每个点计算它到最近障碍物的欧几里得距离。想象一下在一个布满障碍物的房间里我们需要为每个空气分子计算它到最近墙壁或家具的距离——这就是EDT要解决的问题。传统暴力计算方法简单直接对于空间中的每个点遍历所有障碍物计算距离并取最小值。这种方法的时间复杂度高达O(NM)其中N是空间点数M是障碍物数量在3D场景中完全不可行。Fast Planner采用的是一种更聪明的策略维度分解法。它将复杂的3D问题分解为三个连续的1D计算通过巧妙的数学变换将时间复杂度降低到O(N)。这种方法源自Felzenszwalb和Huttenlocher在2012年提出的经典算法。关键突破点在于发现高维EDT可以通过低维EDT的组合来实现。具体来说先在z轴方向计算1D EDT然后在y轴方向计算同时利用z轴的结果最后在x轴方向计算利用前两步的中间结果 这种分而治之的策略是算法高效的关键。2. 1D情况下的EDT算法解析理解3D EDT的基础是彻底掌握1D情况。让我们从一个简单的1D场景开始假设有一条直线上面分布着若干障碍点我们需要计算直线上每一点到最近障碍点的距离。2.1 抛物线包络法算法使用了一种称为抛物线包络的几何方法。对于每个障碍点q我们可以构造一条抛物线f_q(p) (p-q)² f(q)其中p是直线上任意一点q是障碍点的位置f(q)是初始值对于纯1D情况通常为0核心观察对于给定的p点我们只需要考虑所有抛物线在p点的最小值这个最小值就是p点到最近障碍物的平方距离。2.2 算法实现步骤Fast Planner中的fillESDF函数实现了这一过程。让我们拆解它的工作原理初始化准备两个数组v和zv[k]存储第k个抛物线的顶点位置z[k]存储第k个和第(k-1)个抛物线的交点位置处理新障碍点对于每个新障碍点q计算它与当前最右侧抛物线的交点s如果s z[k]将q对应的抛物线加入包络否则移除当前最右侧抛物线重复比较过程计算最终距离对于每个查询点p找到包含p的抛物线区间使用对应的抛物线计算最小距离template typename F_get_val, typename F_set_val void SDFMap::fillESDF(F_get_val f_get_val, F_set_val f_set_val, int start, int end, int dim) { int v[mp_.map_voxel_num_(dim)]; double z[mp_.map_voxel_num_(dim) 1]; int k start; v[start] start; z[start] -std::numeric_limitsdouble::max(); z[start 1] std::numeric_limitsdouble::max(); for (int q start 1; q end; q) { k; double s; do { k--; s ((f_get_val(q) q * q) - (f_get_val(v[k]) v[k] * v[k])) / (2 * q - 2 * v[k]); } while (s z[k]); k; v[k] q; z[k] s; z[k 1] std::numeric_limitsdouble::max(); } k start; for (int q start; q end; q) { while (z[k 1] q) k; double val (q - v[k]) * (q - v[k]) f_get_val(v[k]); f_set_val(q, val); } }注意这段代码中的f_get_val和f_set_val是函数对象分别用于获取和设置距离值这种设计提高了代码的复用性。3. 从1D到3D维度扩展策略1D算法虽然巧妙但真实世界是三维的。Fast Planner采用了一种优雅的维度扩展方法将1D算法推广到3D空间。3.1 维度传递原理关键思路是将前一维度的计算结果作为下一维度的初始值。具体来说首先在z维度计算1D EDT结果存储在临时缓冲区然后在y维度计算但此时f(q)的值取自z维度的结果最后在x维度计算f(q)的值取自y维度的结果这种顺序(z→y→x)不是随意的而是经过精心设计的。因为每个后续维度的计算都依赖于前一步的结果顺序选择会影响计算效率和准确性。3.2 3D实现代码解析Fast Planner中的updateESDF3d函数实现了完整的3D EDT计算void SDFMap::updateESDF3d() { Eigen::Vector3i min_esdf md_.local_bound_min_; Eigen::Vector3i max_esdf md_.local_bound_max_; // 计算正向距离场 // 第一遍z维度 for (int x min_esdf[0]; x max_esdf[0]; x) { for (int y min_esdf[1]; y max_esdf[1]; y) { fillESDF( [](int z) { return md_.occupancy_buffer_inflate_[toAddress(x,y,z)] 1 ? 0 : std::numeric_limitsdouble::max(); }, [](int z, double val) { md_.tmp_buffer1_[toAddress(x,y,z)] val; }, min_esdf[2], max_esdf[2], 2); } } // 第二遍y维度使用z维度的结果 for (int x min_esdf[0]; x max_esdf[0]; x) { for (int z min_esdf[2]; z max_esdf[2]; z) { fillESDF( [](int y) { return md_.tmp_buffer1_[toAddress(x,y,z)]; }, [](int y, double val) { md_.tmp_buffer2_[toAddress(x,y,z)] val; }, min_esdf[1], max_esdf[1], 1); } } // 第三遍x维度使用y维度的结果 for (int y min_esdf[1]; y max_esdf[1]; y) { for (int z min_esdf[2]; z max_esdf[2]; z) { fillESDF( [](int x) { return md_.tmp_buffer2_[toAddress(x,y,z)]; }, [](int x, double val) { md_.distance_buffer_[toAddress(x,y,z)] mp_.resolution_ * std::sqrt(val); }, min_esdf[0], max_esdf[0], 0); } } // 类似的过程也用于计算负向距离场障碍物内部的距离 // ... // 最后合并正向和负向距离场 // ... }缓冲区使用策略tmp_buffer1_存储z维度的中间结果tmp_buffer2_存储y维度的中间结果distance_buffer_存储最终x维度的结果4. 算法优化与实现细节在实际应用中EDT算法的实现需要考虑许多工程细节这些优化使得Fast Planner能够高效运行。4.1 增量更新策略完整的3D EDT计算量很大Fast Planner采用了增量更新策略局部更新只重新计算地图中发生变化的区域边界传播变化的影响会传播到周围一定范围双缓冲技术避免在计算过程中破坏原始数据这些优化使得算法复杂度与实际变化量成正比而非与整个地图大小成正比。4.2 数值处理技巧在实现过程中有几个数值处理的细节值得注意无穷大表示使用std::numeric_limitsdouble::max()表示无障碍物的情况平方距离算法内部计算的是平方距离最后一步才开平方避免不必要的开方运算分辨率缩放最终结果要乘以地图分辨率将栅格单位转换为实际距离单位4.3 并行计算潜力虽然Fast Planner的实现是单线程的但EDT算法本身具有很好的并行潜力维度内并行同一维度不同切片如不同x-y平面的计算可以并行向量化使用SIMD指令加速核心计算部分GPU加速整个算法非常适合在GPU上实现这些优化方向为性能进一步提升留下了空间。5. 可视化理解与调试技巧理解EDT算法的好方法是通过可视化。下面介绍几种常用的可视化手段。5.1 2D案例逐步可视化考虑一个简单的7×10二维栅格地图其中黑色表示障碍物(0,0) (0,9) --------- | # | # at (2,8) | | | ## | ## at (4,5) and (4,6) | | |# # | # at (6,1) and (6,9) | #| # at (7,10) ---------第一步列方向计算每列独立计算1D EDT结果如下表显示平方距离列距离值序列1[∞,∞,∞,∞,∞,0,∞]......5[∞,∞,∞,0,∞,∞,∞]6[∞,∞,∞,0,∞,∞,∞]......9[∞,0,∞,∞,∞,0,∞]10[∞,∞,∞,∞,∞,∞,0]第二步行方向计算以第6行为例包含障碍物在(6,1)和(6,9)在x1和x9处有障碍物f(q)0其他点的f(q)值取自列方向计算结果最终得到的距离场可以用Matlab简单可视化% 生成2D栅格矩阵 bw zeros(7,10); obs [2 8; 4 5; 4 6; 6 1; 6 9; 7 10]; idx sub2ind(size(bw), obs(:,1), obs(:,2)); bw(idx) 1; % 计算距离场 D1 bwdist(bw,euclidean); % 可视化 figure imagesc(D1), title(Euclidean Distance Transform) hold on, contour(D1,w) % 添加等高线 colorbar5.2 调试技巧在实现EDT算法时以下几个调试技巧很有帮助单元测试先从简单的1D案例开始验证中间输出保存和检查每个维度的中间结果可视化对比与已知正确的实现如Matlab的bwdist进行对比边界检查特别注意地图边界的处理是否正确6. 性能分析与优化方向理解EDT算法的性能特征对于实际应用至关重要。6.1 时间复杂度分析对于3D网格地图设尺寸为N×N×N方法时间复杂度适用性暴力法O(N⁶)完全不实用维度分解法O(N³)Fast Planner采用并行实现O(N²)理论最佳实际测量表明Fast Planner的实现可以在几毫秒内完成中等规模(100×100×20)地图的增量更新。6.2 内存使用分析EDT算法需要额外的临时缓冲区缓冲区大小用途tmp_buffer1_N³存储第一维度结果tmp_buffer2_N³存储第二维度结果distance_buffer_N³最终结果对于高分辨率地图内存消耗可能成为瓶颈。可以考虑使用更紧凑的数据类型分块处理大型地图按需计算的策略6.3 未来优化方向多分辨率表示在不同区域使用不同精度稀疏数据结构对空旷区域采用特殊表示近似算法在保证安全的前提下允许一定误差硬件加速利用现代CPU/GPU的并行能力
Fast Planner的ESDF地图是怎么算出来的？手把手拆解3D欧几里得距离转换（EDT）算法
Fast Planner的ESDF地图是怎么算出来的手把手拆解3D欧几里得距离转换EDT算法在机器人路径规划领域ESDF欧几里得符号距离场地图是许多先进规划器的核心组件。它不仅能告诉机器人哪里有障碍物还能精确量化离障碍物有多远。这种距离信息对于生成平滑、安全的轨迹至关重要。Fast Planner作为一款高效的规划算法其性能很大程度上依赖于ESDF地图的快速构建。本文将深入解析ESDF地图背后的3D欧几里得距离转换EDT算法带你从原理到实现全面理解这一关键技术。1. 欧几里得距离转换EDT基础概念EDT算法的核心任务是对于给定的三维空间中的每个点计算它到最近障碍物的欧几里得距离。想象一下在一个布满障碍物的房间里我们需要为每个空气分子计算它到最近墙壁或家具的距离——这就是EDT要解决的问题。传统暴力计算方法简单直接对于空间中的每个点遍历所有障碍物计算距离并取最小值。这种方法的时间复杂度高达O(NM)其中N是空间点数M是障碍物数量在3D场景中完全不可行。Fast Planner采用的是一种更聪明的策略维度分解法。它将复杂的3D问题分解为三个连续的1D计算通过巧妙的数学变换将时间复杂度降低到O(N)。这种方法源自Felzenszwalb和Huttenlocher在2012年提出的经典算法。关键突破点在于发现高维EDT可以通过低维EDT的组合来实现。具体来说先在z轴方向计算1D EDT然后在y轴方向计算同时利用z轴的结果最后在x轴方向计算利用前两步的中间结果 这种分而治之的策略是算法高效的关键。2. 1D情况下的EDT算法解析理解3D EDT的基础是彻底掌握1D情况。让我们从一个简单的1D场景开始假设有一条直线上面分布着若干障碍点我们需要计算直线上每一点到最近障碍点的距离。2.1 抛物线包络法算法使用了一种称为抛物线包络的几何方法。对于每个障碍点q我们可以构造一条抛物线f_q(p) (p-q)² f(q)其中p是直线上任意一点q是障碍点的位置f(q)是初始值对于纯1D情况通常为0核心观察对于给定的p点我们只需要考虑所有抛物线在p点的最小值这个最小值就是p点到最近障碍物的平方距离。2.2 算法实现步骤Fast Planner中的fillESDF函数实现了这一过程。让我们拆解它的工作原理初始化准备两个数组v和zv[k]存储第k个抛物线的顶点位置z[k]存储第k个和第(k-1)个抛物线的交点位置处理新障碍点对于每个新障碍点q计算它与当前最右侧抛物线的交点s如果s z[k]将q对应的抛物线加入包络否则移除当前最右侧抛物线重复比较过程计算最终距离对于每个查询点p找到包含p的抛物线区间使用对应的抛物线计算最小距离template typename F_get_val, typename F_set_val void SDFMap::fillESDF(F_get_val f_get_val, F_set_val f_set_val, int start, int end, int dim) { int v[mp_.map_voxel_num_(dim)]; double z[mp_.map_voxel_num_(dim) 1]; int k start; v[start] start; z[start] -std::numeric_limitsdouble::max(); z[start 1] std::numeric_limitsdouble::max(); for (int q start 1; q end; q) { k; double s; do { k--; s ((f_get_val(q) q * q) - (f_get_val(v[k]) v[k] * v[k])) / (2 * q - 2 * v[k]); } while (s z[k]); k; v[k] q; z[k] s; z[k 1] std::numeric_limitsdouble::max(); } k start; for (int q start; q end; q) { while (z[k 1] q) k; double val (q - v[k]) * (q - v[k]) f_get_val(v[k]); f_set_val(q, val); } }注意这段代码中的f_get_val和f_set_val是函数对象分别用于获取和设置距离值这种设计提高了代码的复用性。3. 从1D到3D维度扩展策略1D算法虽然巧妙但真实世界是三维的。Fast Planner采用了一种优雅的维度扩展方法将1D算法推广到3D空间。3.1 维度传递原理关键思路是将前一维度的计算结果作为下一维度的初始值。具体来说首先在z维度计算1D EDT结果存储在临时缓冲区然后在y维度计算但此时f(q)的值取自z维度的结果最后在x维度计算f(q)的值取自y维度的结果这种顺序(z→y→x)不是随意的而是经过精心设计的。因为每个后续维度的计算都依赖于前一步的结果顺序选择会影响计算效率和准确性。3.2 3D实现代码解析Fast Planner中的updateESDF3d函数实现了完整的3D EDT计算void SDFMap::updateESDF3d() { Eigen::Vector3i min_esdf md_.local_bound_min_; Eigen::Vector3i max_esdf md_.local_bound_max_; // 计算正向距离场 // 第一遍z维度 for (int x min_esdf[0]; x max_esdf[0]; x) { for (int y min_esdf[1]; y max_esdf[1]; y) { fillESDF( [](int z) { return md_.occupancy_buffer_inflate_[toAddress(x,y,z)] 1 ? 0 : std::numeric_limitsdouble::max(); }, [](int z, double val) { md_.tmp_buffer1_[toAddress(x,y,z)] val; }, min_esdf[2], max_esdf[2], 2); } } // 第二遍y维度使用z维度的结果 for (int x min_esdf[0]; x max_esdf[0]; x) { for (int z min_esdf[2]; z max_esdf[2]; z) { fillESDF( [](int y) { return md_.tmp_buffer1_[toAddress(x,y,z)]; }, [](int y, double val) { md_.tmp_buffer2_[toAddress(x,y,z)] val; }, min_esdf[1], max_esdf[1], 1); } } // 第三遍x维度使用y维度的结果 for (int y min_esdf[1]; y max_esdf[1]; y) { for (int z min_esdf[2]; z max_esdf[2]; z) { fillESDF( [](int x) { return md_.tmp_buffer2_[toAddress(x,y,z)]; }, [](int x, double val) { md_.distance_buffer_[toAddress(x,y,z)] mp_.resolution_ * std::sqrt(val); }, min_esdf[0], max_esdf[0], 0); } } // 类似的过程也用于计算负向距离场障碍物内部的距离 // ... // 最后合并正向和负向距离场 // ... }缓冲区使用策略tmp_buffer1_存储z维度的中间结果tmp_buffer2_存储y维度的中间结果distance_buffer_存储最终x维度的结果4. 算法优化与实现细节在实际应用中EDT算法的实现需要考虑许多工程细节这些优化使得Fast Planner能够高效运行。4.1 增量更新策略完整的3D EDT计算量很大Fast Planner采用了增量更新策略局部更新只重新计算地图中发生变化的区域边界传播变化的影响会传播到周围一定范围双缓冲技术避免在计算过程中破坏原始数据这些优化使得算法复杂度与实际变化量成正比而非与整个地图大小成正比。4.2 数值处理技巧在实现过程中有几个数值处理的细节值得注意无穷大表示使用std::numeric_limitsdouble::max()表示无障碍物的情况平方距离算法内部计算的是平方距离最后一步才开平方避免不必要的开方运算分辨率缩放最终结果要乘以地图分辨率将栅格单位转换为实际距离单位4.3 并行计算潜力虽然Fast Planner的实现是单线程的但EDT算法本身具有很好的并行潜力维度内并行同一维度不同切片如不同x-y平面的计算可以并行向量化使用SIMD指令加速核心计算部分GPU加速整个算法非常适合在GPU上实现这些优化方向为性能进一步提升留下了空间。5. 可视化理解与调试技巧理解EDT算法的好方法是通过可视化。下面介绍几种常用的可视化手段。5.1 2D案例逐步可视化考虑一个简单的7×10二维栅格地图其中黑色表示障碍物(0,0) (0,9) --------- | # | # at (2,8) | | | ## | ## at (4,5) and (4,6) | | |# # | # at (6,1) and (6,9) | #| # at (7,10) ---------第一步列方向计算每列独立计算1D EDT结果如下表显示平方距离列距离值序列1[∞,∞,∞,∞,∞,0,∞]......5[∞,∞,∞,0,∞,∞,∞]6[∞,∞,∞,0,∞,∞,∞]......9[∞,0,∞,∞,∞,0,∞]10[∞,∞,∞,∞,∞,∞,0]第二步行方向计算以第6行为例包含障碍物在(6,1)和(6,9)在x1和x9处有障碍物f(q)0其他点的f(q)值取自列方向计算结果最终得到的距离场可以用Matlab简单可视化% 生成2D栅格矩阵 bw zeros(7,10); obs [2 8; 4 5; 4 6; 6 1; 6 9; 7 10]; idx sub2ind(size(bw), obs(:,1), obs(:,2)); bw(idx) 1; % 计算距离场 D1 bwdist(bw,euclidean); % 可视化 figure imagesc(D1), title(Euclidean Distance Transform) hold on, contour(D1,w) % 添加等高线 colorbar5.2 调试技巧在实现EDT算法时以下几个调试技巧很有帮助单元测试先从简单的1D案例开始验证中间输出保存和检查每个维度的中间结果可视化对比与已知正确的实现如Matlab的bwdist进行对比边界检查特别注意地图边界的处理是否正确6. 性能分析与优化方向理解EDT算法的性能特征对于实际应用至关重要。6.1 时间复杂度分析对于3D网格地图设尺寸为N×N×N方法时间复杂度适用性暴力法O(N⁶)完全不实用维度分解法O(N³)Fast Planner采用并行实现O(N²)理论最佳实际测量表明Fast Planner的实现可以在几毫秒内完成中等规模(100×100×20)地图的增量更新。6.2 内存使用分析EDT算法需要额外的临时缓冲区缓冲区大小用途tmp_buffer1_N³存储第一维度结果tmp_buffer2_N³存储第二维度结果distance_buffer_N³最终结果对于高分辨率地图内存消耗可能成为瓶颈。可以考虑使用更紧凑的数据类型分块处理大型地图按需计算的策略6.3 未来优化方向多分辨率表示在不同区域使用不同精度稀疏数据结构对空旷区域采用特殊表示近似算法在保证安全的前提下允许一定误差硬件加速利用现代CPU/GPU的并行能力
