在第18讲 《Coursera自动驾驶课程第18讲：The Planning Problem》 我们对自动驾驶中的规划问题有了一个全面的了解，理解了规划问题中的约束和目标；同时我们还讨论了如何分层如解决规划问题（任务规划、行为规划、路径规划和速度曲线生成）。

在本讲中，我们将学习两种环境建图方法：占用网格图（occupancy grid map） 和 高清地图（high-definition road map），这两种地图在运动规划任务中都发挥着关键作用。

---

19.1 Occupancy Grids

19.1.1 Overview

占用网格是围绕当前自车位置的离散化网格，这种离散化可以在两维或三维上完成。在本讲中我们只讨论 2D 版本。占用网格的每个网格方块用以表示该网格位置中是否存在静态物体。如果是的话，则该网格位置被归类为已占用。可以被分类为占用网格单元的静态物体的示例可以包括树木、建筑物、路标和灯杆。

在自动驾驶汽车领域，其它一些静态物体也应归类为占用空间，包括所有不可驾驶的表面，例如草坪或人行道。占用网格的每个方块可以使用 $m^i$ 来表示是否被占用，其中 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$ 表示占用网格的单个方块，其中 $i$ 可以由测量值 $Y$ 和车辆位置 $X$ 构成。$m^i$ 的置信度等于在给定传感器测量值的情况下当前单元格 $m^i$ 被占用的概率。

为了转换回二进值地图，可以建立一个阈值，当置信度大于阈值则表示该单元格被占用。任何置信度低于设定阈值的单元格都将被设置为空闲。举个例子，下图中被占据的方块的概率为 0.94，方块分类为被占据。另一方面，在街道上找到的广场只有 0.12 的概率表示被占用，因此将被归类为空闲位置。

为了实现更准确的占用置信度，通常会对多个时间的测量进行组合。在时间 $t$ 对地图单元 $m^i$ 的置信度定义为在给定所有测量值和从时间 $1$ 到 $t$ 的车辆位置的情况下 $m^i$ 被占用的概率。
$${\mathrm{bel}}_t\left(m^i\right)=p\left(m^i\mid (y,x{)}_{1:t}\right)\text{\hspace{1em}(19.1)}$$

要将多个测量值组合成单个置信度图，可以使用贝叶斯公式。对于占用网格，我们得到一个以下形式的贝叶斯更新公式。
$${\mathrm{bel}}_t\left(m^i\right)=\eta p\left(y_t\mid m^i\right){\mathrm{bel}}_{t-1}\left(m^i\right)\text{\hspace{1em}(19.2)}$$

其中 $\eta$ 是一个归一化常数，以确保最终结果仍然是概率分布。

让我们看看实际使用中的占用网格。在这段视频中，我们将跟随自动驾驶汽车驶出车道并沿着道路行驶，同时占用网格会实时更新。较浅的网格单元代表自由正方形，而黑色网格单元代表占用的正方形。我们还可以看到红色的原始激光雷达数据，以及橙色的过滤输出。请注意地图如何跟随车辆运动，这是使用我们在状态估计中介绍的技术进行估计的。在此视频中，将物体归类为障碍物所需的置信阈值设置为非常高，因此只有大型静态物体被识别为被占用。降低此阈值将导致更多单元格被标记为已占用，但也会导致地图更加嘈杂。

让我们总结一下我们刚刚学到的东西：

• 我们了解了占用网格地图的基本定义，并了解了如何过滤和压缩 LiDAR 传感器数据以创建占用网格。
• 然后，我们学习了如何将占用网格表示为置信度图，并应用贝叶斯公式将新测量值合并到占用网格中。

---

19.2 Populating Occupancy Grids from LiDAR Scan Data (Part 1)

19.2.1 Issue with Bayesian Probability Update

在本小节中，我们会先讨论在上一小节中看到的贝叶斯概率更新的问题。然后，我们将使用对数几率表示来解决此问题。

