激光雷达SLAM室内导航对比分析

基于激光雷达SLAM的室内导航在自动驾驶车辆中的对比分析

摘要

同步定位与建图（SLAM）是大多数自主车辆和机器人室内导航系统中的基础技术模块。SLAM旨在在构建环境全局一致性地图的同时，确定机器人在此地图中的位置和姿态。近年来，视觉SLAM技术取得了显著进展。然而，由于在缺乏纹理的环境中（例如墙壁为纯白色的仓库）跟踪特征点性能脆弱，视觉SLAM难以提供可靠的定位。与视觉SLAM相比，激光雷达SLAM能够直接利用激光雷达点云捕获的三维空间信息，在室内环境中通常提供更稳健的定位。因此，激光雷达SLAM技术常被应用于自动导引车（AGVs）等工业领域。在过去几十年中，已有多种激光雷达SLAM方法被提出。然而，各类激光雷达SLAM的优势与劣势尚不明确，这可能会困扰研究人员和工程师。本文对不同的基于激光雷达SLAM的室内导航方法进行了分析与比较，并在真实环境中进行了广泛的实验以评估其性能。对比分析与结果可帮助学术界和工业界的研究人员构建适用于其自身应用场景的合适激光雷达SLAM系统。

索引术语 —室内导航，同步定位与建图（SLAM），自动驾驶，粒子滤波，图优化。

一、引言

NAVIGATION 是自动驾驶车辆在工业应用中的一项基本功能[1]。然而，在室内环境中，由于缺乏卫星定位信号，传统导航技术将失效[2]。而当前的定位方法通常存在困难。为了为室内厘米级定位提供稳定解决方案，室内自主车辆必须通过配备一些附加传感器来构建自身的导航系统。一种名为SLAM的导航技术已被研究以满足这一需求[3]–[5]。

SLAM指的是同步定位与建图，最早于20世纪80年代初被提出作为一个研究课题[6],[7]。此后，人们投入了大量努力，将定位与建图作为一个整体问题而非两个独立问题来解决[8],[9]。SLAM的可行性在[10]中得到了理论证明，其中估计地图被证明单调收敛到相对地图。如今，SLAM技术在移动机器人系统的室内导航中正发挥着越来越重要的作用。

SLAM技术大致可分为基于视觉和基于激光雷达两类。过去二十年中提出了许多视觉SLAM方法[11]。受人类视觉启发，许多研究利用立体相机实现视觉SLAM[12]–[15]。同时，单目相机也被证明能够构建有效的SLAM[16]。[17]中的重要工作开创了使用两个不同线程处理特征跟踪和建图的方式。[18]中提出了一种基于ORB特征描述符[19]的流行视觉SLAM，其在无GPU的情况下实现了实时性能。该方法的增强版本在[20]中开发。许多其他视觉SLAM方法如LSD-SLAM[21]和SVO[22]也是公认的解决方案。然而，视觉SLAM本质上会遭受视觉传感器缺点的影响。首先，单目、立体和深度相机系统都对环境中的环境光和光学纹理敏感。其次，捕获的图像可能缺乏纹理，并且在平台高速移动时容易模糊[23]。这些弱点常常导致视觉SLAM在工业应用中的局限性。

利用主动光源，激光雷达传感器通常能够实现高精度测量，并在光照变化的情况下仍获取环境的更密集的信息，相较于传统相机传感器具有优势。激光雷达传感器在快速移动条件下也能稳定工作。因此，激光雷达SLAM在室内环境中通常能提供更稳健的定位性能。激光雷达SLAM系统主要依赖于扫描匹配技术，该技术基于一种流行的点云配准算法——迭代最近点算法（ICP）[24]及其改进版本[25]–[27]。

对于激光雷达SLAM，扫描匹配技术可分为帧间匹配[28]–[32]和扫描到地图匹配[30],[33]–[36]。在前者中，通过将激光扫描彼此对齐来计算相对位姿变化，该方法易于理解和实现，但随着时间推移，误差累积较快。而在后者中，将激光扫描与全局一致地图对齐，因此尽管系统更复杂，但比帧间匹配方法更高效且鲁棒。然而，由于建筑物地图无法做到完全一致，误差仍然存在。因此，当机器人到达一个曾经访问过的位置时，其估计的轨迹在大多数情况下并未闭合，这就是著名的闭环问题[37],[38]。

对于闭环检测，基于词袋（BoW）技术的方法曾被广泛使用[18],[20],[37]，其中通过优化位姿图可以大幅消除累积误差。然而，这些方法仅对视觉SLAM有效。对于激光雷达SLAM，闭环通常通过基于点云的地图匹配[39]或基于匹配学习的特征比较[40]来检测。

近年来，提出了许多新的激光雷达SLAM方法。在[41]中，IMLS-SLAM被提出，通过使用隐式移动最小二乘曲面表示地图来提高定位精度。然而，它无法实时运行。在[42]中，MC2-SLAM通过密集激光雷达点的地图变形保持全局地图一致性，显著提升了CT-SLAM[43]在长期运行中的性能。在[44]中，通过融合相机和激光雷达构建SLAM以实现鲁棒的特征跟踪。其他人则使用期望最大化（EM）[36]、surfel表示[35]和机器学习[45]构建SLAM系统。这些方法为SLAM问题提供了新的解决方案，并将推动相关研究。

