CN114563000A - 一种基于改进型激光雷达里程计的室内外slam方法 - Google Patents

Abstract

本发明公开了一种基于改进型激光雷达里程计的室内外SLAM方法。首先，对4PCS点云匹配算法进行优化并设计定位节点，使用改进后的4PCS点云匹配算法进行点云粗匹配；接着，利用粗匹配提供的转换矩阵初值进行迭代最近算法ICP点云精细配准，完成定位节点的实现；最后，设计辅助的校正节点，对点云匹配帧进行下采样，对采样的关键帧利用NDT算法进行周期性校正，将采样的关键帧与局部子图进行匹配。本发明所用方法根据以往传统的SLAM单节点定位节点所改造的两节点结构，在不具备GPS和IMU的环境下也可以很好地实现定位和同步地图构建；本发明原理简单，实时性和精确度均优于传统的LOAM算法。

Description

一种基于改进型激光雷达里程计的室内外SLAM方法

技术领域

本发明属于智能无人平台同时定位与构建环境高精度地图技术领域，特别是涉及一种基于改进型激光雷达里程计的室内外SLAM方法。

背景技术

利用传感器感知信息作为建图的基础已经成为行业内的重点关注，其中SLAM技术作为一种即时定位与地图构建方法，成为移动无人平台实现自主定位与导航的核心，它可以被看做是一个鸡生蛋蛋生鸡的问题：完美的定位需要用到一个无偏差的地图；但这样的地图又需要精确的位置估测来描绘。SLAM技术要求无人操作平台在一个未知的环境中在不知道自己位置的先验信息的情况下，增量式地构建具有全局一致性的地图，同时确定自身在这个地图中的位置。这一过程是通过车身传感器来获取周围环境的信息，依靠这些信息一边确定自车的位置，一边构建环境的地图。如下图1-2展示了SLAM系统的基本技术框架，一般分为5个模块，包括传感器数据、前端里程计、后端优化、建图及回环检测。传感器数据模块主要用于采集实际环境中的各类型原始数据，包括激光扫描数据、点云数据等，并进行数据预处理。里程计模块主要用于不同时刻间移动目标相对位置的估算，包括特征匹配、直接配准等算法的应用。后端优化模块主要用于优化里程计带来的累计误差，包括滤波器、图优化等算法应用。回环检测主要用于空间累积误差消除。

其工作流程大致为：传感器读取数据后，里程计估计两个时刻的相对运动(Ego-motion)，后端处理里程计估计结果的累积误差，建图则根据前端与后端得到的运动轨迹来建立地图，回环检测考虑了同一场景不同时刻的图像，提供了空间上约束来消除累积误差。

目前SLAM算法常用的传感器有激光雷达和相机，分别对应了激光SLAM算法和视觉SLAM算法。其中基于单目、鱼眼相机的视觉SLAM方案，利用多帧图像来估计自身的位姿变化，再通过累计位姿变化来计算距离物体的距离，并进行定位与地图构建。但是其受环境光影响大，在暗处或无纹理区域无法工作。而且本身算法运算负荷大，测量噪声大，构建的地图也难以直接用来无人平台的路径导航。相比之下，激光SLAM利用激光雷达扫描环境信息，采集到的物体信息呈现出一系列分散的、具有准确角度和距离信息的点，被称为点云。通常，激光SLAM系统通过对不同时刻两片点云的匹配与比对，计算激光雷达相对运动的距离和姿态的改变，也就完成了对智能化无人平台自身的定位。激光雷达测量精度高且范围广，误差模型简单以及运行稳定，点云的处理也比较容易。同时，激光SLAM理论研究也相对成熟，可靠性更高。目前激光雷达也有向低成本发展的趋势。因此，本发明采用基于激光雷达来对智能化无人平台在缺少IMU模块和GPS的环境下进行同步定位与建图。

