基于幾何特征關聯的室內掃描匹配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）。

廖自威，男，碩士，研究方向為激光雷達數據融合與室內導航。