无人驾驶高精度定位技术（4）-城市环境下基于概率地图的定位技术

本文是斯坦福大学2010年的论文>的阅读笔记。

论文概述

在动态城市环境下，基于GPS的惯性导航系统无法提供足够的定位精度。本文提供的基于概率地图的定位方法可以提供很高的定位精度、持续学习和改善的地图、应对环境变化与动态障碍物的高鲁棒性定位能力。具体的实现策略如下：

1、把地图空间分成一个个的小网格，每个网格以高斯分布的方式记录了落在该网格内的激光反射值的强度；

2、采用贝叶斯定理优先加权地图中最可能是静态物体和角反射率一致的物体，从而减小地图的不确定性和灾难性的误差。

3、采用离线SLAM来对多次经过同一地点的激光点云对齐。随着时间的累积和同一地点被采集次数的增加，概率地图对环境的描述会越来越精确，从而使得基于概率地图的车辆定位也越来越准确。

安装64线激光雷达的无人驾驶测试车辆

在实验中，我们的车辆配备GPS/IMU以200HZ的频率来完成惯性更新和全局位姿估计；Velodyne 64线激光雷达以10HZ的旋转频率，每秒可以提供大约100万的3D激光点以及对应的激光反射值数据，如此高密度的点云数据保证了每一个地图网格都可以覆盖很多次，从而使得我们能够针对每一个网格计算激光反射值强度的方差。

概率地图

我们的终极目标在于构建一个能否描述物理环境的网格地图，其中每个网格存储了落在该网格内的激光点的反射值强度的均值和方差。我们采用了三个步骤来生成概率地图：1) 采用后处理方法保证重复覆盖区域的采集轨迹对齐；2) 对每一个激光束的反射值强度做标定，保证不同激光束针对相同物体相同的反射率;3) 把从对齐的轨迹上返回的标定后激光点云投影成一个概率地图。

采用GraphSLAM做地图轨迹对齐

相邻的车辆Pose之间通过里程计和惯导数据构建连接关系，车辆的Pose采用全局的位置估计，从车辆的Pose轨迹寻找匹配区域，用alignment offset构建匹配邻近区域的边约束，然后采用图优化理论做全局最优化。

匹配区域的alignment offset通过对每一个区域5帧连续的360度激光扫描的点云做ICP计算获取，x, y, z, roll,yaw,pitch都需要做优化。

一个城市区域的激光反射值网格地图，网格大小15cm*15cm

激光束标定

对于经过良好标定的激光雷达，这一步可以跳过，但是对于标定效果差的激光雷达，如果不标定，对生成概率地图会有很大影响。

1) 每个网格得激光反射值大小完全依赖于哪个激光束打在这个网格中；

2) 每个网格反射值强度的方差会被放大；

可以看到标定之后不同激光束的强度基本一致了

在实践中，我们发现不需要每次使用都对激光雷达做标定校准，但是完全不校准直接使用原始的激光反射结果是非常不靠谱的。因此只要对激光雷达做一次标定校准，以后就完全可以依赖这个结果了。

在标定过程中，我们不是对每个激光束计算一个转换参数，而是针对每个激光束计算完整的响应曲线，然后通过影响曲线把观测到的反射值强度转换为最终的输出反射值强度，这种方法虽然复杂但是很有效，因为每一个激光束都可能有自己独特的非线性的响应函数。

我们采用非监督的方法使得标定过程在任意环境下进行。标定的过程如下：

a) 记录落在每个地图网格(map cell)的激光束ID、反射值强度、点云坐标；

b) 假设要映射ID为j的反射值强度为a的响应值为c(a, j)，c(a,j) = 所有ID为j的反射值强度为a的所有地图网格的平均反射值强度；对于不存在的反射值强度a，可以通过插值的方式获取。

c) 激光雷达的反射值强度为0-255，对于64线的激光雷达，最终的标定结果为64*256的映射表。

地图创建

创建反射值地图的过程就是计算落在每个地图网格(Map Cell)中的激光点的反射值均值和方差的过程，本文中的网格大小设置为15cm*15cm。想对于Camera，激光点云数据反射值地图可以彻底解决阴影区域的问题。

在创建的地图中不可避免会引入动态的物体(主要是车辆)，这部分动态的物体很难彻底从地图中剔除，但是在包含动态物体的网格的方差一般都比较大，这样的地图用作定位时，即使存在车载传感器采集的反射值强度与地图不匹配，也不应该对实时采集的反射值强度做过大的惩罚措施。

在线定位

环境地图构建完成后，我们就可以用来它来做实时定位。

运动更新(Motion Update)

GPS/IMU系统以200HZ的频率做惯导更新和全局位置估计，通过对惯导输出做积分，可以得到一个平稳连续变化的smooth坐标系统，它与全局坐标系统的真值坐标的offset可以通过一个均值为0的随机高斯噪声模拟建模。Motion Update按照如下方式更新每个网格的概率。

其中：

在实际应用中，由于定位更新频率非常高，定位偏移量非常低，所以我们一般只需要考虑网格相邻的2~3个网格的范围就够了。

测量更新(Measurement update)

利用激光雷达返回的反射值测量数据构建数据网格，并与事先采集的地图进行比较。假设z为激光雷达的测量值，m为事先制作的地图，x，y为与真实坐标的偏移，根据贝叶斯定理：

其中：

因此：

即使使用高端惯导设备和差分GPS，GPS定位容易出现偏移

本文用到的定位方法，即使旁边有大客车之类的动态障碍物，定位也没有明显偏差

位置估计

计算出后验概率后，最后一步是计算x和y的offset最优估计。从理论上讲，应该取

作为坐标位置的最优估计。但是由于多峰分布的最大值会在多个峰值之间不连续的跳动，导致定位结果跳变，所以我们采用后验概率质心来表示定位的位置。

试验结果

定位的横向均方根误差由66cm降至9cm

定位的纵向均方根误差由87cm减少至12cm