从2D栅格到3D距离场：用Matlab复现Fast Planner的ESDF构建过程-平芜编程栈

从2D栅格到3D距离场：用Matlab复现Fast Planner的ESDF构建过程

在机器人路径规划领域，欧几里得距离场（ESDF）作为环境表示的核心数据结构，直接影响着运动规划算法的性能与安全性。不同于传统的二值栅格地图仅标记障碍物位置，ESDF为每个自由空间体素提供了到最近障碍物的精确距离值，这种"距离感知"特性使得规划算法能够生成既安全又平滑的轨迹。本文将脱离复杂的C++工程环境，使用Matlab从零实现ESDF构建过程，通过可视化手段揭示距离场计算背后的数学原理。

1. 欧几里得距离变换（EDT）基础

欧几里得距离变换（Euclidean Distance Transform, EDT）是构建ESDF的核心算法，其本质是将二值图像中的每个像素映射到最近障碍物像素的几何距离。与城市街区距离（Manhattan）或棋盘距离（Chessboard）不同，EDT严格遵循欧几里得几何中的距离定义：

$$ D(p) = \min_{q \in O} \sqrt{(p_x-q_x)^2 + (p_y-q_y)^2} $$

其中$O$表示障碍物像素集合，$p$为待计算点。这种距离度量方式与机器人实际运动空间完全吻合，但计算复杂度显著高于其他距离度量方法。

Matlab基础验证：

% 创建5x5测试栅格 test_grid = zeros(5); test_grid(3,3) = 1; % 中心障碍物 % 计算EDT [D, idx] = bwdist(test_grid, 'euclidean'); disp('距离矩阵:'); disp(D) disp('最近障碍物索引矩阵:'); disp(idx)

执行结果将显示每个栅格到中心点的精确距离，以及对应的最近障碍物索引。这种直观验证是理解算法的基础步骤。

2. 二维EDT的分步实现

Fast Planner采用的EDT算法源自Felzenszwalb的经典论文，其核心思想是通过维度分解将二维问题转化为两次一维计算。我们通过7x10的示例栅格演示这个过程：

2.1 列方向距离计算

首先定义障碍物矩阵：

bw = zeros(7,10); obs_pos = [2,8; 4,5; 4,6; 6,1; 6,9; 7,10]; bw(sub2ind(size(bw), obs_pos(:,1), obs_pos(:,2))) = 1;

列计算伪算法步骤：

对每列独立扫描，标记障碍物位置
计算该列每个像素到垂直方向最近障碍物的平方距离
存储结果为中间距离场$D_{col}$

手动实现代码：

D_col = inf(size(bw)); % 初始化距离场 [rows, cols] = size(bw); for c = 1:cols % 找出当前列的障碍物行号 obs_rows = find(bw(:,c)); if isempty(obs_rows) continue; % 无障碍物列保持inf end % 计算每个行到最近障碍物的平方距离 for r = 1:rows [min_dist_sq, ~] = min((r - obs_rows).^2); D_col(r,c) = min_dist_sq; end end

2.2 行方向距离合成

将列计算结果$D_{col}$作为初始值，进行行方向计算。此时每个栅格的$f(q)$值即为$D_{col}(r,c)$：

D_final = inf(size(bw)); for r = 1:rows % 获取当前行的距离初始值 f_q = D_col(r,:); % 抛物线扫描算法实现 v = zeros(1, cols); z = zeros(1, cols+1); k = 1; v(1) = 1; z(1) = -inf; z(2) = inf; for q = 2:cols s = ((f_q(q) + q^2) - (f_q(v(k)) + v(k)^2)) / (2*q - 2*v(k)); while s <= z(k) k = k - 1; s = ((f_q(q) + q^2) - (f_q(v(k)) + v(k)^2)) / (2*q - 2*v(k)); end k = k + 1; v(k) = q; z(k) = s; z(k+1) = inf; end % 计算最终距离 k = 1; for q = 1:cols while z(k+1) < q k = k + 1; end D_final(r,q) = (q - v(k))^2 + f_q(v(k)); end end % 转换为真实距离 ESDF_2D = sqrt(D_final);

2.3 可视化对比分析

将手动实现结果与Matlab内置函数对比：

figure; subplot(1,3,1), imagesc(bw), title('原始障碍物'); subplot(1,3,2), imagesc(ESDF_2D), title('手动ESDF'); subplot(1,3,3), imagesc(bwdist(bw,'euclidean')), title('bwdist结果'); colorbar;