正如我们在上一小节中看到的，我们可以应用贝叶斯定理将前一时刻的置信图与当前的测量相结合，从而在每个时间步创建一个高度准确的占用网格。这是通过以下函数实现的：
$${\mathrm{bel}}_t\left(m^i\right)=\eta p\left(y_t\mid m^i\right){\mathrm{bel}}_{t-1}\left(m^i\right)\text{\hspace{1em}(19.3)}$$

其中 $\eta$ 是一个归一化常数，$p\left(y_t\mid m^i\right)$ 是在给定 $m^i$ 下接收到的当前测量值 $y_t$ 的概率，${\mathrm{bel}}_{t-1}\left(m^i\right)$ 是 $t-1$ 时刻的置信图。

然而，使用这个贝叶斯更新存在一个问题。为了说明这个问题，我们来看一个示例。假设我们有一个低置信度的占用方块 ${\mathrm{bel}}_{t-1}\left(m^i\right)$ ，置信度为0.000638，此时观察到的测量值的概率也很低，$p\left(y_t\mid m^i\right)$ 为0.000012；二者相乘将会得到一个非常小的置信值：
$$\begin{array}{ccc}be{l}_t(m)& =np\left(y_t\mid m\right)& be{l}_{t-1}(m)\\ & & \\ 0.000000008& 0.000012& 0.000638\end{array}\text{\hspace{1em}(19.4)}$$

可以看到，置信值接近于零。在计算机上对浮点数进行乘法运算会在乘以小数时导致显着的近似误差，进而导致概率估计的不稳定。此外概率相乘也被证明是执行置信更新的一种低效方式。

所以我们应用贝叶斯公式来更新占用单元的置信度看起来并不合适。 但是有一个解决方案。 我们可以使用对数函数将我们的置信度转换为对数概率，而不是存储值范围为 0-1 的置信图。这导致单元格值从负无穷大到正无穷大 ${\mathrm{bel}}_t(m)\to (-\infty ,\infty )$，避免了数字接近零的问题。对数函数取概率 $p$ 和 $1-p$ 之比的自然对数。因此，它可以将用 0-1 的概率值映射到整个实轴，对数概率公式为：
$$logit(p)=\log \left(\frac{p}{1-p}\right)\text{\hspace{1em}(19.5)}$$

也可以将对数概率 $logit(p)$ 转换回概率 $p$，可以通过如下公式转换：
$$p=\frac{e^{\mathrm{logit}(p)}}{1+e^{\mathrm{logit}(p)}}\text{\hspace{1em}(19.6)}$$

---

19.2.2 Bayesian log odds Update

现在我们有了对数概率表示，那么让我们看看对数概率如何影响我们的贝叶斯更新方程。根据贝叶斯公式，有：
$$p\left(m^i\mid y_{1:t}\right)=\frac{p\left(y_t\mid y_{1:t-1},m^i\right)p\left(m^i\mid y_{1:t-1}\right)}{p\left(y_t\mid y_{1:t-1}\right)}\text{\hspace{1em}(19.7)}$$

其中 $m^i$ 是当前占用网格位置 $i$ 处的方块，$y_{1:t}$ 是从时刻 1 到 $t$ 的测量值。接下来我们将应用马尔可夫假设，如果当前地图方块 $m^i$ 状态已知，则该假设可以确保当前测量值独立于先前的测量值，我们可以得到下面的方程：
$$p\left(m^i\mid y_{1:t}\right)=\frac{p\left(y_t\mid m^i\right)p\left(m^i\mid y_{1:t-1}\right)}{p\left(y_t\mid y_{1:t-1}\right)}\text{\hspace{1em}(19.8)}$$

同样根据贝叶斯公式，我们可以得到当前测量 $y_t$ 和地图方块 $m^i$ 之间的关系：
$$p\left(y_t\mid m^i\right)=\frac{p\left(m^i\mid y_t\right)p\left(y_t\right)}{p\left(m^i\right)}\text{\hspace{1em}(19.9)}$$

