郭 哲，徐愛功，隋 心，2，喬 智

(1.遼寧工程技術(shù)大學(xué) 測繪與地理科學(xué)學(xué)院，遼寧 阜新 123000；2.武漢大學(xué) 衛(wèi)星導(dǎo)航定位技術(shù)研究中心，武漢 430079)

激光掃描匹配室內(nèi)定位方法探討

郭 哲1，徐愛功1，隋 心1，2，喬 智1

(1.遼寧工程技術(shù)大學(xué) 測繪與地理科學(xué)學(xué)院，遼寧 阜新 123000；2.武漢大學(xué) 衛(wèi)星導(dǎo)航定位技術(shù)研究中心，武漢 430079)

針對室內(nèi)環(huán)境下基于ICP匹配的算法比較復(fù)雜，運算量大，并且ICP算法高度依賴初始預(yù)估，限制了其實際應(yīng)用范圍的問題，提出一種改進的二維激光掃描匹配方法：不需要先驗信息，降低特征匹配的復(fù)雜度；采用特征線篩選的方法，減少匹配算法運算量。實驗結(jié)果表明，改進的二維激光掃描匹配方法在保證定位精度不變的條件下，能夠提高掃描匹配算法的穩(wěn)定性和效率，適用于較高精度的室內(nèi)定位。

掃描匹配；特征提?。惶卣髌ヅ?；二維激光掃描；室內(nèi)定位

0 引言

在室外環(huán)境中，文獻[1]介紹了依靠全球衛(wèi)星導(dǎo)航系統(tǒng)(global navigation satellite system，GNSS)能獲得高精度的定位信息；但在室內(nèi)環(huán)境下，由于無法接收到GNSS信號，依靠GNSS及其相關(guān)的技術(shù)已無法進行高精度的定位服務(wù)。業(yè)界已提出許多方法來解決這個問題。其中使用最多的解決方案是文獻[2]于1986年提出的基于擴展卡爾曼濾波(extended Kalman filter，EKF)的地圖構(gòu)建方法，即同步定位和映射技術(shù)。文獻[3]介紹了迄今為止國內(nèi)外學(xué)者對此開展的相關(guān)研究，以及取得的進展結(jié)果。文獻[4]介紹了南京航空航天大學(xué)采用激光測距儀與微機電系統(tǒng)(micro-electro-mechanical system，MEMS)慣導(dǎo)組合導(dǎo)航的方法，并且實現(xiàn)了基于小型無人機上的同步定位與建圖。文獻[5]介紹了美國賓夕法尼亞大學(xué)采用激光測距掃描儀、慣性測量單元(inertial measurement unit，IMU)、攝像機以及2個折射鏡，使用基于網(wǎng)絡(luò)搜索的迭代最鄰近點(iterative closest points，ICP)算法作為定位算法，實現(xiàn)同步建圖、定位以及軌跡描述等功能。文獻[6]介紹了新加坡國立大學(xué)利用激光測距掃描儀和一個單目攝像頭，實現(xiàn)了四旋翼飛行器沿著室內(nèi)墻壁飛行一周并返回原點的任務(wù)。從而進一步促進了掃描匹配算法在室內(nèi)定位方面的應(yīng)用。

激光掃描的室內(nèi)定位方法就是通過掃描匹配算法對激光測距儀測量的掃描數(shù)據(jù)進行解算處理，得到移動設(shè)備在建立的導(dǎo)航坐標(biāo)系中的位置和方位信息。其關(guān)鍵技術(shù)就是激光掃描匹配算法，該算法的主要功能就是對激光測距儀的連續(xù)2組掃描數(shù)據(jù)，即當(dāng)前掃描和參考掃描，進行一系列的運算，得到當(dāng)前掃描下的狀態(tài)位置和方位信息，與估算的參考值進行對比，得到移動設(shè)備的精確位置。