在各种激光雷达SLAM方法中，研究人员和工程师在为其自身使用场景选择适合的室内导航解决方案时会感到困惑。这是因为缺乏在相同评估设置下针对真实应用环境中不同SLAM性能的对比研究。在本研究中，我们分析了不同激光雷达SLAM方法的优缺点，并在相同数据集上对它们进行评估，以用于自动驾驶车辆的室内导航。我们对各对比方法获得的结果进行了讨论，并得出结论，以指导构建适用于室内定位的激光雷达SLAM系统。

本文的主要贡献如下：
- 对于SLAM技术，我们系统地分析了七种代表性基于激光雷达的SLAM方法的优缺点，这有助于研究人员快速而全面地理解它们的主要思想和贡献。
- 对于室内导航解决方案，我们通过在真实环境中进行大量实验，比较了现有基于激光雷达SLAM的方法的性能，实验中使用了不同类型的自主车辆和不同的运行场地。我们分析并总结了实验结果，这些结果可帮助工程师根据自身的环境选择适合的SLAM解决方案，用于实际的室内导航。

本文其余部分组织如下。第二节介绍SLAM的问题表述。第三节描述几种典型的基于激光雷达SLAM的定位算法。第四节介绍实验设置、评估结果以及一些讨论。第五节总结了我们的工作。

II. 问题表述

对于在环境中移动并观测多个未知路标的移动机器人，在时间戳 t 定义了以下变量：
- xt：描述机器人位置和方向的状态向量，x1:t表示状态集合 {x1·,· x·，2xt} 在不同时间戳下的值。
- ut: The control vector, applied at time t −1 to drive the robot to a state xt at time t, and u1:t denotes a set of control vector{u1, u2, ···, ut}.
- zt: The measurement data acquired by the exteroceptive sensors. Different from the definition in[7], we define it as a set of landmarks observed at timestamp t and zit is used to describe the ith landmark observed at t.

然后，在定位过程中，SLAM系统必须在给定xt、u1:t和测量值z1:t的情况下获得估计。该问题可以表述为 B(xt)= p(xt|u1:t,z1:t)，其中 B(xt)表示真实世界中时间戳t时刻状态x的状态估计置信度。

关于计算B(xt)的不同策略，SLAM解决方案可分为三类：基于高斯滤波、基于粒子滤波和基于图优化。

A. 基于高斯滤波的解决方案

高斯滤波器是一种参数化滤波器，通常由一对值构成，即均值和协方差。卡尔曼滤波器及其一种变体——扩展卡尔曼滤波器(EKF)——是基于高斯滤波的SLAM典型解决方案。在基于粒子滤波器的SLAM中，粒子滤波器是一种非参数化滤波器，它利用从 B(xt)中抽取的一组随机状态样本。如[7],[46]所示，基于高斯滤波器和基于粒子滤波器的 SLAM方法均可归类为贝叶斯滤波方法，其数学推导见公式(1)。

B(xt)= p(xt |z1:t, u1:t)
= p(xt |z1:t−1, zt, u1:t)
= ηp(zt |xt, z1:t−1, u1:t) p(xt |z1:t−1, u1:t)
= ηp(zt |xt) p(xt |z1:t−1, u1:t)
= ηp(zt |xt)B(xt), (1)

其中 η= 1 p( z t | z 1 : t − 1 , u 1 : t ) 是一个归一化常数，不包含状态 x 的信息。在推导公式(1)的过程中，第二步使用了贝叶斯全概率规则和马尔可夫假设。

公式(1)的第二行和第三行。设 B(xt)为机器人在时刻tᵢ的机器人位姿先验，则其可表示为公式(2)。

B(xt)=∫ p(xt |xt−1, z1:t−1, u1:t) p(xt−1 |z1:t−1, u1:t) dxt−1 = ∫ p(xt |xt−1, ut)B(xt−1) dxt−1. (2)

请注意，此推导过程还依赖于贝叶斯全概率规则和马尔可夫假设。贝叶斯算法可分为两个步骤，即公式(3)中的预测步骤和公式(4)中的更新或关联步骤：

B(xt)= ∫ p(xt |xt−1, ut)B(xt−1)dxt−1, (3)
B(xt)= ηp(zt |xt)B(xt). (4)

在预测步骤中，p(xt| xt−1, ut)表示在给定控制向量ut和状态向量xt−1的情况下，估计机器人状态xt 。在更新步骤中，将修正上一步的估计误差，以便为机器人预测更准确的位置和姿态信息。总之，基于贝叶斯滤波的SLAM的核心思想是通过结合运动中的状态转移方程和观测中的预测方程来预测最可能位姿。

1) 卡尔曼滤波器

基于高斯滤波和基于粒子滤波的方法在SLAM中被广泛应用。卡尔曼滤波器（KF）和扩展卡尔曼滤波器（EKF）是高斯滤波器的两种典型应用[47]。参数可以定义如下[46]。
- At：一个 n × n 矩阵，用于描述在无控制和噪声的情况下状态从 t −1 到 t 的演化，其中 n 是状态向量 xt 的维度。
- Bt：一个 n × m 矩阵，用于描述控制向量 ut 如何使状态从 t−1 变化到 t，其中 m 是控制向量 ut 的维度。
- Ct：一个 k × n矩阵，用于描述如何将状态xt映射到测量值zt，其中k是测量向量zt的维度。
- εt, δt: two random variables representing the process and measurement noise that are assumed to be independent and multivariate normally distributed with covariance Rt and Qt, respectively.
- μ t， t：时刻 t 状态 x 的均值和协方差。

