一種基于激光測距儀的掃描匹配方法

劉 俊，李 霖,2

(1.武漢大學 資源與環境科學學院，湖北 武漢 430079；2.武漢大學 地球空間信息技術協同創新中心，湖北 武漢 430079)

激光測距儀因其快速、成本低、環境適應力強等特點，常被用作移動機器人的外部傳感器[1]，掃描匹配通過計算使相鄰掃描重疊最大的最優剛體變換，以此獲取相鄰時刻的運動量估計，廣泛應用于移動機器人同步定位與環境建圖(Simultaneous Localization and Mapping, SLAM)領域[1-10]。目前應用最廣泛的掃描匹配算法是迭代最近鄰算法(Iterative ClosestPoint, ICP)[1, 11-17]，但該算法對離群掃描點與稀疏掃描點較為敏感，由于缺少實際對應點，這些掃描點容易建立錯誤對應關系。此外，ICP算法根據最近點規則建立對應關系，可能存在一對多和對應距離極端大的問題[12-13]，這會影響最優變換計算的準確性，從而影響匹配的準確性。

Zhang等人提出一種單向對應的方法[13]，通過尋找多個最近點，根據其定義的規則選取某個點作為對應點，從而減少一對多對應，但該方法仍有可能建立對應距離極端大的對應關系。Chetverikov等人提出一種基于截斷思想的迭代最近鄰算法[18]，通過設置截斷比例η，僅保留對應距離前η%的對應關系，從而剔除對應距離極端大的對應，但該方法對η參數較為敏感[19]，若η設置過大，可能保留對應距離極端大的對應，從而影響匹配的準確性，若η設置過小，可能剔除過多的對應關系，導致算法陷入局部最優。Pomerleau等人提出一種基于相對移動量的剔除方法[20]，但當旋轉量較大時，剔除閾值的估算值并不準確[13]。

為了減少掃描數據中的離群掃描點和稀疏掃描點，本文提出一種基于連通格序列的方法，通過對掃描點進行空間網格劃分并建立連通格序列，認為連通度較低的連通格是掃描數據的離群或稀疏部分，予以剔除，為ICP算法提供較為理想的輸入；針對對應關系中的一對多和距離極端大的問題，本文通過建立唯一對應，避免建立一對多對應關系，并基于四分位數法[21]計算對應距離的上截斷點，對應距離超過該截斷點的為錯誤對應的可能性較大，予以剔除，減少錯誤對應數量，從而提高匹配的準確性。

1 ICP算法

ICP算法是目前應用最廣泛的掃描匹配方法，其本質是一個“建立對應關系-求解最優變換”的迭代優化過程[22]，在每次迭代中，尋找參考掃描中最近點作為對應點，建立目標掃描與參考掃描的對應關系，基于該對應關系，求解一個使得目標函數最小的最優變換，并應用于目標掃描，作為下一次迭代的輸入，直到達到收斂條件，最終實現兩幅掃描貼合，并輸出一個最優剛體變換。ICP算法步驟如下：

1)建立對應關系。對于目標掃描中的每一個點，尋找參考掃描中距離最近的掃描點作為對應點，建立對應關系。一般使用K-d樹以加速最近點搜索過程[23]；

2)計算最優變換。計算一個最優變換，使以下目標函數最小，一般使用單位四元數法[23]求解旋轉分量R和平移分量t；

(1)

3)應用最優變換。對目標掃描應用最優變換，得到新點集，作為下一次迭代的輸入。

重復上述步驟，直到滿足收斂條件或迭代次數超出限制。

2 掃描預處理

ICP算法采取點-點對應，對離群掃描點較為敏感，然而，由于室內環境的復雜性，激光測距儀在采集數據時可能會產生離群掃描點，如圖1(b)所示，由于不存在實際對應點，這些離群掃描點容易建立錯誤的對應關系，如圖1(c)所示。此外，在距離激光測距儀較遠的區域，掃描點較為稀疏，這些稀疏掃描點容易建立方向上矛盾的對應關系，如圖1(d)所示。因此，本文提出一種基于連通格序列的方法，通過對掃描點進行空間網格劃分并建立連通格序列，認為連通度較大的連通格序列是掃描數據的主體部分，保留其范圍內的掃描點，認為連通度較小的連通格序列是掃描數據的離群或稀疏部分，剔除其范圍內的掃描點，從而為ICP算法提供較為理想的輸入。

圖1 離群點與稀疏點

首先，對所有掃描點進行空間網格劃分。以傳感器為坐標原點，計算所有掃描點的空間坐標，得到掃描點的最小外接矩形，根據一定的網格邊長進行空間劃分，如圖2(a)所示，網格邊長根據激光測距儀的分辨率與最大掃描距離決定，本文使用UTM-30LX激光測距儀，測量范圍R=30 m，角度分辨率θ=0.25°，網格邊長l=R|sinθ，取值為0.13 m。

