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

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

自动驾驶运动规划中提到可以使用占位图(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)制图

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

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

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

3.2 贝叶斯更新过程的推导

3.3 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坐标系，完成地图制作的计算。

假设2D Lidar在Map坐标系中的姿态为是传感器朝向。通过该姿态，可以将2D Lidar测量结果转换到Map坐标系。

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

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

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

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

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

更高效的Inverse Measurement Model计算方法

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

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

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

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

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