公式 （19.8） 和 （19.9）结合可以得到：
$$p\left(m^i\mid y_{1:t}\right)=\frac{p\left(m^i\mid y_t\right)p\left(y_t\right)p\left(m^i\mid y_{1:t-1}\right)}{p\left(m^i\right)p\left(y_t\mid y_{1:t-1}\right)}\text{\hspace{1em}(19.10)}$$

我们使用对数概率对公式（19.10）进行简化。在此之前，我们先讨论 $1-p$ 的表达式，其概率为：
$$p\left(\neg m^i\mid y_{1:t}\right)=1-p\left(m^i\mid y_{1:t}\right)=\frac{p\left(\neg m^i\mid y_t\right)p\left(y_t\right)p\left(m^i\mid y_{1:t-1}\right)}{p\left(\neg m^i\right)p\left(y_t\mid y_{1:t-1}\right)}\text{\hspace{1em}(19.11)}$$

这表示的是占用网格中方块未被占用的概率。下面我们求概率比，即占用概率与未占用概率之比：
$$\frac{p\left(m^i\mid y_{1:t}\right)}{p\left(\neg m^i\mid y_{1:t}\right)}=\frac{p\left(m^i\mid y_t\right)p\left(\neg m^i\right)p\left(m^i\mid y_{1:t-1}\right)}{p\left(\neg m^i\mid y_t\right)p\left(m^i\right)p\left(\neg m^i\mid y_{1:t-1}\right)}\text{\hspace{1em}(19.12)}$$

我们使用 $1-p$ 代替 $\neg p$，得到：
$$\frac{p\left(m^i\mid y_{1:t}\right)}{p\left(\neg m^i\mid y_{1:t}\right)}=\frac{p\left(m^i\mid y_t\right)\left(1-p\left(m^i\right)\right)p\left(m^i\mid y_{1:t-1}\right)}{\left(1-p\left(m^i\mid y_t\right)\right)p\left(m^i\right)\left(1-p\left(m^i\mid y_{1:t-1}\right)\right)}\text{\hspace{1em}(19.13)}$$

最后，我们使用对数概率，得到：
$$\mathrm{logit}\left(p\left(m^i\mid y_{1:t}\right)\right)=\mathrm{logit}\left(p\left(m^i\mid y_t\right)\right)+\mathrm{logit}\left(p\left(m^i\mid y_{1:t-1}\right)\right)-\mathrm{logit}\left(p\left(m^i\right)\right)\text{\hspace{1em}(19.14)}$$

上式可以用一种方便的简写形式来写，
$$l_{t,i}=\mathrm{logit}\left(p\left(m^i\mid y_t\right)\right)+l_{t-1,i}-l_{0,i}\text{\hspace{1em}(19.15)}$$

我们现在得出了对数概率的贝叶斯更新公式，用于在占用网格地图上进行更新。它由三项组成：

• $\mathrm{logit}\left(p\left(m^i\mid y_t\right)\right)$；
• $l_{t-1,i}$ ；
• $l_{0,i}$，初始置信度，通常为0.5；

可以看到初始置信度在每个时间步都出现了，这有点令人惊讶，但这只是我们在本小节中的推导的结果，并调整了前两项的相加以确保更新后的置信度与对数概率形式一致。与直接更新概率相比，贝叶斯对数概率有两个强大的优势。

• 由于对数概率是整个实轴上的映射，数值更新是稳定的；
• 并且在计算上它也显着提高了效率，因为它完全依赖于加法来完成占用网格的更新。

总结一下我们在本小节中学到的东西：

• 我们首先通过简单的贝叶斯概率更新确定了存储和更新占用网格的一些问题。
• 然后我们看到了如何通过使用概率空间的对数概率表示来解决这个问题。
• 最后，我们看到了贝叶斯对数概率更新是如何从贝叶斯公式推导出来的。

