基于几何特征关联的室内扫描匹配SLAM方法

廖自威，李荣冰，雷廷万2，杭义军

（1.南京航空航天大学导航研究中心，南京210016；2.成都飞机设计研究院，成都610041）

基于几何特征关联的室内扫描匹配SLAM方法

廖自威1，李荣冰1，雷廷万2，杭义军1

（1.南京航空航天大学导航研究中心，南京210016；2.成都飞机设计研究院，成都610041）

为实现室内结构化环境中仅依靠激光雷达数据进行实时自定位并创建精确的特征地图，提出了一种基于几何特征关联的室内扫描匹配SLAM方法。几何特征关联与匹配方法的优劣很大程度上影响SLAM的实时性和精度。结合室内结构化环境的特点，提出了一种完备端点定义与提取方法，将直线段特征与完备端点进行关联，优化几何特征扫描匹配过程。此外，姿态角收敛是SLAM进行机器人位姿估计和求解一致性的关键。为确保姿态角准确收敛，采用了基于直线拟合认知的姿态角加权几何平均求解方法。实验证明，提出的SLAM方法得到的定位精度在100mm内，建图精度也较高，能胜任室内SLAM。

激光雷达；扫描匹配；特征关联；即时定位与地图构建

0　引言

机器人处于未知的室内结构化环境中时，GPS失去导航作用，而惯性元件在航位推算过程中存在累计误差，此时机器人的自定位能力就显得尤为重要。因此通常利用外部传感器多次观测的强相关性，进行室内结构化未知环境的即时定位与地图构建（Simultaneous Localization and Mapping，SLAM）［1］。

SLAM研究方法主要分为基于概率与基于扫描匹配（Scan-Matching）的方法，其中，基于概率的方法包括基于卡尔曼滤波（KF）的SLAM和基于粒子滤波（PF）的SLAM［2］。基于KF的SLAM算法近年来得到了广泛的关注，但是随着时间状态维数增加，计算量增大，并且难以有效解决闭环回路问题。基于PF的SLAM算法能很好地应用于非线性模型，有效解决闭环回路问题，但是存在粒子重采样及粒子衰竭等问题。基于扫描匹配的SLAM算法简单，其基本思想是计算相邻两帧扫描数据的2维姿态转换矩阵并更新地图［3-4］。此外，扫描匹配算法的结果也可作为基于KF或基于PF的SLAM算法的状态扩充。

扫描匹配实现相对简单，但它受到激光雷达传感器累积噪声的影响，往往需要在定位建图精度及算法实时性之间权衡，因此出现了不同扫描匹配的方法，其中大部分都是通过迭代实现的，如ICP及其改进算法等。ICP算法通常用于点与点的局部匹配，尽管许多学者对ICP算法进行了研究改进［5］，但由于需要多次迭代求解最优解，实时性待优化，仍然限制了该方法在SLAM实现中广泛有效的应用。文献［6］是基于特征和特征的扫描匹配方式实现的，但在进行特征点匹配时采用复杂的相似三角形匹配算法，算法复杂度较高，且没有充分利用直线段特征与点特征的相关性特点。文献［7］虽然采用聚类思想将一帧激光雷达扫描点进行区域分割，但是在处理每一个区域的点簇时，对点簇中每个点都进行了扫描点关联微段求取，这种基于点与点的全局特征匹配对SLAM的实时性是不利的。

针对室内结构化环境存在广泛的直线段、角点等特征信息以及分布稀疏的特点，本文提出了基于几何特征关联的室内扫描匹配SLAM方法，利用二维激光雷达完成了工程化实现。该方法首先提取结构化室内环境中直线段特征和充分有效的角点特征，在此基础上，提出完备端点定义和提取的方法，将直线段特征与完备端点特征进行关联，优化几何特征匹配过程。由于姿态角收敛是SLAM进行机器人位姿估计和求解一致性的关键［8］，在姿态角求取时，通过加权几何平均求解的方法确保姿态角准确收敛。采用了基于 “直线段匹配、姿态角求取、完备端点匹配、位移量求取”策略的全局扫描匹配SLAM方法，根据几何特征匹配规则，筛选出匹配好的几何特征后便进行位姿解算。本文基于几何特征关联的室内扫描匹配SLAM方法，仅利用激光雷达数据，避免了基于概率SLAM方法计算复杂度随时间增加的问题，满足SLAM实时性需求，且位姿解算和建图精度均较高。

