線面特征結(jié)合的三維激光SLAM算法及性能評估

王雅儀，余 萌，朱 鋒

（武漢大學(xué) 測繪學(xué)院，武漢 430079）

0 引言

自動駕駛是1個集精準(zhǔn)定位、環(huán)境感知、決策控制等多功能于一體的復(fù)雜系統(tǒng)，而定位又是這些功能發(fā)揮作用的先決條件。當(dāng)前的定位定姿技術(shù)眾多，但均存在缺陷，如衛(wèi)星導(dǎo)航容易在信號遮擋的環(huán)境下失效；高精度的慣導(dǎo)設(shè)備成本極高、且定位誤差隨著時間增加而快速發(fā)散；視覺導(dǎo)航易受光照和視角變化的影響，基于無線信號即無線保真（wireless fidelity, WiFi）、藍(lán)牙、發(fā)光二極管（light emitting diode, LED）等的定位方案，須預(yù)先布置使用場景[1]。文獻(xiàn)[2-3]指出：激光雷達(dá)（light detection and ranging, LiDAR）具有測量精度高、時間和空間分辨率精細(xì)、無需事先布置場景、能夠快速響應(yīng)環(huán)境變化、可融合多傳感器等優(yōu)點(diǎn)，與基于視覺同步定位和制圖（simultaneous location and mapping, SLAM）技術(shù)相比，激光SLAM自主定位更加安全、穩(wěn)健，成為了國內(nèi)外研究的熱點(diǎn)。

激光SLAM框架，一般分為前端掃描匹配、后端優(yōu)化、回環(huán)檢測、地圖構(gòu)建等4個關(guān)鍵模塊。前端掃描匹配是SLAM最核心的功能，實(shí)現(xiàn)前后兩幀點(diǎn)云的相對位姿恢復(fù)，也被稱為前端里程計。前端里程計的主流匹配算法有：迭代最臨近點(diǎn)法（iterative closest point, ICP）及其變種、相關(guān)性掃描匹配法（correlation scan match, CSM）、正態(tài)分布變換（normal distribution transformation, NDT）、基于特征的匹配等等。ICP是由文獻(xiàn)[4]在1992年提出的，迭代轉(zhuǎn)換矩陣使得待匹配的兩幀點(diǎn)云歐氏距離最小化，但是傳統(tǒng)的ICP需要對全點(diǎn)云進(jìn)行優(yōu)化，存儲和計算消耗大；基于特征的匹配，通過抽象出3維點(diǎn)云的幾何信息，能更好地輔助前端進(jìn)行點(diǎn)云匹配，在相同時間下準(zhǔn)確度更高，是當(dāng)下的研究熱點(diǎn)。

2009年，文獻(xiàn)[5]首次在點(diǎn)特征直方圖（point feature histograms, PFH）的基礎(chǔ)上，提出了更加穩(wěn)定的快速點(diǎn)特征直方圖（fast point feature histograms,FPFH），類似于視覺特征子的描述符一樣，描述了3維（3D）點(diǎn)云集中1個點(diǎn)周圍的局部幾何特征，通過直方圖的相似度確定同名點(diǎn)對，再進(jìn)行基于同名點(diǎn)的ICP配準(zhǔn)；文獻(xiàn)[6]提出利用建筑物邊界約束進(jìn)行點(diǎn)云數(shù)據(jù)配準(zhǔn)，通過探測建筑物垂直外墻和地面相交處的邊角點(diǎn)，預(yù)先構(gòu)建出地圖，然后通過ICP實(shí)現(xiàn)地圖匹配來校正車輛位置；文獻(xiàn)[7]采用分開—合并框架提取點(diǎn)線特征，還融合了不同觀測值的置信度；文獻(xiàn)[8]采用“點(diǎn)－切平面”的特征對重疊點(diǎn)云進(jìn)行配準(zhǔn)；2016年，文獻(xiàn)[9]提出的激光雷達(dá)里程計與地圖構(gòu)建（LiDAR odometry and mapping, LOAM）成為3維激光SLAM最流行的點(diǎn)云匹配方法，它基于線面特征進(jìn)行了點(diǎn)云特征提取，并將激光雷達(dá)里程計的輸出和點(diǎn)云子地圖進(jìn)行特征匹配，對里程計恢復(fù)的位姿進(jìn)行優(yōu)化，但系統(tǒng)實(shí)時性差；基于LOAM的基本思想，列戈-洛姆（LeGO-LOAM）[10]引入了基于歷史位置的最近鄰搜索做閉環(huán)檢測，采用兩步優(yōu)化的方式降低設(shè)計矩陣的維數(shù)，可以滿足系統(tǒng)實(shí)時性的要求。

