1 引言（Introduction）

近年来，随着无人驾驶技术和UGV（无人地面车辆）技术的兴起，SLAM算法成了机器人领域的研究热点。SLAM算法根据主要采用的外部感知传感器类型可分为视觉SLAM与激光SLAM。其中，基于激光雷达的SLAM算法因其对光照不敏感、距离度量准确、便于生成导航地图等优势，已成为目前无人驾驶和UGV领域不可缺少的关键性技术^[1]。

传统的2D激光SLAM算法，如Hector^[2]和Cartographer^[3]等，在几何特征丰富的室内环境中具有良好的表现。但在室内长走廊、空旷停车场等场景中，若没有精确的轮式里程计辅助，上述算法会产生极大的定位误差。

在3D激光SLAM中，一般使用ICP（iterative closest point）算法^[4]和NDT（normal distributions transform）算法^[5]或它们的变种算法进行激光点云配准，以估计2帧点云之间的相对位置和姿态（简称为“位姿”）。经典ICP算法基于欧氏距离进行数据关联，点云配准的精度高，但运行速度慢，需要根据场景调整参数，依赖于准确的迭代初值。其思想在LOAM^[6]（LiDAR odometry and mapping in real-time）和LeGO-LOAM^[7]（lightweight and ground-optimized LOAM）算法中均有体现。NDT算法则假设激光点云数据符合正态分布，并引入非线性优化法迭代求解相对位姿，运行速度和鲁棒性较ICP算法有极大提升。基于NDT算法的激光SLAM方法中最具代表性的是Koide所设计的hdl_graph_slam算法^[8]。

然而，大多数纯激光SLAM算法在长廊、隧道、矿道和桥梁等场景中，定位性能均会快速劣化，甚至完全失效。在这些场景中，激光雷达观测到的数据会在某一维度呈现高度的相似性，以致点云配准结果收敛至错误位姿，严重地影响了SLAM整体的性能，也限制了机器人在此类环境下的自主作业能力。在视觉SLAM的研究中，纹理缺失的场景同样会导致其定位效果严重劣化，因此被一些学者称为“退化场景”^[9]。本文所讨论长廊、隧道等场景更侧重于场景几何结构上的相似性，而非其光学纹理上的相似性。为了与视觉SLAM场景区分，本文将此类场景称为“相似场景”。

目前处理该问题的一种措施是使用鲁棒性更高的点云配准算法。文[10]在地下矿道场景中对比了NDT ^[5]和GICP^[11]（generalized-ICP）算法的建图性能，最终选择了鲁棒性更高的GICP。然而，GICP算法的耗时远大于NDT算法，不利于在算力限制较大的机器人上广泛应用。另外一种常见的解决措施是在SLAM中进行多传感器融合，实现信息互补，提高整体精度。文[12]在激光里程计之外，使用相机来识别场景中事先放置的二维码路标，以产生闭环信号，提高SLAM的全局一致性。然而，该方法需要额外增加光学相机，对场景光照条件要求较高，且需要事先布置路标，在实际应用中局限性较大。文[13]在无人机上搭载了激光雷达、光学相机和IMU（inertial measurement unit），使用UKF算法融合了激光里程计、视觉里程计和IMU数据，提高了在走廊中的定位性能。然而，相机、激光雷达和IMU三者的外部参数标定较为复杂，而这些都会显著地影响SLAM算法的性能表现^[14]，且视觉里程计的精度同样会受到光照条件的限制。

为解决上述问题，本文结合实验对NDT算法在相似场景中失效的具体原因展开讨论，并据此对hdl_graph_slam的激光里程计部分（在经典SLAM框架中通常被称为前端）作出改进。相比于文[10-12]，本文算法仅依赖于激光雷达数据，而无需引入其他传感器，更易于在不同机器人上快速配置。本文提出的改进主要包括：(1) 基于匀速平滑运动假设，改进运动预测模型，为点云配准算法提供一个更准确的迭代初值；(2) 引入局部地图的概念，将若干连续关键帧拼接成局部地图参与点云配准，使目标点云稠密化，提高点云配准精度。

2 通用符号定义（Definition of universal symbols）

本文所使用的传感器为单旋转轴、多线束激光雷达，会以固定频率向主机输出表征周围环境的3D点云帧$ S $。全局坐标系记作$ \{ W \} $，在$ i $时刻采样得到的点云帧$ S_{i} $的坐标系记作$ \{{S_{i}} \} $。每帧点云包含了若干激光点，每个激光点位置记作$ {{{\mathit{\boldsymbol{p}}}}}=[{x, y, z} ]^{{\rm T}} $，其中$ x $、$ y $、$ z $分别为激光点$ p $在$ \{{S_{i}} \} $中的3维坐标，表征实际空间中障碍物的3维位置。当激光雷达运动时，$ \{{S_{i}} \} $的位姿也会随之发生变化，本文规定$ S_{i} $的位姿等于采样时刻激光雷达在全局坐标系$ \{W \} $中的相对位姿。

