基于點云聚類評估的激光雷達魯棒定位方法

方 瑋，賴際舟，呂 品，鄭國慶，溫燁貝

(南京航空航天大學自動化學院，南京 211106)

0 引言

工業無人車是一種具有自主定位、感知、規劃和運行能力的智能機器人。目前，在園區巡檢、無人物流等方面有著越來越廣泛的應用。導航定位作為無人車自主運行的前提，其精度直接影響了無人車運行的可靠性以及任務完成能力。

目前,無人車大多依賴衛星進行定位，因此只能運行于衛星信號良好的開闊環境。然而,隨著需求的多樣化，無人車需要在一些復雜環境中(例如工業園區內)進行作業。該環境內由于受到建筑物、樹木等的遮擋，衛星信號的質量難以得到保障，從而影響了無人車的定位精度。對此，一般采取的做法是增加其他類型的傳感器，例如慣性測量單元(Inertial Measurement Unit，IMU)、輪式編碼里程計、激光雷達(Light Detection and Ranging，LiDAR)、視覺傳感器等，最終通過融合算法獲取最終的載體位置和姿態。

激光雷達由于不依賴環境光照、測距精度高的特點，相較于視覺傳感器有著更高的穩定性和精度，因此逐漸成為無人車和無人機導航主要的傳感器[1-2]。通過激光雷達同步定位與構圖(Simultaneous Localization and Mapping，SLAM)的方法對無人車進行定位，在未發生回環的情況下定位誤差會隨行駛里程而發散。近年來，基于先驗地圖的激光雷達定位方法成為無人車定位的主要方式之一，通過事先構建環境的全局無漂地圖，無人車將激光雷達點云實時匹配至先驗地圖，從而可以消除激光雷達里程計的漂移，定位精度可以達到厘米級。

Zhang J.等[3-4]提出了基于激光雷達的SLAM框架LOAM(Lidar Odometry and Mapping)，首先基于曲率計算公式提取一幀點云中的角點和平面點，然后針對提取的特征點分別構建了點到直線以及點到平面的距離誤差函數，最終采用Levenberg-Marquardt法最小化距離誤差函數以獲取載體的位姿。Shan T.等[5]在LOAM的基礎上，針對地面車輛的應用場景進行了改進，提出了LeGO-LOAM框架，首先利用地面點云估計車輛在橫滾、俯仰和高度方向的運動變化，然后在此基礎上利用其他非地面點云估計另外三個自由度的變化，最終得到車輛的六自由度運動信息，相較于LOAM，它的精度相當但實時性更好。

不同于衛星的幾何式定位，IMU、里程計、激光雷達或者視覺都是通過累積每兩個時刻之間的載體位姿變化進行遞推式的定位，因此不可避免地會存在累積誤差。消除累積誤差的方法是在定位過程中融合全局信息，通常采用位置信息已知的路標或者地圖提供全局的位置信息。R.Mur-Artal等[6]提出了一種基于ORB(Oriented FAST and BRIEF)特征[7]的視覺里程計，但其對環境光照條件較為敏感。為了解決該問題，文獻[8]首先使用激光雷達構建了全局的先驗地圖，然后基于該地圖，提出了一種混合光束平差法獲取當前相機的位姿。文獻[9]同樣使用激光雷達構建了場景地圖作為先驗信息，但其使用相機的測量匹配至激光雷達生成的地圖以獲取載體位姿。Koide等[10]使用基于圖優化的SLAM方法融合激光雷達和全球定位系統(Global Positioning System，GPS)量測，前端使用正態分布變換(Normal Distributions Transform，NDT)[11]方法進行激光雷達點云的掃描匹配，獲取激光雷達里程計信息，在后端采用GPS信息作為位姿圖的約束，最終通過因子圖融合的方式獲取載體位姿。Zhu Y.等[12]前端采用LOAM[3]進行點云匹配，后端基于g2o庫[13]融合激光雷達和GPS量測，首先構建了場景的點云地圖，然后采用迭代最近點(Iterative Closest Point，ICP)[14]方法將激光雷達點云匹配至該地圖對無人車進行定位，定位精度達到10cm。N.Steinke等[15]通過提取激光雷達點云中的線、面特征匹配至地理數據庫，實現了厘米級的定位精度，但該方法需要首先獲取目標區域內的地理數據庫。相較于LOAM中簡單的點特征，文獻[16]通過提取結構性強、區分度高的線、面、角等特征，在將點云匹配至先驗地圖時得到了更精確和魯棒的定位結果。