---

19.3 Populating Occupancy Grids from LiDAR Scan Data (Part 2)

19.3.1 Inverse Measurement Model

在本小节中，我们将讨论逆测量模型，并了解如何为激光雷达传感器构建简单的逆测量模型。我们还将描射线追踪，这将有助于降低逆测量模型的计算需求。

回想一下上一小节，为了对我们的占用网格进行贝叶斯更新，我们得到了贝叶斯对数概率更新公式。
$$l_{t,i}=\mathrm{logit}\left(p\left(m^i\mid y_t\right)\right)+l_{t-1,i}-l_{0,i}\text{\hspace{1em}(19.16)}$$

我们需要计算 $p\left(m^i\mid y_t\right)$，这表示在当前测量 $y_t$ 下网格 $m^i$ 不为空的概率。然而到目前为止，我们只看到了测量模型 $p\left(y_t\mid m^i\right)$，它表示在占用网格中的一个单元被占用的情况下，获得测量值的概率。因此，对于占用网格更新，我们需要对测量模型进行逆操作。

也就是说，我们必须构建一个逆测量模型，该模型在给定测量 $y_t$ 的情况下求 $m^i$ 不为空的概率 。为简单起见，我们将激光雷达视为2D距离传感器。设得到的方位角和距离测量值为：
$${\varphi }^s=\left[\begin{array}{lll}-{\varphi }_{\max }^s& \dots & {\varphi }_{\max }^S\end{array}\right]{\varphi }_j^s\in {\varphi }^s\text{\hspace{1em}(19.17)}$$

$$r^S=\left[\begin{array}{lll}{r}_1^S& \dots & r_j^S\end{array}\right]r_j^S\in \left[0,r_{\max }^S\right]\text{\hspace{1em}(19.18)}$$

每个光束对应的视角在最大视角范围内 ${\varphi }_{\max }$ 是均匀分布，同时每个光束得到的距离信息不超过最大距离 $r_{max}$。今天的大多数激光雷达也会返回无回波信号，如果特定波束没有返回回波，则表明该方向范围内不存在任何物体。

我们假设单次旋转的整个激光雷达扫描测量值是在同一时刻测量的。（对于使用旋转激光雷达头高速移动的车辆来说，这不是一个准确的模型，因为我们必须校正车辆的运动，可以使用状态估计来完成校正。）假设下图是带有激光雷达传感器的车辆，它在环境中收集不同方位的距离测量值。 我们构建了一个临时占用网格，其中包含所有方向上的最大光束范围。

该测量网格的坐标系使用占用网格地图坐标，因此我们在占用网格坐标中可以定义传感器的位置 $(x_{1,t},x_{2,t})$ 以及方向 ${\theta }_{1,t}$（这里以左下角为坐标原点）。

但是在实际中，常常选择车辆坐标原点为占用网格地图坐标原点，占用网格地图在每一步都会根据我们的状态估计进行转换。我们现在使用占用概率填充占用网格。根据当前的激光雷达数据，我们可以定义占用网格的三个不同区域：

• 首先，有一个激光雷达光束无法到达的无信息区域。当前的激光雷达扫描没有提供有关该区域环境的信息。
• 然后是一个物体概率低的区域，因为所有的光束都通过了这个区域而没有遇到任何东西。
• 最后，存在一个物体概率较高的区域，在该区域中，激光雷达已经接触到物体，并返回了一个测量值。

为了将这些区域转换到测量网格上，每个网格方块将被分配一个相对于车辆当前位置的方位和距离。每个单元格的相对距离就是传感器到单元格的欧几里得距离。

• 令 $r_i$ 是网格方块 $i$ 的相对距离，其计算公式为：
  $$r^i=\sqrt{{\left(m_x^i-x_{1,t}\right)}^2+{\left(m_y^i-x_{2,t}\right)}^2}\text{\hspace{1em}(19.19)}$$

