自动驾驶地图构建(Mapping)-占位栅格图(Occupancy Grid Map)

前面文章《自动驾驶运动规划(Motion Planning)》中提到可以使用占位图(Occupancy Grid Map)表示自动驾驶行驶区域的哪些区域被障碍物(如静止的车辆、路中间的石墩子、树木、路肩等)占用，Motion Planning模块会通过查询占位地图避开这些道路障碍物，避免与它们碰撞，从而达到安全驾驶的目的。

1、占位栅格地图(Occupancy Grid Map)

如上图所示，将车辆行驶道路环境用网格(Cell)切分，并且每个网格(Cell)用二值数值0和1填充，0表示该网格(Cell)被占用，1表示该网格(Cell)没有被占用。

由此，可以得到如下所示的一张栅格占位地图。

要制作理想的占位栅格地图必须满足的以下几个假设条件：

1）占位栅格地图是对道路行驶区域中的静态环境(Static Environment)的描述。也就意味着，我们在制图前必须将地面、动态物体(车辆、行人等)从传感器数据中移除掉；

2）每个网格(Cell)与其它的所有网格的状态是相互独立的，即它的状态不受周围其它网格状态的影响；

3）在每个时刻，车辆的位置是精确的、已知的。

2、概率占位栅格地图(Probabilistic Occupancy Grid Map)

在实际的应用中，车辆传感器的数据测量是存在误差的，车辆的定位结果也是存在误差的，动态障碍物的识别也是存在误差的，因此用概率表示一个网格(Cell)被占用的可能性是一个更加可行的方案。每个网格存储一个[0, 1]之间的概率值，这个值越大，表示网格被占用的可能性越大；这个值越小，表示网格被占用的可能性越小。

3、概率占位栅格图(Probabilistic Occupancy Grid Map)制图

栅格地图的每个Cell的概率值计算公式如下：

其中

是1到t时刻的车辆位置和传感器测量结果，通过历史信息的累计，可以提升制作的地图的准确性。

如何将1到t时刻的所有传感器测量结果融合起来呢？贝叶斯理论(Bayes Theorem)是一个不错的选择。

其中

是归一化参数,

是传感器的测量模型。通过贝叶斯理论(Bayes Theorem)将多次传感器测量结果融合到同一个Cell中，从而获得高可信度的网格占用概率。

3.1 贝叶斯理论(Bayes Theorem)更新存在的问题

重复的浮点数乘法运算导致计算结果的数值变得很小而难以精确表达和运算。Logit函数可以把自变量从(0,1)连续单调地映射到正负无穷。logit函数的定义如下：

所以我们使用Logit函数替代标准的Bayes更新过程。

3.2 贝叶斯更新过程的推导

贝叶斯理论(Bayes Theorem)更新网格(Cell)占用概率的公式如下：

根据一阶马尔科夫(Markov Assumption)假设，t时刻的状态只与t-1时刻的状态有关，因此公式(1)可写为如下形式：

对测量模型应用贝叶斯(Bayes Theorem)更新过程：

将公式3)代入公式2)，可得：

然后计算1-p的值：

将p和1-p代入logit函数：

对公式6）等号两侧取log，进行整理后，得到：

于是得到Bayes更新递推公式：

其中:

是Inverse Measurement Model，

是网格i在t-1时刻的置信度(belif)，

是Initial belief。

可以看到，该递推公式应用的关键是Inverse Measurement Model：

，如何计算该值呢？

3.3 Inverse Measurement Model

占位栅格地图的传感器测量模型为：

，表示基于已有的地图Cell概率，叠加传感器测量结果，得到新的占位概率值。

而现在我们要求解的是：

，这也是为什么该公式被成为Inverse Measurement Model的原因。

下面来看看Inverse Measurement Model如何计算？下面以二维激光雷达扫描模型来说明(注意：实际应用的激光雷达是3D的，这里用2D Lidar是为了简化模型，所用理论可以很好推广到3D模型)。

2D Lidar模型

它在2D平面上进行扫描，包含两个参数：Scanner bearing和Scanner rangers。Scanner bearing均匀的分布在

之间，一般的我们可以认为它们均匀分布在360度的各个方向上。Scanner rangers是从Lidar中心到障碍物的距离，Lidar发出激光、接收回波，从而计算出到周围障碍物的距离；为了简化期间，我们也假设Lidar发送激光后立即收到回波，不存在时间延迟。

Map坐标系&Vehicle坐标系&传感器坐标系

数学模型构建过程中坐标系是不可或缺的。这里主要涉及到三个坐标系：Map坐标系、Vehicle坐标系以及传感器坐标系。2D Lidar的测量结果都是相对于自身传感器中心的，即以2D Lidar中心为坐标原点；所有的测量结果最终都要转换到Map坐标系，完成地图制作的计算。

Lidar测量结果与Map Cell关联匹配

如何将2D Lidar模型与Map Cell关联起来呢？如下图所示，第i个Map Cell用

表示。

然后通过2D Lidar bearing与Map Cell相对于传感器的方位进行最小误差匹配，得到影响当前Map Cell的激光束。

匹配的过程如下：首先定义两个值

和

，各个网格Cell的概率计算如下：

如下图所示，红色区域为高概率被占用区域，灰色区域为未知区域，其余区域为低概率被占用区域。

至此，有了Inverse Measurement Model，Bayes更新的过程可以正常进行了。

更高效的Inverse Measurement Model计算方法

采用光线跟踪(Ray Tracing)的Bresenham's Line Algorithm可以大大减少复杂的浮点数计算，提升计算效率。

4、移除Lidar中地面和动态物体

实际应用中的激光雷达(Lidar)是3D的，会扫描到大量的地面点，这些地面点如果不被移除，按照计算匹配模型，会被当做障碍物处理。所以需要将地面点点云数据从激光雷达点云中移除掉。如何移除呢？一种可行的方法是，通过自动化识别算法从Lidar点云中将地面识别并剔除。

地面识别的难度是比较高的，因为很多道路路面内外的界限在点云中是不明确的，自动化识别算法会误把道路边界外的区域识别为道路路面，从而导致错误的地图信息等。通过视觉分割算法辅助点云识别可以提升路面的识别率。

动态物体(行人、车辆等)也需要从点云数据中移除，这依赖于基于点云和图像的感知技术。但同样也存在很多技术难题，比如如何提升识别的准确率，如何将静止的车辆识别出来等等。

参考链接

本文主要整理自Coursera自动驾驶课程：Motion Planning for Self-Driving Cars第二周课程的学习笔记。