然而，當前的激光雷達定位方案僅僅通過點層次的幾何特征來篩選用于匹配的點云，一方面易受激光雷達測量噪聲的影響；另一方面，在動態物體較多的環境中，也無法區分動態點云和靜態點云，從而影響最終的定位精度。本文從聚類的層次考慮，對點云聚類的整體誤差進行評估，從而不易受到測量噪聲的影響，同時可以將對應于動態物體的點云進行整體性地辨識和移除，從而提高了最終的定位精度。

此外，在車輛實際運行過程中，也會存在很多意外情況導致點云匹配結果出錯，例如車輛線/角速度變化過快導致點云匹配初值偏差過大而無法收斂至正確解，傳感器視野大面積受遮擋導致用于匹配的有效點過少而無法正確解算位姿變換，傳感器數據異常導致的解算異常，環境變化過大導致實時點云和地圖點云之間相差過大而無法正確匹配至地圖。這些情況下點云匹配常常會收斂至錯誤解導致無人車的定位結果完全錯誤，如果無法及時發現并做出應對措施，可能會導致無人車運行至錯誤路線，甚至導致無人車發生碰撞等較為嚴重的事故。因此，如何評估點云匹配結果是否正確，對于無人車的穩定運行是必不可少的。

基于以上考慮，本文提出了一種動態環境下基于先驗地圖的三維激光雷達魯棒定位方法，主要包含以下工作：

1)提出了一種基于角度和距離雙閾值的點云深度圖像分割方法，相較于傳統僅基于角度或者距離閾值的分割方法，分割結果對點云噪聲更為魯棒；

2)提出了一種基于點云聚類評估的魯棒點云匹配方法，首先對原始掃描點云進行聚類分割，然后對點云聚類的匹配度進行評估，通過剔除匹配度較差的聚類，保留匹配度較好的聚類進行匹配定位，提高了在動態場景中點云匹配定位結果的精度；

3)提出了一種基于聚類評估的兩階段點云匹配結果評估方法，第一個階段首先評估每一個聚類的匹配度，第二個階段基于所有聚類的匹配度判斷匹配結果成功或者失敗，相較于傳統的評估方法，提高了評估結果的準確度；

4)構建了面向動態場景的基于先驗地圖的三維激光雷達魯棒點云匹配與容錯定位系統，并在公開數據集和實際試驗中驗證了本文方法的有效性。

1 動態環境下基于點云聚類評估的三維激光雷達魯棒定位方法

本文提出的動態環境下基于點云聚類評估的三維激光雷達魯棒定位方法，相較于傳統方法僅僅只是提取特征點并進行點云匹配的簡單過程，增加了點云聚類分割以及基于分割后聚類匹配度評估的二次匹配過程，使得匹配結果更為準確。方法主要分為點云分割、點云匹配I、聚類評估、點云匹配II、匹配評估、全局位姿初始化六個模塊(見圖1)。相較于傳統方法(如LOAM)只是從原始點云提取特征點后直接匹配至地圖點云的方式，本文提出的方法首先將原始掃描點云分割為地面點和若干聚類，經降采樣后通過點云匹配I模塊將掃描點云匹配至地圖，然后通過聚類評估模塊對每個點云聚類的匹配度進行評估，通過點云匹配II對地面點和匹配度良好的聚類點云再次進行匹配，得到最終的位姿估計。通過這樣的兩階段匹配方法，剔除原始點云中匹配不良的動態點或是噪點，從而可以獲得更精確的匹配結果。最后，通過匹配評估模塊對點云匹配的結果進行評估，判斷匹配結果是否出錯，若正確，則繼續進行下一幀點云的匹配，若錯誤，則通過全局位姿初始化模塊重新初始化傳感器的位姿。相較于傳統的激光雷達定位框架無法在定位結果錯誤后自行修正的現狀，本文算法通過計算掃描點云和地圖點云之間的匹配度，從而對匹配結果的正確性進行評估，并在匹配錯誤時通過全局重定位進行修正。

