1.1 机器人环境建模技术

对于智能移动机器人来说，其在完全未知的环境下获得自主能力的基础，是对环境模型进行创建——建立环境地图，并在环境模型的基础上同步实现定位，即同步定位与建图技术（Simultaneous Localization and Mapping，SLAM）。SLAM也是机器人在环境中完成各种智能任务的前提。

1.1.1 基于SLAM的环境建模

SLAM作为机器人领域的技术难题，涉及地图表示、不确定性信息处理、数据关联、自定位、探索规划等一系列高度相关的环节，根据地图描述环境的范围可分为局部地图和全局地图两种。大部分移动机器人采用激光测距传感器或可见光视觉传感器来创建局部地图，用于机器人的局部、自主避障。全局导航的目标点由用户进行人工选择、设置。全局地图的创建将为高自主性的全局自定位和导航提供可能，如基于递增极大似然法的全局混合地图的创建方法、采用用户人工引导方式快速创建大规模环境的拓扑和概率栅格混合地图等。

对于移动机器人来说，其SLAM过程中遇到的最大问题是未知环境所带来的挑战，比如室内环境的拥挤、动态变化，室外环境的大范围、特征稀疏等。区别于一般的智能汽车，警用机器人的应用场合包括机场、高铁站、商场以及室外广场等。在SLAM领域，前人已经做出了大量卓有成效的工作，其中具有代表性的有扩展卡尔曼滤波（Extended Kalman Filter，EKF）算法、扩展信息滤波（Extended Information Filter，EIF）算法以及粒子滤波（Rao-Blackwellized Particle Filters，RBPF）算法等。这些算法的核心思想是采用递归贝叶斯原理对系统状态（机器人位姿及环境特征）的后验概率进行估计，将SLAM问题本质等效为后验概率估计问题，即

P（x_1：t，m|z_1：t，u_0：t-1）

式中，m是环境地图；x_t是机器人在时刻t的位姿；z_t是激光测距传感器的观测；u_t是机器人控制量；x_1：t={x₁，…，x_t}是机器人从起始时刻到时刻t的行驶轨迹；u_1：t={u₁，…，u_t}是从起始时刻到时刻t的机器人控制序列。

在SLAM问题中，一般通过式（1-1）对环境地图m和机器人位姿x进行估计

由式（1-1）可以看出，地图m主要依赖移动机器人的行驶轨迹以及传感器的观测，只要确定机器人在各时刻的位姿x，就可以从（x_k，z_k）的信息组中解算出环境地图m。因此，SLAM问题就转变为一个轨迹估计问题

地图创建过程并不需要求取地图的完整分布P（m），只需求取地图的极大似然估计m^*即可，即不需直接求取P（x_1：t，m|z_1：t，u_0：t-1）的完整分布，只需求取P（x_1：t，m|z_1：t，u_0：t-1）的极大似然估计，则可得到

在SLAM研究中，为了计算与分析的方便性，假设移动机器人运动学模型及传感器观测模型的噪声均满足高斯分布，即

这样便可得到如下的关系

式中，⊕是坐标系的变换符号；T_ij和Σ_ij分别是位姿x_i与x_j间对应的坐标变换关系和协方差。对T_ij和Σ_ij进行分解，便可得出机器人的位姿以及环境地图。

考虑复杂环境下一般场景规模大，且特征稀疏、动态因素较多等特点，为保证机器人在该环境下导航时的定位精度、降低算法的计算复杂度，本章节主要针对二维、三维激光雷达在移动机器人SLAM中的应用提出了基于分层匹配的增量式SLAM算法，将SLAM问题简化为数据关联和最小二乘优化两个部分，然后通过以下步骤加以解决：首先，通过分层迭代最近点（ICP）匹配算法解决数据关联问题，并且对传感器观测与局部地图以及局部地图之间进行匹配，匹配结果的不确定性采用Fisher信息矩阵描述；其次，采用增量式正交三角（QR）分解对机器人位姿进行优化；最后，将SLAM问题简化为最小二乘问题，并通过增量优化算法求解，可提高计算效率。提出的建图算法可保证在大规模室内外场景下的建图精度，同时能够满足实时应用的需求。

在进行环境建模时，由于二维、三维激光雷达在数据量规模、地图表示、应用场景等方面存在差异，下面将分别进行阐述。

1.1.2 基于二维激光雷达的环境建模

不论在室内环境还是室外环境，当范围较大时，由于观测噪声以及匹配误差的存在，通过传感器观测信息配准得到的环境地图经常会出现不一致的描述。当机器人回到之前已经探索过的区域时，算法需要进行针对性处理，从而消除不一致性描述。在SLAM过程中，这个做法也被称为环形闭合（Loop Closure）。环形闭合对于移动机器人的地图创建非常重要。正确的环形闭合能够修正建图过程中产生的累积误差，从而提高地图精度；而错误的环形闭合将会在后续不确定性处理过程中引入累积误差，甚至破坏已建立的环境地图。

移动机器人环境建模需要机器人在定位、导航阶段具有较高的精度（厘米级），这样才能满足用户对巡检、安保等任务执行，以及拥挤环境下安全性导航的要求。针对这些要求，提出基于分层匹配的增量式SLAM（Multilayer Matching based Incremental SLAM，M2ISLAM）算法（图1-1）。该算法不提取环境特征，而采用基于图优化融合的方法对所有已知信息（传感器观测和机器人运动轨迹等）进行估计，并在图优化融合的方法中采用图论的思想将环境特征和机器人位姿作为顶点，将传感器观测信息作为边；作为边的观测信息描述了位姿间的空间约束关系，对特征点和位姿点的位置进行优化后，便可满足边所表示的约束关系。

从图1-1可以看出，在M2ISLAM算法中，SLAM问题被简化为数据关联和最小二乘图优化两个部分。

首先对数据关联进行求解。数据关联的求解是该算法的关键部分，决定了算法的精度，因此采用SLAM中常见的迭代最近点（Iterative Closet Point，ICP）匹配算法。根据观测z_t及z_1：t-1，估计x_t与x_1：t-1间的约束关系T_ij及其协方差Σ_ij，从而确定T_ij。同时，由于将z_t与z_1：t-1均进行ICP匹配在计算上是不可行的，所以采用分层匹配的方法进行z_t与局部地图以及局部地图与局部地图间的匹配，从而在保证匹配精度的同时降低算法复杂度。不同于常见的匹配策略——仅将z_t与z_t-1进行匹配或将z_t与所有邻域的观测进行匹配，这种匹配策略既有效减小了累积误差，也避免了算法在特征稀疏的环境中陷入局部极小。另外，由于ICP算法无法直接处理不确定性，所以Fisher信息矩阵被用来对匹配结果的不确定性进行定量估计，从而得到x_t与x_1：t-1间最终的协方差Σ_ij。