定位和测绘任务的重点是应在各种条件下实时获得最准确位置，同时还接收有关环境的信息(例如估算武装部队中的任务，或火灾时的操作信息)，故而系统对实时性和精确性有较高的要求。传统激光雷达里程计通常采用单个定位节点结构，并且4PCS点云匹配算法具有大平面点易错误匹配的局限性。故而本发明激光雷达里程计采用前端定位节点加上后端校正节点的两节点结构，并且优化了点云匹配算法，大大提高了定位的实时性和精确性，最终制定了一种室内及室外定位扩展场景和同步定位构图(SLAM)技术的用例。

发明内容

本发明的目的在于，提供一种基于改进型激光雷达里程计的室内外SLAM方法，实现了SLAM评价模型中实时性和精确性上的双重提高。

为了实现本发明目的，本发明提供了一种基于改进型激光雷达里程计的室内外SLAM方法，包括以下步骤：

步骤1、对4PCS点云匹配算法进行优化并设计定位节点，使用改进后的4PCS点云匹配算法进行点云粗匹配；

步骤2、利用粗匹配提供的转换矩阵初值进行迭代最近算法ICP点云精细配准，完成定位节点的实现；

步骤3、设计辅助的校正节点，对点云匹配帧进行下采样，对采样的关键帧利用NDT算法进行周期性校正，将采样的关键帧与局部子图进行匹配。

进一步地，步骤1包括如下步骤：

步骤1-1、设置本地栅格地图；

步骤1-2、通过基于高程差的地面滤波器滤除大平面点，通过减少潜在匹配对的数量提高实时性，消除大平面点对4PCS点云匹配算法造成大量错误匹配影响；

步骤1-3、利用栅格地图的二维二值图像投影对小型动态对象进行聚类和过滤，以加快聚类过程，提高配准的效率和精度。

进一步地，步骤1-1具体为：

设置本地栅格地图的原点位于激光雷达的几何中心，每个体素的大小设置为50厘米*50厘米，地图的实际大小与激光雷达的扫描范围一致；同时，通过使用体素网格压缩点执行下采样，减少点云的数量且保持点云的形状特征，提高算法处理速度。

进一步地，步骤1-2具体为：

将每个被映射到单个对应的本地栅格地图网格单元中的所有激光点构成点集P_mn，对于P_mn，找出不同的最大高程h_mn，如果小于预先确定的阈值，则单元将被视为接地单元；通过上述步骤遍历所有单元并移除检测到的地面单元后，获得不含大平面点的点云。

进一步地，步骤1-3将动态对象近似为小目标，并将本地栅格地图网格映射到二维图像上，二维图像由二值图像表示；随后使用图像侵蚀算子，并采用区域标记算法对点云进行聚类，具体为：

步骤1-3-1、分别从行和列遍历图像，如果当前像素值为0，检查左侧和顶部的两个像素值是否为0；

步骤1-3-2、如果如果左侧和顶部的两个像素值不为0，则将左侧和顶部的两个像素值合并，并使用标签将其标记为新群集；通过上述步骤，使得具有相同标签的所有像素被视为在同一簇中，并计算簇的矩形边界框，过滤面积小于预定阈值的集群。

进一步地，步骤2在对点云进行粗匹配后，利用迭代最近算法ICP实现两点云之间的精细匹配，采用具有三维Kdtree结构的ICP算法将时间复杂度降低到O(N_PlogN_Q)，其中N_P、N_Q分别表示点云P,Q中点的数量，迭代最近算法ICP的具体步骤为：

步骤2-1、搜索最近点对，对于源点云P中的点p_i，将其按照初始转换参数变换，即R^k-1p_i+t^k-1，这里k代表迭代次数，R代表3×3的旋转矩阵，t表示3×1的平移矩阵；在目标点云Q中寻找一个点q_i使这两个点之间的欧氏距离最近，构成最近邻点对(p_i,q_i)；

步骤2-2、搜索完成的最近点对进行配准变换，构建误差函数作为度量标准：

其中，N为最近邻点对的数量，为使误差函数最小，使用Hessian矩阵法和高斯-牛顿优化转换为对应似然函数的最值问题，计算出最优匹配转换矩阵的旋转参数R^k，平移参数t^k；

步骤2-3、利用步骤2-2中求出的转换参数对p_i再进行旋转和平移转换到新点集p_i′，计算p_i′到q_i的平均欧氏距离d，