• $(x,y)$ 是网格方块中心的坐标，$(x_{1,t},x_{2,t})$ 是传感器当前位置，可以得到方位角 ${\varphi }^i$：
  $${\varphi }^i={\tan }^{-1}\left(\frac{m_y^i-x_{2,t}}{m_x^i-x_{1,t}}\right)\text{\hspace{1em}(19.20)}$$

对于每个网格方块，我们通过激光雷达光束角和单元方位角之间误差最小的测量值来找到最相关的激光雷达光束：
$$k=\mathrm{argmin}\left(\left|{\varphi }^i-{\varphi }_j^s\right|\right)\text{\hspace{1em}(19.21)}$$

然后，我们定义了两个参数 $\alpha$ 和 $\beta$，它们定义了每个光束能覆盖的一个区域，该区域将被分配特定光束的测量信息。$\alpha$ 控制光束的距离，在距离范围内将被标记为高概率；$\beta$ 控制光束的角度，单元格将被标记为低概率或高概率。

基于上面三种类型接收到的激光雷达测量值，我们现在可以求得任一单元格被占用的概率。

• 无信息区域对应于所有大于最大光束测量范围的单元格，或处于它所关联的光束对应覆盖角度 $\beta$ 之外（这里是大于 $\beta /2$ ）；
  $$r^i>\min \left(r_{\max }^s\right)\text{ or }\left|{\varphi }^i-{\varphi }_k^s\right|>\beta /2\text{\hspace{1em}(19.22)}$$
• 高信息区域对应单元格为距离与关联光束之差小于 $\alpha /2$，落在关联光束覆盖角度 $\beta$ 之内，我们令其概率大于0.5。
  $$r_k^s<r_{\max }^s\text{ and }\left|r^i-r_k^s\right|>\alpha /2\text{ and }\left|{\varphi }^i-{\varphi }_k^s\right|<\beta /2\text{\hspace{1em}(19.23)}$$
• 低信息区域对应单元格距离小于光束测量值，且落在关联光束覆盖角度 $\beta$ 之内，令这些单元格占用概率小于0.5。
  $$r^i<r_k^s\text{ and }\left|{\varphi }^i-{\varphi }_k^s\right|<\beta /2\text{\hspace{1em}(19.24)}$$

来看一个示例，假设激光雷达光束在红色 $X$ 标记的位置返回测量值，红色区域被标记为高概率。增加 $\alpha$ 的值会增加受影响区域的范围，增加 $\beta$ 的值会影响受影响区域的角度。

---

19.3.2 Bresenham Line Algorithm

我们现在可以构建完整的逆测量模型，用于我们的对数概率更新。然而，这种简单的逆测量模型在计算上可能很昂贵。它需要对测量图中的每个单元格进行全面更新，并依赖多个浮点运算来识别哪些测量值对应于哪些单元格。

另一种方法是使用光线追踪算法，例如计算机科学经典的 Bresenham 算法。 Bresenham 的线条算法最初是在 1960 年代初设计的，目的是有效解决当时可用硬件上的显示器和打印的线条绘制问题。通过沿着激光雷达扫描的光束进行光线追踪，我们减少了需要处理的单元的数量，并依靠整数加减法和位移来更快地识别它们，从而沿着激光雷达光束穿过网格。有趣的是，许多光束通过汽车附近的相同单元大大增加了测量的可信度。

让我们总结一下：

• 我们为对数概率更新步骤所需的激光雷达数据构建了一个简单的逆测量模型。
• 我们还简要介绍了通过使用光线追踪来加速计算的潜在改进。

---

19.4 Occupancy Grid Updates for Self-Driving Cars

19.4.1 Filtering of 3D LiDAR

