一文详解SLAM关键技术

来源：我想我思

SLAM(Simultaneous Localization and Mapping)同步定位与地图构建,一种同时实现设备自身定位和环境地图构建的技术。

原理是使用相机、激光雷达、惯性测量单元等传感器，来收集环境信息，然后用算法将这些信息融合起来，以确定设备在未知环境中的位置，并构建一张环境地图。

通俗解释为：

用眼睛观察周围地标如建筑、大树、花坛等，并记住他们的特征（特征提取）

在自己的脑海中，根据双目获得的信息，把特征地标在三维地图中重建出来（三维重建）

当自己在行走时，不断获取新的特征地标，并且校正自己头脑中的地图模型（bundle adjustment or EKF）

根据自己前一段时间行走获得的特征地标，确定自己的位置（trajectory）

当无意中走了很长一段路的时候，和脑海中的以往地标进行匹配，看一看是否走回了原路（loop-closure detection）。

SLAM包括以下几个方面

传感器：激光雷达、摄像头、惯性测量单元（IMU）、里程计等。这些传感器可以提供设备在运动过程中所需要的信息，如距离、角度、速度、方向等。

设备运动模型：运动模型用于估计设备的位姿，例如里程计模型、速度模型、加速度模型等。

视觉算法：视觉算法主要用于设备在环境中的感知和定位，如特征提取、特征匹配、图像配准等。

滤波算法：滤波算法用于估计设备的位姿和地图中不同位置的概率分布，如卡尔曼滤波器、粒子滤波器等。

优化算法：优化算法用于优化设备的位姿和地图，如扩展信息滤波器、位姿图优化等。

地图表示：地图表示方式可以有多种，如栅格地图、拓扑地图、点云地图等。

常用的SLAM传感器

激光雷达：激光雷达是一种通过发射激光束来探测目标并测量距离的传感器。它可以提供高精度、高分辨率的距离和角度信息，被广泛应用于SLAM中。

摄像头：摄像头可以通过拍摄环境中的图像来获取场景的信息，如特征点、轮廓等。在SLAM中，摄像头可以用于实现视觉SLAM或者辅助其他传感器进行定位。摄像头种类有单目、双目、单目结构光、双目结构光、ToF。

惯性测量单元（IMU）：IMU可以测量机器人的加速度和角速度等信息，通过积分可以得到机器人的位姿信息。IMU通常与其他传感器结合使用，用于实现多传感器融合。

里程计：里程计是一种通过测量轮子旋转的信息来推算机器人位移的传感器。虽然里程计的精度较低，但在SLAM中仍然是一个非常重要的传感器。

此外，还有其他一些传感器，如GPS、磁力计等，可以用于SLAM中。不同的传感器有不同的特点和适用场景，根据具体的应用需求选择合适的传感器非常重要。

常用的两种地图类型

覆盖栅格地图（Occupany Grid Map）

一种常见的地图表示方式，主要用于描述环境中每个位置的占据状态。在覆盖栅格地图中，环境被分成一个个网格单元，每个网格单元表示一个位置，并记录该位置的占据状态，一般用二进制值表示，例如0表示空闲，1表示占据。

覆盖栅格地图可以通过激光雷达等传感器获得环境中障碍物的位置信息，将障碍物所在的网格单元标记为占据状态。同时，通过机器人的位姿信息，可以将机器人所在的网格单元标记为占据状态，并在地图中绘制出机器人的轨迹。

覆盖栅格地图的优点在于简单、易于实现和可视化，可以用于快速构建地图和进行导航。但是，由于覆盖栅格地图要求环境被划分为网格单元，因此地图的精度和分辨率可能会受到限制。同时，覆盖栅格地图还需要消耗大量的存储空间和计算资源，因为需要维护大量的网格单元。

需要注意的是，覆盖栅格地图只记录了占据状态，并没有记录障碍物的形状和大小等信息。如果需要更精确的地图表示方式，可以使用其他地图表示方法，如点云地图、边缘地图等。