若d小于指定阈值则迭代终止，得出最终的最佳匹配参数，否则重新进行步骤2。

进一步地，步骤3在校正节点中，当选择关键帧时，校正节点使用NDT将关键帧与动态局部子图匹配，从而通过直接替换结果变换来校正定位节点的输出，具体为：

将参考点云占用的空间划分为体素网格，然后根据扫描数据在每个体素中的分布计算描述体素的概率密度函数PDF，形成动态体素栅格点云地图，所述动态体素栅格点云地图根据新出现的点进行动态更新；所述动态体素栅格点云地图使用HashMap体素存储结构动态插入和删除点；每个体素的属性包括索引、质心平均值、协方差矩阵、点和点数；将超过预设数量点的体素运用于校正算法；

为了防止碰撞，点q所在的体素指数I(q)计算为：

I(q)＝t.x+t.y＜＜h_bits+t.z＜＜2h_bits

其中，<<表示左移位运算符，T_mg表示从当前坐标系到地图坐标系的转换；r_es表示网格分辨率，t表示3×1的平移矩阵，t.x表示x方向上的平移向量，t.y表示y方向上的平移向量，t.z表示z方向上的平移向量，h_bits表示h指数位；

索引I(q)用作HashMap的键值，所有点都存储在它们相应的HashMap中；因此，在每次迭代中，只需重新计算具有新插入点的体素的平均值和协方差矩阵，当n个点q_i(i＝1,2，…，n)插入到已经有m个点的体素中时，体素的平均值μ将更新为：

协方差矩阵重新计算为：

其中，m表示更新前体素栅格内的点数，n表示新插入该体素内的点数，q_i表示激光点云点，使用固定大小的体素栅格动态矫正定位的误差，更新并输出定位结果。

与现有技术相比，本发明的显著进步在于：1)本发明所用方法是根据以往传统的SLAM单节点定位节点所改造的两节点结构，在不具备GPS和IMU的环境下也可以很好地实现定位和同步地图构建；2)本发明原理简单，实时性和精确度均优于传统的LOAM算法。

为更清楚说明本发明的功能特性以及结构参数，下面结合附图及具体实施方式进一步说明。

附图说明

此处所说明的附表及附图用来提供对本发明的进一步理解，构成本申请的一部分，本发明的示意性实施例及其说明用于解释本发明，并不构成对本发明的不当限定。在附图中：

图1为传统4PCS点云匹配算法原理图；

图2为4PCS错误匹配情况图；

图3a为地面滤波前的点云示意图；

图3b为地面滤波后的点云示意图；

图3c为栅格单元二值图像化示意图；

图3d为二值图像侵蚀处理结果示意图；

图3e为区域标记聚类示意图；

图3f为滤除地面大平面点和动态小目标后的结果示意图；

图4a为点云帧间改进的4PCS粗匹配结果示意图；

图4b为点云帧间ICP精细匹配结果示意图；

图5为定位算法流程图；

图6为校正算法流程图；

图7a为改进型激光雷达里程计与LOAM算法的耗时对比图；

图7b为改进型激光雷达里程计与LOAM算法的精确度对比图；

图8为在KITTI集上验证系统定位的定性结果图；

图9为本发明总体流程示意图。

具体实施方式

下面将结合本发明实施例中的附表及附图，对本发明实施例中的技术方案进行清楚、完整地描述，显然，所描述的实施例仅是本发明一部分实施例，而不是全部的实施例；基于本发明中的实施例，本领域普通技术人员在没有做出创造性劳动前提下所获得的所有其他实施例，都属于本发明保护的范围。

标准的4PCS算法通过使用四个点的共面集合来解决全局3D配准问题。如图1所示，图1显示了该算法的原理，根据几何原理，在仿射变换和刚体运动下，线段的交叉比保持不变。假设从点云P中选择的集合B中四个点{a，b，c，d}是共面的，线段ab和cd在点e处相交，两条线段的交叉比为：r1＝||a-e||/d1，r2＝||c-e||/d2，其中d1，d2是两条线段的长度：d1＝||a-b||，d2＝||c-d||,根据交叉比r1、r2的不变性，所有对应的4点集U＝{U1，U2，…，Uv}可以在点云Q中找到。对于每个,i＝1,2,...,v,计算B和Ui之间的最佳刚性变换矩阵。选择P和Q之间重叠率最大的解作为真实对应的4点集。