典型输出将显示两种实现方式得到的距离场在视觉上完全一致，验证了算法的正确性。细微差异可能来自边界条件处理或浮点精度限制。

3. 三维ESDF的扩展实现

将二维EDT扩展到三维空间需要增加一次维度计算。Fast Planner采用z→y→x的顺序进行计算，每个维度的输出作为下一维度的输入：

3.1 三维栅格初始化

创建包含障碍物的3D矩阵：

grid_3d = zeros(10,10,10); obs_3d = [3,5,2; 4,6,7; 7,2,9; 8,8,4]; grid_3d(sub2ind(size(grid_3d), obs_3d(:,1), obs_3d(:,2), obs_3d(:,3))) = 1;

3.2 分维度计算流程

三维EDT实现的关键在于维度间的数据传递：

% 第一维度(z轴)计算 D_z = inf(size(grid_3d)); for x = 1:size(grid_3d,2) for y = 1:size(grid_3d,1) obs_z = squeeze(find(grid_3d(y,x,:))); if ~isempty(obs_z) for z = 1:size(grid_3d,3) [min_dist, ~] = min((z - obs_z).^2); D_z(y,x,z) = min_dist; end end end end % 第二维度(y轴)计算 D_yz = inf(size(grid_3d)); for x = 1:size(grid_3d,2) for z = 1:size(grid_3d,3) f_q = squeeze(D_z(:,x,z)); % 抛物线扫描算法（同2D实现） % ... 省略具体实现代码 D_yz(:,x,z) = (1:length(f_q) - v(k)).^2 + f_q(v(k)); end end % 第三维度(x轴)计算 ESDF_3D = inf(size(grid_3d)); for y = 1:size(grid_3d,1) for z = 1:size(grid_3d,3) f_q = squeeze(D_yz(y,:,z)); % 抛物线扫描算法 % ... 省略具体实现代码 ESDF_3D(y,:,z) = sqrt((1:length(f_q) - v(k)).^2 + f_q(v(k))); end end

3.3 三维可视化技巧

使用等值面展示3D距离场：

figure; [x,y,z] = meshgrid(1:10,1:10,1:10); isosurface(x,y,z,ESDF_3D, max(ESDF_3D(:))/2); axis equal; xlabel('X'); ylabel('Y'); zlabel('Z'); title('3D ESDF等值面');

对于特定平面切片，可采用二维热力图形式：

figure; imagesc(squeeze(ESDF_3D(:,:,5))); % z=5平面 colorbar; title('Z=5平面距离场');

4. 算法优化与工程实践

4.1 计算效率对比

通过计时分析不同实现方式的性能差异：

% 测试数据准备 large_grid = rand(100,100) > 0.9; % 稀疏障碍物 % Matlab内置函数 tic; D_matlab = bwdist(large_grid, 'euclidean'); t_matlab = toc; % 手动实现 tic; % ... 二维EDT实现代码 t_manual = toc; fprintf('Matlab bwdist: %.4f秒\n手动实现: %.4f秒\n', t_matlab, t_manual);

典型结果显示内置函数通常有10-100倍的性能优势，这源于：

高度优化的底层实现
可能的并行计算
避免了解释型语言的循环开销

4.2 增量更新策略

实际应用中，ESDF需要动态更新。Fast Planner采用局部更新策略：

function updateESDF(global_map, changed_voxels) % 获取受影响区域边界 min_bound = min(changed_voxels) - 2; max_bound = max(changed_voxels) + 2; % 裁剪局部区域 local_region = global_map(min_bound(1):max_bound(1), ... min_bound(2):max_bound(2), ... min_bound(3):max_bound(3)); % 重新计算局部ESDF local_esdf = computeLocalESDF(local_region); % 合并回全局地图 global_map(min_bound(1):max_bound(1), ... min_bound(2):max_bound(2), ... min_bound(3):max_bound(3)) = local_esdf; end

4.3 数值稳定性处理

实际实现中需注意的特殊情况：

空障碍物集合处理
边界条件处理
浮点数比较容差
大距离值的数值溢出

改进后的抛物线交点计算：

function s = safeIntersection(q, r, fq, fr) denominator = 2*(r - q); if abs(denominator) < 1e-10 s = (q + r) / 2; % 避免除以零 else s = ((fr + r^2) - (fq + q^2)) / denominator; end end