傳統(tǒng)的掃描匹配算法，如文獻[7]中提到的經(jīng)典的ICP算法、文獻[8]中提到的極坐標(biāo)掃描匹配(polar scan matching，PSM)算法等，均依賴良好的先驗信息，而降低了算法的實際應(yīng)用能力。為了解決這一問題，本文提出一個改進的二維激光掃描匹配算法：該方法不需要提供先驗信息，采用距離直方圖的方法對提取的特性進行評價，能夠提高特征提取的準確率；并采用特征線篩選的方法來減少匹配算法的運算量。

1 掃描匹配算法描述

掃描匹配算法通過比較參考位置處獲得的掃描與移動平臺實際位置處獲得的掃描，將實際掃描轉(zhuǎn)換到參考掃描坐標(biāo)系中進行匹配，獲得移動平臺實際位置(局部地圖)和參考位置(全局地圖)之間的相對距離和角度，依次更新移動平臺的位置。

用Sk、Pk和Lk分別代表激光掃描儀在k時刻的掃描數(shù)據(jù)、特征點的集合和特征線的集合。則有

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

(1)

因此掃描匹配問題可以描述為：計算得出2個激光掃描基于點和線特征的位姿變換參數(shù)rk-1、tk-1，以函數(shù)H(r，t)作為評價標(biāo)準，即

(2)

式中：N為掃描點的個數(shù)；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 特征提取

激光掃描儀掃描頻率高、數(shù)據(jù)量大，如果直接對掃描數(shù)據(jù)進行匹配會降低匹配精度；因此處理原始掃描數(shù)據(jù)時，需要提取特征點及特征線。

1.1.1 點特征提取

在室內(nèi)環(huán)境下存在的特征點包括特征線端點、角點，而特征線的端點可以直接從線特征提取中給出；因此點特征提取主要是對角點進行提取。

圖1給出了角點特征的描述：設(shè)Pk為標(biāo)記的角點，角點函數(shù)f(k)=sinθ的大小與點Pk左右兩側(cè)的點及Pk本身所擬合的直線之間的夾角θ有關(guān)。θ越接近90°，f(k)越接近1，說明點Pk成為角點的可能性越高；θ越接近0°或180°，f(k)越接近0，說明點Pk成為角點的可能性越低。角點函數(shù)計算公式為

(3)

圖1 點特征描述

采取非極大值抑制的方法，對于點pk及其一鄰域D={pi|i∈(k-m，k+m)}，若點pk所對應(yīng)的角點函數(shù)值在其鄰域中為最大值，則保留pk的角點函數(shù)值f(k)，獲取特征點。反之，則令f(k)取特定值，本文中取0值。非極大值抑制公式為

(4)

圖2給出了角點函數(shù)計算的結(jié)果：f(k)為角點函數(shù)；k為提取的角點編號。圖3給出了角點函數(shù)非極大值抑制結(jié)果：f(k)為角點函數(shù)非極大值抑制結(jié)果。

圖2 角點函數(shù)計算結(jié)果

圖3 非極大值抑制結(jié)果

如圖2、圖3所示：角點函數(shù)計算結(jié)果中存在大量與極大值相近的非極大值點，從而對角點提取的唯一性造成極大困難；非極大值抑制結(jié)果更好地保證了角點特征提取的唯一性。

1.1.2 線特征提取

目前最普遍的線特征提取方法主要有：文獻[9]介紹的分裂聚合(Split-and-Merge)算法、文獻[10]介紹的線跟蹤(line tracking，LT)算法和文獻[11]介紹的隨機抽樣一致(random sample consensus，RANSAC)算法。然而，經(jīng)典的線提取方法存在一定的誤差。本文提出一種基于距離直方圖的特征提取評定算法。對分裂聚合算法所提取的特征線進行評價，剔除異常特征線。該方法在提高線特征提取精確的基礎(chǔ)上，可以直觀地對特征線進行描述。

