面向點云退化的隧道環境的無人車激光SLAM方法

李帥鑫，李九人，田 濱，陳 龍，王 力，李廣云

1. 信息工程大學地理空間信息學院，河南 鄭州 450000； 2. 慧拓無限科技有限公司，北京 100089； 3. 中國科學院自動化研究所復雜系統管理與控制國家重點實驗室，北京 100190； 4. 中山大學數據科學與計算機學院，廣東 廣州 510275

在大數據和人工智能等新技術浪潮的席卷下，全球新一輪的科技革命和產業革命已悄然而至，傳統測繪技術的轉型升級勢在必行[1]。打破傳統格局，研究更智能高效的地理信息數據獲取新方法，實現地理信息數據對現實環境的支持和增強，是現代測繪發展的重要方向之一[2]。近年來，自動駕駛產業正以前所未有的速度迅猛發展，這一切都離不開高精度地圖(high definition map,HD Map)的強有力支持。然而，無論是大規模城市環境數據的快速采集還是高精度地圖的生產制備，在傳統測繪技術下都是極富挑戰的任務[3-4]。

激光雷達(light detection and ranging，LiDAR)能夠快速、精準地獲取環境信息，現已被廣泛應用于機器人、無人駕駛和測繪導航等領域，被視為無人系統感知外部環境的“眼睛”。基于LiDAR的SLAM技術具有不受環境遮擋和光照變化影響的特點，能夠很好地彌補全球導航衛星系統(Global Navigation Satellite System，GNSS)的信號在室內環境下的失鎖[5-6]。過去的20年來，已涌現出一批優秀的2D激光SLAM解決方案，如：Gmapping[7]、Hector SLAM[8]、Cartograper[9]、Spline SLAM[10]等，它們已在掃地機器人、倉庫物流車等諸多商業化產品中得以應用。隨著傳感器技術的不斷發展和用戶對產品需求的不斷升級，3D SLAM技術近年來備受關注。文獻[11]提出的LiDAR定位與地圖構建系統(LiDAR odometry and mapping，LOAM)是最經典的3D SLAM系統之一。它采用線和面特征配準點云，并在兩個并行線程上分別以高頻和低頻運行激光里程計與地圖構建模塊，保證了系統的時效性。鑒于LOAM出色的表現，后續的諸多工作均在其基礎上展開，重點是解決一些極端或特殊場景下的挑戰性問題。文獻[12]提出了面向無人小車(unmanned ground vehicle，UGV)的輕量級LOAM(lightweight and ground-optimized LOAM，LeGO-LOAM)，通過優化特征提取并采用兩步法配準點云，降低系統運算量，使其能夠在輕量級運算平臺上實時運行。文獻[13]針對非機械轉動的固態激光雷達提出Livox-LOAM，將LOAM算法擴展到Livox Mid-40 LiDAR的應用中。文獻[14]提出M-LOAM (multi-LiDAR LOAM，M-LOAM)，實現了多LiDAR的同時定位與地圖構建。以上這些方法均是LOAM的擴展，它們都沿用了線面特征匹配方法進行運動估計。還有一些學者嘗試采用其他點云配準方法，如：文獻[15]在SLAM系統中采用經典的迭代最近鄰點(iterative closest point，ICP)算法；文獻[16]采用表面元(surfel)模型構建點云地圖，并采用正態分布變換算法(normal distribution transformation，NDT)實現點到地圖的配準；文獻[17]提出基于Surfel的SLAM方法(surfel-based SLAM，SuMa)，在系統中采用surfel ICP配準點云并利用GPU加速；文獻[18]提出多分辨率柵格(multi-resolution map，MRS-Map)地圖表達方法，并采用概率點集配準算法(Coherent Point Drift，CPD)匹配點云。點云強度能夠反映目標的表面材質，可以幫助系統更全面的感知環境。然而，上述系統都只考慮了點云的幾何信息而忽視了強度信息，信息利用不夠充分。