为方便坐标运算，通常使用$ 4\times 4 $的齐次矩阵来描述3维位姿：

$ \begin{equation} _{B}^{A} {{{\mathit{\boldsymbol{T}}}}}=\begin{bmatrix} {_{B}^{A} {{{\mathit{\boldsymbol{R}}}}}} & {_{B}^{A}{{\tau}}} \\ {{{{\bf{0}}}}} & {\bf{1}} \\ \end{bmatrix} \end{equation} $

(1)

其中，$ _{B}^{A} {{{\mathit{\boldsymbol{R}}}}} $为$ 3\times 3 $旋转矩阵，表示将3维姿态从$ \{A \} $转换到$ \{B\} $；$ _{B}^{A}{{\tau}}=\big[\, x, y, z \, \big]^{{\rm T}} $为$ 3\times 1 $的平移向量，表示将3维位置从$ \{A\} $转换到$ \{B \} $；$ _{B}^{A} {{{\mathit{\boldsymbol{T}}}}} $的最后一行仅用于将位姿矩阵补齐为齐次矩阵。因此，位姿从$ \{W\} $到$ \{S_{i}\} $的转换矩阵写作$ _{S_{i}} ^{W} {{{\mathit{\boldsymbol{T}}}}} $，其逆矩阵$ _{S_{i} }^{W} {{{\mathit{\boldsymbol{T}}}}}^{-1}={}_{W}^{S_{i}} {{{\mathit{\boldsymbol{T}}}}} $则表示从$ \{S_{i}\} $到$ \{W\} $的位姿转换关系。

由矩阵知识易知，位姿矩阵可以用于坐标转换。记$ \odot $为4阶齐次矩阵与3维坐标点的乘法运算子，则有

$ \begin{equation} ^{W}{{{\mathit{\boldsymbol{p}}}}}={}_{S_{i}} ^{W} {{{\mathit{\boldsymbol{T}}}}}\odot {}^{S_{i}} {{{\mathit{\boldsymbol{p}}}}} \end{equation} $

(2)

式(2) 表示将$ i $时刻的激光雷达坐标系$ \{{S_{i}}\} $中的点$ p $坐标转换到世界坐标系$ \{W \} $中。此外，从$ \{C \} $到$ \{A \} $的转换矩阵$ _{A}^{C} {{{\mathit{\boldsymbol{T}}}}} $亦可由从$ \{C \} $到$ \{B \} $的转换矩阵$ _{B}^{C} {{{\mathit{\boldsymbol{T}}}}} $和从$ \{B \} $到$ \{A \} $的转换矩阵$ _{A}^{B} {{{\mathit{\boldsymbol{T}}}}} $相乘得到，即

$ \begin{equation} _{A}^{C} {{{\mathit{\boldsymbol{T}}}}}={}_{B}^{C} {{{\mathit{\boldsymbol{T}}}}} {} _{A}^{B} {{{\mathit{\boldsymbol{T}}}}} \end{equation} $

(3)

3 前端算法改进（Improvements on the front-end algorithm） 3.1 激光SLAM整体框架

以LOAM^[6]、LeGO-LOAM^[7]为代表的基于线、面特征的SLAM算法严重依赖于特征提取的质量，一旦面向几何特征较少的场景（仅有少量的特征点），则无法收敛到一个准确的位姿估计。因此本文选择泛用性更高的hdl_graph_slam算法^[8]作为主体框架，来阐述相似场景对激光SLAM带来的挑战，以及相应的改进措施。需要注意的是，本文结论并不单单局限于hdl_graph_slam算法，在作适当修改后，可以方便地移植到其他激光SLAM算法中。

SLAM算法框架图如图 1所示，分为点云预处理、前端里程计、地板检测、后端图优化和建图5个部分（可参阅文[8]了解更多算法细节）。其中，前端里程计部分负责利用NDT算法进行点云配准，估计每一帧点云在全局坐标系中的位姿，后端图优化节点则对所有的估计位姿进行批量优化，提高位姿轨迹的全局一致性。由于前端里程计的精度直接决定了整个SLAM算法的定位和建图效果，因此本文主要讨论如何对前端里程计算法进行改进，以克服相似场景下的SLAM性能退化问题。

3.2 问题分析

经典的NDT算法^[5]进行点云配准的步骤如下：

(1) 将目标点云$ S_{1} $栅格化，并计算每个栅格的正态分布参数；

(2) 利用给定的位姿，将源点云$ S_{2} $转换到目标点云坐标系$ {\rm \{}S_{1} {\rm \}} $，得到$ S_{2} ' $；

(3) 计算$ S_{2} ' $中每个点落入$ S_{1} $中对应栅格的概率，并累加所有的概率，得到评价得分$ \sigma $；

(4) 基于牛顿迭代法来优化$ \sigma $，得到新的估计位姿；

(5) 回到步骤(2)，直至达到收敛判据。