相對于點(diǎn)線面特征，更加抽象的語義特征能如實(shí)地反映3維場景，提供正確的特征匹配。蘇黎世聯(lián)邦理工大學(xué)開發(fā)的LeGO-LOAM軟件[11]，則基于分割算法進(jìn)行回環(huán)檢測，首先從3D點(diǎn)云中聚類并標(biāo)注分割物，將它們用于匹配已經(jīng)走過的地方和使用幾何驗(yàn)證對比的方法選出回環(huán)檢測的候選點(diǎn)云。文獻(xiàn)[12]提出的塞格·馬特赫（SegMatch），采用漫水填充方法消除錯誤的類別標(biāo)簽，識別動態(tài)物體并剔除，構(gòu)建帶有語義約束的ICP模型，提高了定位的穩(wěn)定性。

本文采用1種線面特征相結(jié)合的3維激光SLAM算法，通過基于特征的ICP變種算法，對在不同時刻掃描的重疊點(diǎn)云進(jìn)行配準(zhǔn)，恢復(fù)行駛車輛的6個自由度位姿；為提高解算精度，提出全球衛(wèi)星導(dǎo)航系統(tǒng)（global satellite navigation system,GNSS）數(shù)據(jù)融合激光SLAM的方案。

1 算法理論及實(shí)現(xiàn)

3維激光SLAM的任務(wù)是估計載體自身的位姿，同時建立周圍的環(huán)境地圖。因?yàn)榧す饫走_(dá)可以準(zhǔn)確測量障礙點(diǎn)的角度與距離，環(huán)境地圖通過對正確的位姿進(jìn)行點(diǎn)云疊加即可獲得，可見激光SLAM的關(guān)鍵在于提高位姿估計的精度。本文提出的SLAM基本框架可以分為：預(yù)處理、前端里程計和后端優(yōu)化3個模塊，如圖1所示，圖1中的ICP是指基于特征的迭代最臨近點(diǎn)法，實(shí)驗(yàn)中采用的激光點(diǎn)云采樣率設(shè)為10 Hz，而GNSS接收機(jī)位置先驗(yàn)信息則以低頻1 Hz的速度輸入系統(tǒng)，最終的位姿估計的頻率與點(diǎn)云采樣率一致，在GNSS接收機(jī)良好的場景下可以用GNSS接收機(jī)輔助構(gòu)建點(diǎn)云地圖。

1.1 預(yù)處理

預(yù)處理分為點(diǎn)云分割和特征提取2個步驟，點(diǎn)云分割能提高特征提取的速度和精度。對原始點(diǎn)云進(jìn)行聚類，去除冗余點(diǎn)，僅留下包含結(jié)構(gòu)化特征的分割點(diǎn)云，包括地面點(diǎn)和大型物體上的點(diǎn)（如樹干）。采用基于密度的聚類方法，以某一點(diǎn)為啟發(fā)點(diǎn)向四周探索，計算相鄰方位角、相鄰掃描線上鄰點(diǎn)的距離和角度，足夠靠近的一群點(diǎn)云組成1個簇，只有點(diǎn)云簇含30個點(diǎn)以上才被認(rèn)為是候選點(diǎn)云。接著，從分割點(diǎn)云中再提取對應(yīng)的線特征點(diǎn)和面特征點(diǎn)。

圖1 本文提出的3維激光SLAM算法流程