但是，在使用原4PCS算法进行点云匹配时，存在以下缺点：一方面，4PCS可能会在大平面点云中发现错误的对应点对，从而导致匹配失败。如图2所示，平面s1上的四个点p₁、p₂、p₃、p₄是共面的，并且可以在平面S2上找到许多对应的共面点集，例如{p₁、p₂、p₃、p₄}、{q₁、q₂、q₃、q₄}，…。事实上，错误的点集大量存在，因为这个平面上的所有点都是共面的。找到正确的集合并排除这些集合中的其他错误集合非常耗时。一种可行的解决方案是在匹配之前过滤掉点云中的这些大平面，以避免不必要的计算，提高算法的效率。另一方面，环境中的动态对象也会影响注册。如果共面4点集中的一个点位于动态对象上，则很难在点云Q中找到与B真正相等的4点集，并且验证这些错误的点集将浪费大量时间。因此，为了提高配准的效率和精度，需要在配准前去除点云中的动态对象。

本发明对其进行了改进，以提高该配准方法的性能。针对第一方面的缺点，提出了一种基于高程差的地面滤波方法来滤除大平面点，以避免大量错误匹配，同时通过减少潜在匹配对的数量来提高实时性。对于另一方面的缺点，使用栅格地图的二维二值图像投影对小型动态对象进行聚类和过滤，以加快聚类过程。

实施例

图3a-图3f为过滤掉地面和动态对象的过程，具体地，在本实施例中：

正如在现实生活中的实验所看到的，来自平坦地面地形的点通常形成点云中最大的平面，这可能导致错误匹配。因此，为了提高匹配效率，首先需要对大平面点进行滤波。考虑到实时性能，高差概念用于粗略过滤大平面点，而无需准确提取地面和障碍物。本地栅格地图的原点位于激光雷达的几何中心。每个体素的大小设置为50厘米*50厘米，地图的实际大小与我们激光雷达的扫描范围一致。每个激光点将被映射到单个对应的网格单元中的所有点构成点集P_mn。对于P_mn，找出不同的最大高程h_mn。如果小于预先确定的阈值，则单元将被视为接地单元。通过上述步骤遍历所有单元并移除检测到的地面单元后，可以获得不含大平面点的点云。图3a显示了地面滤波前的点云，图3b显示了地面滤波后的点云。可以发现，地面滤波有效滤除了地面平面点。

对于动态对象的去除，本申请考虑并过滤掉小的动态对象而不是动态对象，即根据观察到现实世界中的动态对象通常都会变大的情况，将动态对象替换为小对象。此外，为了大大加快聚类过程，将网格映射到二维图像上。2D图像由二值图像表示，如图3c所示，其中黑点表示地面上的物体。

因为现实世界中的对象可能会被遮挡，如果直接聚集在原始图像上，它们可能会变得不连续。鉴于在聚类之前使用图像侵蚀算子，如图3d所示。然后，采用区域标记算法对点云进行聚类，步骤如下：(1)分别从行和列遍历图像；(2)如果当前像素值为0(黑色)，检查左侧和顶部的两个像素值是否为0；(3)如果没有，则将它们合并并使用标签将其标记为新群集。通过上述标记步骤，具有相同标签的所有像素被视为在同一簇中，并且可以计算其矩形边界框，如图3e所示，面积小于预定阈值的集群将被过滤掉。图3f展示了最终滤除地面大平面点和动态小目标的效果。