距離直方圖線特征提取評價算法描述：采用點特征提取將原始掃描數(shù)據(jù)分成多組掃描數(shù)據(jù)，分別連接各組掃描數(shù)據(jù)的首末特征點，初步獲取多組特征線；通過提取第i條特征線fl(k，i)∈Lk中的3個點，將該特征線分成4等分，如圖4所示，距離直方圖橫軸表示待監(jiān)測點與特征點之間的距離，單位為cm，縱軸表示點的個數(shù)；檢索每個特征點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)算法為目前最普遍的特征匹配算法，然而該方法需要給出初始對應(yīng)關(guān)系Q={Rq，Tq}來估計下一幀坐標(biāo)，其中Rq為旋轉(zhuǎn)矩陣，Tq為距離改正矩陣，下標(biāo)q表示對應(yīng)關(guān)系。通過計算下一幀每個點的實際坐標(biāo)與估計坐標(biāo)之間的歐氏距離來更新初始對應(yīng)關(guān)系，進行多次迭代，直到對應(yīng)關(guān)系Q基本穩(wěn)定不再有大的變化；因此該方法高度依賴高精度的初始對應(yīng)關(guān)系(先驗信息)，先驗信息選取的好壞直接影響迭代的次數(shù)。

本文提出改進的點線特征匹配算法是直接根據(jù)點、線2方面特征進行匹配，通過判斷對應(yīng)點、線特征之間的距離值來分析匹配結(jié)果的好壞。若計算求得的距離差值越小，則說明匹配的精度越高；距離差值越大，則說明匹配精度越低。該方法不需要先驗信息，提高了特征匹配實際應(yīng)用方面的能力，降低了特征匹配的復(fù)雜程度。

1.2.1 特征點匹配

從連續(xù)2個激光掃描數(shù)據(jù)Sk-1和Sk中提取特征點集合為Pk-1和Pk，計算特征點fp(k-1，i)與Pk中所有點的匹配程度為

(5)

式中：s.t.表示限制條件；dp(·)為點描述符；‖·‖代表歐氏距離。

算法描述：計算連續(xù)2個激光掃描數(shù)據(jù)的scorep值，待匹配點fp(k，i)與特征線fp(k-1，i)中對應(yīng)點間的scorep值越接近，則獲取的fp(k-1，i)匹配點fp(k，i)∈Pk越精確。

1.2.2 特征線匹配

從連續(xù)2個激光掃描數(shù)據(jù)Sk-1和Sk中提取特征線集合Lk-1和Lk，需要計算特征線fl(k-1，i)與Lk中所有線的匹配程度為

(6)

式中：dl(·)為線描述符；scorel描述特征線fl(k-1，i)與Lk中fl(k，i)線中所有對應(yīng)點間的距離。

傳統(tǒng)的特征線匹配算法在應(yīng)用中的難點在于如何查找相應(yīng)的2組關(guān)聯(lián)線。解決的方法往往是通過大量掃描數(shù)據(jù)計算減少匹配過程中出現(xiàn)的誤差；因此本文對特征線匹配算法進行了改進。由于在特征線匹配時只存在著剛體變換，因此長度是不變量。若篩選出的匹配線與特征線的長度不同，則會直接影響匹配精度。在特征線匹配前，首先判斷篩選出的特征線與待匹配線之間的長度是否相同，若特征線與待匹配線長度相同，則進行特征線匹配；若特征線與待匹配線長度不同，則需重新篩選特征線。經(jīng)該方法篩選得到的待匹配線fl(k，i)與特征線fl(k-1，i)中對應(yīng)點間的scorel值越接近，說明線的匹配程度越高。該方法在保證特征匹配精度的前提下，減少了匹配算法運算量。

2 實驗及結(jié)果分析