除了贝叶斯滤波器的马尔可夫假设外，卡尔曼滤波器还依赖于以下假设：状态转移概率和测量概率必须是带有加性高斯噪声的线性函数，此外，初始置信度必须是正态分布。这样，后验概率才能始终服从高斯分布[46]。

The Kalman filter is illustrated by Eqs.(5)-(9), where the prediction of the robot pose is expressed by Eq.(5), and the uncertainty is expressed by the covariance matrix, as shown in Eq.(6). In other words, this corresponds to Eq.(2), which means the robot moves to a new position as estimated using Eqs.(5) and(6): μt= Atμt−1+ Btut, (5)
t= Att−1A T t+ Rt. (6)

最小化残差误差Kt的最优增益，称为卡尔曼增益，由公式(7)定义，

Kt=tCT t(Ctt C T t+ Qt) −1 . (7)

然后，机器人位姿的相关性可以表示为

μt=μt+ Kt(zt − Ctμt), (8)

以及更新后的不确定性是

t=(I − Kt Ct)t. (9)

这两个操作共同对应于公式(4)，意味着机器人利用环境的测量信息来修正预测的位姿。

定位过程如图1所示。在时间戳 t−1，通过控制 xt 的作用，从当前状态 xt−1 预测下一状态。然而，由于机器人无法完全按照预期被精确控制，该步骤将引入误差，导致此过程的不确定性增加。而在更新过程中，通过结合测量信息，可以降低预测状态 xt 的不确定性。

2) 扩展卡尔曼滤波器

如上所述，卡尔曼滤波器的前提是状态转移和测量过程必须为线性方程。然而在实际中它们往往是非线性的，这将导致卡尔曼滤波器无法适用。为了解决这一严重问题，提出了扩展卡尔曼滤波器。如 [46]所示，扩展卡尔曼滤波器假设状态转移和测量方程均为非线性，其定义可用公式(10)和公式(11)表示，

xt= g(ut, xt−1)+ εt, (10) zt= h(xt)+ δt, (11)

其中h和g是非线性函数。但由于高斯分布的特性，预测状态和测量不再服从高斯分布。为了使后验信念传播等效于卡尔曼滤波器，EKF使用一阶泰勒展开对非线性函数g和h进行线性化。换句话说，EKF使用该线性函数来近似非线性函数。因此，EKF能够像KF一样有效地传播其信念和状态。

然而，由于非线性状态转移方程和预测方程均被各自的线性近似函数所替代，EKF估计的状态置信度在很大程度上依赖于非线性函数与其线性近似函数之间的一致性。高斯分布经过非线性函数与其线性近似函数投影后的均值和协方差是不同的。换句话说，线性函数无法准确逼近对应的非线性函数，这将导致更大的状态不确定性。此外，不确定性越大，高斯函数的估计均值和协方差就越差。结果，估计位姿将偏离真实位姿[46]越来越远。

B. 基于粒子滤波的解决方案

与卡尔曼滤波器不同，粒子滤波器是一种非参数化滤波器。估计的机器人位姿的后验分布由一组粒子表示，而不是多元高斯分布。如图2所示，所有粒子代表机器人在环境中的可能位姿，它们是后验分布的实例化。粒子滤波器系统的参数定义如下[46]：
- M：粒子的数量，由粒子滤波算法手动初始化，
- m：一个索引值，满足 1 m M，
- x [ m ] t：第 m 个粒子在时刻 tᵢ 的状态，
- ω [ m ] t：第 m 个粒子在时刻 tᵢ 的归一化权重。

As shown in Algorithm 1, the particle filter system predicts the next state of every particle according to the control ut, and calculates its weight ω according to the measurement zt acquired by exteroceptive sensors. Afterwards, the system will perform resampling or important sampling for the particles according to their weights. The algorithm runs in a recursive way. In each round of computation, some particles are assigned with low weights while some others are with high weights. For a particle, the heavier its weight, the larger possibility it will be sampled. All the sampled particles together will be used to calculate the position and orientation of the robot.

Algorithm 1 Particle Filtering
1: procedure PARTICLEFILTER
2: input:
3: Xt−1: all the particles’ states and weights at time t−1.
4: ut：在时间 t 的控制向量。
5: zt：在时间 t 的测量值。
6: M：粒子的总数。
7: 输出：
8: Xt：在时间 t 的后验状态和权重。
9: % 执行预测步骤：
10: X t= Xt= ∅
11: 对于 (m= 1到 M) 执行
12: % 预测粒子 x[m] t在时刻 m t 的新状态：
13: x[m] t ← predictNewState(ut, x[m] t−1);
14: % Calculate the weight of particle m:
15: ω[m] t ← calculateWeight(x[m] t, zt);
16: % Add the predicted particle to a priori set X t:
17: X t ← AddToSet([x[m] t,ω[m] t]);
18: 结束循环
19: % 执行重采样步骤：
20: 对于 (i= 1到 M) 执行
21: % Sample a particle from X t based on the weights:
22: (x[i] t,ω[i] t) ← drawParticle(X t,i);
23: 将采样的粒子添加到 Xt 中：
24: Xt ← AddToSet(x[i] t,ω[i] t);
25: 结束循环
26: 结束过程