在得到x_t与x_1：t-1间的约束关系T_ij及其协方差Σ_ij之后，再采用增量式QR分解对机器人位姿进行优化，从而得到一致的地图描述及机器人位姿信息。每个模块的算法描述具体如下。

1.1.2.1 数据关联

在移动机器人SLAM问题求解过程中，数据关联是至关重要的部分。在将传感器观测信息与已知地图信息融合建立新的地图描述后，数据关联将成形并不能再被修改。也就是说，数据关联是不能动态调整的，稳定、精确且可靠的数据关联将保证地图描述的一致性与准确性，但是错误、松散的数据关联就将带来较大的误差和不一致的地图描述，甚至会导致地图创建的失败。因此，SLAM算法中关键的一部分就是解决提高数据关联的稳定性与精度问题，尤其在遇到Loop Closure问题时，数据关联将显得更为重要。

以几何地图为例，假设机器人通过传感器检测并获得环境中的特征为D={d₁，…，}，那么数据关联算法的作用就是建立一个集合H={j₁，…，j_p}，使得每一环境特征d_i均能与环境地图中的实际路标m_ji相对应（如果传感器对环境的观测与环境中的特征无关，则j_i=0）。图1-2所示为一种树状的数据关联结构，它表现的是传感器对环境的观测、环境特征d_i以及地图中的实际路标之间的关系。

对于图1-2中的任意一层，所有节点均表示环境路标d_i可能与实际路标存在数据关联；“*”表示环境中不存在任何实际路标与环境特征d_i相关联；M={m₁，…，m_q}为地图实际存在的路标。每个节点均会有p+1个分枝，由此便可形成（p+1）^q种可能的数据关联。而数据关联解算算法就是通过某种搜索算法，从全部可能的关联中找出需要的关联组合。由于计算量巨大，不可能将所有的观测信息均进行匹配，通常需要进行简化：

1）只考虑时间上相近的观测：由于只考虑了与当前时刻相近的观测信息，将不可避免地产生累积误差。因此，当环境范围逐渐增大时，地图描述的不一致性也将增大。

2）只考虑在空间上相近的观测：与上一简化方法相近，也忽略了部分观测信息，但这种方法可减小累积误差，策略更为合理。不过，当环境复杂或特征稀少时，空间上相近的观测信息间的差异度不大，使得匹配算法陷入局部极小。

3）将匹配对离散化，只将观测与地图信息进行匹配：此方法本质上与第二种简化方法类似，空间上不相近的观测对于匹配结果不会产影响，但该方法不需要限定邻域范围，较第二种方法具有一定的优势。然而，该方法并没有解决地图不一致描述的问题。

针对上述所分析的数据关联问题，采用分层匹配的方法，在观测信息与局部地图、局部地图与局部地图间分别进行匹配；匹配算法均采用迭代最近点匹配算法。由于ICP算法并不能直接处理信息的不确定性，故采用Fisher信息量来估计匹配协方差。下文将分别对基于ICP的迭代最近点匹配、分层匹配方法以及不确定性估计等部分进行详细的描述。

1.1.2.2 基于ICP的最近邻域匹配

最近邻域（Nearest Neighbor，NN）匹配方法，是通过将传感器对环境特征的观测与根据历史信息得到的环境特征预测进行比较，判断二者所得到的位置信息是否足够接近（一般由量化指标来表示），从而判定该特征是否与已知地图路标存在关联。

给定特征参考点集M={m₁，…，m_p}与观测点集D={d₁，…，d_p′}间的相对坐标变换关系为T=（R，t），其中，R是旋转矩阵，t是平移向量。基于ICP的迭代最近点匹配算法步骤如下：

Step1：通过最近邻域原则建立M与D之间的对应关系如下

式中，D_{inline_thread}是常数，需要根据传感器的噪声进行设定。

Step2：计算M与D的匹配误差，即

Step3：寻找最优的（R，t）使得上述的配准误差最小，即

Step4：重复进行上述步骤，直至R^*、t^*不再变化或达到最大迭代次数时，终止迭代过程。

根据已有的算法推导，上述过程中的Step3可采用奇异值分解（SVD）方法有效求解。同时，该迭代过程的算法耗时主要在于匹配误差的计算，因此采用K-D树对参考点集M进行再组织从而降低算法复杂度。另外，由于噪声以及未知障碍物等原因的影响，需要将无法匹配的观测值舍弃，即时，在Step2及Step3中将该组观测值舍弃。

1.1.2.3 分层匹配方法

假设在时刻t，机器人采集的观测信息为z_t，则z_t所包含的环境信息与z_1：t-1存在关联，此关联根据时间特性分为两类（连续与不连续）。考虑图1-3所示的位姿约束情景，机器人从x₀出发，在t=9的时刻，机器人回到之前已经探索过的区域（x₀），此时（x₉，z₉）同时与（x₈，z₈）及（x₀，z₀）存在约束关联。

对于时间上连续的位姿约束，可将z_t与邻域间观测构成的局部地图l进行匹配，如图1-4所示。其中，匹配的初始坐标变换可采用里程计信息或x_t-1，局部地图l为z_t-m：t-1信息的合集

下面考虑机器人回到τ时刻已探索的区域时的情形：在机器人行驶较长距离时，误差的累积将导致匹配算法迭代初值与真实值相差较大。因此，仅依靠单帧观测进行匹配易陷入局部极小。为此，将z_t-m：t构成的l_t与z_{τ-q-n：τ-q}的信息合集l_τ-q进行匹配，当l_t与l_τ-q描述的环境范围出现重合时，即机器人回到已探索区域，建立x_t与x_τ-n：τ之间的约束。

局部地图的大小决定了匹配算法的复杂度，这里根据机器人行驶路径的长度离散化τ，即τ∈{τ_i}，其中τ_i满足

当l_t与l_τ-q（τ∈{τ_i}）出现重叠，即两者所描述边界的凸包（Convex Hull）的交集大于一定面积时，进行l_t与l_τ间的ICP匹配。

图1-4形象地描述了分层ICP匹配算法过程，该算法可表述为如下几个步骤：

Step1：初始化，d=0，t=0，L_τ=Φ，l=Φ，q′=0。

Step2：l=l∪{（x_t，z_t）}，并采用K-D树进行描述，t=t+1。