針對這一問題，文獻[19]將同名點強度差值與空間歐氏距離同時作為約束加入ICP的優化求解中，大大縮減了ICP的迭代次數，從而降低了2D SLAM前端配準的計算消耗。文獻[20]提出一種基于統計方法的LiDAR強度信息標定方法，并在Hector SLAM[8]中加入同名點強度差值約束，試驗結果顯示系統的定位精度有所提升。類似情況，Intensity SLAM[21]構建局部強度柵格圖，在點到局部地圖匹配時融入強度信息。文獻[22]針對LOAM的特征點匹配，提出一種強度差定權方法，為具有相似強度值的同名點賦予更高的權。事實上，在結構性特征明顯的環境下，僅依靠點云幾何信息進行匹配已足夠滿足精度要求，無須消耗更多計算資源加入點云強度信息。而在結構特征較弱但強度特征豐富的環境下，則需要利用強度信息輔助點云匹配。綜上，系統應具備自適應強度特征提取以及強度融合判斷的能力，而這些均是上述方法所缺乏的。

本文針對現有激光SLAM在幾何結構不明顯的隧道環境下存在點云配準退化的問題，在文獻[11]和[12]的基礎上提出一種面向點云退化的隧道環境的無人車激光SLAM方法。本文在以下方面對LOAM進行了優化和改進。

(1) 特征提取部分。一是在LOAM基于點云的特征提取方法基礎上，改進為基于點云柱面投影圖的特征提取方法。相較于在三維空間中對離散點云搜索，在投影圖中點云的空間位置關系更加明確，無須使用樹形結構對點云進行組織和管理。二是提出基于統計方法的自適應強度特征提取方法，能夠根據環境情況自適應提取線、面、地面、反射標志物等4種空間特征。

(2) 地圖構建與位姿優化部分。一是采用基于統計的退化檢測方法，能夠自動評估位姿估計退化情況，并確定退化方向；二是結合無人車的特性提出一種基于三次B樣條柵格圖的強度點云匹配方法對平面方向的退化位姿進行修正。

試驗結果表明，采用本文方法能夠在幾何特征較弱但強度特征豐富的長直隧道內實現無人車的魯棒定位與地圖構建。

1 系統概述

系統接收點云數據，實現自主位姿推估和環境地圖構建。整體分為3個部分：①點云數據處理。從原始點云中分割地面點，對剩余非地面點聚類并從中自適應提取線、面和強度特征點。②激光里程計。將相鄰幀的線、面特征配準，估計車輛的相對運動并推估在地圖中的位姿。③地圖構建。將提取的幾何特征與局部地圖配準修正位姿，檢測位姿估計的退化情況并確定退化方向，采用強度特征匹配修正出現退化的位姿估計，最后更新局部特征地圖。本文系統的整體框架如圖1所示。

圖1 系統框架Fig.1 The overview of the system

2 方法介紹

2.1 點云數據處理

2.1.1 地面分割

結合KITTI數據集[23-24]的處理實例對地面分割算法進行說明，主要步驟如下。

(1)

(2)

圖2 點云投影深度圖實例Fig.2 Example of points-to-depth image projection

(2) 點云分塊。將點云劃分為i個扇形區域Bk,i，并以水平距離ρ劃分為j個扇環Ck,i,j(圖3)

(3)

(3) 直線擬合。為避免噪點影響，采用增量式擬合方法對Bk,i中各Ck,i,j進行直線l=aρ+b擬合，a和b分別表示擬合直線的斜率和截距。首先由Ck,i,1和Ck,i,2擬合直線參數a1和b1，再加入Ck,i,3將直線參數更新為a2和b2，依此類推。若更新后引起擬合直線斜率出現較大變化，則判定Ck,i,3為噪聲數據，將其跳過。最終擬合得到Bk,i的地表直線(如圖3中紅色虛線所示)。

圖3 點云地面分割實例Fig.3 Example of ground points segmentation

rk,n=ak,iρk,n+bk,i-zk,n

(4)

式中，ak,i和bk,i為Bk,i的擬合直線參數。當ak,i和bk,i存在時，將滿足rk,n

2.1.2 特征提取

特征點的自適應提取算法基于深度圖實現，主要步驟如下。