点云地图（Point Cloud Map）

主要用于描述环境中物体的三维形状和位置信息。在点云地图中，环境被表示为一组三维点云，每个点表示一个位置，并记录该位置的属性信息，如颜色、方向量、反射率等。

点云地图可以通过激光雷达、摄像头等传感器获得环境中物体的位置和形状信息，通过点云配准和滤波等算法，可以将多个点云融合成一个完整的地图，用于机器人的定位、导航和避障等任务。

相对于覆盖栅格地图等二维地图表示方式，点云地图的优点在于可以提供更加真实的环境信息，可以描述物体的三维形状和位置信息，同时也可以用于识别和分类物体。但是，点云地图的缺点在于数据量较大，需要消耗大量的存储空间和计算资源，同时对点云数据的处理和配准等算法也比较复杂。

需要注意的是，点云地图的精度和分辨率取决于传感器的精度和采样率等因素，而点云地图的质量对机器人的定位和导航等任务影响很大，因此需要对点云地图进行精细的处理和优化。

SLAM工作原理

不同的SLAM算法，实现的具体细节会有所不同，但一般都包含前端和后端。

前端：

从传感器中获取原始数据，并将这些数据与已有地图进行关联，从而确定机器人轨迹的过程。

数据采集：通过传感器获取机器人周围环境的数据，如激光点云数据、图像数据等。

数据时空同步：将从不同传感器或不同时间戳接收到的数据进行同步，以便后续配准。

特征提取：从采集的数据中提取用于建图的特征点，如关键点、特征描述等。

数据融合：将不同传感器获取的数据融合起来，提高建图的准确性和稳定性。

数据关联：将当前帧的特征与之前的地图，或者其他帧之间的特征进行匹配，以确定机器人的运动轨迹。

运动估计：通过数据关联得到机器人的运动轨迹，可以是平移、旋转等运动。

后端：

根据前端获取的运动轨迹和地图信息，对机器人的状态、地图和传感器误差等进行估计和优化的过程。

非线性优化：通过非线性最小二乘法等，对机器人姿态和地图进行优化，使得机器人的位置和地图更加准确。

回环检测：识别机器人经过的相似位置，避免累积误差的产生。可以有效降低机器人的定位误差，提高SLAM算法的精度和鲁棒性。

根据传感器的不同，SLAM算法可以分为二维激光SLAM、三维激光SLAM，以及视觉SLAM。

二维激光SLAM常用的有Cartographer、Karto，三维激光SLAM较流行的是LIO-SAM和LOAM系列，视觉SLAM主流的方案为ORB-SLAM3、VINS-Fusion

激光SLAM

激光SLAM发展已相对较为成熟，它的研究内容主要包括：传感器数据处理、前端配准方法、回环检测、后端优化、图优化、已知定位的建图、3D激光与视觉融合、多传感器融合。

光探测与测距（激光雷达）方法主要使用激光传感器（或距离传感器）。对比相机、ToF 和其他传感器，激光可以使精确度大大提高，常用于自动驾驶汽车和无人机等高速移动运载设备的相关应用。激光传感器的输出值一般是二维 (x, y) 或三维 (x, y, z) 点云数据。激光传感器点云提供了高精确度距离测度数据，特别适用于 SLAM 建图。一般来说，首先通过点云匹配来连续估计移动。然后，使用计算得出的移动数据（移动距离）进行车辆定位。对于激光点云匹配，会使用迭代最近点 (ICP) 和正态分布变换 (NDT) 等配准算法。二维或三维点云地图可以用栅格地图或体素地图表示。但就密度而言，点云不及图像精细，因此并不总能提供充足的特征来进行匹配。例如，在障碍物较少的地方，将难以进行点云匹配，因此可能导致跟丢车辆。此外，点云匹配通常需要高处理能力，因此必须优化流程来提高速度。鉴于存在这些挑战，自动驾驶汽车定位可能需要融合轮式测距、全球导航卫星系统 (GNSS) 和 IMU 数据等其他测量结果。仓储机器人等应用场景通常采用二维激光雷达 SLAM，而三维激光雷达点云 SLAM 则可用于无人机和自动驾驶。