Step3：z_t匹配，将z_t与l匹配，匹配结果即T_tj（t-n＜j＜t），如图1-4a所示。

Step4：d=d+||x_t-x_t-1||，如果d≥d_threshold，则转至Step5，否则转至Step2。

Step5：判断l_τ-q∈L_τ与l_t的凸包是否重合，若重合转至Step6，否则转至Step7。

Step6：进行局部地图l_t与l_τ-q之间的ICP匹配，匹配结果即T_tj（τ-q-n′＜j＜τ-q），并且q=q+1，如图1-4d所示。

Step7：L_τ=L_τ∪{l_t}，d=0，l=Φ，转至Step2。

在机器人未回到和回到之前已经探索过的某个区域时，算法的时间复杂度分别为常数和O（n）+T_ICP，这不仅满足实时运用，而且在地图闭合时能够产生可靠的约束T。

1.1.2.4 不确定性估计

在完成基于ICP的迭代最近邻域匹配之后，需要对匹配结果进行不确定性估计。准确的不确定性估计将会消除地图创建的误差，而不准确估计将加大地图的不一致性，不仅不会提高地图的精度，甚至有可能破坏已建立的地图。

假定匹配结果的不确定性满足均值为零的高斯分布，那么只需确定其协方差Σ即可，这里采用Fisher信息矩阵的逆作为协方差Σ。

定位问题中的Fisher信息矩阵定义为期望激光数据及激光扫描到的环境表面斜率的函数，将其离散化后可用于匹配结果的不确定性估计。观测z_t的Fisher信息矩阵为

式中，r_i是激光测距传感器的第i个激光束到障碍物的长度；σ²是激光数据的噪声方差；s_i是第i个激光束的影响因子，即对匹配的“贡献”大小，是观测值r_i的函数。

r_i越偏离在局部地图中的期望值，匹配结果的不确定性越高。因此，可将s_i定义为激光测距传感器观测的概率分布函数

根据Cramér-Rao界（CRB）定理可知，I（T）以矩阵的形式描述了当前定位的整体性能及方向性，为机器人匹配结果概率分布的协方差下界（不妨假设匹配结果达到下界），即有

当机器人工作在二维平面环境中时，有

分析式（1-14）及式（1-16）可知，只需确定即可确定协方差Σ。

考虑，一个关键问题是如何估计假定约束T′=T+ΔT成立时的观测。图1-5所示为模拟二维激光雷达扫描示例，当约束为T时，传感器获得观测{r_i}；即假定约束为T′时，传感器的观测应为，关系为

式中，θ是机器人转角；Δx是机器人在x方向的位移增量；Δy是机器人在y方向的位移增量；f（i）是对应于观测值r_i的扫描角度；f^-1是f（i）的逆函数。

考虑位姿及观测的序列{（x₁，z₁），（x₂，z₂），…，（x_t，z_t）}，扫描匹配算法根据观测z_t及z_1：t-1建立x_t与x_1：t-1间的约束关系T_ij。

1.1.2.5 基于QR分解的位姿优化

在通过分层匹配算法建立机器人位姿之间的约束之后，需要对位姿估计进行优化。这里采用图优化算法对其进行优化，从而得到全局一致的位姿估计。图优化算法是决定该地图创建算法能否满足实时运用的关键，特别是当图的节点上升时是否会出现维数灾难。采用基于QR分解的增量式平滑和建图方法（Incremental Smoothing and Mapping，iSAM）对图进行优化，以控制算法复杂度，满足实时运用的目的。

在二维情形下，x=（x，y，θ）^T，T可表示为（x_T，y_T，θ_T）^T，则⊕运算的具体形式可以表示为

于是，式（1-3）可更新为

令f_j（x_i）=x_i⊕T_ij，其为非线性函数，为方便处理需要将其线性化，即有

式中，是f_j（x_i）的雅可比矩阵。

在线性化之后，便可得到线性最小二乘优化问题

式（1-24）是式（1-22）的简化，其中。

记，则式（1-24）可转换成一般表达形式

式中，Q（R 0）^T是A的QR分解，这里采用增量式方法进行QR分解。

下面将对QR分解做进一步的讨论：由于QR分解的复杂度较高，而且是增量式的，一般是当出现新的约束或者添加新的变量时才进行完整的QR分解，这将导致算法的复杂度持续增加，从而无法满足实时使用的要求。

考虑t时刻出现新的约束或有新的变量添加，有

由于每次迭代过程中A的变化不大，即t-1时刻的A_t-1与t时刻的A_t中大部分的元素均是相同的，不妨采用上一时刻QR分解的吉文斯旋转（Givens Rotation）矩阵作为迭代初值（在两者不同时可直接向A_t-1添加元素0进行扩充），则有

此时，QR分解是增量进行的，而上三角阵求逆的复杂度较小，因此该方法可满足实时运用。

1.1.3 基于三维激光雷达的环境建模

经过近几十年的发展，可以认为SLAM技术在理论及概念层面已得到基本解决，尤其在室内结构化场景中，基于SLAM技术的移动机器人完成定位导航任务已经得到产品级应用，但是在大范围室外环境中仍然存在大量问题亟待解决，因此仍需致力解决如下问题：

1）大规模场景下，随着机器人行走距离的延长，数据关联不可避免的累积误差会导致机器人重新回到已访问区域或对路径的重复探索时，破坏地图的一致性，需要寻找合理的方式来解决此时的数据关联问题。同时，场景的大尺度对于后端优化的计算复杂度也是一大挑战。

2）室外环境相比室内复杂性更高，场景中特征匮乏与无规则（自然地形）、动态（广场、道路）与噪声数据（树木较多的场景）较多时，如何保证数据关联的鲁棒性与位姿估计的精度也是要解决的问题。

为解决上述问题，本节提出了针对三维激光雷达的基于多层ICP匹配的SLAM算法（Multilayer ICP Matching based SLAM，MIM_SLAM），其框架如图1-6所示。

从MIM_SLAM算法框架可以看出：

1）首先，一种点云降采样策略被提出。考虑激光点云数据量太大，同时场景中可能存在噪声且动态的物体，因此该方法可有效提取环境中稳定的几何特征来参与匹配，以减小匹配过程的不确定性。