1　基于室内结构化环境的特征描述

1.1激光雷达数据预处理

在提取室内环境特征之前，需要对激光雷达数据进行预处理，主要包括激光雷达数据点集的平滑与区域分割。为了尽量消除激光雷达原始数据本身受激光雷达器件噪声的影响，对激光雷达数据点集进行中值滤波平滑处理。

区域分割是根据激光雷达扫描点的位置分布情况划分可能属于同一个物体的分割段。激光雷达扫描点依托于分割段能表达更多的信息，分割段则表征激光雷达扫描点整体分布情况。激光雷达数据点集的区域分割方法参照文献［9］，为了保证载体每次不同角度和距离都能进行准确的区域分割，采用了自适应阈值的区域分割方法。设其中用于分割判断的增量阈值为为自适应比例系数，初始增量阈值计算公式为:

1.2完备端点特征定义与提取

几何特征关联与匹配的优劣很大程度上影响了SLAM方法的实时性和精度。为避免基于EKF 的SLAM方法难以保证实时性的缺陷，结合室内结构化环境的特点，提出了完备端点定义与提取的方法，将直线段特征与完备端点进行特征关联，优化完备端点特征匹配过程。

激光雷达原始数据点集采用基于平面扫描激光雷达自适应环境特征提取方法［9］，通过自适应阈值的区域分割和基于直线段分割的迭代折线角点求解方法充分有效提取出室内结构化环境的直线、分裂点与角点特征。完备端点提取在此基础上进行。

完备端点定义:属于某一区域分割后的物体点集被相邻另一区域分割后的物体点集遮挡，当激光雷达位姿发生改变时，被遮挡物体点集的分裂点相对位姿发生变化，具有这样特性的分裂点称为非完备端点；而遮挡住该物体的另一物体点集的分裂点相对位姿不随激光雷达位姿改变而改变，像这样的分裂点称为完备端点。

设D｛Di|i=1，2，…｝表示呈直线分布的点集。其中，n为点集D的个数。使用最小二乘法对D进行拟合，可获得相应直线段特征，这些获得的直线段端点通常并不是点集Di的端点。完备端点从Di的端点中提取，而Di又被成对的分裂点或角点分割开，因而完备端点是分裂点和角点的筛选及重新集合。若完备端点用点集PP｛PPi|i=1，2，…｝表示，则角点CP｛CPi|i=1，2，…｝和分裂点BP｛BPi|i=1，2，…｝可以按照一定的处理规则将满足条件的点转化到PP中。从完备端点划分的定义可知，角点CP两边的物体不会相互遮挡，因而CP⊆PP。而BP因为两边可能存在遮挡则仅有部分包含于PP。

图1为激光雷达完备端点提取示意图，其中包含D1、D2和D3三个呈直线状分布的点集，同时也包含两个分裂点和一个角点。完备端点进行定义描述时虽然使用了不同帧的激光雷达数据进行了说明，但是完备端点的提取仅仅使用一帧激光雷达数据即可完成。一帧激光雷达数据中，在两个相邻的分割区域的分界处，距离激光雷达较近的分裂点即为完备端点，同时，较远的分裂点则被确定为非完备端点。如图1所示，点集D1的分裂点划分为D1的非完备端点，而点集D2的分裂点划分为D2的完备端点1。D2与D3之间的角点特征则直接划分为完备端点2。

图1　激光雷达完备端点提取示意图Fig.1 Extracted diagram of perfect endpoints of laser ladar

1.3直线段特征匹配度

室内结构化环境存在广泛的直线段、角点等特征信息，且其分布稀疏。采用最小二乘法提取直线段，其特征Fl如下:

其中，a和b分别表示拟合直线在当前激光雷达坐标系下的斜率和截距；Pstart、Pend、Pmiddle分别表示拟合直线的起点、终点以及重心点（亦即中点）；l表示该直线段的长度；θl表示直线段在当前激光雷达坐标系下与x轴横轴的夹角；PP表示直线段两端的完备端点，表征直线段特征与完备端点特征相关联。

将度量两帧激光雷达原始数据的匹配度转化为度量两帧数据的直线特征与完备端点特征的匹配度。首先，分别对两帧激光雷达数据的任意直线段L1和L2进行匹配度计算。在这之前，将L1与L2转换到相同的坐标系。考虑到全局坐标系的统一性，将L1与L2均转化到全局坐标系，使L1与L2坐标系统一到一起。二者的匹配程度表示如下:

其中，

式中，lmin为可用于匹配的直线段的最短要求长度，DLmax为可匹配的两直线段最大重心距离差，Δθmax为可匹配的两直线段最大夹角差。Rl1和Rl2分别表示直线段L1和L2的长度比例，Rm表示直线段L1和L2之间的重心匹配度，Rθ表示直线段L1和L2之间的夹角匹配度。直线段匹配度Ml反应了两直线段之间的几何空间分布的匹配程度，由其定义可知，Ml∈［0，1］且Ml越大，直线段匹配度越高。

1.4完备端点特征匹配度

直线段匹配成功后，便可求得稳定收敛的当前帧激光雷达的姿态角θk。此时，在局部完备端点坐标转换时，将上一时刻定位结果 Pk-1（θk-1，xk-1，yk-1）中的θk-1用θk替换，将当前帧激光雷达的完备端点转化到全局坐标系中。

完备端点匹配依附在直线段匹配的基础上，完备端点与直线段关联使得完备端点的匹配度计算更加高效。由于激光雷达的噪声影响，在进行直线段特征的点集分割时，本该是完整的一条直线段L可能会被分裂为两条分开的直线段La和Lb，不妨设Lb为较长直线段。当La和Lb长度差异较大时，在直线段特征匹配中，存在Lb与全局地图中相应的直线段L成功匹配的可能性。这种情况下Lb与L虽然都分别存在两个完备端点，但其中只有一对是匹配的。因此，需要对完备端点进行进一步的匹配。完备端点匹配程度表示如下:

其中，

R为完备端点PP和激光雷达扫描点之间的距离大小，ω表示完备端点可匹配的最大允许间距DPmax占R的比重。Rp表示两完备端点进一步匹配的程度。由Mp定义可知，Mp∈［0，1］，且Mp越大，完备端点匹配程度越高。

2　基于几何特征关联的室内扫描匹配SLAM算法设计

2.1室内结构化环境下SLAM方案设计SLAM算法的目标是根据当前帧激光雷达获取的室内环境距离参数解算得到环境特征fi=｛fi|i= 1，2，…｝以及机器人的当前位姿状态Pk（θk，xk，yk）。其中，θk为当前帧激光雷达相对于全局坐标系的旋转量，（xk，yk）为当前帧激光雷达相对于全局坐标系的位移量。提出的SLAM方案结构图如图2所示。

图2　SLAM方案结构图Fig.2 The solution structure of SLAM

整体方案主要分为三部分，实现流程如下:

1）特征提取:从激光雷达原始数据中提取室内局部直线段和角点特征，进一步提取出完备端点，建立初始全局特征地图。

2）特征匹配与位姿解算:采用 “直线段匹配、姿态角求取、完备端点匹配、位移量求取”的全局扫描匹配策略，通过基于直线拟合认知的姿态角加权几何平均方法保证姿态角求解的稳定收敛，利用完备端点与直线段相关联的匹配方法优化完备端点匹配过程，最终得到机器人稳定收敛的位姿信息。

3）全局地图更新:采用缓冲全局特征地图的筛选方法优化建立全局特征地图的精度。

2.2基于直线拟合认知的姿态角加权几何平均方法