其中步骤(1) 所得到的栅格化正态分布参数的准确性和差异性至关重要，决定了NDT最终收敛位姿的准确性。相似场景正是影响了步骤(1) 的差异性而使得激光里程计失效的。图 2为沿走廊方向间隔1.1 m采集到的2帧点云数据。虽然红色椭圆区域的墙壁点云偏移现象体现了1.1 m的位移，但点云的其余部分几乎完全一致。这说明在相似场景中，激光雷达观测到的相邻2帧点云之间存在高度相似性。

当hdl_graph_slam算法使用NDT算法匹配此类点云时，给定的初始迭代位姿$ _{S_{2} }^{S_{1}} {{{\mathit{\boldsymbol{T}}}}} $默认为4阶单位矩阵$ {{\mathit{\boldsymbol{I}}}}_4 $，即默认无旋转、无平移。在进行迭代优化时，由于在场景延伸方向上没有足够的差异来帮助更新位姿矩阵在该方向的迭代量，最终得到的估计位姿矩阵必然接近于初始的$ {{\mathit{\boldsymbol{I}}}}_4 $，即倾向于激光雷达始终停留在原地。图 3为图 2(a)和图 2(b)的NDT配准结果。其中红色点云为目标点云$ S_{1} $，绿色点云是投影到$ \{{S_{1}} \} $的源点云$ S_{2} $。点云的平移量真值$ ^{S_{1}}_{S_{2}} {{\tau}} =\big[\, {{\rm 1.095}, {\rm 0.010}, {\rm 0.011}} \, \big]^{{\rm T}} $，而原始前端里程计估计的平移量$ ^{S_{1} }_{S_{2}}{\tilde{\mathit{\boldsymbol{\tau}}}} =\big[\, {0.004, -{\rm 0.002}, {\rm 0.020}} \, \big]^{{\rm T}} $，从图 3(b)亦可看到，移动了1 m的路人被重叠在了一起。显然，激光雷达被判断为静止在原地。

通过上述分析可知，激光SLAM的失效问题，实质上的原因是NDT算法在相似场景中得到的差异性几何特征不足，无法使估计位姿从初始单位矩阵迭代至位姿真值。因此，若可以为NDT算法提供一个更准确的初始迭代位姿，同时增加感知覆盖范围和点云密度来提高场景差异性，相似场景下激光SLAM的失效现象就会得到改善。

本文在原本的前端算法基础上进行如下改动：(1) 改进运动预测模型；(2) 构建局部地图参与点云配准。改进后的前端里程计算法框图如图 4所示，具体细节将于之后进行讨论。

3.3 运动预测模型的改进

假设激光雷达运动时，线速度和角速度关于时间是平滑且连续的，且在较短的时间间隔内近似看作常数。根据牛顿运动学可知，激光雷达在2次连续采样间隔内的运动近似相等，即当前点云$ S_{i} $相对于上一帧点云$ S_{i-1} $的位姿变换$ _{\kern 6pt S_{i}} ^{S_{i-1}} {{{\mathit{\boldsymbol{T}}}}} $近似于$ S_{i-1} $相对于前一帧点云$ S_{i-2} $的位姿变换$ _{S_{i-1} }^{S_{i-2}} {{{\mathit{\boldsymbol{T}}}}} $。由于点云配准是一个非凸优化问题，其计算结果为初值附近的局部最优解。因此，当使用$ _{S_{i-1}}^{S_{i-2}} {{{\mathit{\boldsymbol{T}}}}} $作为NDT算法的初始迭代位姿来求解$ _{\kern 6pt S_{i}} ^{S_{i-1}} {{{\mathit{\boldsymbol{T}}}}} $时，理论上可使配准结果更接近位姿真值。

为减少矩阵连乘带来的累积误差，沿用文[8]中关键帧的概念，即抽取一部分帧作为关键帧，作为点云配准的目标和最后建图的素材。假设当前帧点云$ S_{i} $与上一个关键帧点云$ K $之间的变换矩阵为$ _{S_{i}} ^{K} {{{\mathit{\boldsymbol{T}}}}} $，平移量$ \Delta x_{i} $和轴角$ \Delta \theta_{i} $的计算公式如式(4) 所示：

$ \begin{equation} \begin{cases} \Delta x_{i} =\|_{S_{i}} ^{K}{{\tau}} \|_{2} \\ \Delta \theta_{i} =\arccos \dfrac{{\rm tr} {}_{S_{i}}^{K} {{{\mathit{\boldsymbol{R}}}}}}{2}\\ \end{cases} \end{equation} $

(4)

其中，$ _{S_{i}}^{K}{{\tau}} $为$ _{S_{i}} ^{K} {{{\mathit{\boldsymbol{T}}}}} $中的平移分量，$ \|\cdot \|_{2} $表示求向量的2范数；$ _{S_{i}} ^{K} {{{\mathit{\boldsymbol{R}}}}} $为$ _{S_{i}} ^{K} {{{\mathit{\boldsymbol{T}}}}} $的旋转矩阵，$ {\rm tr} $表示求矩阵的迹。关键帧选取条件如式(5) 和式(6) 所示：