2）其次，SLAM的数据关联问题通过多层ICP匹配来解决。该算法充分考虑了时间相邻帧以及地域相邻帧的数据关联问题；结合Scan-to-Scan（10Hz频率）以及Scan-to-SubMap（1Hz频率）的匹配方式，可以构建实时低漂移的激光里程计，以针对激光里程计不可避免的累积误差；利用提出的Scan-to-KeyScan匹配来解决机器人重新访问已经历区域时的数据关联问题，其中的关键技术是提出一种基于随机森林学习的场景识别算法；经过上述步骤，可以得到位姿x_i与位姿x_j之间的约束关系T_ij以及不确定性Σ_ij，并且以位姿图（顶点即表示位姿，边表示位姿约束）的形式保存。

3）最终，增量QR分解为核心的增量优化被用来优化该位姿图，降低计算复杂度。

整个算法可保证实时性要求，各部分算法模块将在后续章节详细介绍。

1.1.3.1 点云降采样策略

通常，三维激光雷达一帧的数据量非常大。例如，自动驾驶领域较常用的16线激光雷达（Velodyne VLP-16传感器）一帧扫描具有约30000个激光点，而另一款64线激光雷达（Velodyne HDL-64E传感器）一帧扫描具有约250000个激光点。如此庞大的数据量显然不适合直接匹配，这会导致算法无法达到实时性的要求，而常见的降采样算法有：

1）距离滤波：激光探测范围很远。由激光观测方式可知，距离越远的激光点越稀疏，因此距离过远的激光点并不能正确地描述观测物体的整体形状，可根据距离剔除。

2）体素滤波：对于密集点云进行降采样的方式。首先对输入的点云数据进行三维栅格的划分，然后遍历每个体素，用体素中所有点的重心来近似显示该体素中的其他点。该方法可通过栅格分辨率调节降采样后的点云数量，同时保证点云的整体形状特征。

3）统计滤波器：这是一种去除离群点的滤波方式。对于输入数据中的每个点，计算到所有K个临近点的平均距离与方差（假设满足高斯分布，其形状是由均值和标准差决定的），不在标准范围内的点被认为是离群点进行剔除。

上述几种方式虽然简单且计算量小，但在复杂环境下却有很大的局限性，因为场景中会有大量的动态（行人、车辆等）干扰、噪声（树叶、草坪等）等数据，显然这些点云数据对于接下来的匹配算法是不利的，而上述降采样方法无法处理该问题。学者们曾给出一种更好的采样策略——利用激光点云的协方差矩阵来寻找几何稳定点，然而该方法在匹配过程中是非常耗时的，很难在线应用。受其启发，这里提出了一种快速的点云降采样策略。

点云匹配的实质是寻找一个变换矩阵，使得输入点云转移到参考点云坐标系下，并具有最小的关联误差。对于移动机器人处于三维的场景中，该变换矩阵（旋转矩阵+平移向量）是6自由度（Degree of Freedom，DOF）的，即（t_x，t_y，t_z，θ_roll，θ_pitch，θ_yaw）。因此这里提出的快速点云降采样策略的实质是寻找利于匹配求解该6DOF位姿的点云数据，例如地面点云对决定（t_z，θ_roll，θ_pitch）是有帮助的，平行于激光雷达X轴坐标方向的墙面对决定（t_y，θ_yaw）是有帮助的。

图1-7所示为所提出的点云降采样算法框图。首先，对于三维激光雷达输入的点云数据P，提取地面点云信息为P_g，对去地面点云P_s进行基于深度图像的点云分割，得到一系列分割好的点云块{S_i|i=1，…，n}，然后对其进行稳定点云块{T_i|i=1，…，m}的提取，最终与地面点云一起作为输出点云数据。如图1-7左下图所示，输入点云含有大量的噪声数据，包括大量树叶以及远处的稀疏点，经过上述处理后得到的输出点云（其中红色表示稳定点云块集合，蓝色表示地面点云）如图1-7右下图所示，仅包含地面、柱子、墙面、静态车辆等有助于匹配的静态稳定点云数据。

由于三维激光雷达产生的点云数据具有无序性，所以还需将其转换为深度图像进行有序化处理。图1-8所示为Velodyne VLP-16 型号激光雷达的观测方式，绝大多数的国产三维激光雷达的观测方式也相同。其中，垂直视域范围为-15°～15°，角分辨率为2°，可离散化为16条扫描线；水平视域范围为360°，角分辨率为0.18°。对周围环境感知的一帧观测数据如图1-8c所示。

每个激光点均可以用（α，ω）来表示，其中α∈（-90°，90°），ω∈（0°，360°）。α表示该点投影到XY平面上与Y轴方向的夹角，ω表示该点与XY平面的夹角。如此，一帧点云数据便可以转换为深度图像。对于Velodyne VLP-16产生的点云数据，转换为深度图像是16×2000的矩阵，每个矩阵元素存储的是该激光点的坐标以及距离信息，映射到行与列的计算方式如下

式中，vert_bottom是最下面一层扫描线的垂直角度；vert_res是垂直角分辨率；horz_res是水平角分辨率。

（1）地面点提取

将激光雷达的点云数据进行有序化处理过后，即可方便提取水平地面点。本节考虑采用一种快速且简单的地面提取方法，相比网格法和分类器法，在基于扫描线的稀疏点云中表现更好。因为是应用在移动平台上，激光雷达的架设方式一般为水平放置，所以对于垂直角度大于水平视角的扫描线不作考虑。对于深度图像中的每个激光点p_c，寻找上一行对应列的激光点记为p_u，则计算如下

当上式求解结果小于一定阈值angThre时，即被认为是地面点。具体的算法流程见算法1-1。

（2）基于深度图像的点云分割

从三维点云数据中进行物体分割是移动机器人领域的一大研究课题。当机器人在环境中进行导航时，需要知道物体是否改变或移动，因此对场景中的物体进行分割是首要前提。由此，利用点云分割后的结果提取稳定、利于匹配的点云数据，可以看出，准确的物体分割对于后续处理是至关重要的。这里使用了前人研究所提出的一种快速且精准的点云分割算法，该算法用于判断两个相邻（水平或垂直）激光束是否来自同一个物体表面。该点云分割算法示意如图1-9所示。

假设激光雷达所在位置为O，OA和OB分别表示两个相邻（水平或垂直）激光束，AB连线可能在同一个物体表面也可能不在。该算法断言：如果β＞θ（θ是固定阈值），则表示两束激光打在同一物体表面，否则没有打在同一物体表面。图1-9b中，紫色与黄色的两块点云是激光雷达扫描到的两个行人，图中下部是进行的俯视图投影，绿色线表示两束激光来自同一物体（β＞θ），而红色线的β值小于一定阈值，则两束激光被标记为来自不同的物体。算法中β值的计算也非常简单，即