接下來，獲取連通格序列。網格分為占據格和空格，占據格內包含若干掃描點，空格內不包含掃描點，如圖2(b)所示。首先，以第一個占據格G0為起點，初始化一個連通格序列，搜索G0的8鄰域范圍是否存在其他占據格G1；若存在，則將G1加入連通格序列，繼續搜索G1的8鄰域范圍是否存在其他占據格；若不存在，該連通格序列建立完畢，以下一個占據格為起點，初始化一個新的連通格序列，重復上述過程。上述步驟一直執行，直到所有占據格都歸屬于某個連通格序列，如圖3(a)所示。

最后，剔除離群掃描點與稀疏掃描點。定義連通格序列包含的占據格的數量為該序列的連通度，主體部分的掃描點較為連續，因此所歸屬的連通格序列的連通度較大，離群掃描點與主體部分距離較遠，稀疏掃描點之間相隔較遠，因此所歸屬的連通格序列的連通度較小。因此，通過剔除連通度較小的連通格序列內的掃描點，可以達到剔除離群掃描點與稀疏掃描點的目標。本文取最小連通度為5，判定連通度小于5的連通格序列內的掃描點為離群掃描點，予以剔除，結果如圖3(b)所示，可以看出，離群點與稀疏掃描點被成功去除，掃描點的主體部分得以保留。

圖2 網格劃分

圖3 掃描預處理

3 改進ICP算法

3.1 唯一對應

ICP算法假設目標掃描中每個點都存在實際對應點，然而，隨著移動機器人探索新環境，以及環境中的動態障礙物等因素，該假設在實際應用中很難成立。例如，移動機器人在經過拐角時，傳感器在上一時刻無法掃描到的障礙物，在當前時刻卻可以掃描到，如圖4(a)和圖4(b)所示。ICP算法依據最近點規則建立對應關系，對于目標掃描中每一個點，選取參考掃描中距離最近的作為對應點，如圖4(c)所示，可以看出，多個掃描點錯誤地對應參考掃描中同一個點，如圖4(d)所示，這是由于這些掃描點不存在實際對應點，仍根據最近點規則建立對應關系所致，這些對應關系會顯著影響最優變換計算的準確性。

因此，本文提出一種建立對應關系的新規則，通過限定每個參考點只能分配給最近的掃描點作為對應點，從而保證對應關系的唯一性，如圖5(a)所示。例如，給定目標掃描中某掃描點pi，尋找參考掃描中距離最近點mj，作為候選對應點，判斷mj是否已被分配給其他掃描點作為對應點，若未分配，則直接作為pi的對應點，若已分配給掃描點px，則比較pi和px到mj的距離，若pi距離更近，則替代px，并更新對應關系，否則跳過pi，不建立對應關系。根據該規則建立對應關系，結果如圖5(b)所示，可以看出，一對多對應問題被成功解決。然而，該方法只能保證對應的唯一性，無法保證正確性，因此仍可能建立少數對應距離極端大的對應關系，在下一節將介紹一種穩健的剔除方法予以剔除。

圖4 根據最近點規則建立的對應關系

圖5 根據新規則建立的對應關系

3.2 剔除對應

上述新規則只能保證對應關系的唯一性，無法保證其正確性，因此仍可能建立少數距離極端大的對應關系，這些對應關系為錯誤對應的可能性較大。因此，本文基于四分位數法計算對應距離的上截斷點，剔除對應距離大于該上截斷點的對應關系。

四分位數法是一種常用的異常值剔除方法，與傳統的標準差法、Z分數法、經驗法相比，具有簡單方便、計算量小、受極端值影響小的優點，該方法將數據升序排列后劃分為四個部分，每個部分包含25%的數據，Q1為第1四分位數(第25百分位數)，Q2為第2四分位數(第50百分位數)，Q3為第3四分位數(第75百分位數)，四分位極差R=Q3-Q1，在此基礎上，該方法定義數據的上截斷點為Q1-1.5×R，下截斷點為Q3+1.5×R，判定大于上截斷點的為極大值，小于下截斷點的為極小值，從數據集中去除。以一組對應關系為例，對應距離的分布如圖6(a)所示，根據四分位數法計算對應關系距離的上截斷點為0.119，可以看出，大于該上截斷點的僅有少數極端大的對應距離，根據該上截斷點進行剔除，結果如圖6(b)所示，可以看出，距離極端大的對應被成功剔除。

由于剔除目標主要是對應距離極端大的對應關系，因此，本文僅計算對應距離的上截斷點dmax，剔除對應距離大于dmax的對應關系。dmax的計算步驟如下：