(1) 目標分割。根據上節所述的地面點分割結果得到非地面點深度圖Vk=Dk-Gk。采用基于夾角值的快速目標分割方法將點云聚類，算法細節可參考文獻[25]，聚類后得到如圖4的聚類點云Lk。若Lk中某一類的點數少于閾值nthr，則該類的所有點將作為野點從Pk中剔除。

(2) 線、面特征點提取。在Dk上計算各像素點的粗糙度s，方法與文獻[11]中相同

(5)

式中，d表示深度；Pneig為深度圖上的鄰域像素。遍歷Vk，將滿足s>sE的nE個像素標記為線特征Ek,滿足s

圖4 目標聚類結構結果Fig.4 Result of the object clustering

(3) 強度特征點自適應提取。首先將非地面點投影為強度圖Ik，像素位置與Vk一一對應。為自適應提取局部范圍內的強度特征，將Ik分為M×N個子塊，并統計子塊內各強度區間的占比，建立直方圖,如圖6所示。直方圖橫軸為強度區間，區間間隔ΔI，范圍為[0,255]；縱軸為該強度區間占比。在直方圖中取中值作為該子塊強度特征提取的閾值Ithr，將強度滿足I>Ithr且大于最小閾值I>Imin的像素標記為強度特征Rk。圖6中彩色像素為提取的強度特征，顏色越紅表示反射強度越強。

圖5 強度特征提取結果Fig.5 Result of the extraction of intensity features

圖6 自適應閾值設置Fig.6 Adaptive intensity threshold setting

強度特征提取結果與真實環境的對照見圖5，從中可以看出，環境中存在許多諸如車牌、尾燈、路標、鋼架結構等穩定的強度特征，能夠為點云配準提供良好的約束。

2.2 激光里程計

2.2.1 點云配準

(6)

(7)

(8)

2.2.2 位姿推估

(9)

2.3 地圖構建

2.3.1 地圖配準

(10)

地圖配準僅采用幾何特征，即Ek到MEk和Sk到MSk的配準，與激光里程計中所用方法一致，算法細節可參考文獻[11]。

2.3.2 退化檢測

車載點云配準時，地面點可以提供俯仰角、橫滾角及z軸方向的約束，非地面目標則可以提供偏航角、x軸及y軸方向的約束。在空曠場地或長直隧道等環境下，由于難以建立準確的同名點數據關聯，導致狀態空間上某些方向約束不足，點云配準解算的法方程矩陣呈現病態，產生退化現象。

本文采用文獻[27]中提出的退化因子(degeneracy factor，DF)算法進行退化檢測及退化方向判斷。對于方程Ax=b，退化因子rD定義為一個僅與ATA的特征值λi相關的量

rD=λmin+1

(11)

式中，λmin表示最小特征值。在本文點云配準問題中，A即為目標函數的一階雅克比矩陣

(12)

2.3.3 配準優化

無人車點云數據包含大量地面點，因此對俯仰、橫滾及z方向的約束是充分的，一般不會出現退化問題，而偏航、x及y方向則不然。圖7為長直隧道中相隔約10 m的兩幀點云的俯視圖和側視圖。顯然僅利用隧道幾何特征很難得到車輛前進方向的準確估計，即配準在x方向出現退化。

圖7 點云配準退化Fig.7 The points registration

(13)

(14)

s(τ)=φ(τ)Tcφ(τ)T=vec(bx(x)·by(y))

(15)

點云到地圖的配準可表示為點云在強度概率地圖上對應位置的概率和最大，由此寫出強度目標方程

(16)

采用L-M算法可求解位姿優化量ΔT2D。

(17)

需要說明的是，式(17)并非嚴格意義的向量相加，其中⊕表示對應項相加。

2.3.4 地圖更新

本文采用多尺度地圖模型建立線、面特征的局部地圖MEk和MSk[28]，采用三次B樣條概率地圖模型建立強度特征的局部地圖MLk。局部地圖均以傳感器為中心，隨車輛運動而滑動更新。

圖8為多尺度柵格地圖的示意圖，紅黃藍色柵格分別表示不同尺度的柵格地圖；黑色點表示柵格中的點；LiDAR運動時，地圖隨之移動，地圖中心由Oold向Onew移動。