式中，d₁是两束激光中距离值大的那个；d₂是两束激光中距离值小的那个；α是水平角分辨率（两束激光水平相邻）或垂直角分辨率（两束激光垂直相邻）。

具体的点云分割算法和标记函数见算法1-2和算法1-3。

图1-10所示为应用该算法在三个场景进行点云分割的结果，其中阈值参数θ取10°。

1）在室内结构化场景中，场景信息比较单一，只存在墙面和一些动态的行人，分割难度低，由图1-10a可以看出室内环境的分割效果很好。

2）图1-10b所示为在室外道路上的测试结果。该场景中存在较多环境因素，如行人、车辆、树木、草坪、墙面等信息，也均被正确地分割。

3）也在较为空旷的结构化变电站场景中进行了测试，如图1-10c所示。近处的冬青树以及墙面、路沿石被成功分割，环境中存在大量较细的变电柱也可以很好地被分割，对于远处的墙和树叶也具有较好的分割效果。

（3）稳定点云提取

从上一小节可以看出环境中的物体被正确分割，但是其中包含了很多动态的、不稳定的环境因素，例如树叶（是不稳定的特征，因为同一片树叶很难在连续的两帧观测中存在）、行人（不是静态的，对于估计连续两帧观测间的位姿是不利的）等。因此本小节对于分割后的结果提出一种稳定点云的提取方法。

由于降采样后的点云数据是输入匹配算法中用于求解连续两帧数据间的变换矩阵，故针对移动机器人在三维场景的应用进行位姿估计，该变换矩阵为6DOF的，即（t_x，t_y，t_z，θ_roll，θ_pitch，θ_yaw）。因此本小节提出的算法实质是寻找有助于求解该6DOF位姿的点云数据，例如地面点云对决定（t_z，θ_roll，θ_pitch）是有帮助的，平行于激光雷达X轴坐标方向的墙面对决定（t_y，θ_yaw）是有帮助的。

首先，为保证算法运行的实时性要求，对于提取的地面点云降采样2s个激光点进行保存，因为地面点云对决定（t_z，θ_roll，θ_pitch）是有帮助的。

其次，由上一小节进行点云分割提取的点云块集合{S_Label}，对于每个点云块S_Label={p_i|i=1，…，N}计算均值和协方差矩阵：

对式（1-33）中协方差矩阵C进行特征值分解，得到特征向量e₀、e₁、e₂，对应的三个特征值为λ₀、λ₁、λ₂，特别地，λ₀≥λ₁≥λ₂且被归一化。由主成分分析（Principal Component Analysis，PCA）可知，对于场景中物体的三种特征类型具有不同的特征值表示，若λ₀≈λ₁≈λ₂，则表示散乱特征；若λ₀＞＞λ₁≈λ₂，则表示线性特征；若λ₀≈λ₁＞＞λ₂，则表示平面特征。定义线性度与平面度如下

对每个点云块计算如下三个值

式中，X_v是X轴单位向量；Y_v是Y轴单位向量；Z_v是Z轴单位向量；f₁是该点云块为平面特征且对决定（t_x，θ_yaw）贡献量的大小，f₂是该点云块为平面特征且对决定（t_y，θ_yaw）量的大小；f₃是该点云块为线性特征且对决定（t_x，t_y）贡献量的大小。

其目的是尽量提取环境中对定位（t_x，t_y，θ_yaw）方向有帮助的平面特征（建筑墙面等）以及线性特征（树干、柱子等）。对于所有点云块，这三个值按照降序进行排列，每一列只取前s个激光点保存。

最终，提取出环境中有利于定位（t_z，θ_roll，θ_pitch）方向的地面点，以及有利于定位（t_x，t_y，θ_yaw）方向的墙面、树干、柱子等点云数据。共计有5s个激光点云数据，实验中一般设置s=1000。将降采样后的点云数据输入后文匹配算法中完成位姿估计。

为验证所提出的点云降采样策略的有效性，与传统的体素滤波降采样算法开展对比工作，为保证两种算法降采样后的点云数量的一致性，算法的关键参数s设置为1000，体素滤波的体素栅格设置为0.8m。两种算法在两个场景下点云降采样的示例如图1-11所示，直观上两种算法基本保证了环境的整体外观形状特征。

1）图1-11a与图1-11b所示为室外道路的场景下测试。其中，蓝色点云所示是激光雷达观测到的一帧完整数据，可以看出包含了较多的不稳定环境因素，例如草坪、树叶等。图1-11a中的红色点云是经过所提出的点云降采样策略后的输出点云，仅包含墙面、柱子、树干、地面等静态、稳定的特征；反观图1-11b室外道路环境中的红色点云，却包含了大量的树叶、远处杂点等噪声数据，这对于匹配算法显然是不利的。

2）图1-11c与图1-11d所示为结构化变电站场景下的测试。由图1-11c可以看出算法的输出点云只保留了环境中利于匹配的特征，如墙面、地面、变电柱、变电设备等，图1-11d所示的体素滤波后的点云输出中包含较多打在近处冬青树木上的激光点。

因此，所提出的点云降采样策略是非常有效的，可以避免保留不利于匹配的树叶、草坪、行人等动态噪声数据，对于提升匹配精度是至关重要的。

1.1.3.2 多层ICP匹配方法

对输入点云进行降采样处理后，可以得到一帧用于匹配的静态稳定点云数据。本节介绍MIM_SLAM算法中的多层ICP匹配方法，如图1-6所示。这里所说的多层匹配，包括Scan-to-Scan匹配、Scan-to-SubMap匹配和Scan-to-KeyScan匹配等。其中利用Scan-to-Scan和Scan-to-SubMap相结合的匹配方式，可构建实时低漂移的激光里程计，用于解决在稀疏、特征匮乏场景下时间相邻帧的数据关联不可靠的问题。但是对于复杂场景下的应用，激光里程计不可避免的累积误差问题可能导致地图的不一致性，于是Scan-to-Key-Scan匹配被用来解决地域相邻帧的数据关联问题，也被称为回环检测问题。下面分别从三个方面来阐述。

（1）Scan-to-Scan匹配

假设已知t-1时刻机器人的位姿，对于t时刻输入的一帧激光观测数据，如何确定当前位姿即为Scan-to-Scan匹配。很多杰出的算法被提出来解决该类问题，例如ICP、NDT、栅格相关性等。其中，迭代最近点（Iterative Closest Point，ICP）算法发展最为迅速。近年来，各种ICP变种算法也被提出。

