本文是Fast Planner构建ESDF地图部分中距离场计算相关函数的说明。ESDF中距离场的计算过程其实就是计算出地图更新范围内每个空闲体素到附近障碍物体素的最小距离的过程。

Fast Planner采用了文献[1]中提出的方法，来进行ESDF地图中距离的计算。该方法通过多个1-D维度上的距离计算，最终得到3-D空间上的距离值。

对于1-D的情况，首先计算一系列抛物线的下包络集合，然后 q q q 点在下包络上对应的数值 f ( q ) f(q) f(q) 即为该点到最近障碍物的距离 D f ( p ) \mathcal{D}_f(p) Df​(p)。 D f ( p ) = min ⁡ q ∈ G ( ( p − q ) 2 + f ( q ) ) \mathcal{D}_f(p)=\min_{q \in \mathcal{G}}\left((p-q)^2+f(q)\right) Df​(p)=q∈Gmin​((p−q)2+f(q)) 其中， G \mathcal{G} G 表示一系列的一维栅格； q q q 表示这些一维栅格中是障碍物的部分； p p p 是需要计算最小障碍物距离的位置； f ( q ) f(q) f(q) 是一个偏置量，对于仅有1-D的情况 f ( q ) = 0 f(q)=0 f(q)=0，若存在多个维度 f ( q ) f(q) f(q) 的数值为上一维度计算出来的 D f ( p ) \mathcal{D}_f(p) Df​(p)。

在一系列抛物线构成的下包络中，用 k k k 表示用来构成下包络的抛物线个数； v [ k ] v[k] v[k] 表示第 k k k 个抛物线的顶点； z [ k ] z[k] z[k] 和 z [ k + 1 ] z[k+1] z[k+1] 表示第 k k k 个抛物线在整个下包络中的有效范围，其中 z [ k ] z[k] z[k] 表示的是第 k k k 个抛物线与第 k − 1 k-1 k−1 个抛物线的交点；

假设一顶点为 q q q 的新抛物线与原有下包络线最右侧抛物线 v [ k ] v[k] v[k] 的交点为 s s s，该交点的位置只存在两种可能：交点 s s s 在 z [ k ] z[k] z[k] 左边或在 z [ k ] z[k] z[k] 右面，如下图所示。需要注意的是，该情况下任意两个抛物线有且仅有一个交点 s s s ，且该交点的水平位置为 s = ( f ( r ) + r 2 ) − ( f ( q ) + q 2 ) 2 r − 2 q s=\frac{(f(r)+r^2)-(f(q)+q^2)}{2r-2q} s=2r−2q(f(r)+r2)−(f(q)+q2)​ 其中， q q q 和 r r r分别表示对应的两个抛物线顶点的水平位置。

（1）若交点 s s s 在 z [ k ] z[k] z[k] 右边，即 s > z [ k ] s > z[k] s>z[k]，则将抛物线 q q q 添加为下包络最右边的抛物线，有 k = k + 1 k = k+1 k=k+1， v [ k ] = q v[k] = q v[k]=q， z [ k ] = s z[k] = s z[k]=s， z [ k + 1 ] = + ∞ z[k+1] = +\infty z[k+1]=+∞；

（2）若交点 s s s 在 z [ k ] z[k] z[k] 左边，即 s < z [ k ] s < z[k] s 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 Eigen::Vector3i min_esdf = md_.local_bound_min_; Eigen::Vector3i max_esdf = md_.local_bound_max_; /* ========== compute positive DT ========== */ for (int x = min_esdf[0]; x fillESDF( [&](int z) { return md_.occupancy_buffer_inflate_[toAddress(x, y, z)] == 1 ? 0 : std::numeric_limits::max(); }, [&](int z, double val) { md_.tmp_buffer1_[toAddress(x, y, z)] = val; }, min_esdf[2], max_esdf[2], 2); } } for (int x = min_esdf[0]; x //md_.tmp_buffer1_是上一维度计算的结果，作为本维度计算的基础赋给 f(q) 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); } } for (int y = min_esdf[1]; y //md_.tmp_buffer2_是上一维度计算的结果，作为本维度计算的基础赋给 f(q) 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(mp_.resolution_ * std::sqrt(val), // md_.distance_buffer_[toAddress(x, y, z)]); }, min_esdf[0], max_esdf[0], 0); } } /* ========== compute negative distance ========== */ for (int x = min_esdf(0); x md_.occupancy_buffer_neg[idx] = 1; } else if (md_.occupancy_buffer_inflate_[idx] == 1) { md_.occupancy_buffer_neg[idx] = 0; } else { ROS_ERROR("what?"); } } ros::Time t1, t2; for (int x = min_esdf[0]; x fillESDF( [&](int z) { return md_.occupancy_buffer_neg[x * mp_.map_voxel_num_(1) * mp_.map_voxel_num_(2) + y * mp_.map_voxel_num_(2) + z] == 1 ? 0 : std::numeric_limits::max(); }, [&](int z, double val) { md_.tmp_buffer1_[toAddress(x, y, z)] = val; }, min_esdf[2], max_esdf[2], 2); } } for (int x = min_esdf[0]; x 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); } } for (int y = min_esdf[1]; y fillESDF([&](int x) { return md_.tmp_buffer2_[toAddress(x, y, z)]; }, [&](int x, double val) { md_.distance_buffer_neg_[toAddress(x, y, z)] = mp_.resolution_ * std::sqrt(val); }, min_esdf[0], max_esdf[0], 0); } } /* ========== combine pos and neg DT ========== */ for (int x = min_esdf(0); x