【SLAM】同时定位与地图创建在无人车领域的应用探索

毕业答辩总算结束了，主要研究的是GraphSLAM进行地图创建的可行性。其中，理论部分继承了前一篇博客的内容，实验部分在室内室外两个场景下分别进行了尝试，最终结果还是不错的。
下面我按照答辩的顺序，简要梳理一下毕设的思路。

研究背景

同时定位与地图创建也被称为SLAM（Simultaneous Localization and Mapping），简单来说就是机器人在自身位置不确定的情况下，探索静态的未知环境，并且同时建立地图和定位的技术。
它的应用平台也十分广泛，从家用的扫地机器人到道路上行驶的无人车，都离不开SLAM技术的支持。
那么我们为什么要研究SLAM在无人车上的应用呢？上文也说了，目的是为了进行高精度的地图创建。在城市道路上行驶的无人车需要对自身有较为准确的定位，而GPS会受到高楼、树木等干扰，一般会有10m左右范围的误差，这一误差对于无人车而言是不可接受的：比如环路主路和辅路常常只隔了一个车道（2m左右）的距离。因此，如果有一个高精度地图，无人车根据传感器观测与地图进行实时匹配定位，就可以将定位精度提高到车道级别，满足实际需求。

SLAM过程示意

下面用一副图片简单的描述一下SLAM的过程：

无人车按照红色箭头所示的路径行驶，在这个过程中，它会通过各种传感器（GPS、惯导、编码器等）对自身进行定位，但是这个定位肯定是存在误差的，我们可以认为这个误差服从高斯分布，就用图中黑色的模糊点云表示。无人车行驶的过程中，定位的累计误差会越来越大。同时，外部传感器可以观测到环境中的很多参照物，也被称为Landmark，观测车辆和Landmark的相对位置可以为SLAM提供很多有用的信息。
另外，当无人车绕了一圈回到原点的时候，就产生了一个闭环。在SLAM问题中，无人车此时必须知道自己回到了原点，并且根据之前的观测校准当前的位姿。闭环成功之后，可以消除累计误差。在地图和车辆行驶轨迹没有真值的时候，闭环检测是评判SLAM方法好坏的重要标准之一。

SLAM问题定义

SLAM问题可以用这张图来形象的表示。

图中的xt表示无人车的位姿，m 表示静态的环境地图，它们是SLAM问题中的未知量，而ut 和 zt 则分别是无人车的控制量（速度、转向等）和观测量（由外部传感器获取），它们是SLAM问题中的已知量。
SLAM问题面临的主要难点有：

• 控制量和观测量都存在误差，想通过带误差的已知量去估计精确的地图和定位
• 制图和定位两个部分相互依赖，类似“鸡生蛋和蛋生鸡”的关系
• 闭环检测

除了这几个SLAM问题本身的难点以外，在无人车上应用SLAM还面临着一些特殊的问题：

• 动态环境：无人车行驶的环境中充满了行人、车辆等动态物体，而SLAM数学模型中定义的地图是静态的
• 室外场景的结构性很差：相比于SLAM通常应用的室内场景，室外比较少有清晰的墙壁、走廊这些特征
• 室外环境复杂：这点显而易见，想要从环境中提取有效的特征进而用于定位和建图是一件非常困难的事情

基于图模型的SLAM方法

SLAM主要有两种技术路线：滤波器模型SLAM和图模型SLAM，其中图模型的也被称为GraphSLAM或Graph-Based SLAM。
本课题主要研究的是GraphSLAM，它的特点是先将SLAM问题转化为图模型，再将图模型的优化转化为非线性最小二乘问题。
这个过程中引入了两种约束，在上一篇博客中也提到过，它们分别是：
运动模型约束

(xt−g(ut,xt−1))TR−1t(xt−g(ut,xt−1))

观测模型约束

(zit−h(xt,mj,i))TQ−1t(zt−h(xt,mj,i))

对应到刚才的图中就是这样：

其中红色的箭头对应了运动模型约束，它们是由车辆的位移产生的；黑色虚线对应了观测模型约束，他们是由外部传感器的观测所产生的。这些约束构成了无人车位姿节点xt 之间的约束边，将它们综合到一起就得到了GraphSLAM的优化目标函数：

JGraphSLAM=xT0Ω0x0+∑t(xt−g(ut,xt−1))TR−1t(xt−g(ut,xt−1))+∑t∑i(zit−h(xt,mj,i))TQ−1t(zt−h(xt,mj,i))

得到优化函数后，调用Ceres、g2o、SPA等后端优化库就可以很方便的进行优化了。

系统框架

本课题的系统框架如下图所示：

本研究在理解GraphSLAM算法的基础上，使用无人车平台在室内室外场景下进行了实验，目的是为了进行高精度地图的创建。在算法实现方面，GraphSLAM的前端主要参考了开源项目OpenKarto，我将其改为了离线版本，通过ROS系统进行通信、可视化等；后端优化部分使用了SPA。

室内实验

室内试验的平台使用的是小型的无人车POSS-Mini，大概一台笔记本电脑的大小。实验场景在逸夫楼二层的环形走廊。
下图是建图的对比：左图是航位推算法结合激光帧匹配的结果，右图是GraphSLAM的结果。

下面是红圈部分的放大图：

将建好的图模型可视化的效果：其中红色的点是图模型中的节点，也就是车辆的位姿；蓝色的边是节点之间的约束，可以看到闭环处，开头和结尾轨迹的节点之间由于闭环检测产生的约束边。

室外实验

室外实验使用了实验室的Jeep牧马人作为无人车平台，路线是绕着理科楼群环绕两周，大概2km左右的距离：

室外建图的结果，以及局部对比图：

总结与展望

本课题在理解了GraphSLAM算法的基础上，使用无人车进行了实验和探讨，研究了其在室内室外两种环境下进行高精度地图创建的可行性。从实验结果来看，建图的结果还是不错的，但是仍然有可以改进的空间。
今后可以尝试在图模型中加入GPS约束，提高闭环检测的鲁棒性。
更长期的来看，还可以研究GraphSLAM在长期（Long-Term）动态环境中如何有效地更新和维护的问题。

这次借这个机会简单的总结一下之前的工作，也希望之后的研究和生活能够一切顺利吧~

最后，可爱的小胖刺镇楼~