ICP算法常被用来解决多视点云之间的空间对齐问题，是一种基于最小二乘的点云最优配准方式。该算法的核心思想是对于输入的两帧点云数据，通过计算空间变换矩阵，使得它们可以统一到同一坐标系下，完成点云间的数据拼接。一般的计算步骤包括数据点对的关联、计算空间变换矩阵、迭代此过程直至满足收敛条件。这里使用的是ICP的一种改进算法即Point-to-Plane ICP，相比标准ICP具有更好的配准精度与鲁棒性，其算法流程见算法1-4。ICP算法的迭代初值对算法的收敛情况影响很大，实验中由轮式里程计或惯性测量单元（IMU）给定。利用ICP算法可以很容易地求解出两帧数据间的变换矩阵（也即两个位姿间的位姿约束），却很难描述位姿间的不确定性，而在后端优化中需要明确给定该不确定性，实验中采用配准距离S_fit（计算方式参见算法1-4）来描述，该配准距离越小表示不确定性越小，否则不确定性越大。

虽然上述Scan-to-Scan匹配可以用作构建激光里程计，实现同步定位与建图，但是会带来较大的累积误差，尤其是在特征较为匮乏的场景中很容易导致匹配陷入局部极小，最终破坏了地图的一致性。于是提出Scan-to-SubMap匹配来进一步减小该误差，从而达到构建实时、低漂移激光里程计的目的。

（2）Scan-to-SubMap匹配

Scan-to-SubMap匹配过程如图1-12所示，M_i表示i时刻已构建的全局地图，表示i时刻全局坐标系下的机器人位姿，表示i+1时刻到i时刻机器人位姿的变换矩阵（即Scan-to-Scan匹配的输出结果），z_i+1表示i+1时刻的激光观测数据。由图1-12可以看出，Scan-to-Scan匹配存在的误差可能导致地图的不一致性（橙色与黑色存在偏差）。

归纳Scan-to-SubMap匹配过程如下：

1）首先，对全局点云地图用八叉树结构描述。将z_i+1转换到全局坐标系下，并对每个激光点找到地图中一定半径范围内的近邻点，将所有近邻点转移到局部坐标系下并保存作为m_i+1。

2）其次，为加速搜索，使用PCL库中近似最近邻（Approximate Nearest Neighbor，ANN）算法。

3）最后，将z_i+1和m_i+1作为算法1-4的输入，可得到完成两帧点云数据配准的变换矩阵T_opt。优化后的机器人在i+1时刻的位姿为：。

考虑SLAM算法的实时性问题，上述Scan-to-SubMap匹配过程不可能对每帧数据都进行优化，这会加重算法运行的负担。由于三维激光雷达的数据发布频率为10Hz，所以一般也对Scan-to-Scan匹配采用10Hz处理频率，而Scan-to-SubMap匹配采用1Hz的处理频率，即位姿发布为10Hz的频率，全局地图发布为1Hz的频率，如此可保证算法的实时性要求。

经过上述Scan-to-Scan匹配与Scan-to-SubMap匹配结合的方式，可构建实时低漂移的激光里程计。利用该激光里程计算法进行建图的示例如图1-13所示。

图1-13a与图1-13b是在小范围室内及长直走廊中的测试结果，初步验证了算法的有效性；图1-13c所示是在室内办公区域（场景规模约80m×50m）内，遥控机器人沿走廊行走一圈进行激光里程计测试。此时并没有利用回环检测进行优化处理，由建图结果可以看出地图的一致性较高；图1-13d是在室外道路环境下遥控机器人沿道路行走100m的测试结果，道路旁的车辆、建筑物、树木等清晰可见，并没有地图叠加错乱等现象。

由此可见，所提出的Scan-to-Scan匹配与Scan-to-SubMap匹配结合的激光里程计算法的有效性得到了验证，在多个室内外小场景范围内具有较好的建图效果，但是在大范围室外场景下还需要考虑回环优化以避免激光里程计的累积误差，具体将在后文详细介绍。

（3）Scan-to-KeyScan匹配

通过之前构建的激光里程计算法，已经可以实现在小范围室内外环境下保证SLAM建图的一致性。但是在复杂环境下，激光里程计不可避免的累积误差会破坏地图的一致性，主要表现在当机器人重新回到已访问过的区域时。如果机器人知道该场景曾经访问过，那么便可以修正当前估计的位姿，否则不正确的位姿所关联的观测数据必将破坏已建立的地图。该问题即是SLAM中尤为关键的回环检测问题，也是在复杂环境下建立全局一致性地图的关键。在视觉领域，图像检索技术常被用于回环检测，其中基于词袋模型的方法已被广泛应用，并取得了很好的效果。而在激光雷达领域，由于信息丰富度较视觉匮乏，所以回环检测一直是激光雷达的短板。此外，由于三维激光雷达输出的点云数据量太大，基于点云配准的方式很难保证实时运行，近年来大多数工作也都是融合视觉信息来实现回环检测。因此，针对三维激光雷达的回环检测问题，提出Scan-to-KeyScan匹配算法来解决机器人重新回到已访问区域时的数据关联。

算法流程见算法1-5。算法输入为关键帧InScan以及处理频率f。其中，关键帧的定义为当机器人行走一定距离后（实验中设置该距离为0.5m）才认为是一帧关键帧；为考虑算法的时效性，对于输入的关键帧满足处理频率f（实验中设置f=6）才进行后续的Scan-to-KeyScan匹配，否则直接返回（算法1-5中的3—5行）。在进行匹配时，会与所有历史关键帧进行遍历匹配。为避免相邻帧的相似性太高从而导致误检测，要忽略近邻的几帧观测数据（7行）。这里，首先会进行基于随机森林的场景识别检测（详细阐述见后文），寻找与当前机器人所处环境最为相似的历史关键帧（8 行），然后应用算法1-4 介绍的Point-to-Plane ICP求解变换矩阵以及配准距离（10行），只有配准距离小于一定阈值才被加入到回环的候选集合中（12行）。

对于当前输入的点云数据，检测到的回环候选集合可能包含0个、1个或多个回环信息。若为0个，则表示没有检测到回环信息（17—19行）；若不为0个，则挑选具有最小配准距离S_fit的一组回环信息，因为当前的回环帧对具有最好的配准效果。但此时还不能直接将其输出作为最终检测到的回环信息，因为环境中可能存在高度相似的场景，这可能是误检测，因此在输出前会进行几何验证（21行）。

回环检测中的几何验证过程如图1-14所示。其中，圆圈表示关键帧，圆圈间的箭头表示运动过程。对于机器人观测到第i′-f、i′、i′+f个关键帧时，对应的第i-f、i、i+f个关键帧是检测到的候选回环，只有满足如下条件才认为第i′+f个与第i+f个关键帧是检测到的回环帧对