粒子滤波器常用于构建SLAM系统。然而，它存在一些缺点。首先，它容易出现粒子耗尽问题，即在重采样步骤中，具有低权重的粒子会逐渐被淘汰，导致粒子的多样性过低，无法准确估计机器人的位姿。其次，重采样步骤可能会意外消除正确粒子[48]。第三，由于每个粒子都是对机器人在环境中位姿的一种假设，通常需要数百甚至数千个粒子才能充分估计机器人的位姿，这可能带来较大的计算负载。

C. Graph Optimization-Based Solution

SLAM中图优化的主要思想是对整个路径上的所有状态x0:t和地图进行估计，而不仅仅是当前的位姿xt。它会考虑一段时间间隔内的所有测量值和控制值，并对所有位姿进行全局优化估计。基于图优化的SLAM旨在解决完整 SLAM问题，如图3所示。在计算状态xt+1时，x0与xt之间的所有可用测量值和控制值都将被纳入考虑。这意味着当前的测量可能会影响之前时刻的位姿估计。

The principle of graph optimization is illustrated in Fig. 4. All the constraints in the graph are calculated as residual errors. Then, the sum of the residual errors, F, can be formulated by

F= x T 0x0+∑
t [xt − g(ut, xt−1)]T R−1 t[xt − g(ut, xt−1)]
+∑
t
[zt − h(m, xt)] T Q−1 t[zt − h(m, xt)], (12)

where x T 0x0 is an anchoring constraint that anchors the robot’s initial pose to(0, 0, 0)T in the map as detailed in[49], g(ut, xt−1) is a nonlinear state transition function that transits the state xt−1 to xt, and h(m, xt) is a nonlinear measurement function that maps the robot’s observation values at state xt into the map. F can be minimized by nonlinear optimal methods such as Gaussian-Newton algorithm and Levenberg-Marquard algorithm.

从上述描述中可以看出，基于图优化的SLAM与基于滤波的SLAM之间的主要区别在于，前者估计的是全局最优位姿，并且当前的测量会影响之前时间戳的位姿估计，而后者仅估计当前时刻最优的位姿。

D. 总结

上述提到的三种策略提出了不同的思路来解决SLAM问题。基于高斯滤波的策略假设机器人的估计状态服从多元高斯分布。高斯滤波器使用若干特定参数，例如第二节II‐A.1中提到的均值 μt和协方差 t，来表示机器人的状态，并通过更新这些参数来预测新的状态。

基于粒子滤波的策略使用一组粒子来估计机器人的位姿。每个粒子代表机器人在真实世界中的一种可能状态。与高斯滤波器类似，基于粒子滤波的策略通过前一状态x t−1递归地计算当前状态xt。机器人状态的递归估计遵循贝叶斯滤波的基本思想。

与上述两种方法不同，基于图优化的策略将定位与建图过程建模为一个位姿图，其中节点表示机器人的估计状态，边表示节点之间的关联。值得注意的是，高斯滤波器和图优化被广泛

在激光雷达SLAM和视觉SLAM中都有使用，而粒子滤波器通常仅用于激光雷达SLAM。

III. 激光雷达SLAM技术

第二节中介绍的三种策略是估计机器人当前状态的基本思路。这些策略可以通过多种方式实现，从而导致不同的SLAM方法。在本节中，我们简要介绍七种典型的基于激光雷达的SLAM方法或解决方案，即Gampping、 CoreSLAM、KartoSLAM、HectorSLAM、 LagoSLAM、LOAM和Cartographer。这些方案均在机器人操作系统（ROS）上运行。表I总结了各种方法的特点。

A. Gmapping

Gmapping[33]基于Rao‐Blackwellized粒子滤波器 （RBPF）[53]构建SLAM，自发明以来受到了广泛关注。RBPF是一种基于粒子滤波的解决方案，每个粒子代表机器人在真实世界中的一个可能位姿，并携带环境的独立地图。为了准确估计真实位姿，RBPF需要大量粒子，这将消耗大量计算资源。而Gmapping通过采用自适应采样策略，大幅减少了所需粒子的数量。它通过使用由公式(13)定义的阈值 N来判断是否进行重采样。

N= 1
∑
M m=1(ω[m]) 2
, (13)

其中M为粒子的总数， ω[ m ]是粒子m的归一化权重。 N的值决定了粒子集的离散程度。较大的 N会使粒子集更加集中，从而产生的估计位姿更接近真实值；相反，较小的 N将导致位姿估计出现更大的误差。当 N小于N/2时，进行重采样

该步骤可执行[54]。同时，它考虑了最近的观测值，从而显著提高了建议分布的精度。因此，预测位姿的不确定性将大幅降低。

B. CoreSLAM

CoreSLAM[34] is a brief SLAM algorithm that has only 200 lines of C code. It aims to develop a code friendly, efficient, comprehensible and high-precision SLAM algorithm which can be easily integrated into the existing particle localization framework. It uses an affordable Hokuyo URG04 laser scanner – a short range laser with 10Hz update frequency.

The CoreSLAM algorithm consists of two main functions, namely the scan-to-map distance function and the map-update function. The former converts the physical distance obtained by laser to the pixel distance in occupied map. While the latter updates the map with the robot’s moving. CoreSLAM introduces a concept of latency, which is the time between the laser scan and its integration into the map, and is formulated by Eq.(14),

Latency= Rm ∗ Fl vr , (14)