本文采用文獻(xiàn)[9]提出的線面特征提取方法，定義激光點(diǎn)的局部平滑度為

式中：n為選取與點(diǎn)iP同一激光掃描線的鄰點(diǎn)的個數(shù)；ic為點(diǎn)iP與其相鄰2n個激光點(diǎn)形成的單位向量之和的模。

當(dāng)激光束打在“面”上，iP與其鄰點(diǎn)分布在近似一條直線上，因此這些鄰點(diǎn)與iP構(gòu)成的向量相互抵消，ic值較小。當(dāng)激光束打在“線”上，鄰點(diǎn)與iP形成的向量無法相互抵消，ic值較大。

為獲取空間均勻分布的特征，將每根掃描線等分為6段，在段內(nèi)依據(jù)激光點(diǎn)的局部平滑度進(jìn)行排序，每一段選取平滑度大于0.1的最大2個點(diǎn)為線特征點(diǎn)，選取平滑度小于0.1的最小4個點(diǎn)為面特征點(diǎn)。特征提取的效果如圖2所示。

圖2 線面特征提取示意圖

原始激光點(diǎn)云數(shù)約為11.7萬個，提取特征后，線特征點(diǎn)數(shù)量為667個，面特征點(diǎn)數(shù)量為845個，總特征點(diǎn)數(shù)為1 512個，待估觀測數(shù)據(jù)大大減少，并且如圖2所示，提取出的線特征點(diǎn)主要聚集在樹干、路沿、車輛邊緣、房屋角點(diǎn)等處，而面特征點(diǎn)聚集在地面，能如實(shí)反映3維環(huán)境的結(jié)構(gòu)化特征。

1.2 前端里程計

里程計主要負(fù)責(zé)每幀點(diǎn)云的位姿估計，當(dāng)結(jié)構(gòu)化線面特征輸入系統(tǒng)后，采用畸變校正和掃描匹配并行的算法，恢復(fù)出相鄰幀的位姿關(guān)系。

考慮到常用的激光雷達(dá)都是旋轉(zhuǎn)掃描的工作方式，車輛在激光雷達(dá)一幀掃描的周期內(nèi)，會行駛過一段距離，導(dǎo)致一幀內(nèi)的所有激光點(diǎn)并不是處于同一個局部坐標(biāo)系，經(jīng)過畸變校正可以恢復(fù)到同一局部坐標(biāo)系下。點(diǎn)云的去畸變需要已知幀間運(yùn)動量，而求解幀間運(yùn)動量又依賴于去畸變后掃描匹配，因此可以用位姿插值的辦法將畸變校正和掃描匹配放在1個算法中同時處理。假設(shè)掃描周期內(nèi)汽車的運(yùn)動是平滑的，基于勻速運(yùn)動模型，不同時刻對應(yīng)的位姿可以插值表示，設(shè)在第k幀期間，車體的運(yùn)動量等價于幀尾時刻的局部坐標(biāo)系{Lk+1}相對于幀頭時刻的局部坐標(biāo)系{Lk}的旋轉(zhuǎn)和平移，用Tk表示，Tk=[θx,θy,θz,t x,t y,tz]T。

如果已知某點(diǎn)在一幀掃描內(nèi)的相對時間，記為s，那么該點(diǎn)對應(yīng){Lk}的旋轉(zhuǎn)和平移量為（以繞Y軸旋轉(zhuǎn)和平移為例）

在對每幀點(diǎn)云進(jìn)行特征提取操作后，本文采用PCL庫中的FLANN匹配器快速查找前后兩幀點(diǎn)云的匹配特征點(diǎn)對，線特征點(diǎn)只和線特征點(diǎn)進(jìn)行最近鄰匹配，面特征點(diǎn)只和面特征點(diǎn)進(jìn)行最近鄰匹配。對于線特征點(diǎn)而言，雖然最近鄰點(diǎn)和它可能不是同一個點(diǎn)，但是它們在同一條結(jié)構(gòu)線上的可能性很大，因此在目標(biāo)點(diǎn)云中找到最近鄰點(diǎn)和次近鄰點(diǎn)，觀測方程就變成了最小化點(diǎn)到線/面的距離之和的函數(shù)，相比于傳統(tǒng)全點(diǎn)云ICP，線面-ICP能夠建立更準(zhǔn)確的觀測方程。