圖8 局部柵格地圖Fig.8 Local voxel map

(18)

式中，Δl表示采樣間隔。地圖更新時采用遞歸策略更新離散的概率值控制點

(19)

式中，c-和c+分別表示τocc,n對應控制點的先驗和后驗概率；κ為更新因子，對于遮擋點該值取正，對于非遮擋點該值取負。

圖9為隧道內的局部強度地圖實例。紅色和藍色區域分別表示強度地圖的占據和非占據連通區域，顏色越深表示該點置信度越高。綠色點為當前觀測到的強度特征點。局部地圖隨LiDAR而運動，使傳感器始終保持在地圖中心。

圖9 局部B樣條地圖實例Fig.9 Example of local B-spline map

3 試驗結果和分析

3.1 試驗介紹

本文試驗采用實測和仿真數據。仿真數據由16線LiDAR采集，輸出頻率10 Hz；垂直視場為[+15°,-15°]，水平視場為360°；垂直分辨率為2°，水平分辨率為0.2°；測距的最小和最大范圍分別是0.1 m和180 m(如圖10(b))。實測數據由車載Robosense RS-32 LiDAR(如圖11(a))采集，其輸出頻率為10 Hz；垂直視場為[+15°,-25°]，水平視場為360°；垂直分辨率由0.33°至2°，水平分辨率為0.18°；測距的最小和最大范圍分別是0.2 m和200 m；支持輸出各點的三維坐標和反射率。相較于64線LiDAR，32線更專注于車輛行駛區域的點云采集，多用于無人駕駛領域。

圖10 仿真平臺Fig.10 Simulation platform

圖11 實測數據采集平臺Fig.11 Real-world data collection platforms

實測試驗數據在封閉無人駕駛測試場采集，時間約5 min，車輛行駛速度為30～60 km/h，全長約1.5 km并包含一段約150 m的長直隧道。定位軌跡的參考值由高精度GNSS輔助慣性導航系統Oxford Technical Solution(OxTS) RT3000獲得(圖11(b))，其定位精度優于10 cm，可直接輸出通用橫墨卡托格網系統(universal transverse mercator grid system，UTM)下的坐標。需要說明的是，在進行本文所述的激光定位與地圖構建的數據處理過程中，OxTS數據未參與任何形式的數據融合，它僅在試驗中作為位姿估計的參考值用于定量評估軌跡精度。仿真平臺以DARPA地下城市挑戰賽提供的仿真環境為基礎，模擬了真實井下長直隧道的場景(如圖10(a)和(c))。隧道長約500 m，墻壁兩側以30 m的間隔交錯設置反射標志，仿真車輛運動過程中受到的地面摩擦力、傳感器溫濕度等均最大程度地與實際情況保持一致。數據采集時行車速度保持15～20 km/h，共采集1000幀點云數據，車輛在各個時刻的精確位姿可直接仿真輸出。本文所述方法輸出頻率約為5 Hz，運行環境為Ubantu16.04，CPU為Intel Core i5-6300@2.3 GHz，內存8 GB。

3.2 試驗結果和分析

3.2.1 特征提取結果

本文試驗采用實測數據集，線特征提取閾值設置為sE=0.3，面特征提取閾值設置為sS=0.1，強度特征自適應提取的格網化為16×4。如圖12分別為開闊區域，城市道路，綠化帶和狹長封閉隧道環境下的點云及特征提取結果。圖中分別用紅、綠、藍色表示提取的強度、面和線特征，白色為當前幀的全部激光掃描點。線和強度特征點與實際場景的對應物由相應的顏色框框出。如圖12(a)橙色框內的地面斷點，在地面分割的作用下，可防止因其粗糙度s過大而被誤標記為線特征；如圖12(b)，在一般城市道路中，具有十分豐富的強度特征點云，且這些特征往往源自靜態環境；如圖12(c)，隧道中的有效線特征較面特征少很多，但存在穩定的強度特征；如圖12(d)，在目標聚類的作用下，橙色框內散亂的植被點作為散點被剔除，避免因s過大而被誤標記為線特征。

