高精地图之3D栅格地图的应用

什么是3D栅格地图

3D栅格地图的定义认为一个真实的三维世界可以用一个概率模型来表示。

比如像今天这个会场，我们可以将它离散化成一个个立体方格，每个方格里会存储一个概率，这个概率即方格空间不为空的概率。

上图是一个 3D 栅格地图，从数学的角度讲，它并不是一个连续的描述，而是离散化后的地图，只会保留一些概率值和属性。这里展示的是一张厘米级精度的栅格地图，我们可以很清楚看到周围的环境，包括树、建筑等等。

SLAM与自动驾驶

那么，如何制作高精地图呢？传统上有一种地图制作方法叫SLAM（即时定位与地图构建）。它指运动物体根据传感器的信息，一边计算自身位置，一边构建环境地图的过程。目前，SLAM的应用领域主要有机器人、虚拟现实和增强现实。其用途包括传感器自身的定位，以及后续的路径规划、场景理解。

传统SLAM与自动驾驶SLAM的区别

传统 SLAM：

2D；

视觉传感器，单靠摄像头，在某个空间转来转去，最终通过算法来建模；

没有 GPS 信息，很多室内场景是收集不到 GPS 信息的；

生成地图和定位同时进行。

自动驾驶SLAM：

3D，描绘的是一个三维环境；

激光传感器，需要达到比较高的精度；

GPS 信息，室外场景的优势就是可以接收 GPS 信息；

离线处理，数据不需要在线处理，前面介绍过，如果选择在线，某些场景算法无法得到良好的处理，所以需要抽取一些信息来做离线的验证。

3D栅格地图的挑战

相比于传统 SLAM，我们来看下有哪些问题是3D栅格地图需要解决的：

全局 GPS 信息并不总是好的。上图左下角是3D栅格地图的底图，里面包含一条当时车辆的行驶轨迹。你会发现依据 GPS 的这条行驶轨迹，这辆车已经开到马路牙子（路肩）上了，但实际情况并非如此。因此，当拿到这样一份地图数据时，我们需要从算法层做出优化，并且做好细节。

不同时间段的数据如何align（校准）。一般来说，地图的采集不可能一次完成，因此，从算法层面上看，3D栅格地图需要处理把不同时间段的数据校准到一起的问题。

去除非静态的物体。我们所建立的是一个概率的模型，3D栅格地图只需要保留一些静态的物体以辅助感知系统，许多非静态的物体都需要从地图中去除。

应对挑战：位姿图 Pose Graph

位姿图其实是一个优化问题

整体上我们认为位姿图（Pose Graph）是一个优化问题。假设将路线采集以时间去划分——比如每隔4分钟采一个点，最后能得到不同的位置的环境。举个例子：在一个大厅里，我现在站在某点，一分钟后我可能站在另一处，再绕一圈的话后站在门口，通过把整个GPS获得的位置信息离散化之后，将我在每一点所看到的周围的环境直接拼起来，就可能得到一个三维的地图，因为每个点看到的角度都是不一样的。

但因为原始位置可能跟真实位置有偏差，直接拼起来常常导致很多问题。所以我们把这些点离散化之后，希望能够把所有点都对齐调整到准确的位置。那怎么确保最终的结果是正确的？

对整个图来说， 除了顶点之外，还有边。我们希望通过边的约束将点调好，边的约束你可以认为是个相对约束，假设我知道下个点的准确位置，那么便可倒推另一个点的准确位置。也就是说，在有准确的相对位置的基础上，辅以一些比较少的绝对位置，就可以得到一个全局的准确位置。

总的来说，位姿图的优化目标是把整个图离散化到一个个点之后，通过建立一些边的约束，最终通过优化某些点的位置来满足边的约束。位姿图也就成为了一个优化问题。

位姿图的挑战

位姿图整体的数学模型看似简单，但是存在许多细节问题需要做优化：

◆ 如何选择顶点。比如需要考虑是不是毫秒的点都要加进去，GPS 信号不好时的点要不要加进去。

◆ 如何保证边的准确性。一个顶点到另一个顶点的相对位置，需要通过什么方式去建立。

◆ 如何求解非线性优化。图建好之后如何去做非线性的优化。

◆ 如何评估优化效果。图是否和真实的环境一致，以及如何评估结果满足要求。

◆ 效率和资源。对于工程来讲，数据量这么大，你需要考虑怎么设计工程算法以保证效率和资源。比如说，考虑是不是需要在算法基础上进行定制优化，如何用 GPU 或者分布式的计算方式。

我想通过上图右边的例子解释位姿图——左侧原始的图展示的是有一个物体来回在球面上运动旋转，通过在边与边之间建立相对约束，进行优化后，你会发现整个环境的重建成了右侧一个比较理想、完整的球体，概括来说，我们的工作就是需要把左侧的图来变成右侧的图。

迭代最近点算法：保证边的约束

我们前面提到位姿图需要保证边的约束，为了计算出这个约束，业界用的比较多的传统经典算法叫做 ICP（Iterative Closest Point algorithm）——迭代最近点算法。

举个例子：左边是只红色的兔子，右边是只蓝色的兔子，现实中这两只兔子的位置是一样的，但在图片里两只兔子位置不一样，说明位置存在偏差。也就是说，输入是两组点云 A 和 B，输出是两组点云之间在空间上的旋转和平移。

具体算法的思路大致是：找到点云集合 A 中的每个点在集合 B 中的对应点，通过求解最佳的刚体变换，不断的迭代优化，最终得到一个收敛解。

应对挑战：去除非静态障碍物体

去除非静态障碍体的方法大致有三类：

◆ 通过概率模型去除。

◆ 点云处理，单纯的概率模型是不够的，例如有时候小区路边经常停着车，而某些时候车是被开走的。数据采集时，恰好车停在路边，后续那就需要依赖离线点云处理。

◆ 机器学习，即利用机器学习算法去把非静态的物体从地图里面抠出来。

上图是一个简单的去除非静态障碍物前后对比图，处理之前，你会发现右侧道路有一些车的轮廓，当从空间上去除之后，地图变得清晰许多