1.1 基于角度和距離雙閾值的點云深度圖像分割

點云分割模塊從每一幀點云中提取出地面點云和分別屬于每一個物體的點云，并剔除其他無法被聚為一類的單獨的點。

定義FL為激光雷達坐標系，其原點OL位于激光雷達中心，其三軸XL、YL、ZL分別指向激光雷達的前、左、上。記t時刻獲取的一幀原始激光雷達點云為Pt={pi},i=1,2,…,n，其中，n為一幀點云的總點數，pi=(xi,yi,zi)T為每一個點在FL下的三維坐標。首先，類似于LeGO-LOAM[5]，將原始的激光雷達點云Pt投影至深度圖像It。

對于地面無人車來說，地面是一種穩定的特征，但由于激光雷達掃描點云的稀疏性，深度圖像It同一列內的相鄰像素點對應到實際地面上的兩點相距很遠。因此，直接基于歐式距離準則的方法難以穩定地對地面點進行分割，類似于文獻[5]和文獻[17]，本文基于深度圖像It，采用如下的閾值分割方法提取地面點：

對于It中的任意一點It(ui,vi)，其對應的FL系下的三維點坐標pi=(xi,yi,zi)T，并且設與其處于It中同一列的上一個點It(ui,vi-1)對應的三維點坐標pup=(xup,yup,zup)T，與其處于It中同一列的下一個點It(ui,vi+1)對應的三維點坐標pdown=(xdown,ydown,zdown)T，如果滿足如下的判斷條件，則將其標記為地面點

|zi-hg|<δgh

(1)

(2)

(3)

其中，式(1)保證了只有在一定高度范圍內的點才會被標記為地面點；hg代表激光雷達相對于地面的高度；δgh、δgα代表了地面的平坦度；式(2)和式(3)保證了只有高度的梯度變化在一定范圍內的點才會被標記為地面點，設提取的地面點集為Gt。

圖2所示為一室外復雜場景中各方法的地面點提取效果對比，圖2(a)為激光雷達原始掃描點云，其中藍色框內為地面凸起的路旁人行道，黃色框內為較低矮的灌木叢，紅色圈內為路上的行人和車輛；圖2(b)為本文方法的地面點提取結果；圖2(c)、圖2(d)分別為文獻[5]和基于隨機抽樣一致(Random Sample Consensus, RANSAC)平面擬合的地面點提取結果?？梢钥吹?，圖2(c)中文獻[5]采用方法提取的地面點中包含了較多的灌木叢返回的點云，一部分行人、車輛返回的點云也被包含在了地面點中；圖2(d)中基于RANSAC平面擬合方法提取的地面點雖然剔除了行人、車輛和灌木叢的點云，但其只提取出了地面較為平坦的部分，未提取到藍色框內的地面凸起部分；而本文方法通過綜合考慮地面的平坦度和激光雷達安裝高度，在剔除行人、車輛和灌木叢的點云的同時，成功提取了凸起部分的地面點云，取得了更準確的地面點分割效果。

圖2 各方法地面點提取效果對比Fig.2 Comparison of ground points extraction effects of the methods

地面點分割后，不同于文獻[18]中僅基于角度閾值的分割方法，本文對區域增長的判斷準則上同時設定了角度和距離閾值，做了如下改進：

(4)

(5)

其中，式(4)為文獻[18]中的判斷準則，該準則考慮了掃描點隨距離的增大而變稀疏的非均勻特點，通過相鄰掃描點間距和掃描距離的比值作為區域增長的判斷條件，有效地緩解了由于點云稀疏性而引起的過分割現象。然而，在掃描距離較短時，β值的大小會顯著受到激光雷達測距誤差的影響，常常會由于測距誤差而導致過分割，因此本文加入了式(5)作為另一判斷準則，如果兩點之間的歐氏距離小于一定閾值，同樣也被標記為同一聚類，即只需滿足式(4)或式(5)之一，(ui,vi)和(uj,vj)即被標記為同一聚類。圖3展現了改進前后分割結果的對比，其中不同的顏色代表不同的聚類，要注意的是這里展示的分割結果已經經過了上述地面點提取的過程，圖3(a)和圖3(b)分別為同一場景改進前后的分割結果，可以看到在對具有一定噪聲的物體(如植被)進行分割后，圖3(b)相對于圖3(a)獲得了更完整和連續的分割結果(圖3(a)、圖3(b)中藍色方框內)；圖3(c)和圖3(d)分別為另一場景改進前后的分割結果，右下角的卡車對應的點云在圖3(c)中被分割為了若干個聚類，而在圖3(d)中則被分割為了一個整體(圖3(c)、圖3(d)中藍色方框內)。