圖12 特征提取結果Fig.12 Result of the features extraction

根據多種場景下的特征點云提取結果可以得出以下結論：①面特征數量往往遠多于線特征數量，尤其在相對開闊的區域。將所有特征聯合解算，如LOAM，可能會存在約束不均衡的問題。②強度特征廣泛存在，并且這些特征往往都是靜態目標，如路標或其他的反光標志等，能夠提供穩定的匹配，不應被忽略。③提出的自適應強度特征提取方法為在弱結構環境下人工加設強度目標提供可能。

3.2.2 退化檢測及時效性結果

本文試驗采用實測數據集，分別分析退化檢測結果和本文方法的時效性。

退化檢測結果：如圖13所示為退化因子及退化檢測示意圖。圖13(a)為DF統計結果，其中橫軸表示DF的統計區間，縱軸表示該區間的占比。可以看出，DF統計數據呈現一定的聚類效應，可取聚類分界值為閾值rthr，本文試驗中取rthr=300。圖13(b)為計算DF的關鍵幀幀號， 縱軸為DF數值。圖13中綠色為檢測到的退化幀，藍色為非退化幀。結合實際數據采集情況，紅色虛線段內的區域為進入隧道的幀段，在這兩段區域均自動判定為退化，與實際情況具有較好的契合度，反映出退化檢測的有效性。

時效性分析：當檢測到退化時，系統將利用強度特征進行2D激光匹配，對LiDAR的位姿估計進一步修正。圖14為發生退化時的數據處理時耗，4組箱線圖分別代表DF計算、基于強度特征的二維激光匹配、LOAM地圖構建線程及本文所提方法的地圖構建線程等部分的耗時情況。由圖可以看出，DF的計算及退化檢測幾乎不耗時，2D激光匹配約需20 ms。LOAM的地圖構建線程數據處理時間約為100 ms，本文所提方法約為120 ms。本文系統的地圖構建線程以不超過5 Hz的頻率接收數據，即該線程的響應間隔為200 ms，低于平均處理時長的120 ms，因此能夠達到實時運行。

圖13 退化因子及退化檢測結果Fig.13 Degeneracy factor detection

圖14 時效性分析Fig.14 Timecosts analysis

3.2.3 軌跡估計結果

本文試驗采用實測數據集，從相對精度和絕對精度的角度分析定位軌跡精度。

在實測數據試驗中，將OxTS與LiDAR的軌跡統一至同一參考系，以OxTS的輸出軌跡為真值，內插對應時刻的LiDAR位姿估計，并采用蒙特卡洛試驗對位姿估計進行定量評價。選擇LOAM和HDL-Graph-SLAM兩個具有代表性的系統為比較對象，各系統分別運行10次取各位姿點平均值，軌跡如圖15所示。

由圖15中可以看出，基于點云NDT匹配的HDL-Graph-SLAM和基于幾何特征匹配的LOAM在隧道內沿車輛前進方向出現不同程度的退化，導致位置估計在該方向出現嚴重偏差。而采用本文方法可以有效修正偏差，得到準確的定位結果。

圖15 定位軌跡對比Fig.15 Comparison of the estimated trajectories

相對定位精度：采用文獻[23]中的相對精度評估方法，相對誤差由一段距離區間內的相對位姿變化估值與真值的差值計算得到，相對誤差曲線如圖16所示。

圖16 固定距離的相對定位誤差Fig.16 Relative errors over the fixed lengths

由圖16可以看出，3種方法在1.5 km的范圍內相對位置誤差均優于0.5%，相對角度誤差優于0.014°/m。本文方法的相對位置誤差明顯優于其余兩者，且在不同距離區間內保持穩定。LOAM和HDL-Graph-SLAM在某些距離區間內誤差有波動，反映出軌跡在某一區域出現嚴重偏移。由于隧道僅為直線，因此并未造成過大的姿態偏差，3種方法的相對姿態曲線差異不大。

絕對軌跡精度：采用均方根誤差(root mean square error,RMSE)作為絕對精度的評價指標，結果見表1。

表1 絕對定位誤差