其中，Rm 表示地图分辨率，Fl 是激光的频率，vr 是自动驾驶车辆的速度。延迟会影响相对于地图分辨率的微小位移测量。

作者在室内环境中进行了测试，并得出结论： CoreSLAM 方法表现良好，能够处理回环检测。然而， [55]在机器人操作系统（ROS）上对其进行了评估，评估结果显示 CoreSLAM 并不如预期，未能实现回环检测效果。

C. KartoSLAM

KartoSLAM[50]是一种基于图优化的SLAM方法。对于基于图优化的SLAM，节点表示现实世界中的路标和机器人位姿，边表示由位姿与路标之间的空间距离所引起的约束。KartoSLAM提出了一种名为稀疏位姿调整 (SPA)的有效方法来解决大规模位姿图优化问题。该方法首先从约束图中计算稀疏矩阵，然后使用稀疏非迭代 Cholesky分解技术求解线性系统。观测误差可通过公式 (15)表示。

F=∑
ij (zij −[RT i(ρj − ρi) θj − θi])
ij(zij −[RT i(ρj − ρi) θj − θi]), (15) where ρ and θ are the translation and orientation parameter of the robot pose in the world frame, respectively, Ri is the 2×2 rotation matrix, zij is the measured offset between nodei and nodej, eij is the error function, and F is the total error of all edges in the pose graph. We can see, F is a nonlinear function and nonlinear optimization methods are used to solve it.

In KartoSLAM, the highly-optimized Cholesky decomposition solver is adopted to solve this nonlinear system. Compared with the general graph optimization-based method, KartoSLAM has the advantages of getting more accurate solutions, being robust and tolerant to initialization, converging very fast and being fully non-linear, etc[50].

D. LagoSLAM

LagoSLAM[31]也是一种基于图优化的SLAM方法。它引入了一种闭式近似到

基于激光雷达SLAM的室内导航在自动驾驶车辆中的对比分析

从线性估计的角度来看完整SLAM问题

LagoSLAM在线性逼近过程中不需要初始猜测。此外，其结果足够精确，无需使用非线性优化技术进行 refinement。

Graph optimization-based SLAM aims at minimizing the sum of the residual errors F as shown in Eq.(15). LagoSLAM solves this nonlinear problem by decoupling the the orientation and position estimation under the assumption that the relative position information and the relative orientation information are independent. Under this assumption, Eq.(15) can be simplified as

{A T 1 θ= δ, 2 ρ= R(θ)β, (16)

where A1 and A2 are two reduced incidence matrices, θ and ρ are the orientation and position that we need to solve, δ=[δ1, δ2,…, δt]T is the relative orientation measurements, β=[β T 1, β T 2,…, β T t] is relative position measurements. Afterwards, the linear estimation for orientation and position can be solved.

LagoSLAM应用线性估计和图论来求解机器人位姿的近似解。然而，作为一种基于图优化的SLAM算法，根据[55]中的测试结果，它比KartoSLAM占用更高的 CPU负载，且建图精度相对较低。

E. HectorSLAM

与[31],[33],[34],[50], HectorSLAM相比，[30]是一种可应用于无人机(UAV)的独特SLAM系统，但 HectorSLAM系统的结构更为复杂。为了获得更好的定位结果，必须安装具备高更新率和低测量噪声的3D惯性传感系统以及现代激光雷达，并且这两个系统必须通过硬实时控制器进行同步。与一般系统不同，HectorSLAM仅包含前端系统，后端不提供位姿图优化。

HectorSLAM can be divided into 2D SLAM sub system and 3D navigation sub system. For the 2D SLAM subsystem, scan matching that aligns the laser scans with each other or with the existing map is a key technique. It aims at finding the proper orientation and translation parameter so that the laser beam endpoints and the constructed map can be matched. This procedure is formulated by Eq.(17), f(t, θ)= argmin t,θ n ∑ i=1 [1 − M(ti, θi)] 2 , (17)

其中，ti, θi分别表示在时间i时位姿在二维世界坐标中的平移和方向，M(t, θ)将世界坐标系中的位姿转换到占据栅格地图中，而f(t, θ)是残差误差之和。

对于3D导航子系统，它可以分为导航滤波器和 SLAM集成。在导航系统中，采用EKF技术结合IMU来估计速度与位置。而在SLAM集成系统中，通过EKF获得的三维估计将被投影到二维系统，以提高扫描匹配性能。

F. LOAM

LOAM[51]提出了一种实时SLAM解决方案，该方案通过两个独立的算法分别解决里程计和建图问题。定位算法以高频率进行低精度的里程计计算，而建图算法则以低频率构建高精度的地图。从硬件角度来看，它结合了三维IMU传感器和激光雷达传感器来估计位姿和地图。其实验表明，该方法在定位方面表现出较高的性能，并且在惯性测量单元（IMU）的辅助下能够获得更高的定位精度。

LOAM的工作场景如图5所示。

V‐LOAM[52]是LOAM的一个改进版本，提出了一种融合视觉和激光雷达信息的通用框架，并实现了比 LOAM更优的定位结果。在V‐LOAM中，视觉里程计以 60Hz的高频（图像帧率）进行运动估计，而激光雷达里程计则以1Hz的低频对运动估计进行优化，消除畸变并校正漂移。通过这种方式，系统能够适应快速移动和环境光照变化的环境。因此，其性能优于仅使用激光雷达或仅使用视觉的算法，在KITTI基准上排名第一，结合视觉与激光雷达里程计时的漂移误差为0.55%。

G. Cartographer

Cartographer[39]是谷歌提出的一种SLAM解决方案，支持背包、无人机和自动导引车等多种平台。它能够适应不同的传感器配置，并在5厘米分辨率下实现实时建图和回环检测。在Cartographer中，采用分支定界算法来降低机器人位姿估计和回环检测的计算负载。对于局部二维SLAM，使用Ceres匹配器[56]来寻找与相应子地图最佳匹配的扫描位姿。最后，位姿估计被建模为广泛接受的非线性最小二乘问题，如公式(18)所定义。

f(t, θ)= argmin t,θ n ∑ i=1 (1 − Msmooth(ti, θi)) 2, (18)

其中，ti和 θi分别表示相对于子地图坐标系而非全局世界坐标系的平移和方向。Msmooth(·)用于平滑局部子地图中的概率值。

为了消除扫描与子地图匹配过程中产生的累积误差，采用了回环闭合机制，该机制利用稀疏位姿调整算法[50]来调整所有子地图和扫描的位姿。如公式(12)所述，回环闭合约束可被表述为一个非线性最小二乘问题，其中可使用分支定界扫描匹配技术和Ceres求解器来获取最优解。

IV. 实验与讨论

在本节中，对室内环境中的各种激光雷达SLAM方法进行了广泛的评估。

在这些采集的数据上离线运行方法。通过这种方式，我们可以获得各种方法的定位结果并进行比较。共有三个主要实验。

在实验I中，我们使用配备McLam轮的机器人在普通室内环境中采集数据。基于激光雷达的多种常用方法对采集的数据进行了角度与位置估计。图6展示了实验场地的一个场景，该场地位于一个堆满货物的仓库中，区域足够大，可支持我们的测试。

在实验II中，实验数据由一辆配备McLam轮的自动驾驶车辆在具有挑战性的带玻璃的走廊环境中采集，如图 7(a)所示。为了便于手动测量运动轨迹的

真实值，我们控制机器人沿L形和矩形路径移动。图8显示了实验场地的占据栅格地图。在该图中，两条长走廊由玻璃墙构成，图中心的矩形区域是我们的主实验区域。图 12和图13展示了车辆在主实验区域的运动轨迹。

在实验III中，实验也在如图6所示的仓库中进行。该实验旨在评估在具有相对较高速度[57]的大规模场景中定位与建图的性能。实验使用的是一辆配备差速轮的自动驾驶车辆，如图7(b)所示。与实验I不同，本实验中的车辆在两侧有货物的长走廊中行驶。

B. 实验I

在本实验中，我们首先评估姿态精度，然后分析在规则与不规则形状路径下的定位误差，最后评估使用和不使用惯性测量单元及轮式里程计时Cartographer的性能。

1) Orientation Accuracy