在本小节，我们将了解自动驾驶汽车的特定要求，将激光雷达数据转换为可用于创建占用网格的表示（滤波后）。我们首先学习几种常见的 3D 数据滤波方法。如下图所示，我们有激光雷达扫描后的数据，它是自动驾驶汽车在当地道路上行驶时获得的，常见的滤波处理有：

• 下采样，为了可以实时更新，通常希望将激光雷达扫描的点数下采样到更小的数量。

• 移除汽车上方的点，过滤掉自动驾驶汽车上方的所有激光雷达点。

• 移除属于地面的点，在创建占用网格时，我们不需要考虑驾驶地面，因此会删除所有地面反射的激光雷达点。

• 移除所有动态物体，例如运动中的汽车或行人。

下采样是通过删除或忽略冗余激光雷达点来减少激光雷达点数量的过程。自动驾驶中使用的激光雷达每秒可产生 120 万个点左右，为车辆周围的所有物体提供丰富的几何描述。 这些生成的点实际上是多余的，其中的许多点捕获了相同的物体信息。在下图中，我们可以看到一个路标，该路标被激光雷达点密集覆盖，其中一小部分点足以表示占用网格地图中障碍物位置。 最重要的是，处理这 120 万个点在计算上是不切实际的。 第二，必须删除一些点以满足后续操作所需要的计算。

可以以多种方式执行下采样：其中最简单的方法是在等间距采样，在激光雷达每条扫描环上每隔 $n$ 个点保留一个点； 还可以在距离图像中应用图像下采样技术，并在 3D 网格中进行空间搜索，用单个占用测量替换点集。 在每种情况下，这些方法都可以在开源点云库（如 PCL）或计算机视觉库（如 OpenCV）中轻松获得。

我们的第二个过滤器是一个非常简单的过滤器，它只是删除了车辆高度以上的点。如下图所示，激光雷达安装高度为 2.4 米。 然而，这个滤波器通常假定汽车行驶在一个平坦的地平面上，有时候很难满足这一假设。 这种方法会帮助我们删除树木、电线、桥梁和高空标志等物体。

第三个过滤器是删除落在可驾驶表面上的点。由于激光雷达自身扫描的性质，在下图中可以看到许多同心圆，这是由于激光雷达光束撞击可驱动表面造成的。这些点不应与占用网格图中的占用单元相混淆。这是非常有挑战性的问题：

• 首先，所有道路都有不同的道路几何形状，包括可变的排水凹度、不同的坡度和曲率等等。这种变化也很难预测。但是，如果不移除地面，则占用网格可能会出现伪影，从而导致汽车无法继续行驶，因为它认为道路被阻塞了。
• 第二个问题是路沿在不同的位置有不同的高度，并且道路边界在激光雷达数据中并不总是明确定义的。这些变化可能导致部分路沿或不可驾驶区域作为地平面的一部分被移除。
• 最后，需要检测道路上的小物体，例如足球或小动物，这意味着在处理点云的简单几何方法很难获得前方道路的真实状态。
  

处理此类问题的最佳方法是使用图像语义分割方法。在下图中可以看到，其中地面分割为深紫色。然后，该任务变成了将视觉中检测到的可驾驶表面映射到激光雷达点云，然后进行移除最后保留来自非驾驶表面的点。

最后一种过滤方法是移除所有动态物体，例如移动的汽车或行人。这可以通过感知方法来检测和追踪场景中所有的动态物体。检测到的动态物体的 3D 边界框用于移除受影响区域中的所有点。

然而，很多时候这并不令人满意，原因有两个：

• 首先，并非所有检测到的动态物体实际上都在移动。一些车辆可能停在路边，因此可以被视为占用网格的一部分，因为它们确实是静止的。为了处理这个问题，感知需要使用动态物体轨迹来识别当前哪些物体是静态的。
• 其次，由于感知处理通常需要一定的计算时间，动态物体只有在延迟一段时间后才会被检测到。这会导致使用历史的物体位置更新占用网格，从而导致边界框丢失动态车辆上大部分激光雷达点。
• 我们可以使用运动物体轨迹预测。边界框沿预测路径向前移动，最终从最近的激光雷达扫描中删除更多动态属于物体的点。