圖3 點云分割結果對比Fig.3 Comparison of point cloud segmentation results

圖3(a)、圖3(b)為同一幀原始掃描點云改進前后的分割結果，圖3(c)、圖3(d)為另一幀原始掃描點云改進前后的分割結果，注意這里展示的分割結果不包含地面點云。圖3(a)、圖3(b)中的藍色方框內為一排相連植被返回的點云，盡管相較于墻面等的平坦表面具有更大的起伏，但仍屬于一個物體，可以看到圖3(a)中改進前算法將該部分點云分割為了許多細小的點云聚類，但圖3(b)中改進后算法則成功地將其分割為了一個完整的點云聚類。圖3(c)、圖3(d)中的藍色方框內為一輛卡車返回的點云，圖3(c)中改進前算法將同屬于一輛卡車的點云分割為了若干的點云聚類，而圖3(d)中改進后算法則分割為了一個完整的點云聚類。

1.2 基于殘差卡方檢驗的點云聚類匹配度評估方法

由于地圖變化或環境中存在動態物體的情況，這些變化或動態的物體所返回的點云會對匹配結果產生不良影響，最終影響位姿估計的精度。因此，本文在初次點云匹配的結果下，對每一聚類的匹配良好度進行評估，剔除匹配不良的聚類，之后在點云匹配II模塊中對剩余匹配良好的聚類點云進行二次匹配，進一步優化位姿估計。

上文提到，原始點云Pt經過點云分割模塊處理后，被分割為地面點集以及若干個聚類，Pt={Gt,Ci,t},i=1,2,…,k，對于任一聚類Ci,t(不包含地面點集)，通過如下的卡方檢驗步驟評估其匹配良好度：

(6)

(7)

在本文中，取α=0.05。

1.3 基于點云聚類評估的魯棒點云匹配方法

相較于傳統點云匹配方法直接從原始點云Pt中提取特征點進行匹配，本文方法僅使用地面點以及滿足聚類評估模塊檢驗的聚類所包含的點，通過剔除匹配度較差的聚類進行進一步的點云匹配，以優化位姿估計結果，具體步驟如下：

設分割后點云Pt={Gt,{Ci,t}},i=1,2,…,k是地面點以及所有分割后的聚類所包含的點，Gt為地面點云，Ci,t表示第i個聚類的點云，k為聚類個數。

首先，類似于LOAM，從Pt中提取特征點構建點到面的距離殘差，通過高斯牛頓迭代得到粗匹配位姿Trough。將Pt通過粗匹配位姿Trough進行變換得到Pt,Tr。

Pt,Tr=Trough·Pt

(8)

1.4 基于聚類評估的兩階段點云匹配結果評估方法

相較于傳統僅基于單個點對的距離設定閾值的匹配評估方法，本文提出了一種基于聚類評估的兩階段點云匹配結果評估方法。第一個階段首先評估每一個聚類的匹配度，如果一個聚類匹配成功，就認為該聚類內的所有點匹配成功，反之則聚類內的所有點匹配失敗;第二個階段基于所有聚類的匹配度判斷匹配結果成功或者失敗，計算所有成功匹配的聚類包含的點數與總點數的比值，具體過程如下。

經過點云匹配II模塊后，通過將滿足檢驗評估的聚類點云匹配至地圖得到傳感器估計位姿Tfine，然后按照聚類評估模塊的步驟，首先對地面點集進行同樣的評估，如果評估失敗，即地面點集匹配度較差，則直接判定點云匹配結果錯誤；如果地面點集匹配度良好，即在地面點集通過檢驗評估的前提下，再進行如下的評估步驟：

(9)

