激光扫描匹配室内定位方法探讨

郭 哲，徐爱功，隋 心，2，乔 智

(1.辽宁工程技术大学 测绘与地理科学学院，辽宁 阜新 123000；2.武汉大学 卫星导航定位技术研究中心，武汉 430079)

激光扫描匹配室内定位方法探讨

郭 哲1，徐爱功1，隋 心1，2，乔 智1

(1.辽宁工程技术大学 测绘与地理科学学院，辽宁 阜新 123000；2.武汉大学 卫星导航定位技术研究中心，武汉 430079)

针对室内环境下基于ICP匹配的算法比较复杂，运算量大，并且ICP算法高度依赖初始预估，限制了其实际应用范围的问题，提出一种改进的二维激光扫描匹配方法：不需要先验信息，降低特征匹配的复杂度；采用特征线筛选的方法，减少匹配算法运算量。实验结果表明，改进的二维激光扫描匹配方法在保证定位精度不变的条件下，能够提高扫描匹配算法的稳定性和效率，适用于较高精度的室内定位。

扫描匹配；特征提取；特征匹配；二维激光扫描；室内定位

0 引言

在室外环境中，文献[1]介绍了依靠全球卫星导航系统(global navigation satellite system，GNSS)能获得高精度的定位信息；但在室内环境下，由于无法接收到GNSS信号，依靠GNSS及其相关的技术已无法进行高精度的定位服务。业界已提出许多方法来解决这个问题。其中使用最多的解决方案是文献[2]于1986年提出的基于扩展卡尔曼滤波(extended Kalman filter，EKF)的地图构建方法，即同步定位和映射技术。文献[3]介绍了迄今为止国内外学者对此开展的相关研究，以及取得的进展结果。文献[4]介绍了南京航空航天大学采用激光测距仪与微机电系统(micro-electro-mechanical system，MEMS)惯导组合导航的方法，并且实现了基于小型无人机上的同步定位与建图。文献[5]介绍了美国宾夕法尼亚大学采用激光测距扫描仪、惯性测量单元(inertial measurement unit，IMU)、摄像机以及2个折射镜，使用基于网络搜索的迭代最邻近点(iterative closest points，ICP)算法作为定位算法，实现同步建图、定位以及轨迹描述等功能。文献[6]介绍了新加坡国立大学利用激光测距扫描仪和一个单目摄像头，实现了四旋翼飞行器沿着室内墙壁飞行一周并返回原点的任务。从而进一步促进了扫描匹配算法在室内定位方面的应用。

激光扫描的室内定位方法就是通过扫描匹配算法对激光测距仪测量的扫描数据进行解算处理，得到移动设备在建立的导航坐标系中的位置和方位信息。其关键技术就是激光扫描匹配算法，该算法的主要功能就是对激光测距仪的连续2组扫描数据，即当前扫描和参考扫描，进行一系列的运算，得到当前扫描下的状态位置和方位信息，与估算的参考值进行对比，得到移动设备的精确位置。

传统的扫描匹配算法，如文献[7]中提到的经典的ICP算法、文献[8]中提到的极坐标扫描匹配(polar scan matching，PSM)算法等，均依赖良好的先验信息，而降低了算法的实际应用能力。为了解决这一问题，本文提出一个改进的二维激光扫描匹配算法：该方法不需要提供先验信息，采用距离直方图的方法对提取的特性进行评价，能够提高特征提取的准确率；并采用特征线筛选的方法来减少匹配算法的运算量。

1 扫描匹配算法描述

扫描匹配算法通过比较参考位置处获得的扫描与移动平台实际位置处获得的扫描，将实际扫描转换到参考扫描坐标系中进行匹配，获得移动平台实际位置(局部地图)和参考位置(全局地图)之间的相对距离和角度，依次更新移动平台的位置。

用Sk、Pk和Lk分别代表激光扫描仪在k时刻的扫描数据、特征点的集合和特征线的集合。则有

Sk=rk-1Sk-1+tk-1。

(1)

因此扫描匹配问题可以描述为：计算得出2个激光扫描基于点和线特征的位姿变换参数rk-1、tk-1，以函数H(r，t)作为评价标准，即

(2)