$ \begin{align} \Delta x_{i} &\geqslant \Delta x_{{\rm th}} \end{align} $

(5)

$ \begin{align} \Delta \theta_{i} &\geqslant \Delta \theta_{{\rm th}} \end{align} $

(6)

其中，$ \Delta x_{{\rm th}} $和$ \Delta \theta_{{\rm th}} $分别表示相邻2个关键帧点云的最大平移量和最大轴角。若计算得到的$ \Delta x_{i} $和$ \Delta \theta_{i} $满足上述任一条件，则将$ S_{i} $对应的帧作为新的关键帧，此后的每一帧点云均以$ S_{i} $为目标点云进行配准，直至新的关键帧产生。

综上，若已知从上一关键帧点云$ K $到上一帧点云$ S_{i-1} $之间的坐标系变换矩阵为$ _{S_{i-1}} ^{\kern 3mm K} {{{\mathit{\boldsymbol{T}}}}} $，从上一关键帧点云$ K $到前一帧点云$ S_{i-2} $的变换矩阵为$ _{S_{i-2} }^{\kern 3mm K} {{{\mathit{\boldsymbol{T}}}}} $，则如图 5所示，改进后的运动预测模型可以表示为

$ \begin{aligned} \boldsymbol{T}_{\text {guess }} &={ }_{S_{i-1}}^{K} \boldsymbol{T}_{S_{i}}^{S_{i-1}} \boldsymbol{T} \\ & \approx{ }_{{S_{i - 1}}}^K{\mathit{\boldsymbol{T}}}_{S_{i-1}}^{S_{i-2}} \boldsymbol{T} \\ &={ }_{{S_{i - 1}}}^K{\mathit{\boldsymbol{T}}}_{S_{i-2}}^K{\boldsymbol{T}}^{-1}{ }_{S_{i-1}}^{K} \boldsymbol{T} \end{aligned} $

(7)

式中，$ {{{\mathit{\boldsymbol{T}}}}}_\rm{guess} $为当前帧点云$ S_{i} $与关键帧点云$ K $配准的迭代初值，其余位姿矩阵均可在之前帧的配准中获得。

3.4 局部地图的构建

LOAM^[6]算法的里程计线程将距离当前帧坐标原点一定范围内的特征点转换至当前帧坐标系下，形成局部特征地图参与最近点迭代匹配，这使得激光里程计可以使用更广更密的几何特征进行位姿估计，从而获得更高的配准精度。该思想在SLAM中亦被称为scan-to-map。受其启发，在前端里程计中引入局部地图概念，将邻近的关键帧均投影至同一个坐标系中，拼接成局部点云地图参与NDT配准。其作用主要有3点：

(1) 邻近关键帧拼接后，则历史关键帧中所包含的几何信息也全部被转换到了局部点云地图中，这使得NDT算法可以获得更广范围的几何特征，丰富了相似场景中的几何信息。

(2) 拼接成局部地图，点云密度显著上升。由于目标点云栅格内的激光点数量增加，根据概率学知识可知，此时计算得到的正态分布参数（均值与方差）更能反映真实的环境特征，有利于NDT算法的精确配准。这一点对于低线束激光雷达的作用尤为显著。

(3) 邻近关键帧拼接后，形如墙壁、隧道壁之类的连续立面会作为整体参与点云配准，相比于原始的帧帧匹配方式，可以提高立面的局部一致性，减少里程计在垂直于立面方向的漂移。

局部地图拼接的细节会显著影响前端算法的精度。由于NDT算法的运行速度会随着局部地图规模增大而迅速增大，而过于久远的关键帧既不会包含当前帧可以用到的几何信息，同时也无法提升当前传感器所在位置附近的点云密度，因此将参与局部地图构建的关键帧数量限定在离当前帧最近的$ \lambda $帧。$ \lambda $取不同值对配准精度的影响将于4.3.3节以实验来说明。

将当前时刻$ i $之前所有关键帧的集合记为$ K(i-1) $，集合中的每个关键帧记作$ k_{j} $，$ j=1, 2, $ $ \cdots, $ $ \varphi (i-1) $，其中$ \varphi (i-1) $表示在$ i $之前的关键帧总数，该值随着SLAM的运行不断增大。拼接局部地图时，需要将关键帧$ k_{\varphi (i-1)-\lambda +1} $~$ k_{\varphi (i-1)} $转换到同一坐标系下，称为“归一坐标系”。理论上，该坐标系既可选择全局坐标系$ \{ W \} $，亦可选择关键帧坐标系$ \{k_{\varphi (i-1)}\} $，但由于计算机浮点值精度有限，在编程实践中，选择$ \{k_{\varphi (i-1)}\} $往往优于$ \{W \} $。当选择$ \{W\} $时，点云配准结果表征该帧在$ \{W\} $中的位姿，其值受到先前所有帧的配准误差影响；当选择$ \{k_{\varphi (i-1)}\} $时，点云配准结果表征相邻关键帧之间的相对位姿，其值仅受到最近$ \lambda $帧点云的配准误差影响。为充分说明坐标系选择对SLAM前端里程计精度的影响，相应的实验于4.3.2节进行。