1)計算指數i。將對應關系按照對應距離升序排列后，根據i=(p/100)×n計算指數i，其中n為對應距離的項數，p為所求的百分位數的位置，例如求解第1四分位數，p為25；

2)計算第1四分位數Q1、第3四分位數Q3和分位數極差R。以計算Q1為例，若其指數i為整數，以第i項與第i+1項的平均值作為Q1的值，若i不為整數，將i向上取整后下標對應的值為Q1的值，同理求出第3分位數Q3。在此基礎上，根據Q3-Q1求出R。

3)計算對應距離的上截斷點dmax。根據四分位數法，上截斷點定義為Q3+1.5×R，根據該定義，求解出對應距離的上截斷點dmax。

圖6 基于四分位數法的剔除

4 試驗結果及分析

4.1 實驗設備

實驗設備由一臺移動小車和一個激光測距儀(UTM-30LX, Hokuyo Automatic Co. Ltd)組成，激光測距儀的最大測量距離為30 m，掃描角度范圍270°，角度分辨率為0.25°，安裝在移動小車前方，對室內環境進行水平掃描，獲取二維掃描數據，如圖7所示。

4.2 實驗過程及成果

4.2.1 局部匹配實驗

為了驗證本文算法對于相鄰時刻掃描的匹配效果，本文設計一組實驗，實驗選取一組相鄰時刻的掃描數據，如圖8(a)和圖8(b)所示，可以看出，當前時刻掃描在矩形1區域中存在離群點，在矩形2區域中存在部分稀疏掃描點。

圖7 實驗設備及環境

為了驗證掃描預處理對于匹配結果的影響，實驗1以當前時刻原始掃描為目標掃描，上一時刻原始掃描為參考掃描，應用ICP算法進行匹配實驗，實驗2對當前時刻掃描進行預處理，以處理后的掃描作為目標掃描，上一時刻原始掃描作為參考掃描，同樣應用ICP算法進行匹配實驗。在建立對應關系階段，實驗1建立的對應關系如圖9(a)所示，可以看出，由于目標掃描中存在離群點，且在參考掃描中缺少其實際對應點，從而產生錯誤對應，實驗2建立的對應關系如圖9(b)所示，可以看出，掃描預處理成功剔除了離群點與部分稀疏掃描點，但仍存在一對多對應和部分距離極端大的對應。實驗1和實驗2的匹配結果如圖9(c)和圖9(d)所示，可以看出，目標掃描與參考掃描間均存在較為明顯的偏離，匹配結果都不理想，這說明掃描預處理僅能減少掃描中的離群點和稀疏點，但無法解決一對多對應和少數對應距離極端大的問題。

圖8 相鄰時刻掃描數據

圖9 實驗1與實驗2

為了驗證ICP改進算法處理一對多對應和距離極端大對應的效果，實驗3與實驗4均以掃描預處理后的當前時刻掃描為目標掃描，以上一時刻原始掃描為參考掃描，實驗3應用ICP標準算法進行匹配，建立的對應關系如圖10(a)所示，可以看出，對應關系中仍存在一對多對應和少數極端大的對應關系，其匹配結果如圖10(b)所示，可以看出，掃描之間存在明顯的偏離，匹配結果并不理想。實驗4應用ICP改進算法進行匹配，建立的對應關系如圖10(c)所示，可以看出，一對多對應和少數極端大的對應關系被去除，其匹配結果如圖10(d)所示，可以看出，目標掃描與參考掃描緊密貼合，匹配結果明顯改善。

4.2.2 全局匹配實驗

為了驗證本文方法在實際應用中的匹配表現，我們使用移動小車與激光測距儀采集了武漢大學化學院三樓部分走廊的掃描數據，在缺少里程計提供匹配初值的條件下，以第一幅掃描數據的坐標系為參考，分別應用ICP標準算法和本文算法，對所有相鄰時刻掃描數據進行匹配實驗，得到實驗結果。ICP算法的匹配結果如圖11(a)所示，可以看出，累計誤差較大，匹配結果在走廊的后半段出現彎曲。本文方法的匹配結果如圖11(b)所示，可以看出，匹配結果未出現明顯彎曲，較好地表達了室內的平面結構，說明本文算法在實際應用中具有較好的匹配表現。

圖10 實驗3與實驗4

圖11 實際應用匹配結果

5 結束語

為了剔除掃描數據中的離群掃描點和稀疏掃描點，本文提出一種基于連續格序列的方法，通過對掃描點進行網格劃分并建立連續格序列，剔除連通度較小的網格內的掃描點，實現掃描數據的預處理，為掃描匹配提供較為理想的輸入，實驗結果表明，該方法能夠有效剔除掃描數據中的離群掃描點和稀疏掃描點。此外，針對匹配過程中產生的一對多對應和距離極端大的對應，本文通過限定對應關系的唯一性，從而避免建立一對多對應，然后基于一種穩健的四分位數法計算距離閾值，剔除對應距離大于閾值的對應關系，從而減少對應距離極端大的對應關系，提高匹配的準確性，實驗結果表明，本文方法能夠較好地匹配相鄰時刻掃描數據，且在實際應用中具有較好的全局匹配表現。