视觉SLAM

使用从相机和其他图像传感器采集的图像。视觉 SLAM 可以使用普通相机（广角、鱼眼和球形相机）、复眼相机（立体相机和多相机）和 RGB-D 相机（深度相机和 ToF 相机）。视觉 SLAM 所需的相机价格相对低廉，因此实现成本较低。此外，相机可以提供大量信息，因此还可以用来检测路标（即之前测量过的位置）。路标检测还可以与基于图的优化结合使用，这有助于灵活实现 SLAM。使用单个相机作为唯一传感器的 vSLAM 称为单目 SLAM，此时难以定义深度。这个问题可以通过以下方式解决：检测待定位图像中的 AR 标记、棋盘格或其他已知目标，或者将相机信息与其他传感器信息融合，例如测量速度和方向等物理量的惯性测量单元 (IMU) 信息。vSLAM 相关的技术包括运动重建 (SfM)、视觉测距和捆绑调整。视觉 SLAM 算法可以大致分为两类。稀疏方法：匹配图像的特征点并使用 PTAM 和 ORB-SLAM 等算法。稠密方法：使用图像的总体亮度以及 DTAM、LSD-SLAM、DSO 和 SVO 等算法。

视觉SLAM整体流程大致可以概括为五步：

（1）传感器信息读取

在视觉SLAM中主要为相机图像信息的读取和预处理。如果是在机器人中，还可能有码盘、惯性传感器等信息的读取和同步。

（2）视觉里程计 (Visual Odometry,VO)

视觉里程计的任务是估算相邻图像间相机的运动，以及局部地图的样子。VO又称为前端(FrontEnd)。