局部地图拼接算法的伪代码如算法1所示。

算法1：局部地图拼接算法
1 input：$k_{j}, ~ {}_{k_{j} }^{W} {T}$（$j=1, 2, \cdots, \varphi (i-1)$）
2 output：局部地图点云$L_{i} $
3 begin
4    $n=\max \{1, \varphi (i-1)-\lambda +1\}$
5    for ($j=n$; $j\leq \varphi (i-1)$; $j++$) do
6        $_{\kern 5mm k_{j} }^{k_{\varphi (i-1)} } {T}=(_{k_{\varphi (i-1)} }^{\kern 5mm W} {T})^{-1} \; _{k_{j} }^{W} {T}$
7        $L_{i} =L_{i}\cup \{{}_{\kern 5mm k_{j} }^{k_{\varphi (i-1)} } {T}\odot {p} \, \big|\, \forall {p} \in k_{j} \}$  //将转换后的$k_j$拼接入局部地图
8    end for
9    if ($\varphi (i-1)-n\geq \eta)$ then
10         对局部地图$L_{i} $ 进行体素降采样
11    end if
12    return $L_{i} $
13 end

其中，$ \eta $为局部地图降采样的阈值。当拼接局部地图使用的关键帧数超过该阈值时，为了加快配准速度，对拼接好的点云地图进行体素降采样滤波，即使用体素质心近似体素栅格内的所有激光点。由于降采样滤波会导致局部地图带来的点云稠密效果被削弱，降采样栅格尺寸应比NDT算法的栅格分辨率低1个数量级，确保滤波完成后点云仍有足够的激光点来准确拟合正态分布函数。

3.5 前端里程计的整体结构

改进后的前端里程计结构已在图 4中给出。在构建好局部地图$ L_{i} $后，NDT算法将其作为目标点云，并将新点云帧$ S_{i} $作为源点云，进行点云配准，求得从局部地图坐标系$ \{L_{i} \} $到$ \{S_{i}\} $坐标系的位姿变换$ _{S_{i}} ^{L_{i}} {{{\mathit{\boldsymbol{T}}}}} $。借助式(3)，可得从世界坐标系$ \{W\} $到$ \{S_{i}\} $坐标系的位姿变换$ _{S_{i}} ^{W} {{{\mathit{\boldsymbol{T}}}}} $。前端里程计向后端线程发送该位姿，进行图优化以提高地图SLAM的全局一致性。

由于使用了局部地图代替关键帧参与NDT配准，因此将运动预测模型(7) 重写为式(8) 的形式：

$ \begin{align} {{{\mathit{\boldsymbol{T}}}}}_{\rm{guess}} &={}_{S_{i-1}} ^{\kern 2.5mm L_{i}} {{{\mathit{\boldsymbol{T}}}}} {}_{S_{i} }^{S_{i-1}} {{{\mathit{\boldsymbol{T}}}}} \\ &\approx{}_{S_{i-1}} ^{\kern 2.5mm L_{i}} {{{\mathit{\boldsymbol{T}}}}} _{S_{i-1}} ^{S_{i-2}} {{{\mathit{\boldsymbol{T}}}}} \\ &={}_{S_{i-1}} ^{\kern 2.5mm L_{i}} {{{\mathit{\boldsymbol{T}}}}} {} _{S_{i-2}} ^{\kern 2.5mm L_{i}} { {{\mathit{\boldsymbol{T}}}}}^{-1} {}_{S_{i-1}} ^{\kern 2.5mm L_{i}} {{{\mathit{\boldsymbol{T}}}}} \end{align} $

(8)

完整的前端里程计算法伪代码见算法2。为便于理解，在伪代码中引入中间变量$ {{\mathit{\boldsymbol{T}}}}_{\rm local} $、$ {{\mathit{\boldsymbol{T}}}}_{\rm last} $和$ {{\mathit{\boldsymbol{T}}}}_{\rm prev} $，$ {{\mathit{\boldsymbol{T}}}}_{\rm local} $表示从世界坐标系$ \{ W\} $到当前局部地图坐标系$ \{ L_i\} $的位姿转换，$ {{\mathit{\boldsymbol{T}}}}_{\rm last} $表示从上一张局部地图坐标系$ \{L_{i-1} \} $到上一帧点云坐标系$ \{ S_{i-1}\} $的位姿转换，$ {{\mathit{\boldsymbol{T}}}}_{\rm prev} $表示从坐标系$ \{S_{i-1}\} $到$ \{ S_{i-2}\} $的位姿转换。