---

19.4.2 Projection of LiDAR Onto a 2D Plane

在讨论完所有这些滤波方法之后，我们将激光雷达数据从 3D 投影到 2D，处理方法很简单：

• 首先，将激光雷达点高度值 $Z$ 设为零，这会将激光雷达网格折叠为平面。二维扫描平面被划分为与占用网格相同的网格图案。
• 对于占用网格的每个单元格，所有落入其中的激光雷达点都会被计算在内。单元格中的点越多，在占用网格中该单元格中包含静态物体的机率就越大。
  

简单总结以下：

• 我们掌握了制作适合用作占用网格的激光雷达点云所需的过滤方法。包括下采样、去除汽车上方的激光雷达点、去除地平面，最后去除动态物体。
• 然后，我们看到了一种简单而有效的方法，可以将 3D 激光雷达数据转换为 2D 置信图。

---

19.5 High Definition Road Maps

19.5.1 Overview

在本小节，我们将学习另一种地图：高清道路地图（HD Map）。 我们将通过查看称为 lanelet Map 的数据类型来了解如何有效地存储此地图。然后我们将了解 lanelet 地图的两个主要元素； lanelets 和 intersection，并讨论它们是如何连接的。 紧接着，我们看看可以对 lanelet 元素执行的不同操作。最后，我们将探索记录和创建 lanelet 地图的不同方法。

高清道路地图类似于传统的纸质地图或在线地图。但是，高清地图中包含的信息要详细得多。传统地图存储道路的大致位置，而高清道路地图存储精确的道路位置，包括精确到厘米级的车道；除此之外，高清道路地图还存储了可能影响自动驾驶汽车的所有道路标志和信号的位置。

由于数据的详细和相互关联的性质，需要一种有效的方法来存储地图中包含的所有信息。Lanelet 是在2014年由 Philip Bender、Julius Ziegler 和 Christoph Stiller 提出，由于这种方法在存储和传输高清地图所需的复杂信息方面的有效性，因此得到了广泛的应用。我们会在课程的其余部分进一步研究，特别是它在我们的行为规划中发挥着关键作用。

---

19.5.2 Lanelet Map

Lanelet 地图有两个主要组成部分。Lanelet Element 存储与它所代表的道路上的一小段纵向车道相关的所有信息。Intersection Element 存储单个交叉路口一部分的所有Lanelet Element，以便在运动规划任务期间进行简单检索；此外还包含二者是如何连接的。

---

一个 Lanelet Element 包含有几组信息，这些信息存储了它所代表的路段，信息有：

• lanelet 存储了给定车道的左右边界。
• lanelet 存储可能存在于 lanelet 元素末端的任何监管元素，例如停车标志线或静态标志线。这里我们只为任何监管元素存储一条线，因为这是自动驾驶汽车视为该监管元素的活动位置的点。
• lanelet 存储可能影响该路段的所有监管属性，例如速度限制。
• lanelet 存储自身与周围其他 lanelet 元素的连接性。这允许通过由高清地图中的一组 lanelets 创建的图轻松遍历和计算。

每个 lanelet 以监管元素或遇到监管属性的变化而结束。这意味着，对于作为交叉路口一部分的 lanelet，可以短至只有几米，对于高速公路路段，则可以长达数百米。

---

lanelet 的边界表示为 lanelet 的边缘。这些边界捕获标记的车道边界或禁止驾驶的曲线。车道边界存储为连续曲线的一组点。每个点都存储其 $x、y$ 和 $z$ 坐标。点之间的距离可以细至几厘米，也可以粗至几米，具体取决于所讨论的折线的平滑度。使用这种多项式曲线结构，可以执行许多有用的操作来收集用于运动规划任务的数据。点的顺序定义了 lanelet 的行进方向和航向。lanelet 任何部分的道路曲率也可以从该 lanelet 预先计算。可以对两个边界之间进行插值得到中心线，将其作为自动驾驶车辆在该车道上的所需行驶路径。

