在第18讲 《Coursera自动驾驶课程第18讲：The Planning Problem》 我们对自动驾驶中的规划问题有了一个全面的了解，理解了规划问题中的约束和目标；同时我们还讨论了如何分层如解决规划问题（任务规划、行为规划、路径规划和速度曲线生成）。
在本讲中，我们将学习两种环境建图方法：占用网格图（occupancy grid map） 和 高精度地图（high-definition road map），这两种地图在运动规划任务中都发挥着关键作用。
文章目录
19.1 Occupancy Grids
19.1.1 Overview
占用网格是围绕当前自车位置的离散化网格，这种离散化可以在两维或三维上完成。在本讲中我们只讨论 2D 版本。占用网格的每个网格方块用以表示该网格位置中是否存在静态物体。如果是的话，则该网格位置被归类为已占用。可以被分类为占用网格单元的静态物体的示例可以包括树木、建筑物、路标和灯杆。
在自动驾驶汽车领域，其它一些静态物体也应归类为占用空间，包括所有不可驾驶的表面，例如草坪或人行道。占用网格的每个方块可以使用
m
i
m^{i}
mi 来表示是否被占用，其中 1 表示该方块被静态物体占用，而 0 表示没有。在下面这张地图中，我们可以看到有树木和草地的正方形被标记为 1，而道路被标记为 0。网格中所有占据的方块都是紫色的，对应于可驾驶表面的方块为透明。
我们现在讨论为创建准确的占用网格而做出的假设：
- 首先，当前创建占用网格时的环境
仅对应于静态物体。这意味着，在将传感器数据用于占用网格映射之前，必须从传感器数据中删除所有动态物体。 - 其次，每个网格单元都
独立于所有其它网格单元。做出这个假设是为了简化创建占用网格所需的更新函数。 - 最后，当前
车辆状态与每个时间步的占用图有关。
19.1.2 LiDAR Data Filtering and Noise
在自动驾驶汽车领域，激光雷达是目前最常用的距离传感器。（快速提醒一下）激光雷达传感器使用激光脉冲来测量汽车与周围所有物体的距离，并在整个视野范围内返回测量点云。在下图中，我们可以看到激光雷达传感器的输出。在使用点云数据构建占用网格之前，需要过滤掉一部分 LiDAR 数据。
- 第一步是过滤构成地
平面的所有激光雷达点。在这种情况下，地面是自动驾驶汽车可以安全行驶的路面。 - 接下来，所有出现在
车辆最高点之上的点也被过滤掉。这组激光雷达点可以忽略不计，因为它们不会阻碍自动驾驶汽车的前进。 - 最后，需要移除激光雷达捕获的所有
非静态物体。这包括所有车辆、行人、自行车和动物。
激光雷达数据过滤完成后，需要将 3D 激光雷达数据投影到 2D 平面以用于构建我们的占用网格。（激光雷达数据的过滤和压缩我们会在后面部分进行介绍。） 现在经过过滤和压缩的 LIiDAR 数据类似于来自高清 2D 距离传感器的数据，该传感器可以准确地测量到车辆周围所有静态物体的距离。
但是，仍然存在一个问题，在完成所有过滤后，由于过滤方法、数据的复杂性以及环境和传感器噪声，仍然存在地图的不确定性。
19.1.3 Bayesian Update of the Occupancy Grid
为了处理这种噪音，占用网格将被修改为概率性的。每个方块不再存储表示占用的二进制值，而是存储一个介于 0 和 1 之间的概率值。概率值越高，给定方格被占用的概率就越高。
因此占用网格现在可以表示为由术语 bel 表示的置信图。为简单起见，
m
i
m^i
mi 表示占用网格的单个方块，其中
i
i
i 可以由测量值
Y
Y
Y 和车辆位置
X
X
X 构成。
m
i
m^i
mi 的置信度等于在给定传感器测量值的情况下当前单元格
m
i
m^i
mi 被占用的概率。
为了转换回二进值地图，可以建立一个阈值，当置信度大于阈值则表示该单元格被占用。任何置信度低于设定阈值的单元格都将被设置为空闲。举个例子，下图中被占据的方块的概率为 0.94，方块分类为被占据。另一方面，在街道上找到的广场只有 0.12 的概率表示被占用，因此将被归类为空闲位置。
为了实现更准确的占用置信度，通常会对多个时间的测量进行组合。在时间
t
t
t 对地图单元
m
i
m^i
mi 的置信度定义为在给定所有测量值和从时间
1
1
1 到
t
t
t 的车辆位置的情况下
m
i
m^i
mi 被占用的概率。
bel
?
t
(
m
i
)
=
p
(
m
i
∣
(
y
,
x
)
1
:
t
)
(19.1)
\operatorname{bel}_{t}\left(m^{i}\right)=p\left(m^{i} \mid(y, x)_{1: t}\right) \tag{19.1}
belt?(mi)=p(mi∣(y,x)1:t?)(19.1)
要将多个测量值组合成单个置信度图，可以使用贝叶斯公式。对于占用网格，我们得到一个以下形式的贝叶斯更新公式。
bel
?
t
(
m
i
)
=
η
p
(
y
t
∣
m
i
)
bel
?
t
?
1
(
m
i
)
(19.2)
\operatorname{bel}_{t}\left(m^{i}\right)=\eta p\left(y_{t} \mid m^{i}\right) \operatorname{bel}_{t-1}\left(m^{i}\right) \tag{19.2}
belt?(mi)=ηp(yt?∣mi)belt?1?(mi)(19.2)
其中 η \eta η 是一个归一化常数，以确保最终结果仍然是概率分布。
让我们看看实际使用中的占用网格。在这段视频中，我们将跟随自动驾驶汽车驶出车道并沿着道路行驶，同时占用网格会实时更新。较浅的网格单元代表自由正方形，而黑色网格单元代表占用的正方形。我们还可以看到红色的原始激光雷达数据，以及橙色的过滤输出。请注意地图如何跟随车辆运动，这是使用我们在状态估计中介绍的技术进行估计的。在此视频中，将物体归类为障碍物所需的置信阈值设置为非常高，因此只有大型静态物体被识别为被占用。降低此阈值将导致更多单元格被标记为已占用，但也会导致地图更加嘈杂。
让我们总结一下我们刚刚学到的东西：
- 我们了解了
占用网格地图的基本定义，并了解了如何过滤和压缩LiDAR传感器数据以创建占用网格。 - 然后，我们学习了如何将占用网格表示为
置信度图，并应用贝叶斯公式将新测量值合并到占用网格中。
19.2 Populating Occupancy Grids from LiDAR Scan Data (Part 1)
19.2.1 Issue with Bayesian Probability Update
在本小节中，我们会先讨论在上一小节中看到的贝叶斯概率更新的问题。然后，我们将使用对数几率表示来解决此问题。
正如我们在上一小节中看到的，我们可以应用贝叶斯定理将前一时刻的置信图与当前的测量相结合，从而在每个时间步创建一个高度准确的占用网格。这是通过以下函数实现的：
bel
?
t
(
m
i
)
=
η
p
(
y
t
∣
m
i
)
bel
?
t
?
1
(
m
i
)
(19.3)
\operatorname{bel}_{t}\left(m^{i}\right)=\eta p\left(y_{t} \mid m^{i}\right) \operatorname{bel}_{t-1}\left(m^{i}\right) \tag{19.3}
belt?(mi)=ηp(yt?∣mi)belt?1?(mi)(19.3)
其中 η \eta η 是一个归一化常数， p ( y t ∣ m i ) p\left(y_{t} \mid m^{i}\right) p(yt?∣mi) 是在给定 m i m^i mi 下接收到的当前测量值 y t y_t yt? 的概率， bel ? t ? 1 ( m i ) \operatorname{bel}_{t-1}\left(m^{i}\right) belt?1?(mi) 是 t ? 1 t-1 t?1 时刻的置信图。
然而，使用这个贝叶斯更新存在一个问题。为了说明这个问题，我们来看一个示例。假设我们有一个低置信度的占用方块
bel
?
t
?
1
(
m
i
)
\operatorname{bel}_{t-1}\left(m^{i}\right)
belt?1?(mi) ，置信度为0.000638，此时观察到的测量值的概率也很低，
p
(
y
t
∣
m
i
)
p\left(y_{t} \mid m^{i}\right)
p(yt?∣mi) 为0.000012；二者相乘将会得到一个非常小的置信值：
b
e
l
t
(
m
)
=
n
p
(
y
t
∣
m
)
b
e
l
t
?
1
(
m
)
0.000000008
0.000012
0.000638
(19.4)
\begin{array}{ccc} b e l_{t}(m) & =n p\left(y_{t} \mid m\right) &b e l_{t-1}(m) \\ \\ 0.000000008 & 0.000012 & 0.000638 \end{array} \tag{19.4}
belt?(m)0.000000008?=np(yt?∣m)0.000012?belt?1?(m)0.000638?(19.4)
可以看到，置信值接近于零。在计算机上对浮点数进行乘法运算会在乘以小数时导致显着的近似误差，进而导致概率估计的不稳定。此外概率相乘也被证明是执行置信更新的一种低效方式。
所以我们应用贝叶斯公式来更新占用单元的置信度看起来并不合适。 但是有一个解决方案。 我们可以使用对数函数将我们的置信度转换为对数概率，而不是存储值范围为 0-1 的置信图。这导致单元格值从负无穷大到正无穷大
bel
?
t
(
m
)
→
(
?
∞
,
∞
)
\operatorname{bel}_{t}(m) \rightarrow(-\infty, \infty)
belt?(m)→(?∞,∞)，避免了数字接近零的问题。对数函数取概率
p
p
p 和
1
?
p
1-p
1?p 之比的自然对数。因此，它可以将用 0-1 的概率值映射到整个实轴，对数概率公式为：
l
o
g
i
t
(
p
)
=
log
?
(
p
1
?
p
)
(19.5)
logit(p)=\log \left(\frac{p}{1-p}\right) \tag{19.5}
logit(p)=log(1?pp?)(19.5)
也可以将对数概率
l
o
g
i
t
(
p
)
logit(p)
logit(p) 转换回为概率
p
p
p，可以通过如下公式转换：
p
=
e
logit
?
(
p
)
1
+
e
logit
?
(
p
)
(19.6)
p=\frac{e^{\operatorname{logit}(p)}}{1+e^{\operatorname{logit}(p)}} \tag{19.6}
p=1+elogit(p)elogit(p)?(19.6)