Two experiments are conducted. In the first one, we manually control the robot and rotate it n rounds around itself in situ, then we run the different SLAM methods and get the estimated orientations, which are then compared with the ground-truth value n × 360◦. Figure 9 shows the estimated orientation values of different methods, and Table II gives the estimation errors. From Fig. 9 and Table II we can see, the orientation error of ‘Wheel Odometry’ is 20.585◦, which is much higher than the other three methods. Exactly, with the help of IMU, the estimated errors are reduced by an order of magnitude. Both EKF(IMU and Wheel Odometry) and LOAM(LiDAR) show excellent performance in rotation estimation, which hold an error rate lower than 0.05%.

在第二次实验中，自动驾驶车辆随机移动并返回起点，轨迹形成一个闭合环路。我们在两种不同设置下执行航位推算算法来推断轨迹：“轮式里程计”和“轮式里程计 + IMU”。对于后一种情况，我们使用扩展卡尔曼滤波器 (EKF)融合轮式里程计和惯性测量单元的信息，并将其命名为“EKF”。图10显示了通过这两种方法获得的轨迹。终点应与起点重合。从图10可以看出，终点的

表II 不同SLAM方法的方向误差

and the EKF(in yellow). The red circle denotes the starting point, and the black and green circles indicate the end points.)

EKF 比“轮式里程计”要精确得多，这表明惯性测量单元在提高自动驾驶车辆定位精度方面起着重要作用。因此，在后续实验中，由于轮式里程计的定位精度较差，其结果将不再进行比较。取而代之的是，在后续实验中将对轮式里程计与惯性测量单元的融合结果进行比较和讨论。

2) 规则形状轨迹上的定位精度

本实验在仓库中进行。图11展示了试验场地的布局。灰色块表示混凝土地面，黑线表示块之间的间隙。我们在地面上标记了七个点。根据测量结果， |P1P2| = 11985m， |P2P5| =5.003m， |P1P3| = 18007m， |P2P3| = 6015m。尽管P1, P2和P3并不严格在一条直线上，但从P1到P3的直线距离为18.007m。我们驱动机器人沿三种不同形状的路径移动，分别为直线形、 L形和矩形，每种路径重复三次。直线

TABLE III LOCALIZATION ERRORS OF DIFFERENT METHODS ON STRAIGHT LINE TRAJECTORY. THE GROUND-TRUTH END POINT IS AT(18.007, 0)

表IV L型轨迹上不同方法的定位误差。真实终点位于(11.985, 5.003)

如公式(20)所述，

σi= √(xi −xgt)2+(yi − ygt)2, (20)