由于LOAM和HDL-SLAM在隧道中出現點云配準退化情況，分別造成了高達34.654 m和141.498 m的絕對位置誤差。本文算法的定位精度遠高于其余兩者，1.5 km的路徑范圍絕對位置誤差為4.726 m，偏航角誤差0.747°，在該場景下定位精度較LOAM和HDL-SLAM有大幅提升。統計各位姿點絕對誤差得到如圖17的累計誤差分布圖。

圖17 絕對定位誤差累積分布Fig.17 Cumulative distribution of the absolute errors

由圖17中可以看出，在該場景下利用強度信息優化點云配準結果，保證了點云配準在全軌跡下的穩定與可靠。全局范圍內定位誤差逐步累積，最大不超過10 m，姿態誤差最大不超過4°。對比方法的姿態精度與本文所述方法相差不大，但位置精度差異明顯。兩對比方法的曲線并非呈平滑遞增，而是在某些誤差區間內陡增，反映出點云配準在某些區域的異常，這與隧道內點云配準退化的事實相吻合。

3.2.4 相對位姿變換結果

圖18 相對位姿變換誤差Fig.18 Relative transformation errors

由圖18中可以看出，在本文的仿真隧道環境內，3種方法的相對旋轉變換誤差都很很小，整體均小于0.04°，其中本文方法和HDL-Graph-SLAM小于0.01°。說明隧道內的地面和墻壁點云足以保證姿態估計的準確。相對平移變換誤差分層明顯，其中本文方法的平移誤差0.02 m以下，遠小于LOAM的0.4 m和HDL-Graph-SLAM的0.08 m，說明了基于強度特征匹配的正確性和有效性，也反映出在這種退化環境下融合強度特征的必要性。

3.2.5 地圖構建結果

采用實測數據構建點云地圖，通過地圖結果對比可直觀地看出本文方法在隧道環境下性能的提升。僅采用所采集數據的隧道部分，用3種方法分別構建點云地圖，結果如圖19所示。

圖19 隧道點云構建結果Fig.19 Points map mapping on the remote sensing image

通過對比可以明顯看出，HDL-Graph-SLAM和LOAM出現所謂的“長直走廊困境”，導致構建的隧道比實際隧道出現幾十米的偏差(LOAM為168.299 m，HDL-Graph-SLAM為90.952 m)。本文方法通過自適應特征提取方法獲得更多隧道中的強度特征，以此來彌補點云幾何配準時可靠約束的缺失。從地圖構建結果上看，本文方法能夠在該環境下取得較好的結果，整體隧道結構保持完好。需要注意的是，匹配結果的正確必須建立在有足夠的特征的基礎上，因此對于某些極端環境，可通過在墻面張貼反光標志或涂抹反光漆等方式人為設置強度標志，本文提出的自適應特征提取算法同樣能夠識別并提取出這些強度目標用于配準。

為更直觀的展示本文方法構建地圖的準確性，將構建的完整地圖映射在該場景的遙感影像上，如圖20所示。從圖20中可以看出，所構建的地圖結果與實際場景的契合度良好。

圖20 遙感影像映射點云Fig.20 Points map mapping on the remote sensing image

3.3 待改進之處

車輛動態運動時采集的點云數據質量往往存在較大問題，尤其是在車輛快速轉彎時，點云畸變尤為明顯。這可能會導致轉彎后的方向偏移，造成較大累積誤差。如圖15和圖20左側，車輛在十字路口轉彎后出現方向偏差。

4 結 語

本文針對隧道環境下基于激光的車輛定位與地圖構建問題，提出了強度信息增強的激光SLAM系統。該系統通過自適應算法提取環境中的強度特征，并自動化判斷點云配準的退化情況，在幾何匹配的基礎上增加強度特征的匹配，為點云配準提供更多可靠的約束信息，解決隧道環境下點云匹配的退化問題，使系統能夠在長直隧道環境下穩定運行。本文提出的方法是對國際上主流的LOAM系統性能的補充和完善。后期的工作將集中于與IMU的融合方面，為系統提供更多車輛運動狀態的信息，以幫助校正動態條件下點云的畸變，并為點云配準提供可靠的初始估計。