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

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

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

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

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

本文針對現(xiàn)有激光SLAM在幾何結(jié)構(gòu)不明顯的隧道環(huán)境下存在點云配準(zhǔn)退化的問題，在文獻(xiàn)[11]和[12]的基礎(chǔ)上提出一種面向點云退化的隧道環(huán)境的無人車激光SLAM方法。本文在以下方面對LOAM進(jìn)行了優(yōu)化和改進(jìn)。

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

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

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

1 系統(tǒng)概述

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

圖1 系統(tǒng)框架Fig.1 The overview of the system

2 方法介紹

2.1 點云數(shù)據(jù)處理

2.1.1 地面分割

結(jié)合KITTI數(shù)據(jù)集[23-24]的處理實例對地面分割算法進(jìn)行說明，主要步驟如下。

(1)

(2)

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

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

(3)

(3) 直線擬合。為避免噪點影響，采用增量式擬合方法對Bk,i中各Ck,i,j進(jìn)行直線l=aρ+b擬合，a和b分別表示擬合直線的斜率和截距。首先由Ck,i,1和Ck,i,2擬合直線參數(shù)a1和b1，再加入Ck,i,3將直線參數(shù)更新為a2和b2，依此類推。若更新后引起擬合直線斜率出現(xiàn)較大變化，則判定Ck,i,3為噪聲數(shù)據(jù)，將其跳過。最終擬合得到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的擬合直線參數(shù)。當(dāng)ak,i和bk,i存在時，將滿足rk,n

2.1.2 特征提取

特征點的自適應(yīng)提取算法基于深度圖實現(xiàn)，主要步驟如下。

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

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

(5)

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

圖4 目標(biāo)聚類結(jié)構(gòu)結(jié)果Fig.4 Result of the object clustering

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

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

圖6 自適應(yīng)閾值設(shè)置Fig.6 Adaptive intensity threshold setting

強度特征提取結(jié)果與真實環(huán)境的對照見圖5，從中可以看出，環(huán)境中存在許多諸如車牌、尾燈、路標(biāo)、鋼架結(jié)構(gòu)等穩(wěn)定的強度特征，能夠為點云配準(zhǔn)提供良好的約束。

2.2 激光里程計

2.2.1 點云配準(zhǔn)

(6)

(7)

(8)

2.2.2 位姿推估

(9)

2.3 地圖構(gòu)建

2.3.1 地圖配準(zhǔn)

(10)

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

2.3.2 退化檢測

車載點云配準(zhǔn)時，地面點可以提供俯仰角、橫滾角及z軸方向的約束，非地面目標(biāo)則可以提供偏航角、x軸及y軸方向的約束。在空曠場地或長直隧道等環(huán)境下，由于難以建立準(zhǔn)確的同名點數(shù)據(jù)關(guān)聯(lián)，導(dǎo)致狀態(tài)空間上某些方向約束不足，點云配準(zhǔn)解算的法方程矩陣呈現(xiàn)病態(tài)，產(chǎn)生退化現(xiàn)象。

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

rD=λmin+1

(11)

式中，λmin表示最小特征值。在本文點云配準(zhǔn)問題中，A即為目標(biāo)函數(shù)的一階雅克比矩陣

(12)

2.3.3 配準(zhǔn)優(yōu)化

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

圖7 點云配準(zhǔn)退化Fig.7 The points registration

(13)

(14)

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

(15)

點云到地圖的配準(zhǔn)可表示為點云在強度概率地圖上對應(yīng)位置的概率和最大，由此寫出強度目標(biāo)方程

(16)

采用L-M算法可求解位姿優(yōu)化量ΔT2D。

(17)

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

2.3.4 地圖更新

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

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

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

(18)

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

(19)

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

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

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

3 試驗結(jié)果和分析

3.1 試驗介紹

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

圖10 仿真平臺Fig.10 Simulation platform

圖11 實測數(shù)據(jù)采集平臺Fig.11 Real-world data collection platforms

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

3.2 試驗結(jié)果和分析

3.2.1 特征提取結(jié)果

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

圖12 特征提取結(jié)果Fig.12 Result of the features extraction

根據(jù)多種場景下的特征點云提取結(jié)果可以得出以下結(jié)論：①面特征數(shù)量往往遠(yuǎn)多于線特征數(shù)量，尤其在相對開闊的區(qū)域。將所有特征聯(lián)合解算，如LOAM，可能會存在約束不均衡的問題。②強度特征廣泛存在，并且這些特征往往都是靜態(tài)目標(biāo)，如路標(biāo)或其他的反光標(biāo)志等，能夠提供穩(wěn)定的匹配，不應(yīng)被忽略。③提出的自適應(yīng)強度特征提取方法為在弱結(jié)構(gòu)環(huán)境下人工加設(shè)強度目標(biāo)提供可能。

3.2.2 退化檢測及時效性結(jié)果

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

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

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

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

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

3.2.3 軌跡估計結(jié)果

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

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

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

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

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

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

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

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

表1 絕對定位誤差

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

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

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

3.2.4 相對位姿變換結(jié)果

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

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

3.2.5 地圖構(gòu)建結(jié)果

采用實測數(shù)據(jù)構(gòu)建點云地圖，通過地圖結(jié)果對比可直觀地看出本文方法在隧道環(huán)境下性能的提升。僅采用所采集數(shù)據(jù)的隧道部分，用3種方法分別構(gòu)建點云地圖，結(jié)果如圖19所示。

圖19 隧道點云構(gòu)建結(jié)果Fig.19 Points map mapping on the remote sensing image

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

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

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

3.3 待改進(jìn)之處

車輛動態(tài)運動時采集的點云數(shù)據(jù)質(zhì)量往往存在較大問題，尤其是在車輛快速轉(zhuǎn)彎時，點云畸變尤為明顯。這可能會導(dǎo)致轉(zhuǎn)彎后的方向偏移，造成較大累積誤差。如圖15和圖20左側(cè)，車輛在十字路口轉(zhuǎn)彎后出現(xiàn)方向偏差。

4 結(jié) 語

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