式中：N为扫描点的个数；argmin表示取最小值；fp(k，i)(x(k，i)，y(k，i))为Pk中的第i个特征点；fp(k-1，i)(x(k-1，i)，y(k-1，i))为Pk-1中的第i个特征点。

1.1 特征提取

激光扫描仪扫描频率高、数据量大，如果直接对扫描数据进行匹配会降低匹配精度；因此处理原始扫描数据时，需要提取特征点及特征线。

1.1.1 点特征提取

在室内环境下存在的特征点包括特征线端点、角点，而特征线的端点可以直接从线特征提取中给出；因此点特征提取主要是对角点进行提取。

图1给出了角点特征的描述：设Pk为标记的角点，角点函数f(k)=sinθ的大小与点Pk左右两侧的点及Pk本身所拟合的直线之间的夹角θ有关。θ越接近90°，f(k)越接近1，说明点Pk成为角点的可能性越高；θ越接近0°或180°，f(k)越接近0，说明点Pk成为角点的可能性越低。角点函数计算公式为

(3)

图1 点特征描述

采取非极大值抑制的方法，对于点pk及其一邻域D={pi|i∈(k-m，k+m)}，若点pk所对应的角点函数值在其邻域中为最大值，则保留pk的角点函数值f(k)，获取特征点。反之，则令f(k)取特定值，本文中取0值。非极大值抑制公式为

(4)

图2给出了角点函数计算的结果：f(k)为角点函数；k为提取的角点编号。图3给出了角点函数非极大值抑制结果：f(k)为角点函数非极大值抑制结果。

图2 角点函数计算结果

图3 非极大值抑制结果

如图2、图3所示：角点函数计算结果中存在大量与极大值相近的非极大值点，从而对角点提取的唯一性造成极大困难；非极大值抑制结果更好地保证了角点特征提取的唯一性。

1.1.2 线特征提取

目前最普遍的线特征提取方法主要有：文献[9]介绍的分裂聚合(Split-and-Merge)算法、文献[10]介绍的线跟踪(line tracking，LT)算法和文献[11]介绍的随机抽样一致(random sample consensus，RANSAC)算法。然而，经典的线提取方法存在一定的误差。本文提出一种基于距离直方图的特征提取评定算法。对分裂聚合算法所提取的特征线进行评价，剔除异常特征线。该方法在提高线特征提取精确的基础上，可以直观地对特征线进行描述。

距离直方图线特征提取评价算法描述：采用点特征提取将原始扫描数据分成多组扫描数据，分别连接各组扫描数据的首末特征点，初步获取多组特征线；通过提取第i条特征线fl(k，i)∈Lk中的3个点，将该特征线分成4等分，如图4所示，距离直方图横轴表示待监测点与特征点之间的距离，单位为cm，纵轴表示点的个数；检索每个特征点fp(k，i)∈Pk附近的点，若检索点p(k，i)与特征点fp(k，i)∈Pk间的距离小于检索半径R，则认为p(k，i)是特征点fp(k，i)∈Pk邻近点；比较p(k，i)与特征线fl(k，i)∈Lk之间的距离，绘制成距离直方图；判断特征线提取的准确程度。

图4 线特征描述

1.2 特征匹配

迭代最近点(ICP)算法为目前最普遍的特征匹配算法，然而该方法需要给出初始对应关系Q={Rq，Tq}来估计下一帧坐标，其中Rq为旋转矩阵，Tq为距离改正矩阵，下标q表示对应关系。通过计算下一帧每个点的实际坐标与估计坐标之间的欧氏距离来更新初始对应关系，进行多次迭代，直到对应关系Q基本稳定不再有大的变化；因此该方法高度依赖高精度的初始对应关系(先验信息)，先验信息选取的好坏直接影响迭代的次数。

本文提出改进的点线特征匹配算法是直接根据点、线2方面特征进行匹配，通过判断对应点、线特征之间的距离值来分析匹配结果的好坏。若计算求得的距离差值越小，则说明匹配的精度越高；距离差值越大，则说明匹配精度越低。该方法不需要先验信息，提高了特征匹配实际应用方面的能力，降低了特征匹配的复杂程度。

1.2.1 特征点匹配

从连续2个激光扫描数据Sk-1和Sk中提取特征点集合为Pk-1和Pk，计算特征点fp(k-1，i)与Pk中所有点的匹配程度为