對于線特征點(diǎn)，其算法模型為

以同一數(shù)據(jù)測試全點(diǎn)云，ICP算法和線面特征結(jié)合的ICP算法，得到全點(diǎn)云ICP算法每一幀的計算時間平均為3.183 s，而改進(jìn)后的ICP算法只需要0.292 s，這主要是因?yàn)樘卣魈崛p小了匹配點(diǎn)對的數(shù)量，提高了匹配速率；另外，考慮到用最小二采估計位姿速度慢，本文采用分層優(yōu)化的方法[10]減小設(shè)計矩陣求逆的維度，如圖3所示，在同等精度下可以減少35%的計算時間。

圖3 分層優(yōu)化算法

1.3 后端優(yōu)化

前端里程計每一步的解算都存在誤差，隨著航位推算的距離增加，全局位姿的誤差也在累積，根據(jù)位姿拼接起來的地圖也會發(fā)生畸變。在SLAM領(lǐng)域，又提出了后端優(yōu)化的關(guān)鍵技術(shù)，對位姿進(jìn)行優(yōu)化，得到全局一致的軌跡和地圖。始終假設(shè)后一時刻的估計較前一時刻帶有更多的誤差，因此對已經(jīng)構(gòu)建的地圖的信任程度遠(yuǎn)高于相鄰幀點(diǎn)云配準(zhǔn)后的運(yùn)動估計。

本文通過幀間距離和角度的關(guān)系來劃分關(guān)鍵幀，距離相隔1 m或者角度轉(zhuǎn)過30°就向后端輸入1個關(guān)鍵幀；一旦前端判斷當(dāng)前幀為關(guān)鍵幀后，從后端的關(guān)鍵幀庫中查詢在當(dāng)前位置80 m范圍內(nèi)的關(guān)鍵幀軌跡點(diǎn)；并將對應(yīng)的點(diǎn)云疊加、構(gòu)建局部地圖，進(jìn)行基于地圖的線面-ICP匹配；最后將優(yōu)化后的位姿更新到關(guān)鍵幀點(diǎn)云庫和軌跡庫中。

如果系統(tǒng)中還有GNSS數(shù)據(jù)融合，考慮到GNSS 數(shù)據(jù)具有不發(fā)散的良好特性，可以用來抑制激光SLAM的長時性漂移，保證了SLAM軌跡漂移在1個可控范圍之內(nèi)。將車輛的初始時間和位置和GNSS 時間和坐標(biāo)系對準(zhǔn)后，設(shè)定每1 Hz得到對應(yīng)的GNSS先驗(yàn)數(shù)據(jù)。系統(tǒng)首先進(jìn)行3維激光點(diǎn)云配準(zhǔn)，然后融合GNSS先驗(yàn)位置數(shù)據(jù)和對應(yīng)位姿點(diǎn)，經(jīng)過GNSS融合后的位姿估計才被存儲到關(guān)鍵幀點(diǎn)云庫和軌跡庫中?；谑剑?）的觀測模型，觀測值為源點(diǎn)云到最近鄰目標(biāo)點(diǎn)云形成的線/面的歐氏距離，可以引入1個附加先驗(yàn)信息的最小二乘估計器，假設(shè)觀測方程線性化后得到系數(shù)矩陣為B，觀測值為L，觀測值權(quán)陣為P，則附加先驗(yàn)信息的最小二乘法方程為

式中：待估的6自由度的位姿(R t)是當(dāng)前幀相對于全局坐標(biāo)系的旋轉(zhuǎn)、平移量；N、W的值由公式N=BTPB，W=BTPL求得；PG是具有先驗(yàn)信息的平移量t的權(quán)陣，根據(jù)經(jīng)驗(yàn)設(shè)定；LG表示t的GNSS先驗(yàn)值減去前端ICP位姿估計值的差。