采用 “直线段匹配、姿态角求取、完备端点匹配、位移量求取”的全局特征扫描匹配与位姿解算策略。根据几何特征匹配规则，筛选出匹配好的几何特征后便立即进行位姿解算。

二维平面内当前帧激光雷达定位结果用Pk（θk，xk，yk）表示。全局地图w中得到的直线段和完备端点特征总体表述为fw=｛fwi|i=1，2，…，n1｝，其中，n1为全局地图中的特征的个数。当前帧激光雷达的局部坐标系l下总体特征表述为fl=｛flj|j=1，2，3，…，n2｝，其中，n2为当前帧激光雷达局部特征个数。匹配前，用上一帧定位结果Pk-1（θk-1，xk-1，yk-1）将fl转换为全局特征f′w=｛f′wj|j=1，2，3，…，n2｝。f′w中分别包含直线段特征L′w与完备端点特征PP′w。匹配的过程就是从f′w中找出与fw一致的直线段特征和完备端点特征。匹配成功的直线段特征用于求取当前帧激光雷达的姿态角θk，在完成姿态角求解的基础上，进行完备端点匹配度计算。匹配成功的完备端点特征则用于求取当前帧激光雷达的位移量（xk，yk）。

设（Lli，Lwj）表示当前帧激光雷达数据在局部坐标系下的第i条直线段Lli与全局地图中第j条直线段Lwj匹配成功后得到的匹配对。假设成功匹配m条当前帧拟合直线段Lli，则相应求得m个姿态角θki。

姿态角θk准确收敛关系到整个定位结果的一致收敛，准确求取θk显得至关重要。θki中不可避免地包含有误差，每一条匹配成功的当前帧直线段拟合可信度不同，因而不能直接对姿态角θki求均值。令匹配成功的当前帧拟合直线段Lli的最小二乘拟合残差为Si，其拟合点集的点数为Ni。直线段Lli残差Si越小，且拟合点数Ni越多，则直线段拟合可信度和准确度越高。基于此方法对姿态角θki进行加权求解，θk满足式:

式中，ρ1为拟合点数N所占权重，θNk为通过Ni计算得到的姿态角；ρ2为直线段拟合残差S所占权重，θSk为通过Si计算得到的姿态角。θNk与θSk由式（5）求得:

式中，i=1，2，3，…，m。按照式（3）进行完备端点匹配度计算，假设最终有q对好的完备端点匹配对（PPli，PPwi），则得到的q个位移量求解值满足式（6）:

最终得到的当前帧激光雷达平移量 （xk，yk）为:

3　实验验证与分析

3.1实验硬件设计

为了验证本文基于几何特征关联的室内扫描匹配SLAM算法的实际性能，设计了激光雷达导航系统实验平台，用该平台模拟机器人在室内某一平面内的运动过程。激光雷达测距范围0.1 m～30m，探测角分辨率为0.25°，所构成的激光雷达同步定位与建图系统示意图如图3所示。

图3　系统硬件设计连接框图Fig.3 Connection diagram of the system hardware design

3.2实验及结果分析

图4　室内实验环境照片Fig.4 Photograph of indoor experimental environment

本文使用的运动平台运动速度约为1.5m/s，转动时角速度约为150（°）/s。实验环境如图4所示，为大小约10m×7m的长方形实验室，实验环境中包含有窗户、墙壁、墙角等常规室内环境特征，实验环境中包含一扇打开的门、墙角以及六扇窗户。实验时，运动平台在如图5所示的室内结构化未知环境中按照指示轨迹运动。针对室内环境的几何空间大小，直线匹配阶段选取的匹配阈值为:最短匹配长度lmin取30mm，两直线段最大重心距离差DLmax取为50mm，可匹配的两直线段最大夹角差Δθmax为10°。在直线匹配成功的基础上完成姿态角的求解。图6是使用本文提出的基于直线段拟合认知的姿态角加权几何平均求解方法与姿态角求取的均值法的结果对比。从图6中可以看出，本文提出的基于认知的姿态角加权几何平均方法比均值求解法更加收敛，验证了姿态角几何加权平均求解算法的有效性。