算法2：前端里程计算法
1 input：点云帧序列$S_{i} $，$i=1, 2, \cdots$
2 output：从$\{W\}$到$\{S_{i}\}$的位姿变换$_{S_i}^W{T} $
3 begin
4     for $i=1, 2, \cdots$ do
5      if $i==1$ then
6         将$S_{i} $标记为关键帧
7         拼接局部地图$L_{i+1} $
8        $ {T}_{{\rm local}} \leftarrow {I}_{4} $，$ {T}_{{\rm last}} \leftarrow {I}_{4} $，$ {T}_{{\rm prev}} \leftarrow {I}_{4} $$\kern 52mm $      //初始化中间变量
9         $^W_{S_i}{T} \leftarrow {I}_{4} $
10     return $^W_{S_i}{T} $
11     end if
12     $ {T}_\text{guess} \leftarrow {T}_{{\rm last}} {T}_{{\rm prev}} $
13     以$ {T}_{\rm{guess}} $为迭代初值，运行NDT配准算法，其中    目标点云为${L_i}$，源点云为${S_i}$
14     NDT算法运行结果为$^{L_i}_{S_i}{T}$
15     $ {T}_{{\rm prev}} \leftarrow {T}_{{\rm last}}^{-1} {}^{L_i}_{S_i}{T} $
16     $ {T}_{{\rm last}} \leftarrow {}^{L_i}_{S_i}{T} $
17     $ {}^W_{S_i}{T} \leftarrow {T}_{{\rm local}} {}^{L_i}_{S_i}{T} $
18     利用$ {}^{L_i}_{S_i}{T} $计算$\Delta x_i$和$\Delta \theta _i $
19     if ($\Delta x_i \geq \Delta x_{{\rm th}} \;\text{or}\;\Delta \theta _i \geq \Delta \theta_{{\rm th}} )$ then
20        将$S_{i} $标记为关键帧
21        拼接局部地图$L_{i+1} $
22        $ {T}_{{\rm last}} \leftarrow {I}_{4} $
23        $ {T}_{{\rm local}} \leftarrow {}^W_{S_i}{T} $
24     else
25        $ {L}_{i+1} = L_{i}$
26     end if
27     return $ {}^W_{S_i}{T}$
28  end for
29 end

4 实验与分析（Experiments and analyses）

为测试本文算法性能，采用如图 6所示的UGV设备进行测试。装设的激光雷达有16条激光线束，水平视角360°，垂直视角30°，点云输出频率为10 Hz，因此前端算法的耗时应尽可能低于100 ms。底盘可进行原地旋转，最大行进线速度为1.5 m/s，最大旋转角速度为0.5235 rad/s。算法运行环境为Intel i5-4200H（2.8 GHz，双核），12 G内存。算法主体基于ROS^[15]平台进行开发。

实验一共分为3部分：(1) 在室内相似场景下，对比本文提出的改进算法与其他算法的性能差异；(2) 在室外场景下，对比本文算法与其他算法的性能差异；(3) 通过实验来展开对算法细节设置的讨论。

实验考察的性能指标有：里程计漂移、场景还原度和配准耗时。

(1) 里程计漂移：当行程的起点与终点为同一点时，起点估计位姿和终点估计位姿之间的欧氏距离$ d_{{\rm bias}} $表征了里程计漂移性能。规定起点位姿为$ (0, 0, 0) $，记终点估计位姿为$ (x, y, z) $，则$ d_{{\rm bias}} $计算方式为

$ \begin{equation} d_{{\rm bias}} =\sqrt{x^{2}+y^{2}+z^{2}} \end{equation} $

(9)

在SLAM过程中，有部分点云帧之间的里程计漂移会互相抵消，因此该指标仅作为衡量算法性能的一个相对指标。

(2) 场景还原度：考虑到里程计漂移存在互相抵消的情况，本文额外引入场景还原度$ \gamma $，其计算方式为

$ \begin{equation} \gamma =\frac{l_{{\rm m}}} {l_{{\rm t}}} \times 100\% \end{equation} $

(10)

其中，$ l_{{\rm m}} $是最后建成的点云地图里场景长度的测量值，如走廊点云长度；$ l_{{\rm t}} $是现实中手工测量得到的场景长度值。因此，$ \gamma $直观地反映了SLAM算法对现实场景的还原程度。里程计漂移与场景还原度这2项指标综合地反映了里程计精度。

(3) 配准耗时：前端里程计进行1次点云配准的平均耗时。

4.1 室内相似场景下SLAM算法性能测试 4.1.1 实验说明