[1] LYU J, YUKINORI K, RAVANKAR A A, et al. A solution to estimate robot motion with large rotation by matching laser scans[C]. Society of Instrument and Control Engineers of Japan, 2015: 1083-1088.

[2] PEDROSA E, PEREIRA A, LAU N. Efficient localization based on scan matching with a continuous likelihood field[C]. IEEE International Conference on Autonomous Robot Systems and Competitions, 2017: 61-66.

[3] 錢曉明, 張浩, 王曉勇,等.基于激光掃描匹配的移動機器人相對定位技術研究[J]. 農業機械學報, 2016, 47(3): 14-21.

[4] WANG X, JIA Y, XI N, et al. Mobile robot pose estimation using laser scan matching based on Fourier Transform[C]. IEEE International Conference on Robotics and Biomimetics, 2014: 474-479.

[5] LIU Y, SUN Y. Mobile robot instant indoor map building and localization using 2D laser scanning data[C]. International Conference on System Science and Engineering, 2012: 339-344.

[6] DIOSI A, KLEEMAN L. Fast Laser Scan Matching using Polar Coordinates[J]. International Journal of Robotics Research, 2007, 26(10): 1125-1153.

[7] MINGUEZ J, MONTESANO L, LAMIRAUX F. Metric-based iterative closest point scan matching for sensor displacement estimation[J]. IEEE Transactions on Robotics, 2006, 22(5): 1047-1054.

[8] DIOSI A, KLEEMAN L. Laser scan matching in polar coordinates with application to SLAM[C]. Ieee/rsj International Conference on Intelligent Robots and Systems, 2005: 3317-3322.

[9] PFISTER S T, KRIECHBAUM K L, ROUMELIOTIS S I, et al. Weighted range sensor matching algorithms for mobile robot displacement estimation[C]. IEEE International Conference on Robotics and Automation, 2002. Proceedings. ICRA, 2002: 1667-1674 vol.2.

[10] BENGTSSON O, BAERVELDT A J. Localization by Matching of Range Scans - Certain or Uncertain?[C]. Eurobot, 2001.

[11] 顧文華, 周波, 戴先中.基于ICP匹配算法的室內移動機器人定位[J]. 華中科技大學學報(自然科學版), 2013, 41(增1): 262-266.

[12] POMERLEAU F, COLAS F, SIEGWART R, et al. Comparing ICP variants on real-world data sets[J]. Autonomous Robots, 2013, 34(3): 133-148.

[13] ZHANG L, CHOI S I, PARK S Y. Robust ICP Registration Using Biunique Correspondence[C]. International Conference on 3d Imaging, Modeling, Processing, Visualization and Transmission, 2011: 80-85.

[14] CENSI A. An ICP variant using a point-to-line metric[C]. IEEE International Conference on Robotics and Automation, 2008: 19-25.

[15] ZHU J, ZHENG N, YUAN Z, et al. Point-to-line metric based Iterative Closest Point with bounded scale[C]. Industrial Electronics and Applications, 2009. Iciea 2009. IEEE Conference on, 2009: 3032-3037.

[16] RUSINKIEWICZ S, LEVOY M. Efficient Variants of the ICP Algorithm[M]. 2001: 145-152.

[17] 洪斌斌, 孟正大.基于MBlCP匹配算法的陪護機器人定位方法[J]. 工業控制計算機, 2015, (9): 42-44.

[18] CHETVERIKOV D, SVIRKO D, STEPANOV D, et al. The Trimmed Iterative Closest Point Algorithm[C]. 16 Th International Conference on Pattern Recognition, 2002: 30545.

[19] ZHU J, MENG D, LI Z, et al. Robust registration of partially overlapping point sets via genetic algorithm with growth operator[J]. Iet Image Processing, 2014, 8(10): 582-590.

[20] POMERLEAU F, COLAS F, FERLAND F, et al. Relative Motion Threshold for Rejection in ICP Registration[M]. Springer Berlin Heidelberg, 2009: 229-238.

[21] 鞏斌, 丁華軍. 在統計分析中如何快捷識別極端值[J]. 統計與咨詢, 2007, (3): 14-15.

[22] 王育堅, 廉騰飛, 吳明明, 等. 基于八叉樹與KD樹索引的點云配準方法[J]. 測繪工程, 2017, 26(8): 35-40.

[23] 劉江, 張旭, 朱繼文. 一種基于K-D樹優化的ICP三維點云配準方法[J]. 測繪工程, 2016, 25(6): 15-18.