自动驾驶定位系列教程十三_ 基于地图的定位

自动驾驶定位系列教程十三: 基于地图的定位

一、概述

通过前面的工作，我们已经完成了点云地图的建立。接下来要做的就是基于这张地图做定位了。

定位任务其实就一句话：把当前点云和地图做匹配。

为了完成这句话的内容，需要考虑这样几个问题：

1）由于地图比较大，所以加载地图之后需要做一个滤波。

2）由于匹配需要初始位置，所以要做初始化

3）地图比较大，直接和整张地图匹配，会非常耗时间，但是匹配其实只需要附近的点云就可以了，所以需要从大地图中分割出一个小地图。

4）每移动一次就重新分割一次小地图，同样很耗时间，所以小地图面积可以适当大一些，当车辆快要离开小地图时再重新分割。

解决了以上问题，就完成定位了。

附赠自动驾驶学习资料和量产经验：链接

二、流程及代码实现

1. 地图加载

由于原始地图是非常大的，地图要经过两次稀疏。第一次是加载之前，这一步其实在建图模块生成地图的时候就已经完成了。第二次是加载之后，根据定位时局部地图所需要的稀疏度再进行一次滤波。

为什么要经过两次？是为了调试方便。

第一次滤波可以使定位模块在加载地图时能够更快地加载，因为我们只需要加载滤波后的地图就可以了。在第二次滤波中，我们往往想更详细地了解地图稀疏度和定位质量之间的关系，所以第二次滤波参数往往设置成可调的。所以第一次滤波的稠密度应该设置成第二次滤波的稠密度调节范围的上限，这样就能够同时兼顾到加快加载速度和调节稠密度这两个需求了。

在地图文件夹中，生成的原始地图名字是"map.pcd"，生成的稀疏地图名字是"filtered_map.pcd"，在定位模块的配置文件中，把文件路径设置成"filtered_map.pcd"的路径就可以了。

2. 位姿初始化

由于建图时是以gnss位置为基准进行优化的，所以在初始化时也把第一帧gnss位姿赋值给匹配算法就可以了。

完成初始化以后，就可以只使用雷达点云定位，而不需要gnss数据了。

3. 局部地图分割

从全局地图中分割出局部地图采用的是pcl中的CropBox方法，它的基本原理就是从一个大的点云中按照一个长方体尺寸分割出一块小的点云出来。

局部地图的大小在配置文件中可以设置，对应变量是box_filter_size，六个参数代表的是以长方体内某一点为原点，x、y、z三个维度方向的长方体边界坐标。其实这六个参数就决定了局部地图分割的频繁程度。

下面我们就要设置这个长方体原点在全局地图中的坐标，这个其实就是当前帧点云定位结果，直接使用即可。

接下来要看什么时候更新地图，目前的策略是当前位置和立方体的边缘距离小于50米时，则以当前位置为原点，按长方体尺寸重新分割出一个局部地图来。

4. 代码实现

在工程目录下创建了matching文件夹，里面的matching.cpp文件就包含了建图模块的核心代码，以上提到的各功能都在这个文件里，这个模块功能比较简单，代码还算清晰，所以就不在这里贴了。

三、运行及效果

在配置文件中设置好地图路径，按如下指令启动程序

roslaunch lidar_localization matching.launch

之后播放bag文件。

然后就可以在rviz中看到这样的定位效果了