如果δ低于設定閾值δ0，則判定點云匹配結果錯誤。本文設置δ0為0.5。

在判定點云匹配結果錯誤后，嘗試重新初始化傳感器的位姿，本文采用一種基于激光掃描描述子的方法[19]對傳感器的位姿進行全局初始化，通過在構建地圖時存儲下各位置處的掃描描述子構成描述子庫，再在地圖的描述子庫中搜尋與當前掃描描述子最相似的描述子對應的位置，即為全局初始化的結果，關于描述子的構建方法可以參考文獻[19]，在此不做贅述。

2 驗證與分析

為驗證本文所提定位框架相對于傳統匹配方法在定位精度上的提升，以及匹配評估方法相對于傳統評估方法的優勢，在公開數據集和實際試驗中都進行了驗證。算法運行的配置如下，選擇Intel CORE i9 8950處理器和8GB的內存，算法基于C++實現，運行環境為Linux Ubuntu下的機器人操作系統ROS。

本文用于對比的方法為LOAM和Lio-sam[20]，不同的是去除了LOAM和Lio-sam中的地圖更新部分，改為和已知先驗地圖匹配；用于對比的傳統點云匹配評估方法為基于點云匹配殘差的評估方法，以及另一種基于點云匹配成功率的方法，它們的評估指標分別計算如下：

設P、Q為兩幀待匹配點云，經點云匹配將P匹配至Q后，獲得了P到Q的位姿估計T，設P中總點數為n，配對成功的m對點對為{pi,qi},i=1,2,…,m。其中pi與qi已形成對應關系，滿足|T*pi-qi|

(10)

若ε值小于設定的閾值εthre，ε<εthre，則認為匹配成功，否則匹配失敗?；邳c云匹配成功率的方法計算P中成功匹配的點數占P中總點數的比例

(11)

若匹配成功的點數的比例η大于設定的閾值ηthre，η>ηthre，則認為匹配成功，否則匹配失敗。

2.1 數據集驗證與分析

本文采用文獻[21]中公開的動態環境數據集，該數據集包含了室內大廳、火車站、步行區等各種存在大量行人的環境，通過手持Ouster OS1-64激光雷達以10Hz的頻率采集點云序列，選取了其中在步行區采集的數據驗證本文算法。圖4中展示了其中的一幀掃描點云的強度圖像，用紅色標注的點為場景中的行人返回的點云。

圖4 數據集Ouster激光雷達掃描點云強度圖像Fig.4 Intensity image by Ouster lidar scan in dataset

首先通過因子圖[20]融合點云數據與參考真值構建場景的先驗地圖(圖5中白色點云)，然后將激光雷達實時點云匹配至先驗地圖獲取傳感器位姿。圖5中藍色點云為激光雷達原始掃描點云，紅色點云為通過本文算法提取的匹配度良好的點云，可以看到，本文算法提取的紅色點云中已經不包含行人返回的點云(圖5中黃色方框內的藍色點云)，成功剔除了場景中動態行人的點云，因此定位結果相較于LOAM和Lio-sam更為平穩和準確。

圖5 數據集點云地圖(白色)和實時點云(紅色和藍色)Fig.5 Point cloud map (white) and real-time point cloud (red and blue) in dataset

圖6展示了LOAM、Lio-sam和本文所提方法的定位結果的誤差對比，表1列出了均方根誤差(Root Mean Square Error，RMSE)的對比結果，可以看到，相較于LOAM，本文算法的定位誤差減小了近50%，相較于Lio-sam，本文算法的定位誤差減小了27.5%。

(a) X軸誤差

表1 定位均方根誤差(RMSE)對比

2.2 試驗驗證與分析

2.2.1 匹配定位試驗

本文選擇的試驗場地為南航校園主干道，無人車搭載有Velodyne VLP-16激光雷達傳感器和實時差分GPS(RTK)，RTK用作定位基準，以及輔助構建全局無漂的先驗地圖(見圖7)。通過因子圖[21]融合激光雷達和RTK構建先驗地圖(圖8(b))。無人車的行進路線在圖8(a)中用藍線表示。

(a) 無人車定位驗證平臺

(a) 試驗場景鳥瞰圖