---

有两种类型的 lanelet 监管规则：监管元素位于 lanelet 的末尾；监管属性影响整个lanelet。

• 监管元素表示为由一组共线点定义的线。监管元素通常需要自动驾驶汽车采取行动或做出决定，例如在红绿灯的情况下，继续进行的决定必须基于该红绿灯的当前状态。
• lanelet 的监管属性在整个 lanelet 一直存在。例如速度限制，或者该 lanelet 是否与另一个 lanelet 交叉，如在交叉路口或合并中。

---

每个 lanelet 存在四种连接形式：向前、向后、向左、向右。整个 lanelet 结构连接在一个有向图上，这是高清地图的基础结构。应该注意，在当前 lanelet 之前可能有多个 lanelet，或者在交叉路口的情况下可能有多个后续 lanelet。

为了更好地理解这个概念，让我们看一个简单的例子（下图所示）。一些 lanelet 被编号并表示为图上的节点，因此这些 lanelet 仅代表具有此交叉点的完整 lanelet 的一部分。节点或顶点在该图的车道上标记为 1-5，并形成车道图的顶点。该图的所有边都是有向的并被标记以表示边连接的两个顶点之间的关系。标记为 0 的边表示指向下一个lanelet，1 表示与左侧 lanelet 的连接。标记为 2 的边表示与右侧 lanelet 的连接，而标记为 3 的边表示与之前的 lanelet 相连。

Lanelet element 5 和 lanelet element 2 是离开 lanelet element 1 后可以继续行驶的两个可能的下一个lanelet，因为它们是通过这个路口的两条路。类似地，lanelet element 4 通过一条边连接到 lanelet 1，因为 lanelet 4 位于 lanelet 1 的左侧。

---

Intersection Element 只包含一个指向所有监管元素的指针，这些元素构成了感兴趣的交集。作为 Intersection 一部分的所有 lanelet 也指向该交叉点元素。这种结构很像一个容器，并在分配行为时简化了整个 lanelet 结构的查找。

Lanelet 数据结构的一大优势是它使部分运动规划过程更简单，计算效率更高。穿过复杂的多车道道路网络进行路径规划的过程需要在转弯之前进行多次连接更改，这种数据结构使得每个车道都被视为一个单独的顶点成为可能。我们将在21讲看到，将动态物体定位到已知地图可以改进路径预测。我们将在22讲看到，这种定位能力还通过提供一种简单的方法来规划自动驾驶汽车在繁忙十字路口的行为，从而改善与动态物体的交互。

---

可以通过三种方式创建高清地图：

• 首先是驾驶车辆多次穿过路网收集信息来离线创建地图，然后融合分割和定位信息以提高地图的准确性。在这种方法中，如果从算法中发现任何错误，也可以进行人工校正。
• 第二种方法是在第一次驾驶路网时在线创建地图。通过使用现有的传统地图并依赖分割和物体检测，可以在自动驾驶汽车首次通过路网时创建地图。这种方法在运动规划任务和感知任务方面都非常昂贵。更不用说高度容易出错，因此很少部署。
• 创建高清地图的第三种方法是离线创建地图，然后在检测到更改时在线更新它们。一个这样的变化可能是一个新的建设区域导致出现一个新的监管元素，这是在地图创建过程中无法预测的。

事实上，所有这些方法都使用相同的计算来识别 lanelet element、属性和 intersection。在实践中，具有实时确认和更新功能的离线地图构建可以两全其美，确保对不经常变化的静态元素具有高精度，同时仍能捕捉环境中的新事物并让车辆适应它。