粗匹配后，利用ICP实现两点云之间的精细匹配。标准ICP匹配算法由Besl和McKay首先提出，其复杂性为O(N_P*N_Q)，其中N_P和N_Q分别表示这两个点云中包含的点数。在本申请中，采用具有三维Kdtree结构的ICP算法将时间复杂度降低到O(N_PlogN_Q)。但是，当点云中存在大量点时，该算法仍然会非常耗时。因此，在匹配之前通过使用体素网格压缩点来执行下采样，同时保持点云的形状特征。点数可以大大减少。通过粗匹配得到良好的初始变换矩阵，大大加快了ICP的匹配速度。本发明的主要贡献之一是通过结合几种主流的配准技术，使用从粗到精的匹配方法来形成我们的SLAM系统的前端节点，以实现精度和实时性能之间的良好折衷。图4显示了定位算法的最终匹配结果，图4a显示了优化后4PCS粗匹配方法的匹配结果，图4b显示了ICP精细匹配方法的匹配结果，图5显示整个定位算法流程图。

在校正节点中，关键帧以下采样方式确定。当选择关键帧时，校正节点使用NDT将关键帧与动态局部子图匹配，从而通过直接替换结果变换来校正定位节点的输出。Magnusson等人2009年通过大量实验证明，NDT在稳健性和准确性方面都优于ICP。另外，NDT算法具有良好的实时性和稳定性，对不同环境具有良好的适应性。因此，校正节点采用NDT，将当前关键帧与局部子图进行匹配。

NDT算法将参考点云占用的空间划分为体素网格。然后根据扫描数据在每个体素中的分布计算描述体素的概率密度函数(PDF)。本发明介绍了一种动态体素栅格地图，该地图可以根据新出现的点进行动态更新。该贴图支持使用HashMap体素存储结构动态插入和删除点。每个体素包含若干属性，包括索引、质心(平均值)、协方差矩阵、点和点数。只有包含超过一定数量点的体素才被视为活动的，并在我们的校正算法中使用。为了防止碰撞，点q所在的体素指数I(q)计算为

I(q)＝t.x+t.y＜＜h_bits+t.z＜＜2h_bits (1)

其中<<表示左移位运算符，

T_mg表示从当前坐标系到地图坐标系的转换；r_es表示网格分辨率。索引I(q)用作HashMap的键值，所有点都存储在它们相应的HashMap中。因此，在每次迭代中，只需重新计算具有新插入点的体素的平均值和协方差矩阵。当n个点q_i(i＝1,2，…，n)插入到已经有m个点的体素中时，体素的平均值μ将更新为：

协方差矩阵重新计算为：

其中，m表示更新前体素栅格内的点数，n表示新插入该体素内的点数，q表示激光点云点。以上使用固定大小的体素栅格可以动态矫正定位的误差，更新并输出定位结果

校正算法流程如图6所示，其中，字母j用于表示关键帧的第j次更新，是第j次更新后的局部子图。该算法的输入为点云的当前帧、当前局部子图，上一个关键帧的的位姿变换和定位节点的当前输出。该算法以1hz的频率收集关键帧，并通过对当前关键帧Pj进行下采样获得。然后使用NDT算法从定位节点获得具有初始变换的局部定位子图，将当前关键帧与局部子图匹配。之后，该算法用更新局部子图，并保持固定大小的局部动态体素栅格图。最后，该算法输出M_j、

和

用来作为下一次迭代。

实验验证将改进后的激光雷达里程计与常用的LOAM算法里程计进行耗时与精度比对，分别如图7a和7b所示。可以看到本发明设计的激光里程计在实时性和耗时稳定性上均优于LOAM里程计，具有良好的系统鲁棒性；同时，可以看到本发明设计的激光里程计所输出的定位轨迹更加贴合地面真值，具有更好的系统定位精度。

在KITTI数据集上对我们的系统结果进行定位的定性验证，如图8所示。结果表明本文算法仿真的定位轨迹与地面真值轨迹基本吻合，故在无需GPS/IMU辅助的情况下，本文里程计算法也基本满足无人智能平台在室内复杂环境下进行精准定位。

进一步对里程计定位性能进行定量分析。关于计算里程计定位的误差率，参考KITTI数据集网站上的方法，首先根据数据集的地面真值找到距离当前帧100m，200m，300m，…，600m的帧，编号为1～6，再将这些帧与当前帧的位姿变换矩阵T_true用作真实值，然后在算法测试集中找到同样编号的帧并计算变换矩阵T_test，求出其与真实位姿变换矩阵中的平移误差向量t_error＝[x_e,y_e,z_e]^T及旋转误差向量R_error＝[R_xe(α),R_ye(β),R_ze(γ)]^T，则定位平移误差率λ_error和旋转误差值R_e可计算为：