2 實(shí)驗(yàn)與結(jié)果分析

為了對激光SLAM系統(tǒng)的車載導(dǎo)航性能進(jìn)行測試和評估，采用不同道路環(huán)境的數(shù)據(jù)對算法的整體定位定姿性能以及GNSS融合對定位結(jié)果的影響進(jìn)行分析。本文測試了卡爾斯魯厄工學(xué)院和豐田技術(shù)學(xué)院聯(lián)合采集的開源數(shù)據(jù)集基蒂（KITTI）[13]，其采集環(huán)境包括公路、城市及鄉(xiāng)村。采集設(shè)備有一套OXTS RT3003的GNSS與慣性測量系統(tǒng)（inertial measurement unit, IMU）組合導(dǎo)航系統(tǒng)和一臺Velodyne 64線激光雷達(dá)。激光雷達(dá)以10 Hz的頻率進(jìn)行旋轉(zhuǎn)掃描，測距精度0.02 m（＜1δ），水平視角360°，水平分辨率0.09°，豎直視角為26.8 °；涵蓋三類環(huán)境，“公路”道路寬、車速快；“城市”道路兩旁是建筑物，“鄉(xiāng)村”道路兩旁是大量植被。其實(shí)驗(yàn)設(shè)備、采集環(huán)境如圖4所示。

2.1 整體精度分析

把GNSS/INS的解算結(jié)果視為參考真值，和純激光SLAM實(shí)時輸出的定位定姿結(jié)果進(jìn)行誤差分析，水平軌跡誤差的大小由灰度深淺來表示，灰度越深代表偏差越大，如圖5所示。

圖4 實(shí)驗(yàn)裝備與采集環(huán)境

KITTI共有11組數(shù)據(jù)，但是由于公路場景下車速達(dá)到80 km/h，點(diǎn)云變化速度快，特征提取稀疏，SLAM位姿誤差隨時間很快地發(fā)散了，在圖5中沒有展示出來。由圖5可知，大部分的軌跡和參考真值重合度高，但是仍明顯存在著累計誤差隨行駛距離增大而增大的情況。這里采用平均相對誤差進(jìn)行精度分析，每行駛100 m計算1次誤差，得到整體的定位定姿精度如表1所示。

圖5 整體軌跡誤差結(jié)果

表1 KITTI數(shù)據(jù)描述以及整體解算精度

由表1可知，本文給出的線面特征結(jié)合的純激光SLAM在車速20～50 km/h的結(jié)構(gòu)化環(huán)境下，能較好地維持車輛的位姿，整體定位精度約為1.98%，即行駛100 m，偏離真值1.98 m，角度誤差為0.005 1(°)/m。分析幾個相對誤差較大的軌跡，如序號4、序號8的軌跡，明顯是誤差隨著行駛距離的增加而累積，所以激光雷達(dá)必須融合其他具備絕對定位能力的傳感器數(shù)據(jù)，才能抑制自身長時性漂移；對序號7的異常軌跡分析發(fā)現(xiàn)，車輛經(jīng)過了多次快速轉(zhuǎn)彎、環(huán)境退化（大量相似及不穩(wěn)定特征干擾導(dǎo)致）的場景，在平面上產(chǎn)生了嚴(yán)重漂移。

2.2 后端優(yōu)化對定位定姿結(jié)果的影響

后端優(yōu)化將當(dāng)前關(guān)鍵幀和距離80 m以內(nèi)的歷史關(guān)鍵幀組成的地圖關(guān)聯(lián)起來，用構(gòu)建好的地圖對當(dāng)前幀的位姿進(jìn)行精化。為了具體分析后端地圖優(yōu)化模塊對整體定位定姿精度的影響，以序列6數(shù)據(jù)為例，這是1組城市道路的數(shù)據(jù)，全長700 m左右。分析不加地圖優(yōu)化和加入地圖優(yōu)化后，對比兩種模式下估計出來的軌跡和定位定姿結(jié)果的誤差序列，結(jié)果如圖6至圖8所示。