其中(xgt,ygt)表示真实位置，(xi,yi)表示第 i 次测试中的估计位置。由于误差会随着轨迹长度的增加而累积，我们引入平均误差率 ϕ 作为一个指标，其定义为

ϕ= σL¯ × 100%, (21)

其中 σ¯是平均误差， L是路径的总长度。不同方法在直线、L形和矩形路径上的定位结果分别如表III、IV和V所示。

从表III‐V可以看出，LOAM、Gmapping和 Cartographer在规则形状轨迹上的定位表现出色，它们的平均误差率均低于1.00%。在直线情况下， Cartographer的误差率最低，为0.034%，略低于 Gmapping的0.077%。在L形情况下，LOAM的误差率最低，为0.590%。AMCL在这些规则形状轨迹上的定位表现较差。一个可能的原因是，AMCL需要环境的预构建地图来计算自动驾驶车辆的位姿，其定位精度高度依赖于地图的精度。在我们的实验中，地图由Cartographer构建，因此AMCL的精度低于Cartographer。

3) 不规则形状轨迹上的定位精度

在本实验中，自动驾驶车辆被远程控制以随机方向移动，从而产生不规则轨迹。该测试重复进行了三次。需要注意的是，机器人在每次测试结束时会停在其起点。实验结果如表VI所示。从表 VI可以看出，LOAM、Cartographer和Gmapping获得的平均误差比EKF和AMCL小一个数量级。在上一节中，我们已经讨论了AMCL定位精度较低的原因。对于EKF，我们可以发现，在规则形状轨迹上，其定位精度与 LOAM、Gmapping和Cartographer相近，但在不规则形状轨迹上则明显偏低。根据规则与不规则定位实验结果， Gmapping表现出较高的

表V 不同方法在矩形轨迹上的定位误差。真实终点位于(0, 0)

表VI 不同方法在不规则形状轨迹上的定位误差。地面真值终点位于 (0, 0)

在小范围区域和低移动速度下，其定位性能优于LOAM方法。这表明，在不进行转弯的情况下，EKF可以与LOAM、 Gmapping和Cartographer这三种融合了激光雷达信息进行定位的方法表现相当。需要注意的是，由于McLam轮的灵活性，我们在 90◦角落处并未对车辆进行转弯。

4) 使用与不使用IMU/里程计时Cartographer的定位精度

Cartographer可以利用激光雷达信息进行定位。在本实验中，我们评估了Cartographer在三种不同设置下的性能，即“LiDAR”、“LiDAR + IMU”和“LiDAR + IMU +Wheel_Odometry”。我们使用第四节‐B.2 和IV‐B.3中采集的数据进行评估。表VII显示了不同 Cartographer配置在不同轨迹下获得的定位结果。结果显示，在小范围环境中移动时，各种Cartographer配置之间没有明显差异，即使用或不使用IMU或轮式里程计的激光雷达传感器表现相近。这表明Cartographer在小于一百平方米的区域内的定位性能非常好。

C. 实验二

该实验使用另一辆McLam轮自主车辆进行，如图 7(a)所示。试验场地在一栋建筑物内。场地布局如图8所示，场地大小约为10m × 10m。从图8可以看出，试验场地内有长走廊。门和走廊由玻璃制成。这是一个理想的实验场地，可用于评估LiDAR SLAM算法在包含玻璃和长走廊等挑战性场景中的性能。

机器人配备了轮式编码器、惯性测量单元和激光雷达传感器。需要注意的是，IMU和LiDAR传感器与实验I中相同。测试在L形轨迹和矩形轨迹上进行，每种情况重复三次。与实验I不同的是，本实验中车辆在L形路径的拐角处转弯。图12和图13分别显示了在L形和矩形路径下由轮式里程计、EKF和AMCL方法估计的轨迹。定位误差和平局误差率见表VIII。

从图12和图13可以看出，EKF和AMCL的定位精度明显优于轮式里程计。从表VIII可以看出，AMCL和EKF的定位误差分别为0.656%和0.681%，两者非常接近。而在矩形环境中，AMCL的定位误差比EKF高出约0.5%。一个可能的原因是AMCL所使用的地图不够精确。

表VII 制图师使用激光雷达（带或不带IMU或里程计）获得的定位结果

表VIII 定位 轮式里程计、EKF和AMCL方法的定位误差，使用第二辆自动驾驶车辆 LE

表IX LOAM评估 ’和Cartographer ’ 不同移动速度下直线路径的定位误差。路径的真实长度为84.932米

D. 实验III

This experiment is performed by the third autonomous vehicle, as shown in Fig. 7(b). The experimental task is to evaluate the localization accuracy of LOAM and Cartographer in a large-scale indoor test filed, which is about 100m × 40m. The robot is equipped with the same 16-line velodyne LiDAR sensor as used in Experiment I and II. In this experiment, we compare the performance of LOAM and Cartographer by driving the robot to move in a straight line with a low speed and a high speed. Each speed is tested twice. The trajectories generated by LOAM and Cartographer are shown in Fig. 14.

在图14中，黑线表示真实轨迹，其长度为84.932米。高速部分用虚线表示，低速部分用实线表示。Cartographer和LOAM的结果分别用蓝色和青色标注。表IX显示了这两种方法的定位误差。