其中，||·||为向量范数，表示向量的长度，

L为距离，取对应100m～800m。R_e(姿态变化量)的单位为度每米(deg/m)。定位误差计算结果及配准单帧耗时如下表1所示，表1为在KITTI集上验证系统定位的定量结果。从中可以看出里程计整体的定位平移误差率均值约为1.21％，旋转误差均值约为0.0036deg/m。

表1里程计定位误差及耗时结果

如图9所示，图9为本发明总体流程图，具体包括以下步骤：

步骤1、对4PCS点云匹配算法进行优化并设计定位节点，使用改进后的4PCS点云匹配算法进行点云粗匹配；

步骤1-1、设置本地栅格地图；

步骤1-2、通过基于高程差的地面滤波器滤除大平面点，通过减少潜在匹配对的数量提高实时性，消除大平面点对4PCS点云匹配算法造成大量错误匹配影响；

步骤1-3、利用栅格地图的二维二值图像投影对小型动态对象进行聚类和过滤，以加快聚类过程，提高配准的效率和精度；

步骤2、利用粗匹配提供的转换矩阵初值进行迭代最近算法ICP点云精细配准，完成定位节点的实现；

步骤3、设计辅助的校正节点，对点云匹配帧进行下采样，对采样的关键帧利用NDT算法进行周期性校正，将采样的关键帧与局部子图进行匹配。

需要说明的是，在本申请中，诸如第一和第二等之类的关系术语仅仅用来将一个实体或者操作与另一个实体或操作区分开来，而不一定要求或者暗示这些实体或操作之间存在任何这种实际的关系或者顺序。而且，术语“包括”、“包含”或者其任何其他变体意在涵盖非排他性的包含，从而使得包括一系列要素的过程、方法、物品或者设备不仅包括那些要素，而且还包括没有明确列出的其他要素，或者是还包括为这种过程、方法、物品或者设备所固有的要素。

尽管已经示出和描述了本发明的实施例，对于本领域的普通技术人员而言，可以理解在不脱离本发明的原理和精神的情况下可以对这些实施例进行多种变化、修改、替换和变型，本发明的范围由所附权利要求及其等同物限定。

Claims (7)

1.一种基于改进型激光雷达里程计的室内外SLAM方法，其特征在于，包括以下步骤：

步骤1、对4PCS点云匹配算法进行优化并设计定位节点，使用改进后的4PCS点云匹配算法进行点云粗匹配；

步骤2、利用粗匹配提供的转换矩阵初值进行迭代最近算法ICP点云精细配准，完成定位节点的实现；

步骤3、设计辅助的校正节点，对点云匹配帧进行下采样，对采样的关键帧利用NDT算法进行周期性校正，将采样的关键帧与局部子图进行匹配。

2.根据权利要求1所述的一种基于改进型激光雷达里程计的室内外SLAM方法，其特征在于，步骤1包括如下步骤：

步骤1-1、设置本地栅格地图；

步骤1-2、通过基于高程差的地面滤波器滤除大平面点，通过减少潜在匹配对的数量提高实时性，消除大平面点对4PCS点云匹配算法造成大量错误匹配影响；

步骤1-3、利用栅格地图的二维二值图像投影对小型动态对象进行聚类和过滤，以加快聚类过程，提高配准的效率和精度。

3.根据权利要求2所述的一种基于改进型激光雷达里程计的室内外SLAM方法，其特征在于，步骤1-1具体为：

设置本地栅格地图的原点位于激光雷达的几何中心，每个体素的大小设置为50厘米*50厘米，地图的实际大小与激光雷达的扫描范围一致；同时，通过使用体素网格压缩点执行下采样，减少点云的数量且保持点云的形状特征。

4.根据权利要求2所述的一种基于改进型激光雷达里程计的室内外SLAM方法，其特征在于，步骤1-2具体为：