為了驗證掃描匹配室內(nèi)定位方法的有效性，進行了1次實驗。在本次實驗中，二維激光雷達硬件采用RoboPeak團隊開發(fā)的RPLIDAR模塊，其測距精度為0.2 cm，測量范圍6 m，掃描范圍360°，實驗平臺硬件連接如圖5所示。掃描儀將探測到的數(shù)據(jù)以5.5 Hz的頻率，通過USB接口傳送給筆記本電腦，實時顯示結(jié)果并保存相關(guān)數(shù)據(jù)。圖6為該實驗的走廊，圖7為該實驗的室內(nèi)環(huán)境。

圖5 移動平臺

圖6 走廊

圖7 室內(nèi)環(huán)境

采用角點提取算法與分裂聚合算法對圖7室內(nèi)部分環(huán)境特征進行提取。采用距離直方圖的方法進行線特征提取精度評定，圖8(a)中圓圈為所提取的特征點，圖8(b)中的黑實線為采用距離直方圖法提取的特征線，選取圖8(b)中的1、2、3號3條特征線進行評價。圖9給出了線特征提取評價結(jié)果：特征線提取結(jié)果趨于穩(wěn)定；提取的精度范圍在2～4 cm。

圖10給出了分別采用經(jīng)典ICP算法、無先驗信息的ICP算法以及本文改進的掃描匹配3種算法的匹配結(jié)果。其中無先驗信息的ICP算法是在經(jīng)典ICP算法中減少了先驗信息提取的步驟。

圖中方法1為無先驗信息的ICP算法；方法2為經(jīng)典ICP算法。結(jié)果表明，由于點線特征匹配算法無需先驗信息，因此：1)算法復(fù)雜程度方面，本文方法與方法1相同，比方法2效率高；2)算法匹配結(jié)果方面，3種方法誤差都比較穩(wěn)定，改進的特征匹配算法精度與方法2相近，比方法1準確。

圖8 特征提取結(jié)果

圖9 線特征提取精度評定

圖10 特征匹配精度評定

本文提出的改進的激光掃描匹配算法在保證精度的同時無需先驗信息，降低了特征匹配的復(fù)雜程度。

圖11給出了改進的激光掃描同步建圖與定位(simultaneous localization and mapping，SLAM)匹配算法結(jié)果的示意圖。圖中實線為移動平臺的軌跡；圓圈與方塊部分分別表示圖4中的門與消火栓。

圖11 同步建圖與定位結(jié)果

表1 激光掃描匹配定位結(jié)果

3 結(jié)束語

GNSS可以在室外進行高精度的定位；而在室內(nèi)環(huán)境下，需要采用其他系統(tǒng)來替代GNSS：激光掃描儀是一個較好的選擇。本文針對激光掃描匹配的室內(nèi)定位方法進行研究：采用距離直方圖的方法，對提取的特征進行評價，減少了因特征提取錯誤造成的匹配誤差；并提出基于改進的特征匹配算法，該方法不需要提供先驗信息，在保證定位精度的情況下，降低了特征匹配的復(fù)雜度。實驗結(jié)果表明改進的激光掃描匹配室內(nèi)定位方法具有cm級的精度，穩(wěn)定性更高、可靠性更強，適用于較高精度的室內(nèi)定位。

[1] 李星星.GNSS精密單點定位及非差模糊度快速確定方法研究[D].武漢：武漢大學(xué)，2013.

[2] VALIENTE D，GIL A，F(xiàn)ERNANDEZ 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] 溫熙，郭杭.室內(nèi)移動機器人自定位方法[J].測繪科學(xué)，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，F(xiàn)uxin，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

國家重點研發(fā)計劃項目(2016YFC0803102)；遼寧省高等學(xué)校創(chuàng)新團隊項目(LT2015013)。

郭哲(1992—)，男，山東青州人，碩士研究生，研究方向為二維激光雷達室內(nèi)定位。

郭哲，徐愛功，隋心，等.激光掃描匹配室內(nèi)定位方法探討[J].導(dǎo)航定位學(xué)報，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