本文是Fast Planner构建ESDF地图部分中距离场计算相关函数的说明。ESDF中距离场的计算过程其实就是计算出地图更新范围内每个空闲体素到附近障碍物体素的最小距离的过程。
Fast Planner采用了文献[1]中提出的方法，来进行ESDF地图中距离的计算。该方法通过多个1-D维度上的距离计算，最终得到3-D空间上的距离值。
1. 算法原理
对于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)
ight)
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<z[k]，则将下包络原有的抛物线
v
[
k
]
v[k]
v[k] 删除，并有
k
=
k
?
1
k = k-1
k=k?1，然后重新计算抛物线
q
q
q 与下包络此时最右侧的抛物线
v
[
k
]
v[k]
v[k] 的交点
s
s
s，并重复上述过程，直到交点
s
s
s 在
z
[
k
]
z[k]
z[k] 右边。
上述计算1-D情况下，距离计算的算法伪代码如下图所示。
2. 代码实现
伪代码对应的C++代码如下所示。
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_limits<double>::max();
z[start + 1] = std::numeric_limits<double>::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_limits<double>::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);
}
}
- 1
- 2
- 3
- 4
- 5
- 6
- 7
- 8
- 9
- 10
- 11
- 12
- 13
- 14
- 15
- 16
- 17
- 18
- 19
- 20
- 21
- 22
- 23
- 24
- 25
- 26
- 27
- 28
- 29
- 30
- 31
- 32
- 33
- 34
- 35
上述算法仅对多维空间的一维进行了计算，在实际应用中需要计算3D空间中的距离。在Fast Planner中，通过将上述算法分别在3个维度中计算一次，并将上一个维度的计算出的数值
D
f
(
p
)
mathcal{D}_f(p)
Df?(p) 赋值给下一个维度计算时的
f
(
q
)
f(q)
f(q)，经过3次遍历以后就实现了 3D 空间中空闲体素到最近障碍物物栅格距离的计算。该计算过程详见函数
z
→
y
→
x
z o y o x
z→y→x）。
void SDFMap::updateESDF3d() {
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 <= 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_limits<double>::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 <= max_esdf[0]; x++) {
for (int z = min_esdf[2]; z <= max_esdf[2]; z++) {
//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 <= max_esdf[1]; y++) {
for (int z = min_esdf[2]; z <= max_esdf[2]; z++) {
//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 <= max_esdf(0); ++x)
for (int y = min_esdf(1); y <= max_esdf(1); ++y)
for (int z = min_esdf(2); z <= max_esdf(2); ++z) {
int idx = toAddress(x, y, z);
if (md_.occupancy_buffer_inflate_[idx] == 0) {
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 <= max_esdf[0]; x++) {
for (int y = min_esdf[1]; y <= max_esdf[1]; y++) {
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<double>::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 <= 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);
}
}
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_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 <= max_esdf(0); ++x)
for (int y = min_esdf(1); y <= max_esdf(1); ++y)
for (int z = min_esdf(2); z <= max_esdf(2); ++z) {
int idx = toAddress(x, y, z);
md_.distance_buffer_all_[idx] = md_.distance_buffer_[idx];
if (md_.distance_buffer_neg_[idx] > 0.0)
md_.distance_buffer_all_[idx] += (-md_.distance_buffer_neg_[idx] + mp_.resolution_);
}
}
- 1
- 2
- 3
- 4
- 5
- 6
- 7
- 8
- 9
- 10
- 11
- 12
- 13
- 14
- 15
- 16
- 17
- 18
- 19
- 20
- 21
- 22
- 23
- 24
- 25
- 26
- 27
- 28
- 29
- 30
- 31
- 32
- 33
- 34
- 35
- 36
- 37
- 38
- 39
- 40
- 41
- 42
- 43
- 44
- 45
- 46
- 47
- 48
- 49
- 50
- 51
- 52
- 53
- 54
- 55
- 56
- 57
- 58
- 59
- 60
- 61
- 62
- 63
- 64
- 65
- 66
- 67
- 68
- 69
- 70
- 71
- 72
- 73
- 74
- 75
- 76
- 77
- 78
- 79
- 80
- 81
- 82
- 83
- 84
- 85
- 86
- 87
- 88
- 89
- 90
- 91
- 92
- 93
- 94
- 95
- 96
- 97
- 98
- 99
- 100
- 101
- 102
- 103
3. 计算过程示意
一个2D栅格地图中计算距离的示例，栅格地图中空白栅表示free，黑色实心栅格表示障碍物。
首先对列的维度进行计算，距离的计算结果如下图所示，其中
∞
2
infty^2
∞2 表示该列无障碍物。
然后在列向维度的基础上，将算出来的相应栅格的数值作为行维度计算时
f
(
q
)
f(q)
f(q) 的数值。在计算行维度时，所有有具体数值的栅格都被认为是障碍物，均需要在该栅格处绘制抛物线，上一维度计算出来的不同的数值大小即代表该栅格不同的偏置量
f
(
q
)
f(q)
f(q)。以上图中最后三行为例计算的结果如下图所示。
在列维度计算的基础上，这三行中第一行计算行维度（因为是2D栅格，也即最终距离）中每个栅格距离计算示意如下图所示。
参考文献
[1] Pedro F. Felzenszwalb, Huttenlocher Daniel P. Distance Transforms of Sampled Functions[J]. Theory of Computing, 2012, 8(1): 415-428.
[2] 欧几里得距离转换（EDT）算法
[3] 深蓝学院移动机器人运动规划