大范围室外环境三维激光点云SLAM建图

一家之言，仅作分享，如有不合理或需要改进的地方，欢迎各位讨论。

前言

    在之前项目中为提高系统定位精度，开发激光SLAM建图方案。场景是城市道路，车辆装备RTK-GNSS，IMU和轮速计。离线SLAM建图过程是从三维激光点云中提取地面、中层和高层特征，分别建立同一场景的多层次二维地图，提高对复杂环境的适用性。在karto与cartographer的基础上进行了改进，结合大范围场景特殊性，新增GPS约束，有效减少离线建图过程中的定位累计误差，从而减少闭环时的搜索范围，自动处理大范围场景中离线定位闭环问题。在线定位时采用多层次地图匹配方案，在粒子滤波框架下实现地图匹配定位算法。

多层地图

    地图的表示形式一方面会影响建图时的效率，另一方面会影响在线定位的精度和鲁棒性，一个有效的地图表示方法需要在简化特征复杂度的同时保留有效特征信息，且信息本身需要在一段时间内保持稳定。不同于视觉城市众包方案中可以进行语义特征提取那样保证特征鲁棒性（因为没能力对激光雷达的语义特征提取 -_-），选择根据实际情况合理对三维点云进行多层次划分并建图。在应用环境中，地面和建筑物稳定性较高，因此地面特征和高层特征不易受到干扰；而地面与高层之间的特征却受到移动物体的干扰严重。然而，部分开阔场景地面标线模糊且缺乏建筑物信息，利用地面与高层的特征无法有效定位。因此，在地图表示中仍需考虑中层特征，但就需要对移动物体进行过滤。场景的环境特征因此可划分为：

1. 地面特征–地面，路面标线；
2. 中层特征–包含高度靠下的障碍物特征（路障、花坛、行人和车辆等），特征的数目较多，但受到一定程度移动物体的影响；
3. 高层特征–包含高度较高的障碍物特征（建筑物、灯杆等），特征的数目相对中层较少，但移动障碍物的影响也较小。

每一层特征为一个地图，均使用占据栅格地图表示。确定不同层次的特征，首先要确定地面平面系数，这里的方法是用RANSAC确定地面平面，在之前的博客中有介绍方法。RANSAC思想解决平面拟合问题

复杂大尺度环境下的闭环检测

    离线的地图创建是一种逐帧迭代计算的过程，存在误差积累的问题，其典型的表现为“闭环问题”。闭环问题是指机器人再次访问曾经访问过的场所时，由误差积累等的原因，第二次访问所估计的位置与第一次不吻合，导致创建的地图有偏差。通过匹配两次访问的数据，并通过优化过程轨迹和地图，可以显著地减小累积误差。两次访问的间隔/距离越大，误差积累越严重，自动的闭环检测也越困难。然而复杂大尺度城市环境中，自动驾驶车辆往往经过很长时间/距离的驾驶才能回到过去访问的地点，甚至并不回回到过去访问的地点，传统的闭环检测方法不适用。 考虑到实际场景为室外，因此引入 GPS 约束把闭环前的累积误差降低在米级，使得闭环的搜索范围缩小到可计算的区间。

    闭环检测的依据是节点的欧式距离，采用的逻辑与karto和cartographer一致。
其中候选chain的计算流程总结如下

相关性扫描匹配

    里程计部分的运动估计方法采用的是相关性扫描匹配（Correlation scan match，CSM）。相关性扫描匹配在karto与cartographer中都有使用，相关文献也有很多，这里不做赘述，只贴上当初总结的方法流程图

位姿图优化

    在位姿图模型构建过程中，最容易出错的是闭环检测所构造的匹配边。当车辆行驶一段很长的距离回到原点时，如果期间没有经过相同场景形成闭环，则会因为累积误差过大导致闭环检测完全失效。GPS 可以在闭环处将两者的误差缩小到米级，使得闭环检测的搜索范围缩小到可运算的区间。另一方面，如果仅考虑节点之间的约束，而不考虑全局坐标。所获取的定位结果虽然逻辑上可以自洽，但有可能整体产生了偏移。因此，考虑引入 GPS 约束来控制定位结果的整体偏移。
    位姿图优化采用的是ceres库，节点是车辆位姿，边有三种：相邻边；GPS约束边；回环边。优化方法不做赘述了，在之前的博客有简单源码介绍。
Ceres解决实车SLAM后端优化问题