(5)

式中：s.t.表示限制条件；dp(·)为点描述符；‖·‖代表欧氏距离。

算法描述：计算连续2个激光扫描数据的scorep值，待匹配点fp(k，i)与特征线fp(k-1，i)中对应点间的scorep值越接近，则获取的fp(k-1，i)匹配点fp(k，i)∈Pk越精确。

1.2.2 特征线匹配

从连续2个激光扫描数据Sk-1和Sk中提取特征线集合Lk-1和Lk，需要计算特征线fl(k-1，i)与Lk中所有线的匹配程度为

(6)

式中：dl(·)为线描述符；scorel描述特征线fl(k-1，i)与Lk中fl(k，i)线中所有对应点间的距离。

传统的特征线匹配算法在应用中的难点在于如何查找相应的2组关联线。解决的方法往往是通过大量扫描数据计算减少匹配过程中出现的误差；因此本文对特征线匹配算法进行了改进。由于在特征线匹配时只存在着刚体变换，因此长度是不变量。若筛选出的匹配线与特征线的长度不同，则会直接影响匹配精度。在特征线匹配前，首先判断筛选出的特征线与待匹配线之间的长度是否相同，若特征线与待匹配线长度相同，则进行特征线匹配；若特征线与待匹配线长度不同，则需重新筛选特征线。经该方法筛选得到的待匹配线fl(k，i)与特征线fl(k-1，i)中对应点间的scorel值越接近，说明线的匹配程度越高。该方法在保证特征匹配精度的前提下，减少了匹配算法运算量。

2 实验及结果分析

为了验证扫描匹配室内定位方法的有效性，进行了1次实验。在本次实验中，二维激光雷达硬件采用RoboPeak团队开发的RPLIDAR模块，其测距精度为0.2 cm，测量范围6 m，扫描范围360°，实验平台硬件连接如图5所示。扫描仪将探测到的数据以5.5 Hz的频率，通过USB接口传送给笔记本电脑，实时显示结果并保存相关数据。图6为该实验的走廊，图7为该实验的室内环境。

图5 移动平台

图6 走廊

图7 室内环境

采用角点提取算法与分裂聚合算法对图7室内部分环境特征进行提取。采用距离直方图的方法进行线特征提取精度评定，图8(a)中圆圈为所提取的特征点，图8(b)中的黑实线为采用距离直方图法提取的特征线，选取图8(b)中的1、2、3号3条特征线进行评价。图9给出了线特征提取评价结果：特征线提取结果趋于稳定；提取的精度范围在2～4 cm。

图10给出了分别采用经典ICP算法、无先验信息的ICP算法以及本文改进的扫描匹配3种算法的匹配结果。其中无先验信息的ICP算法是在经典ICP算法中减少了先验信息提取的步骤。

图中方法1为无先验信息的ICP算法；方法2为经典ICP算法。结果表明，由于点线特征匹配算法无需先验信息，因此：1)算法复杂程度方面，本文方法与方法1相同，比方法2效率高；2)算法匹配结果方面，3种方法误差都比较稳定，改进的特征匹配算法精度与方法2相近，比方法1准确。

图8 特征提取结果

图9 线特征提取精度评定

图10 特征匹配精度评定

本文提出的改进的激光扫描匹配算法在保证精度的同时无需先验信息，降低了特征匹配的复杂程度。

图11给出了改进的激光扫描同步建图与定位(simultaneous localization and mapping，SLAM)匹配算法结果的示意图。图中实线为移动平台的轨迹；圆圈与方块部分分别表示图4中的门与消火栓。

图11 同步建图与定位结果

表1 激光扫描匹配定位结果

3 结束语

GNSS可以在室外进行高精度的定位；而在室内环境下，需要采用其他系统来替代GNSS：激光扫描仪是一个较好的选择。本文针对激光扫描匹配的室内定位方法进行研究：采用距离直方图的方法，对提取的特征进行评价，减少了因特征提取错误造成的匹配误差；并提出基于改进的特征匹配算法，该方法不需要提供先验信息，在保证定位精度的情况下，降低了特征匹配的复杂度。实验结果表明改进的激光扫描匹配室内定位方法具有cm级的精度，稳定性更高、可靠性更强，适用于较高精度的室内定位。