将每个被映射到单个对应的本地栅格地图网格单元中的所有激光点构成点集P_mn，对于P_mn，找出不同的最大高程h_mn，如果小于预先确定的阈值，则单元将被视为接地单元；遍历所有单元并移除检测到的地面单元后，获得不含大平面点的点云。

5.根据权利要求2所述的一种基于改进型激光雷达里程计的室内外SLAM方法，其特征在于，步骤1-3将动态对象近似为小目标，并将本地栅格地图网格映射到二维图像上，二维图像由二值图像表示；随后使用图像侵蚀算子，并采用区域标记算法对点云进行聚类，具体为：

步骤1-3-1、分别从行和列遍历图像，如果当前像素值为0，检查左侧和顶部的两个像素值是否为0；

步骤1-3-2、如果如果左侧和顶部的两个像素值不为0，则将左侧和顶部的两个像素值合并，并使用标签将其标记为新群集；通过上述步骤，使得具有相同标签的所有像素被视为在同一簇中，并计算簇的矩形边界框，过滤面积小于预定阈值的集群。

6.根据权利要求1所述的一种基于改进型激光雷达里程计的室内外SLAM方法，其特征在于，步骤2在对点云进行粗匹配后，利用迭代最近算法ICP实现两点云之间的精细匹配，采用具有三维Kdtree结构的ICP算法将时间复杂度降低到O(N_PlogN_Q)，其中N_P、N_Q分别表示点云P,Q中点的数量，迭代最近算法ICP的具体步骤为：

步骤2-1、搜索最近点对，对于源点云P中的点p_i，将其按照初始转换参数变换，即R^k-1p_i+t^k-1，这里k代表迭代次数，R代表3×3的旋转矩阵，t表示3×1的平移矩阵；在目标点云Q中寻找一个点q_i使这两个点之间的欧氏距离最近，构成最近邻点对(p_i,q_i)；

步骤2-2、搜索完成的最近点对进行配准变换，构建误差函数作为度量标准：

其中，N为最近邻点对的数量，为使误差函数最小，使用Hessian矩阵法和高斯-牛顿优化转换为对应似然函数的最值问题，计算出最优匹配转换矩阵的旋转参数R^k，平移参数t^k；

步骤2-3、利用步骤2-2中求出的转换参数对p_i再进行旋转和平移转换到新点集p_i′，计算p_i′到q_i的平均欧氏距离d

若d小于指定阈值则迭代终止，得出最终的最佳匹配参数，否则重新进行步骤2。

7.根据权利要求1所述的一种基于改进型激光雷达里程计的室内外SLAM方法，其特征在于，步骤3在校正节点中，当选择关键帧时，校正节点使用NDT将关键帧与动态局部子图匹配，从而通过直接替换结果变换来校正定位节点的输出，具体为：

将参考点云占用的空间划分为体素网格，然后根据扫描数据在每个体素中的分布计算描述体素的概率密度函数PDF，形成动态体素栅格点云地图，所述动态体素栅格点云地图根据新出现的点进行动态更新；所述动态体素栅格点云地图使用HashMap体素存储结构动态插入和删除点；每个体素的属性包括索引、质心平均值、协方差矩阵、点和点数；将超过预设数量点的体素运用于校正算法；

为了防止碰撞，点q所在的体素指数I(q)计算为：

I(q)＝t.x+t.y＜＜h_bits+t.z＜＜2h_bits

其中，<<表示左移位运算符，T_mg表示从当前坐标系到地图坐标系的转换；r_es表示网格分辨率，t表示3×1的平移矩阵，t.x表示x方向上的平移向量，t.y表示y方向上的平移向量，t.z表示z方向上的平移向量，h_bits表示h指数位；

索引I(q)用作HashMap的键值，所有点都存储在它们相应的HashMap中；因此，在每次迭代中，只需重新计算具有新插入点的体素的平均值和协方差矩阵，当n个点q_i(i＝1,2，…，n)插入到已经有m个点的体素中时，体素的平均值μ将更新为：

协方差矩阵重新计算为：

其中，m表示更新前体素栅格内的点数，n表示新插入该体素内的点数，q_i表示激光点云点，使用固定大小的体素栅格动态矫正定位的误差，更新并输出定位结果。

Images