室内测试场景为校园内某处回廊，该回廊呈标准矩形，长51 m，宽40.9 m，地面铺设了瓷砖，基本符合hdl_graph_slam算法^[8]的地面水平假设。走廊1和走廊3的局部放大图如图 7所示，完整走廊的俯视图及序号标记见图 8(a)。走廊1为封闭走廊，左端和右端均为大面积的立面障碍物（墙壁和不透明门窗），几何特征较少，场景重复度高，符合本文所讨论的相似场景特点。走廊2和走廊4，靠近中间庭院一侧为栏杆，远离庭院一侧则有楼梯间，场景差异性大，属于易于点云配准的非相似场景。走廊3整体上与走廊1对称，但在录制数据时，打开了若干门窗，使得办公室室内环境也被扫描了进来，图 7中蓝色圆圈部分即为房间点云。由于走廊3的开放房间沿着走廊均匀排布，提供了垂直于走廊朝向的几何特征，其配准难度介于相似场景和非相似场景之间，称其为“准相似场景”。

实验时，手动遥控UGV沿回廊行驶1圈然后回到起点，期间保持线速度在0.8 m/s~1.2 m/s，以满足3.3节中的匀速运动假设。该数据集共2200帧点云。

针对该数据集，除了分别运行原始的基于NDT^[5]的hdl_graph_slam^[8]算法（实验中简记为“NDT”），以及本文提出的基于局部地图的改进算法（实验中简记为“Local”）以外，本文还额外地增加了一组基于GICP^[11]配准算法实现的对照组（实验中简记为“GICP”）。该对照组将原始的hdl_graph_slam前端里程计的配准算法由NDT换成GICP。相对而言，GICP虽然运算速度慢，但由于引入了点－面配对和面－面配对，对场景的适应性和收敛精度要远高于单纯的NDT或者ICP算法，可以用于对比改进算法的提升效果。

实验过程的主要参数设置如下：点云有效距离30 m（对于16线激光雷达而言，远处的点过于稀疏，在室内环境下反而容易干扰匹配），$ \lambda =10 $，$ \eta =11 $，$ \Delta x_{{\rm th}} = $ 0.25 m，$ \Delta \theta _{{\rm th}} =1.0 $ rad，即对于Local算法而言，其局部地图不会进行体素降采样滤波。其余参数选择室内环境测试得出的较优值，不在本文讨论范围。

4.1.2 实验结果及分析

3个算法最后建成的点云地图如图 8所示。其中，图 8(a)(c)(e)为无回环检测的建图结果，从中可以测出里程计漂移；图 8(b)(d)(f)为引入回环检测（实验中简记为“$ + $Loop”）后的建图结果，全局一致性得到提升，从中可以评价算法的场景还原度。图中走廊上的圆点表示关键帧位置，红色线条表示前端算法输出的里程计信息，半透明红色圆球表示回环检测的范围。需要补充的是，图 8(b)(d)中矩形房间没有对齐的原因在于NDT与GICP算法的里程累积误差较大，在同样的回环检测参数下，无法形成准确的回环，形成了回环重影。相比之下，Local算法的回环一致性显然更好。

图 9为引入回环检测后的GICP和Local算法的建图结果放大视图。

3种算法的量化指标如表 1所示。结合图 8和表 1可以看出，NDT算法在相似场景和准相似场景下建图效果极差，走廊1和走廊3的平均场景还原度仅为42.02%。GICP算法相比于NDT算法既减少了里程计漂移，又将走廊1和3的平均场景还原度提升至93.03%，但其耗时几乎是原先的3倍。从细节来看，虽然GICP算法在走廊1的场景还原度达到92.44%，但仍会导致地图产生重叠，影响后续导航环节的展开，而Local算法则几乎没有重叠现象。从数据来看，Local算法的里程计漂移在三者中最低，场景还原度最高，耗时则介于NDT与GICP算法之间。

综上所述，本文提出的改进算法以少量耗时增加为代价，使SLAM算法在室内相似场景和准相似场景下的定位和建图效果显著提升。

4.2 室外相似场景下SLAM算法性能测试

室外测试场景为校园某段马路。图 10为UGV的大致行走路径。该路径全长1.1 km，在里程840 m处和1.1 km处各有一次回环，为了确保轨迹起点和终点位置一致，本次实验仅使用840 m处回环来计算里程计误差。UGV的行驶速度约为0.8 m/s~1.2 m/s，符合匀速运动假设。

该轨迹的大部分路段两侧为树木和建筑物，属于非相似场景（便于提取有差异性的特征）。然而，685 m~817 m这段轨迹的两侧为对称的篮球场和网球场，道路开阔无遮挡物，靠马路一侧均使用大网孔的钢丝网隔开，实际场景如图 11所示。由于激光雷达的绝大多数激光点会从空隙穿过，少数被钢丝网反射的激光点也无法形成有效的几何特征，因此该段属于相似场景。整个室外数据集共7300帧点云。

此次室外实验对象仍为NDT、GICP和Local算法。算法参数设置如下：激光雷达的检测距离为100 m，$ \Delta x_{{\rm th}} = $ 1 m，$ \Delta \theta_{{\rm th}} = $ 1.0 rad，$ \lambda =10 $，$ \eta =5 $，降采样体素栅格边长为0.1 m。