图5　平台运动轨迹示意图Fig.5 Diagram of platform motion trajectory

图6　姿态角几何加权求解法与均值求解法对比Fig.6 The attitude angle geometric weighted method compared with the average method

为了比较本文完备端点与直线段进行特征关联方法的有效性，将与直线匹配关联后的完备端点匹配与单独角点匹配结果进行对比。其中，完备端点匹配与单独角点匹配的匹配阈值均采用自适应阈值取值，其取值满足式（8）:

式中，l表示角点或完备端点距离坐标原点的距离，单位为mm。实验时共采集激光雷达数据帧数为1887帧。完备端点匹配与单独角点匹配结果如表1和表2所示。

表1　完备端点匹配法Table 1 Perfect points matching

表2　角点匹配法Table 2 Corners matching

完备端点匹配法在直线匹配基础上进行，在所选匹配阈值的约束条件下，完备端点匹配法得到的误匹配率均为0%。而单独的角点匹配方法由于缺少与直线段的关联，当激光雷达运动转角较大（如第500帧）以及进入新的环境中（如第1500帧）时，其角点误匹配率也较大，因而对定位精度的影响较大。需要说明的是，虽然从完备端点定义上表明角点是完备端点的子集，但是表1中的完备端点并不确定是当前帧所有的完备端点，而是在直线匹配的基础上保留下来的完备端点数目。因此表1中可匹配的完备端点数与表2中的所有角点数并不满足包含关系。表1和表2的对比表明完备端点匹配方法能够精确筛选出可靠的匹配点对，完成点集匹配。

图7是将角点与直线段等几何特征各自单独匹配的扫描匹配SLAM方法的结果。其中，里圈点集表示运动轨迹，外圈线段表示环境特征地图。可以看出，该SLAM算法定位结果的偏差大，后半段定位已经不能反映平台真实的运动状态，且建图结果也出现了明显错误。

图7 几何特征独立匹配SLAM方法结果Fig.7 The SLAM result of geometric features mapping separately

图8为使用本文提出的几何特征关联的室内扫描匹配SLAM方法的建图与定位结果。虽然无法与理想运动轨迹进行精确对比，但是从图8中运动路线重叠的部分仍然可以看出，本文一步全局匹配SLAM方法的定位结果精度较高，其误差在100mm内。

图8　几何特征关联的室内扫描匹配SLAM方法结果Fig.8 The indoor scan-mapping SLAM result of geometric features association

4　结论

为实现室内结构化环境中仅依靠激光雷达进行实时自定位并创建精确的特征地图，提出了一种基于几何特征关联的室内扫描匹配SLAM方法。几何特征关联与匹配方法的优劣很大程度上影响SLAM的实时性和精度。针对室内结构化环境的特点，提出了完备端点定义与提取方法，将直线段特征与完备端点进行关联，优化几何特征扫描匹配过程。此外，姿态角收敛是SLAM进行机器人位姿估计和求解一致性的关键。为确保姿态角准确收敛，采用了基于直线拟合认知的姿态角加权几何平均求解方法。与传统基于ICP及其改进扫描匹配算法不同，采用 “直线段匹配、姿态角求取、完备端点匹配、位移量求取”策略进行SLAM位姿解算，该算法平均每两帧定位与建图解算时间在100ms左右，满足SLAM实时性需求。本文基于几何特征关联的室内扫描匹配SLAM方法不需要里程计进行航位推算的位姿估计，也无需进行多传感器的数据融合，仅使用激光雷达数据实现室内SLAM，定位误差在100mm内，且建图精度也较高，可用于静态或动态环境，胜任室内SLAM任务。在未来的工作上进一步与惯性器件进行组合滤波，优化室内组合导航。

［1］ Bailey T，Durrant Whyte H.Simultaneous localization and mapping（SLAM）：Part II［J］.IEEE Robotics and Automation Magazine，2006，13（3）：108-117.