式中，‖ ‖是二范数；T是两个关键帧间的位姿变换矩阵；trans函数是取变换矩阵的平移部分；rot函数是取变换矩阵的旋转部分；t与r分别是平移与旋转阈值，实验中一般分别设置为0.1m和0.035rad。该方法可有效避免因误检测情况的发生而导致地图的一致性破坏。

最终，整个多层ICP匹配算法结束。经过Scan-to-Scan匹配、Scan-to-SubMap匹配以及Scan-to-KeyScan匹配构建的位姿及位姿约束与不确定性估计，统一保存在位姿图结构中，并通过增量优化算法进行全局位姿优化。至此，整个SLAM问题得到求解。

1.1.3.3 基于随机森林学习的场景识别

由上述可知，使用Scan-to-KeyScan匹配方法来解决SLAM中的回环检测问题，其中的关键步骤便是基于随机森林学习的场景识别。下面详细阐述该算法流程。

图1-15所示为MIM_SLAM所使用的基于随机森林的场景识别算法（以下称为RF-PR算法）框架。具体地，在离线部分采集用于训练的数据集，并且基于随机森林的学习方法训练模型参数，这里的输入为两帧激光观测数据的特征向量表示，输出为两帧数据是否来自同一场景。在线部分对于输入的激光观测先用特征向量表示，并分别与数据库中所有的特征向量表示构建成对（Make Pair），输入到随机森林完成分类，最终输出为1表示两帧观测数据来自同一场景。若用此方法解决SLAM中的回环检测问题，则这里的“特征向量表示数据库”为SLAM中所有历史关键帧的特征向量表示。

（1）特征向量表示

由于三维激光雷达一帧数据量太大，不可能直接输入到随机森林分类器中进行训练，需要用一组特征向量来表示，故提出了图1-16所示的三类特征描述方式。三类特征均具有旋转不变性，分别介绍如下。

1）投影特征：f₁～f₆₀。该特征是将点云数据分别向XY、XZ、YZ平面投影。以投影到XY平面为例（图1-16a），输入点云为S，对于其中每个激光点p_i=（x_i，y_i，z_i）计算到原点（即激光中心）的距离为，然后将平面中到原点距离在[d_min，d_max]范围内的区域划分为20个等间隔的圆环{I_j|j=1，…，20}，最后统计落在每个圆环中的激光点数目，并归一化处理，即得到

同理，投影到XZ、YZ平面上也可以分别得到一个包含20个值的特征向量。该类特征描述了一帧观测数据的所有激光点的分布情况，是一种全局描述子。

2）法向量曲率特征：f₆₁～f₁₀₀。表示方式如图1-16b所示，输入点云为S，对于其中每个激光点p_i=（x_i，y_i，z_i）计算法向量

式中，v₁是当前激光点指向上一条扫描线对应水平角度的激光点；v₂是当前激光点指向同一扫描线其后第三个激光点；v₃是当前激光点指向下一条扫描线对应水平角度的激光点；v₄是当前激光点指向同一条扫描线其前第三个激光点。

对法向量进行单位化处理，并取投影在Z轴的值：。将该区间划分为20个子区间{I_j|j=1，…，20}，对所有激光点计算值，并统计落在每个子区间的点云数目，进行归一化处理

对法向量曲率特征表示中每个激光点p_i=（x_i，y_i，z_i）计算曲率

式中，S是激光点p_i同一条扫描线最近邻的8个激光点组成的点集。

定义c_i取值范围为[c_min，c_max]，将该区间划分为20个子区间{I_j|j=1，…，20}。对所有激光点计算c_i值，并统计落在每个子区间的点云数目，进行归一化处理

该类特征可以很好地描述机器人周围环境中分布物体表面的朝向、起伏等特性，是一种局部描述子。

3）基于扫描线特征：f₁₀₁～f₁₄₈。表示方式如图1-16c所示。对于每条扫描线计算如下三个值，表示该扫描线上所有连续激光点距离之和，表示该扫描线覆盖面积，表示该扫描线上所有激光点的距离标准差。

式中，N_i是第i条扫描线的激光点数目；x、y、z分别是某个激光点在X轴、Y轴、Z轴方向的坐标；r是某个激光点的距离（或深度）值；α是该激光点投影在XY平面与Y轴方向的夹角；L是扫描线的条数。

该类特征是基于扫描线的特征描述方式得到的，它可以很好地描述每条扫描线的激光点分布特性；同时对于环境中存在动态障碍时的描述性更好，因为动态物体通常只改变几条扫描线的状态，而大多数扫描线的状态是不受影响的。具体实现时，若采用具有16条扫描线的Velodyne VLP-16激光雷达，则该类特征包含48个值即f₁₀₁～f₁₄₈；若采用具有64条扫描线的Velodyne HDL-64E，则选择每隔4条扫描线取1条处理，因此最终也包含48个值即f₁₀₁～f₁₄₈。最终，均可得到包含148个值的特征向量来表示一帧三维激光观测数据。

（2）随机森林学习

对于两帧观测数据z_k和z_k+1，利用特征向量表示方法可表示为，，N=148。定义如下函数

则n组训练数据可表示如下

式中，y_i∈{0，1}是二进制变量，y_i=0表示两帧激光观测不来自同一场景，否则表示来自同一场景。

在获取训练数据集之后，便可以使用随机森林学习方法进行训练，其算法流程见算法1-6。随机森林算法利用集成学习的思想，通过将多个学习器进行结合，可获得比单一学习器更显著的泛化性能；此外，自助采样法以及在决策树的训练过程中引入随机属性选择相结合，使得随机森林算法中基学习器的多样性不仅来自于样本扰动还有属性扰动，对于分类的抗干扰性大大加强，最终集成的泛化性能可通过个体学习器之间差异度的增加而进一步提升。

（3）场景识别性能

在训练随机森林分类器之前需要获取训练数据集，传统的手工标注方式是困难且烦琐的，因此考虑一种更为简单的方式获取用于训练的数据集。选取室内地下停车场场景来构建训练数据集，由于缺乏地面真值数据，故利用前述的低漂移的激光里程计算法来产生位姿。为保证位姿的精度，控制机器人以较慢的速度（约为0.5m/s）行驶，同时考虑激光里程计会随着机器人行驶距离的增加会有较大的累积误差，因此控制每次行驶距离也较短。考虑激光雷达频率较高，任意相邻的两帧观测可被认为来自同一场景，同时通过刻意控制机器人重复访问之前经历的场景以产生更多的正样本，对于判断是否两帧观测数据来自同一场景可用如下方式