[1] 李星星.GNSS精密单点定位及非差模糊度快速确定方法研究[D].武汉：武汉大学，2013.

[2] VALIENTE D，GIL A，FERNANDEZ L，et al.A comparison of EKF and SGD applied to a view-based SLAM approach with omnidirectional images[J].Robotics and Autonomous Systems，2014，62(2)：108-119.

[3] 温熙，郭杭.室内移动机器人自定位方法[J].测绘科学，2016，41(6)：97-101.

[4] LI R B，LIU J Y，ZHANG L，et al.LIDAR/MEMS IMU integrated navigation(SLAM)method for a small UAV in indoor environments[C]//The Institute of Electrical and Electronics Engineers(IEEE).Proceedings of Inertial Sensors and Systems Symposium(ISS 2014).Nanjing：IEEE，2014：1-15.

[5] SHEN S，MICHAEL N，KUMAR V.Autonomous multi-floor indoor navigation with a computationally constrained MAV[C]//The Institute of Electrical and Electronics Engineers(IEEE).Proceedings of Robotics and automation(ICRA 2011).Shanghai：IEEE，2011：20-25.

[6] WANG F，CUI J，PHANG S K，et al.A mono-camera and scanning laser range finder based UAV indoor navigation system[C]//The Institute of Electrical and Electronics Engineers(IEEE).Proceedings of Unmanned Aircraft Systems(ICUAS 2013).Atlanta：IEEE，2013：694-701.

[7] BESL P J，MCKAY N D.Method for registration of 3-D shapes[EB/OL].(2007-10-09)[2017-01-18].http：//eecs.vanderbilt.edu/courses/CS359/other_links/papers/1992_besl_mckay_ICP.pdf.

[8] DIOSI A，KLEEMAN L.Laser scan matching in polar coordinates with application to SLAM[EB/OL].(2011-11-30)[2017-01-18].http：//www.diosirobotics.net/publications/diosi_kleeman_iros05.pdf.

[9] BORGES G A.A split-and-merge segmentation algorithm for line extraction in 2D range images[C]//The Institute of Electrical and Electronics Engineers(IEEE).Proceedings of International Conference on Pattern Recognition.Montpellier：IEEE，2000：441-444.

[10] NGUYEN V，GACHTER S，MARTINELLI A，et al.A comparison of line extraction algorithms using 2D range data for indoor mobile robotics[J].Autonomous Robots，2007，23(2)：97-111.

[11] DIETMAYER K，SPARBERT J，STRELLER D.Model based object classification and object tracking in traffic scenes from range images[C]//The Institute of Electrical and Electronics Engineers(IEEE).Proceedings of IV IEEE Intelligent Vehicles Symposium.Tokyo：IEEE，2001：25-30.

Discussiononindoorpositioningoflaserscanmatching

GUOZhe1，XUAigong1，SUIXin1，2，QIAOZhi1

(1.School of Geomatics，Liaoning Technical University，Fuxin，Liaoning 123000，China；2.Research Center of GNSS，Wuhan University，Wuhan 430079，China)

Aiming at the problems that the matching algorithm based on ICP is complex with heavy computation，and highly dependent on the initial estimate，which limits its practical application，the paper proposed an improved 2D laser scan matching method：the priori information was not necessary so as to reduce the complexity of feature matching；the method of feature line filtering was used to reduce the computation of the matching algorithm.Experimental result showed that the proposed method could improve the stability and efficiency of the scan matching algorithm with the same positioning accuracy，suitable for high-precise indoor positioning.

scan matching；feature extraction；feature matching；2D laser scanning；indoor positioning

2017-02-28

国家重点研发计划项目(2016YFC0803102)；辽宁省高等学校创新团队项目(LT2015013)。

郭哲(1992—)，男，山东青州人，硕士研究生，研究方向为二维激光雷达室内定位。

郭哲，徐爱功，隋心，等.激光扫描匹配室内定位方法探讨[J].导航定位学报，2017，5(4)：25-29,97.(GUO Zhe，XU Aigong，SUI Xin，et al.Discussion on indoor positioning of laser scan matching[J].Journal of Navigation and Positioning，2017，5(4)：25-29,97.)

10.16547/j.cnki.10-1096.20170406.

P228.1

A

2095-4999(2017)04-0025-06