圖6 后端優(yōu)化對整體精度的影響

圖7 后端優(yōu)化對位置估計的影響

圖8 后端優(yōu)化對姿態(tài)估計的影響

由圖6可以看出：沒有進(jìn)行后端優(yōu)化的軌跡尺度漂移明顯，各方向的位置、角度逐漸偏離真值。反之，成功的地圖優(yōu)化可使得起點(diǎn)和終點(diǎn)高度重合，最大偏差顯著降低。本文采用均方根誤差（root mean square error, RMSE）作為評價標(biāo)準(zhǔn)，來定量地分析優(yōu)化前后相對精度變化，其結(jié)果如表2所示。

表2 后端優(yōu)化前后RMSE

水平定位精度提升至1～2 m，角度精度約為2°，提高了66.7%的姿態(tài)精度，說明優(yōu)化后整體精度有了顯著提高，驗(yàn)證了后端優(yōu)化的有效性。在沒有其他傳感器的融合輔助下，成功的后端優(yōu)化可以在一定程度上抑制激光SLAM自身的長時性漂移。

2.3 GNSS融合對定位定姿結(jié)果的影響

圖9 GNSS融合對整體精度的影響

為進(jìn)一步分析多傳感器融合對3維激光SLAM位姿估計的影響，在后端優(yōu)化中引入GNSS位置先驗(yàn)數(shù)據(jù)，采用有先驗(yàn)約束的整體平差法進(jìn)行數(shù)據(jù)融合，這是1種松組合的方法，便于多傳感器的拓展。將車輛的初始時間和位置與 GNSS 時間和坐標(biāo)系對準(zhǔn)后，參考真值轉(zhuǎn)換為GNSS約束條件，模擬實(shí)時GNSS約束輸入。以長路程的序列1、序列7的數(shù)據(jù)為例，GNSS融合后估計出來的軌跡和建圖效果如圖9所示。

由圖9可知，融合了GNSS數(shù)據(jù)的計算軌跡和參考真值軌跡的重合度更高：當(dāng)有GNSS數(shù)據(jù)約束時，可抑制激光雷達(dá)里程計的漂移發(fā)散；沒有GNSS數(shù)據(jù)約束時，激光雷達(dá)里程計依賴自身航位推算進(jìn)行位姿估計與建圖。

表3分析了車輛行駛中水平方向上的定位精度。

表3 KITTI數(shù)據(jù)定位結(jié)果分析

由表3可知，通過精度對比，GNSS/激光SLAM的融合算法明顯優(yōu)于純SLAM位姿優(yōu)化算法。該定位方法不存在由里程計引起的累積誤差，在30 km/h左右的速度下，表現(xiàn)出良好的定位性能，適用于在GNSS條件良好下的點(diǎn)云地圖構(gòu)建。定位精度提高至25 cm左右時，和純GNSS定位精度相當(dāng)，但融合算法能輸出更高頻率的位姿結(jié)果，有助于生成點(diǎn)云密度更高、一致性良好的點(diǎn)云地圖。

3 結(jié)束語

本文針對傳統(tǒng)全點(diǎn)云ICP算法存儲和運(yùn)算消耗大、點(diǎn)云誤匹配的問題，提出了1種線面特征結(jié)合的點(diǎn)云精配準(zhǔn)算法。通過對獲取的點(diǎn)云進(jìn)行分割和特征提取，再對結(jié)構(gòu)化的線、面特征點(diǎn)云進(jìn)行精配準(zhǔn)，并在精配準(zhǔn)的過程中，同時實(shí)驗(yàn)姿點(diǎn)云運(yùn)動的畸變校正，最后通過KITTI測試，對算法的可行性和有效性進(jìn)行了評估，驗(yàn)證了該算法的整體定位相對精度約為1.98%，定姿相對精度約為0.005 1(°)/m；后端優(yōu)化在一定程度上可抑制激光SLAM自身的長時性漂移；當(dāng)有GNSS約束時，軌跡無發(fā)散，定位精度約為25 cm，達(dá)到分米級定位精度。