（3）后端优化(Optimization）

后端接受不同时刻视觉里程计测量的相机位姿，以及回环检测的信息，对它们进行优化，得到全局一致的轨迹和地图。由于接在VO之后，又称为后端(Back End)。

（4）回环检测(Loop Closing)

回环检测判断机器人是否到达过先前的位置。如果检测到回环，它会把信息提供给后端进行处理。

（5）建图(Mapping)

它根据估计的轨迹，建立与任务要求对应的地图。

其整体流程图大致如下

SLAM算法实现4要素

地图表示，比如dense和sparse都是它的不同表达方式，这个需要根据实际场景需求去抉择

信息感知，需要考虑如何全面的感知这个环境，RGBD摄像头FOV通常比较小，但激光雷达比较大

数据关联，不同的sensor的数据类型、时间戳、坐标系表达方式各有不同，需要统一处理

定位与构图，就是指怎么实现位姿估计和建模，这里面涉及到很多数学问题，物理模型建立，状态估计和优化

其他的还有回环检测问题，探索问题（exploration），以及绑架问题（kidnapping）。

6大主流SLAM算法

Cartographer

由谷歌开发的一款基于激光雷达和RGB-D相机数据的SLAM算法。可以跨平台使用，支持Lidar、IMU、Odemetry、GPS、Landmark等多种传感器配置，被广泛用于机器人导航、自动驾驶等领域。

Cartographer系统架构图

Cartographer算法在前端完成占据栅格地图的构建，得出激光雷达扫描帧的最佳位姿后，将扫描帧插入到子地图Submap中，得到局部优化的子地图并记录位姿。

后端根据扫描帧间的位姿关系进行全局的地图优化，并使用分支定界法加速求解，进而得出闭环扫描帧在全局地图中的最佳位姿。

Karto

一种基于位姿图优化的SLAM方法，使用了高度优化和非迭代的cholesky矩阵对系统进行解耦并求解。适用于各种室内环境，可以处理静态和动态障碍物。

Karto系统架构图

Karto使用图论的标准形式表示地图，其中每个节点代表了移动机器人运行轨迹上的一个位姿点，以及当前位姿下传感器返回的感知信息。

节点之间的边，代表了相邻机器人位姿之间的位移矢量。对每一个新的位姿点定位，需要节点间匹配关系和边带来的约束，保持定位估计误差的前后一致。

LIO-SAM

一种新型的激光惯性导航系统，结合了激光雷达和惯性测量单元数据，可以实现机器人的高精度定位和运动轨迹的建图。

LIO-SAM系统架构图

前端在传统的LIDAR-SLAM基础上，利用卡尔曼滤波和因子图优化算法，将激光雷达和IMU数据融合，进一步提高机器人的定位精度和建图效果。后端加入优化算法，使得机器人的定位精度和建图精度都得到极大提升。

LOAM系列

一种成熟的基于激光雷达的SLAM算法，包括LOAM、LOAM-Velodyne、LOAM-LiDAR等。

其中LOAM使用3D激光雷达的数据来进行建图和定位，利用点云数据的特征，如空间聚类、连续性约束等来估计位姿。

LOAM系统架构图

LOAM-Velodyne则使用Velodyne激光雷达的3D点云数据，以实现更高精度的地图重建和定位；LOAM-LiDAR采用LiDAR传感器来进行建图和定位，可以高效获取目标物体的三维坐标信息，在一些机器人应用场景具有很大的优势。

基于LOAM系统的地图构建局部

ORB-SLAM3

当前最优秀的基于特征点的视觉SLAM系统之一，支持单目、双目、RGB-D等多种相机模式，在特征提取、关键帧选取、地图维护、位姿优化等方面进行了优化，并能建立短期、中期和长期的数据关联，使得该系统兼具精度和鲁棒性。

ORB-SLAM3系统架构图

前端视觉里程计基于ORB特征，建立图像帧之间特征点的数据关联，以及图像特征点和地图点之间3D到2D的数据关联，具有较好的鲁棒性和提取效率，回环检测和重定位也基于ORB特征实现。

VINS-Fusion

一种基于视觉惯性传感器的视觉SLAM算法。它将视觉和惯性信息进行融合，来提高机器人在未知环境下的定位和导航能力。

VINS-Fusion系统架构图

前端和后端之间通过一个状态传递机制来交换数据。前端将IMU和图像数据合并，生成一组状态量，然后将其传递给后端进行融合和优化；后端将优化后的状态量传递回前端，以便更新机器人的运动估计。

SLAM方案选择

1. 二维SLAM就适合在二维平面上运动的机器人，如扫地机、配送机器人、迎宾机器人等。

具体来看，Cartographer的总体表现最为优秀，定位精度和建图质量都很高，同时鲁棒性也很强，能承受一定的噪声和漂移干扰，可以应用于大范围建图；Karto适合建图面积不是太大的低成本应用场景，可以在一定程度上满足建图需求，同时工程实现难度也不太高。

2. 三维SLAM则适用于机器人需要在三维空间中进行建模和定位的场景，比如无人机在室外飞行、机器人在户外环境中进行探测等。

其中，LIO-SAM的鲁棒性和实时性表现相对优秀，适用于建图面积和硬件资源有限的小型机器人应用场景；在建图面积和鲁棒性方面比较弱的LOAM方案，适用于小而快速的机器人及小型场景的定位、建图任务。

3.视觉SLAM通过摄像头等可视传感器来获取机器人与环境之间的相对位置关系，不需要其他传感器，更为轻量化。但对于光照变化、运动模糊等情况的处理仍存在挑战。

定位精度和实时性都比较高的ORB-SLAM3，面对场景变化能够快速适应，适用于多种场景的建图。与之相比，VINS-Fusion适用于对定位精度要求较高，且建图面积相对较小的机器人应用场景。

总的来说，这些算法各有优劣，如何选择最合适的算法，需要根据具体的应用场景和实际需求来综合考虑。

SLAM应用场景

随着计算机处理速度显著提升，且相机和激光测距仪等低成本传感器大为普及，SLAM技术广泛应用于机器人、AR、VR、无人机、自动驾驶等领域。

使用SLAM的扫地机器人可以使用滚轮转数以及来自相机和其他成像传感器的数据来确定移动位置。机器人还可以同步使用相机和其他传感器创建其周围障碍物的地图，避免同一区域清洁两次。即实现了定位和构图，从而保证了机器人可以按轨迹运动，同时也降低了设备功耗。

SLAM 还用于许多其他应用场景：

利用一个小摄像头实现VR头显空间定位；

让一队移动机器人在仓库中移动并整理货架；

让自动驾驶汽车停泊到空车位；

让无人机在未知环境中完成送货、无人机空中阵列表演等等。

SLAM面临的挑战

虽然 SLAM 已在某些场景下投入实际应用，但是仍面临诸多技术挑战

（1）定位误差累积，导致与实际值产生偏差

SLAM会估计连续移动，并容许一定的误差。但是误差会随着时间累积，导致与实际值产生明显偏差。误差还会导致地图数据瓦解或失真，让后续搜索变得困难。我们来看一个绕正方形通道行驶的例子。随着误差累积，机器人的起点和终点对不上了。这称为闭环问题。这类位姿估计误差不可避免。我们必须设法检测到闭环，并确定如何修正或抵消累积的误差。对策之一是记住之前到过的某处的某些特征，将其作为路标，从而最小化定位误差。构建位姿图有助于修正误差。将误差最小化问题视为优化问题进行求解，以生成更准确的地图数据。这种优化在视觉 SLAM 中称为捆绑调整。

（2）定位失败，地图上的位置丢失

图像和点云建图不考虑机器人的移动特征。在某些情况下，这种方法会生成不连续的位置估计。例如，可能会有计算结果显示，以 1 米/秒速度移动的机器人突然向前“瞬移”了 10 米。避免这种定位失败的办法有两种：一是使用恢复算法；二是将运动模型与多个传感器融合，以基于传感器数据计算。有多种方法可以实现运动模型的传感器融合。一种常见方法是使用卡尔曼滤波进行定位。由于大部分差速驱动机器人和四轮车辆一般都使用非线性运动模型，因此通常会使用扩展卡尔曼滤波器和粒子滤波器（蒙特卡罗定位）。某些情况下，也可以使用无迹卡尔曼滤波器等更加灵活的贝叶斯滤波器。一些常用传感器是惯性测量装置，例如惯性测量单元 (IMU)、航姿参考系统 (AHRS)、惯性导航系统 (INS)、加速度计传感器、陀螺仪传感器和磁力传感器。安装到车辆的轮式编码器通常用于测距。定位失败时，一种恢复对策是记住之前经过的某个位置的关键帧，将其作为路标。搜索路标时，会以特定方法进行特征提取以便高速扫描。有些方法基于图像特征，例如特征袋 (BoF) 和视觉词袋 (BoVW)。近年来，人们也使用深度学习来比较特征距离。

（3）图像处理、点云处理和优化带来高计算成本

在车辆硬件上实现 SLAM 时，计算成本是个问题。计算通常在处理能力有限的紧凑型低功耗嵌入式微处理器上执行。为了实现准确定位，必须高频率执行图像处理和点云匹配。此外，闭环等优化计算都是高成本计算流程。此处的挑战在于如何在嵌入式微处理器上执行这种高成本处理。对策之一是并行运行多个不同流程。例如，用于匹配流程前处理的特征提取就相对适合并行运行。使用多核 CPU 进行处理时，单指令多数据 (SIMD) 计算和嵌入式 GPU 在某些情况下可以进一步提升速度。而且，由于位姿图优化可以在相对长的周期里执行，降低其优先级并以规律间隔执行也能提高性能。

审核编辑：汤梓红