从图14可以看出，在低速情况下，LOAM和 Cartographer均表现良好；而在高速情况下，LOAM的性能优于Cartographer。根据表IX，在低速时， LOAM和Cartographer的定位误差均低于1%；而在高速时，Cartographer的定位误差相对于84.932米的总路径长度超过10米。这表明Cartographer不适用于快速移动平台。然而，LOAM在低速和高速情况下均表现出较高的定位精度，其定位误差小于10厘米，体现了LOAM算法的鲁棒性。

E. 计算负载与内存使用分析

除了对定位精度的分析外，还进行了内存使用和计算负载的评估。实验采用一台配备英特尔酷睿i5‐3210M处理器和6GB内存并运行Ubuntu v14.04的笔记本电脑。各对比算法运行时的CPU负载和内存使用情况分别如图15(a)和 (b)所示。我们随机选取两组测试数据对这些算法进行评估，通过Linux操作系统中的命令‘top ‐n 300 ‐b | grep [ algorithm]’获取CPU负载和内存使用信息。在记录过程中未同步各算法的启动时间，这确实是一项困难的工作，但由于我们仅旨在对不同方法的资源消耗进行整体比较，因此该因素不会对分析造成显著影响。从图15(a)可以看出， Gmapping的CPU负载最高，其次是Cartographer。EKF方法的CPU负载最低，在整个运行过程中稳定在约 10%左右。

关于内存使用，我们从图15(b)中得出以下观察结果。首先，AMCL和EKF在整个运行过程中保持几乎恒定的内存使用量。这是因为AMCL和EKF只需要固定大小的内存空间来存储其状态信息，因为新估计的状态将覆盖旧状态。其次，AMCL占用的内存空间远大于 EKF。原因是AMCL必须维持大量粒子，每个粒子代表机器人的一种可能状态，并需要占用内存空间，而EKF只需为自动驾驶车辆维护一个新估计的状态。第三， Gmapping在运行过程中内存使用持续增加，是四种方法中最高的。Gmapping基于粒子滤波器，且必须记录不断扩大的地图。Cartographer的内存使用起初较低且保持稳定，但后期逐渐增加。一个可能的原因是初始内存分配较少，初期可支持运行，但当地图扩大时，内存需求随之增加。

我们还评估了不同最小更新距离对AMCL方法的影响。最小更新距离作为一个参数，用于确定执行滤波器更新前所需的平移移动量。具体评估了1厘米、2厘米、5厘米、10厘米和20厘米的最小更新距离。结果如图15(c)所示。从图15(c)可以看出，当最小更新距离设置为1厘米或2厘米时， CPU负载非常高；当设置为5厘米、10厘米或20厘米时， CPU负载保持在约5%左右。这表明不应将AMCL的最小更新距离设置得过小。

和(b)显示了在相同数据集上运行不同SLAM方法时的CPU负载和内存使用情况。(c)显示了在不同最小更新距离下运行AMCL时的CPU负载。)

F. 讨论

在本节中，我们通过三种不同的自动驾驶车辆来评估各种SLAM方法的定位性能。实验I和实验II基于两种均配备McLam轮的自动驾驶车辆。我们选择这类车辆的原因是McLam轮能够向任意方向移动，且已在工业界广泛应用。为了进行公平比较，我们采用规则形状和不规则形状的路径来测试对比的SLAM方法。除了使用不同路径进行广泛评估外，我们还在低速和高速条件下，于小范围环境和大范围环境中运行自动驾驶车辆，以使评估场景更贴近实际应用。

在实验中，我们发现，对于方向估计，轮式里程计不可靠，而EKF、AMCL和 LOAM的表现要好得多。这一结论基于我们车轮精度的局限性。如果轮式编码器的精度足够高，轮式里程计将产生更高的精度。为了在不同速度下进行评估，由于车辆的限制，我们将高速设置为0.8米/秒。实验表明，在低速和高速情况下，LOAM在四次测试中的定位误差均低于10厘米，显示出其良好的鲁棒性。AMCL的定位精度不如 Cartographer。我们认为原因是，AMCL方法需要由其他建图方法（如Gmapping或Cartographer）预先构建的地图，这导致AMCL在定位方面的性能低于 Gmapping或Cartographer。

关于内存使用，实验表明Cartographer的内存消耗比 AMCL低。但这仅基于有限时间长度的实验。随着时间推移，Cartographer将需要越来越多的内存来存储新构建的地图，最终其内存使用将超过AMCL。

V. 结论

本文对基于SLAM的室内导航技术进行了综述，分析了不同激光雷达SLAM方法的特点和性能。为了进行公平比较，我们实现了这些算法，并在相同的实验设置下进行了评估。在实验中，评估了不同激光雷达SLAM方法的定位精度，并分析了这些算法的CPU负载和内存使用情况。实验结果表明：1）在使用轮式里程计进行位姿估计时， IMU有助于校正其方向估计误差，从而获得更优的位姿估计；2）Gmapping和Cartographer在小范围环境中均具有良好的定位性能；当构建相同分辨率的地图时， Gmapping比Cartographer消耗更多内存；3）尽管轮式里程计和AMCL的CPU负载较低且所需内存空间较小，但它们的定位精度不如Gmapping和Cartographer；4） LOAM在小范围和大范围测试场地中均能取得良好的性能。

对于未来的研究方向，我们认为：1）视觉特征与点云特征的融合将提升SLAM系统的鲁棒性；2）可以利用云计算来降低终端的计算负载，同时促进多自主车辆协同 SLAM的构建。