圖9展示了本文算法對點云的篩選作用，圖9(a)中的白色點為地圖點云，彩色點為聚類后的點云(除地面點)，每種顏色代表一個聚類；圖9(b)中的藍色點云為圖9(a)中所有聚類構成的點云，紅色點云為經過聚類評估模塊提取的點云，可以看到本文算法成功地剔除了掃描點云中和地圖不匹配的部分。

(a) 點云分割結果

圖10展示了LOAM、Lio-sam和本文所提改進匹配方法的定位結果的誤差對比，表2列出了均方根誤差(RMSE)的對比結果，可以看到，在剔除了匹配度較差的聚類點云后，相較于LOAM，本文算法的定位誤差減小了近70%，相較于Lio-sam，本文算法的定位誤差減小了48%。

(a) 東向誤差

表2 試驗場景下定位均方根誤差(RMSE)對比

2.2.2 算法運行時間分析

圖11所示為本文算法各主要模塊運行時間，表3列出了各模塊運行的平均時間，可以看到，加入的點云分割模塊平均耗時為6.59ms，聚類評估模塊平均耗時為4.54ms，點云匹配I和II的總時間為62.58ms，算法總體平均運行時間為73.71ms，激光雷達掃描頻率為10Hz，因此本文算法可以實時處理每幀點云數據。

(a)

表3 各模塊運行平均時間

2.2.3 匹配評估試驗

本文選取了8個典型場景對比本文匹配評估方法和上述兩種傳統方法，如圖12所示，1～4號為匹配成功的情況，5～8號為匹配失敗的情況，表4列出了對應的評估指標的計算值。在諸如1號的一般情形下，三種方法均能正確進行評估；然而在2、3、4中，當傳感器附近經過大型車輛，視野被大量遮擋或環境點云較為嘈雜時，基于殘差或匹配率的方法出現了較大波動，而本文算法則受影響較小。5～8中模擬了點云匹配失敗的情形，在5中當點云和環境差別較大時，三種方法均能正確評估出匹配失??；然而在6、7、8中，通過將點云從正確位置平移數米模擬匹配失敗的情形中(實際運行過程中通過衛星定位或是其他全局定位算法可能會出現的情況)，誤匹配的點云和環境相似度較高，此時6、7中點云匹配的殘差甚至低于在3、4中的殘差值，7、8中的點云匹配率高于2、4中的匹配率，從而無法正確評估出匹配結果是否成功，而本文算法在匹配失敗時的匹配率明顯小于匹配成功時的匹配率，即使是點云和環境的相似度較高時，也能正確做出評估。圖12中5(b)～8(b)的黃色點云為傳統僅基于距離閾值篩選所提取的成功匹配的點云，紅色點云為本文算法經過聚類評估后提取的成功匹配的點云。可以看到，如果僅僅基于距離閾值進行篩選，哪怕是在匹配錯誤的情況下，依舊有很多點會被認為是正確配對的點云，從而可能出現最終將匹配錯誤的情況誤判斷為正確；但在經過聚類評估后，盡管單獨的某些點在匹配錯誤時可能仍能找到距離較近的點作為對應點，但聚類中所有點的總體誤差在匹配錯誤時通常會顯著變大，從而能夠剔除多數誤匹配的點云，最終可以更為準確地對匹配結果進行評估。

圖12 點云匹配成功(1～4)和失敗(5～8)情況Fig.12 Matching success (1～4) and failure (5～8) situations

表4 圖12中各情況對應評估指標的計算值

3 結論

本文針對動態環境下的激光雷達定位問題，提出了一種基于點云聚類評估的三維激光雷達魯棒定位方法。主要工作為以下幾點：

1)基于雙閾值的區域增長法對點云進行分割聚類，提高了聚類結果對噪聲的魯棒性；

2)通過卡方檢驗剔除誤匹配的動態點云聚類，以提高匹配精度；

3)通過聚類的匹配成功比例，評估匹配結果的正確性。

經過公開數據集與實際試驗驗證，結果表明，相較于傳統匹配方法，本文方法有效提高了在行人、車輛等移動物體較多的動態場景下的定位精度和匹配結果評估的準確度。在后續工作中，將會針對激光雷達全局重定位方法開展研究，以提高無人車全局位姿初始化的速度和準確率。