2.3.3 SLAM算法

同步定位与地图构建（Simultaneous Localization and Mapping, SLAM）是指搭载特定传感器的主体，在没有环境先验信息的情况下，运动过程中建立环境的模型，并同时估计自身运动。目前，SLAM主要应用于机器人、无人机、无人驾驶、AR、VR等领域，用途包括传感器自身定位及路径规划、场景理解等功能。

根据传感器种类和安装方式的不同，SLAM实现方式主要分为基于激光传感器的激光SLAM和基于视觉传感器的视觉SLAM（Visual SLAM,VSLAM）两大类。其中，激光SLAM比VSLAM起步早，在理论、技术和产品落地上都相对成熟。VSLAM方案目前主要有两种实现路径：一种是基于RGB-D（RGB Depth）地图的深度摄像机，如Kinect；另外一种是基于单目、双目或鱼眼摄像头。

1. 激光SLAM

激光SLAM技术指搭载激光雷达的主体在运动中估计自身位置与姿态，与此同时建立周边环境地图。精确定位依赖精确地建图，而精确建图离不开准确定位。激光SLAM所需传感器一般有激光雷达、惯性测量单元、里程计。更新频率较高的激光雷达可实现里程计的功能。由于激光雷达具有误差模型简单、测距比较准确、点云处理容易、在光照变化明显的环境中运行稳定等特点，因此激光SLAM理论研究相对成熟，落地产品丰富。由于激光雷达能够快速地采集到的物体信息（激光点云）的准确距离与角度等丰富的信息，激光SLAM技术成为无人驾驶定位稳定的解决方案。

激光SLAM系统的工作原理是通过对不同时刻两帧激光点云进行比对与匹配，然后计算激光雷达载体在前后两个时刻的相对距离和姿态变化，最终完成载体定位。

（1）激光SLAM建模框架

激光SLAM建模框架分为前端扫描匹配、后端优化、闭环检测、点云地图构建四个核心模块，如图2-23所示。前端扫描匹配，是激光SLAM的关键步骤，工作原理是已知前一帧位置与姿态并利用下一帧的位姿变换关系估计当前帧的位姿；激光雷达的扫描频率决定前端扫描匹配间隔的位姿变换与局部地图，但是由于不可避免地出现帧间匹配的累计误差，因此需要后端做长时间增量式扫描匹配后里程计及地图信息的优化；闭环检测环节的任务是，通过检测历史帧判断是否曾经到过此地从而减少全局地图的漂移，进一步减少前端匹配所产生的累计误差，以便生成全局一致性地图；激光点云地图构建的主要任务是，生成和维护全局地图。

（2）点云匹配模型的建立

搭载激光雷达的无人车在处于运动状态时会实时构建环境点云数据，地图点云数据会随时间与行程的增加而累积，要匹配的激光点云与地图数据不断扩大，搜索量随即增加。在搜索步长不变的情况下，为保证匹配的精度，点云匹配的耗时也必然增加。为了控制匹配时间，激光SLAM的前端拟采用时间前后两帧点云匹配方法[30]。

该匹配方法的原理：以第一帧载体所在位置为建立全局地图的坐标系原点，以后所有帧都是基于第一帧与前一帧的点云进行匹配，定义第n次扫描的数据为d_n，第i次匹配过程，首先将d_i作为待匹配数据向d_i-1进行匹配，然后根据d_i-1与d₁之间的变换矩阵，将d_i转换至d₁所在坐标系下。当前帧点云（x_i,y_i）与上一帧点云（x_i-1，y_i-1）进行配准，获取旋转角度为θ_i，平移量设为（Δx_i，Δy_i），旋转平移后得到的点云为上一帧点云与起始点第一帧点云存在准关系，即角度为，平移量为，匹配后的点云为，变换关系如下：

因此

其中

已经获得的作为当前帧与原始点集配准的参数。通过此参数将当前帧的点集转换到全局地图所处的坐标系下，完成当前时刻的点云匹配。然而，由于时间前后两帧点云配准的配准会存在一定的误差，当多帧匹配时，误差便会累积。假设第i-1次匹配过程中引入误差，定义第i次匹配时d_i向d_i-1匹配的误差为，因此通过位姿变换关系转换到d₁上时所引起的误差可表示为

因此每一次匹配均有的增量。当激光雷达运动为直线时，由于角度的变化不大，可以认为当前的角度，因此累计误差为

由于每一次的匹配均有的误差增量，所以随着匹配的数量上升，累计误差随即增加，综上为点云匹配的误差建模的建立过程。

2. VSLAM

VSLAM是将图像作为主要环境感知信息源的SLAM系统。VSLAM以计算相机位姿为主要目标，通过多视几何方法构建三维地图，VSLAM还处于实验室研究阶段，实际应用较少。

VSLAM系统的处理过程一般都是分为2个阶段：帧间估计和后端优化。在整个SLAM系统中，帧间估计是根据相邻两帧间的传感器信息获取该时间间隔内的运动估计，后端优化指对之前帧间估计产生的路径累积漂移误差做优化，解决机器检测到路径闭环后历史轨迹的优化问题。与激光SLAM相比，VSLAM对色彩和纹理等信息更敏感，在提高帧间的估计精度和闭环检测方面有巨大潜力。

一般的VSLAM分为特征点法和直接法。特征点法，首先从每帧图片中提取分布均匀的角点和周围的描述子，通过这些角点周围的描述子的不变性完成帧间匹配，接下来使用对几何恢复相机姿态并确定地图坐标，最终根据最小化重投影误差完成相机位姿和地图的微调。直接法是根据光度误差确定相机位姿和地图的，不用提取角点和描述子。正因为这样，直接法不能表征一张图像的全局特征。另外，直接法的闭环检测面临的累积漂移的消除问题一直没有得到很好的解决。

近些年，计算机视觉与深度学习相互结合，促使视觉相关任务的准确率、执行效率及鲁棒性等实际表现得到巨大提升，如实例分类[31]、对象检测[32]、行为识别[33]等领域的表现。VSLAM系统以计算机视觉为基础，为神经网络在该领域的应用提供很大的发挥空间。将深度学习与VSLAM结合[34]，有以下优势：

1）基于深度学习的VSLAM系统有很强的泛化能力，可以在光线复杂的环境中工作。

2）对于动态物体的识别和处理更加有效。

3）采用数据驱动的方式，对模型进行训练，更符合人类与环境交互的规律。有很大的研究和发展空间。

4）采用神经网络可以更好地将图像信息与其他传感器的数据融合，优化帧间漂移。

5）更高效地提取语义信息，有益于语义SLAM[35]的构建和场景语义的理解。

6）端到端的VSLAM，舍去前端点跟踪、后端优化求解的一系列过程，直接输入图像给出位姿估计。