式中，P₁和P₂分别是两帧观测的位置坐标；d_thre是距离阈值，选择3m。

由此，可产生大量的正负样本来训练随机森林分类器。在这个训练数据集中，最终生成了21700的正样本以及21700的负样本。为了保证测试的合理性，在该场景下，又用同样方式生成了包含12000正样本和12000负样本的测试数据集。

利用随机森林学习方法对构建好的训练数据集进行训练，在得到模型参数后，在测试数据集上验证所提出的RF-PR场景识别算法的有效性，并与基于激光帧描述的方法进行对比：

1）Small-Size Signature（S-SS）：该算法对三维激光雷达产生的点云数据提取法向量特征构建直方图，并采用EMD距离（Earth Mover’s Distance）度量两个直方图之间的相似性。

2）Fast Histogram（FH）：该算法对三维激光雷达产生的点云数据提取高度特征构建直方图，并采用卡方距离度量两个直方图之间的相似性。

为定量评估场景识别精度，引入监督学习方法中两个重要的评价指标，分别为分类精度和受试者工作特征（Receiver Operating Characteristic，ROC）曲线。此外，该数据集使用的激光雷达型号为Velodyne VLP-16，因此所提出算法的特征向量表示的长度为148，对于另外两种对比算法直方图的bin的个数也设置为148。

1）分类精度：为了评价在测试数据集上的预测结果，引入如下错误率指标

式中，I（·）是一个函数，当为真时返回1，否则返回0；y_i是真值结果；是预测结果；m是样本总数。

给定不同的d_thre，三种算法的分类精度见表1-1，显然RF-PR算法具有更低的分类错误率。

2）ROC曲线：ROC曲线横坐标为假正例率（False Positive Rate，FPR），即实际为负样本中预测为正样本的比例；纵坐标为真正例率（True Positive Rate，TPR），即实际为正样本中预测为正样本的比例。曲线下方围成的面积被称为AUC（Area Under ROC Curve），该面积越大表示分类性能越好。

三种算法的ROC曲线如图1-17所示。图1-17a中的黄色曲线相比蓝色（0.87）和红色（0.80）有更大的AUC值（0.95），绿色曲线表示随机猜想，显然所提算法（RF-PR）的表现更好。对于两帧观测数据，当关联的位姿距离值小于阈值d_thre时认为是正样本（即来自同一场景），不同的d_thre值可能影响算法性能，因此选择几个离散的不同距离值：d_thre=2m，d_thre=3m，d_thre=5m，d_thre=10m，并在S-SS和所提算法开展对比测试（图1-17b）。显然，当该距离阈值越大时识别效果越差，因为此时很难描述两帧观测数据之间的相似性或差异性；然而该取值也不能取太小，以防止实际为同一场景却无法识别的情况。由此，实验中一般设置为d_thre=3m。

通过上述对比实验，在采集的大范围地下停车场场景下的数据集上，由定量结果可以验证所提出的场景识别算法相比另外两个算法具有更好的识别性能。

1.1.3.4 算法性能测试

为验证MIM_SLAM算法整体性能，首先在室外道路场景中进行测试。在实验中，遥控移动机器人以1m/s的速度行驶了约950m，共采集10472帧数据。机器人的行驶路径如图1-18a所示（场景规模约180m×150m），红色实线显示的路径序列为ABCDEFGCAHFGBH，其间经历了多次重复路径。

MIM_SLAM算法建立的场景点云地图如图1-18b所示。由图可以看出地图创建精度较高，地图一致性较好。主要原因是该场景结构化信息较为丰富，MIM_SLAM算法构建的低漂移的激光里程计均保证了较小的累积误差，所以地图的一致性较高，几乎不存在点云的错乱累积现象。

接下来，在公开数据集KITTI上对提出的MIM_SLAM算法进行了验证。实验中，特别挑选了其中3种典型类型的场景，并且与标准ICP（Standard ICP）、LOAM算法进行定量对比分析。

1）乡村道路（Sequence 03）：道路较窄，动态的行人、车辆等较少，周围存在大量的田园树木、自然地形等。该数据集被用来验证提出的MIM_SLAM方法可以在室外特征稀疏的乡村田园场景下实现低漂移的位姿估计。建图结果如图1-19a所示，无人车平台的运行路径及真值数据如图1-20a所示。由于在该场景下稳定特征点较少，尤其是在运行约420m之后，实际的运行轨迹与真值存在些许偏差，但整体建图效果并没有存在错乱等现象，仍然保持了较高的一致性。为了定量评估算法的性能，这里引入了KITTI中评测测距数据集的方法，它对所有的可能的子序列长度（100m，200m，…，800m）计算平均平移及旋转误差。图1-21a所示为该场景下的位姿估计误差，相比标准ICP算法，LOAM算法与MIM_SLAM算法均具有较高的激光里程计精度，可保证平移误差在1%以内，旋转误差在0.005°/m以内。

2）城市道路（Sequence 07）：道路上动态行人、车辆等较多，周围具有较多的墙面、建筑物等显著特征。该数据集被用来验证提出的算法可有效处理回环区域的数据关联问题，并通过增量优化算法建立全局一致性高的地图。如图1-19b所示，MIM_SLAM算法建立的全局点云地图中建筑物、柱子等场景信息清晰可见，几乎不存在错误累积，点云地图一致性很高。图1-20b所示的运行轨迹与地面真值轨迹也几乎是重合的。如图1-21b所示，MIM_SLAM算法相比LOAM算法平移与旋转误差更低。

3）高速道路（Sequence 06）：车速较快，道路上动态行人、车辆等较多。该数据集是城市道路场景，场景中具有较高的动态、噪声数据，且规模较大。KITTI无人车以6.2m/s的速度行走约600m停止。MIM_SLAM算法建立的点云地图结果如图1-19c所示，KITTI无人车平台的运行轨迹及地面真值数据如图1-20c所示。直观上看，点云地图的轮廓是清晰可见的，地图一致性较高，估计的位姿和地面真值数据几乎是重合的，具有较高的建图精度。位姿估计的误差如图1-21c所示。由此可见，MIM_SLAM算法在动态因素较高的城市道路场景上也具有较好的表现，几乎不输LOAM算法。

由此可见，在公开数据集KITTI上再一次验证了MIM_SLAM算法的有效性与较高的建图精度，相比该数据集上的榜首算法LOAM也具有相似或更好的表现性能，可满足在大范围室外场景下的实时高精度建图的应用。