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

王雅儀，余 萌，朱 鋒

（武漢大學 測繪學院，武漢 430079）

0 引言

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

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

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

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

本文采用1種線面特征相結合的3維激光SLAM算法，通過基于特征的ICP變種算法，對在不同時刻掃描的重疊點云進行配準，恢復行駛車輛的6個自由度位姿；為提高解算精度，提出全球衛星導航系統（global satellite navigation system,GNSS）數據融合激光SLAM的方案。

1 算法理論及實現

3維激光SLAM的任務是估計載體自身的位姿，同時建立周圍的環境地圖。因為激光雷達可以準確測量障礙點的角度與距離，環境地圖通過對正確的位姿進行點云疊加即可獲得，可見激光SLAM的關鍵在于提高位姿估計的精度。本文提出的SLAM基本框架可以分為：預處理、前端里程計和后端優化3個模塊，如圖1所示，圖1中的ICP是指基于特征的迭代最臨近點法，實驗中采用的激光點云采樣率設為10 Hz，而GNSS接收機位置先驗信息則以低頻1 Hz的速度輸入系統，最終的位姿估計的頻率與點云采樣率一致，在GNSS接收機良好的場景下可以用GNSS接收機輔助構建點云地圖。

1.1 預處理

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

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

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

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

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

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

圖2 線面特征提取示意圖

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

1.2 前端里程計

里程計主要負責每幀點云的位姿估計，當結構化線面特征輸入系統后，采用畸變校正和掃描匹配并行的算法，恢復出相鄰幀的位姿關系。

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

如果已知某點在一幀掃描內的相對時間，記為s，那么該點對應{Lk}的旋轉和平移量為（以繞Y軸旋轉和平移為例）

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

對于線特征點，其算法模型為

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

圖3 分層優化算法

1.3 后端優化

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

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

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

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

2 實驗與結果分析

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

2.1 整體精度分析

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

圖4 實驗裝備與采集環境

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

圖5 整體軌跡誤差結果

表1 KITTI數據描述以及整體解算精度

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

2.2 后端優化對定位定姿結果的影響

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

圖6 后端優化對整體精度的影響

圖7 后端優化對位置估計的影響

圖8 后端優化對姿態估計的影響

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

表2 后端優化前后RMSE

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

2.3 GNSS融合對定位定姿結果的影響

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

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

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

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

表3 KITTI數據定位結果分析

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

3 結束語

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