［2］ 祝继华，郑南宁，袁泽剑，何永健.基于ICP算法和粒子滤波的未知环境地图创建［J］.自动化学报，2009，35（8）：1107-1113. ZHU Ji-hua，ZHENG Nan-ning，YUAN Ze-jian，HE Yong-jian.A SLAM approach by combining ICP algorithm and particle filter［J］.Acta Automatica Sinica，2009，35 （8）：1107-1113.

［3］ Tsardoulias E，Petrou L.Critical rays scan match SLAM ［J］.Journal of Intelligent&Robotic Systems，2013，72 （3/4）：441-462.

［4］ Nieto J，Bailey T，Nebot E.Scan-slam：combining ekfslam and scan correlation［C］.Field and Service Robotics，Springer Berlin Heidelberg，2006：167-178.

［5］ Holz D，Behnke S.Sancta simplicitas-on the efficiency and achievable results of SLAM using ICP-based incremental registration［C］.Robotics and Automation（ICRA），2010 IEEE International Conference on IEEE，2010：1380-1387.

［6］ 武二永，项志宇，沈敏一，等.大规模环境下基于激光雷达的机器人 SLAM算法［J］.浙江大学学报（工学版），2008，41（12）：1982-1986. WU Er-yong，XIANG Zhi-yu，SHEN Min-yi，et al.Robot SLAM algorithm based on laser range finder for large scale environment［J］.Journal of Zhejiang University（Engineering Science），2008，41（12）：1982-1986.

［7］ 郭瑞，孙凤池，苑晶，等.一种基于几何统计特征的全局扫描匹配方法［J］.模式识别与人工智能，2011，24 （3）：314-320. GUO Rui，SUN Feng-chi，YUAN Jing，et al.A global scan matching method based on geometric statistic features ［J］.Pattern Recognition and Artificial Intelligence，2011，24（3）：314-320.

［8］ 李久胜，李永强，周荻.基于EKF的SLAM算法的一致性分析［J］.计算机仿真，2008，25（6）：155-160. LI Jiu-sheng，LI Yong-qiang，ZHOU Di.Analysis of the consistency of EKF-based SLAM ［J］.Computer Simulation，2008，25（6）：155-160.

［9］ 杭义军，刘建业，李荣冰，等.基于混合特征匹配的微惯性/激光雷达组合导航方法［J］.航空学报，2014，35 （9）：2583-2592. HANG Yi-jun，LIU Jian-ye，LI Rong-bing，et al.MEMS IMU/LADAR intergrated navigation method based on mixed features matching［J］.Acta Aeronautica et Astronautica Sinica，2014，35（9）：2583-2592.

Indoor Scan-matching SLAM Method Based on Geometric Features Association

LIAO Zi-wei1，LI Rong-bing1，LEI Ting-wan2，HANG Yi-jun1
（1.Navigation Research Center，Nanjing University of Aeronautics and Astronautics，Nanjing 210016；2.Chengdu Aircraft Design and Research Institute，Chengdu 610041）

To realize the real-time positioning and precise mapping which only using ladar data in structured indoor environment，an indoor scan-matching SLAM method based on geometric feature association is proposed.The performance of geometric feature association and matching method greatly influence the timeliness and precision of SLAM.Combined with the characteristics of the structured indoor environment，the perfect endpoints definition and extraction method proposed associate points and lines together which optimize the process of geometric features matching.Besides，the convergence of heading is the key to the consistency of pose estimation and calculation of the robot in SLAM method.To ensure the convergence of the heading，an heading weighted geometric average method based on the cognitive of line fitting was proposed.Experiments shows that with the application of the SLAM solution proposed，the accuracy of the positioning is limited in100mm，and the accuracy of mapping is high enough and compete for indoor SLAM.

laser ladar；scan matching；feature association；simultaneous localization and mapping（SLAM）

TP24

A

1674-5558（2016）01-01132

10.3969/j.issn.1674-5558.2016.03.005

2015-06-03

江苏省2014年度普通高校研究生科研实践计划项目（编号:SJLX_0134）。

廖自威，男，硕士，研究方向为激光雷达数据融合与室内导航。