实验结果如图 12和表 2所示。显然，NDT算法在相似场景中彻底失效，GICP和Local算法的建图结果则基本与实际场景一致。耗时方面，GICP算法由于超过了100 ms，只能以0.5倍速播放数据集，无法满足实时性要求。

综上所述，在室外的相似场景中，本文提出的改进算法依然能够显著提升SLAM算法的定位和建图效果，且算法实时性明显优于GICP算法。

4.3 算法细节讨论 4.3.1 消融实验

本文针对相似场景提出的算法改进共有2点：(1) 改进了运动预测模型；(2) 在点云配准中引入了局部地图。本节通过引入对比实验组，构成消融实验，来说明2个改进点在SLAM中分别起到的作用。其中，实验所用的数据集和参数与4.1节相同，对比实验组（实验中简记为“NDT$ + $”）仅使用改进后的运动预测模型，而不引入局部地图。实验结果如图 13和表 3所示，其中NDT组与Local组的实验数据来自4.1.2节。

对比NDT与NDT$ + $组，走廊1和3的场景还原度分别提升了18.65% 和25.87%。走廊2和4的场景还原度则分别提升了4.83% 和1.19%。由于NDT算法在走廊1和3均完全失效，因此部分里程计漂移互相抵消；而在改进运动预测模型后，NDT$ + $组在准相似场景（走廊3）的效果有较大提升，在相似场景（走廊1）的效果提升略小，致使里程计漂移无法互相抵消，数值上反而大于NDT组。实验结果表明，改进运动预测模型通过提供更好的迭代初值，可以有效地改善前端里程计的性能。

Local组中所有走廊的场景还原度都达到98.5%以上，里程计漂移仅为1.6534 m（占轨迹全长的0.92%），这表明局部地图的引入可以大大改善相似场景和准相似场景下的前端里程计性能。

综上所述，改进运动预测模型与构建局部地图的做法分别为NDT组提供了更准确的初值和更广阔、更稠密的目标点云，切实地提高了前端里程计的精度以及SLAM算法整体的建图效果。

4.3.2 探讨局部地图坐标系选择的影响

本节通过设计对比实验来说明3.4节中提到的归一坐标系选择对SLAM算法的影响。

实验数据集和参数与4.1.1节所述一致，实验组（实验中简记为“Ori”）将归一坐标系设置为全局坐标系，其余均与对照组Local一致。

实验结果如图 14和表 4所示。从图 14与图 9(b)的对比以及表 4数据可知，相比于选择全局坐标系，将最后一帧关键帧的坐标系定为归一坐标系，可以减少前端里程计的漂移并提高场景还原度。

4.3.3 探讨局部地图尺寸$ \lambda $的影响

本节设计对比实验，说明不同局部地图尺寸$ \lambda $对算法性能的影响。实验数据集与参数与4.1.1节所述一致。$ \lambda $分别取为3、5和10，其中$ \lambda=10 $与前述的Local组为同一实验组别。

实验结果如图 15和表 5所示。随着$ \lambda $值逐步增大，里程计漂移减小，各段走廊的场景还原度也在不断增加，逐渐逼近100%。需要注意的是，$ \lambda=3 $和$ \lambda=5 $时的里程计漂移大于NDT组的原因与4.3.1节中NDT$ + $组大于NDT组的原因类似，即规模较小的局部地图虽然可以帮助SLAM算法在准相似场景中准确稳定地工作，但在相似场景中的提升稍显不足，这种差异使得里程计漂移无法相互抵消。此外，由于随着$ \lambda $的增大，配准耗时也会相应地提升，因此在实践中应权衡精度与实时性，选择合适的$ \lambda $。

综上所述，随着$ \lambda $的增大，SLAM算法在各场景下的里程计精度和建图效果均有所提升，但应注意前端配准耗时也会随之增加。

5 结论（Conclusion）

针对相似场景下激光SLAM算法退化问题，本文提出了一种基于局部地图的激光SLAM前端改进算法。该算法通过改进原有的运动预测模型和引入局部地图概念，以增加少量CPU耗时为代价，显著提升其在相似场景下算法的定位和建图精度。在室内相似场景中，该算法对实际场景的还原度达到99.54%，相比改进前提升了57.25%；在室外场景中，该算法使得里程计漂移从111.62 m降至7.65 m，且基本还原了实际场景。实验结果表明，本文提出的改进算法能有效地缓解激光SLAM算法在相似场景中的退化问题，显著地提升了前端里程计精度和整体的建图效果。出于实时性考虑，文中选择将局部地图与NDT算法搭配使用，实际上若进行离线建图，亦可方便地将本文算法与GICP算法结合，进一步提升3维重建的精度。

未来的研究一方面考虑结合IMU传感器放宽文中的匀速运动假设和地面水平假设，另一方面考虑在激光SLAM算法引入新的回环检测机制，改善回环性能。