面向高精度城市測繪的激光緊耦合SLAM方法

孫喜亮，關宏燦，蘇艷軍，徐光彩，郭慶華

1. 中國科學院植物研究所植被與環境變化國家重點實驗室，北京 100093； 2. 中國科學院大學，北京 100049； 3. 北京大學城市與環境學院，北京 100871

近年來，集成了激光雷達(light detection and ranging,LiDAR)、慣性測量單元(inertial measurement unit,IMU)和全球導航衛星系統(Global Navigation Satellite System,GNSS)的移動激光雷達平臺被廣泛用于獲取城市三維地理信息數據[1-2]。GNSS/IMU組合導航是目前移動測量平臺最常用的定位方法，然而由于高樓、橋梁、隧道、行道樹等城市地物對GNSS信號的遮擋，城市中易存在大量弱、無GNSS信號區域，僅憑這種定位方法難以有效實現大范圍城市場景的高精度制圖[3]。激光同時定位與地圖構建(simultaneous localization and mapping,SLAM)技術可利用激光雷達增量式構建地圖的同時，實現移動測量平臺自身的相對定位，為解決弱、無GNSS信號環境下的制圖問題提供了有效的解決方案[4]，從而被廣泛應用于高精度制圖中[5-6]。

目前，激光SLAM方法按照激光和IMU的融合方式可分為松耦合和緊耦合[7-9]。松耦合方法獨立計算激光和IMU各自的運動量，然后，對它們的位姿估計結果進行融合。文獻[10]提出的LOAM方法是目前較為經典的松耦合方法，該方法通過結合高頻里程計粗定位和低頻建圖精定位的方式，實現了基于旋轉2D LiDAR的同步定位和制圖。文獻[11]在LOAM的基礎上，引入視覺里程計作為激光里程計的前端，形成了V-LOAM方法，進一步提高了SLAM的精度。文獻[12]在LOAM基礎上，利用點云分割降低特征數量，結合兩步配準和閉環約束削弱累積誤差，形成了適合地面無人車的LeGO-LOAM方法。然而，松耦合方法未能聯合優化各傳感器的觀測量，在特征缺乏區域易存在累積誤差大、穩健性差等問題[4]。

與松耦合方法相比，緊耦合方法直接融合了激光原始特征信息和慣性單元測量值等輸出運動狀態量，具有穩健性強且制圖精度高的優點[8]。近年來，多種緊耦合方法被相繼提出。例如文獻[13]將IMU預積分值應用到點云的運動畸變補償中，實現了LiDAR和IMU的外參校正；文獻[14]基于視覺慣性緊耦合SLAM方法引入了一種緊耦合的激光慣導SLAM框架，聯合優化激光雷達特征點和IMU的觀測量，與LOAM相比獲得了更準確的結果，但是其效率較低，慣導初始化條件復雜，場景特征缺乏區域穩健性較差。文獻[15]提出了一種以機器人為中心的激光雷達慣性狀態估計器R-LINS，該方法利用迭代誤差狀態卡爾曼濾波器修正了機器人的狀態估計，但存在長期運行會產生漂移的問題。文獻[16]提出一種面向移動機器人的激光慣導緊耦合實時定位方法，該方法具有較好的實時性，但在開闊的室外環境下定位精度達分米級。文獻[17]構建了基于因子圖的緊耦合激光慣導里程計框架(LIO-SAM)，將不同來源的IMU預積分因子、激光里程計因子、GNSS因子和閉環環因子進行聯合優化，取得了較高的測圖精度，但該方法只是簡單地將GNSS因子加入聯合優化中，未考慮復雜環境下GNSS異常對SLAM結果的影響。總體而言，目前已有方法主要針對小范圍的室外街道或室內建筑場景，應用的地理覆蓋范圍有限，在場景復雜、GNSS信號不穩定、制圖范圍廣且精度要求高的城市測繪應用中仍存在著累積誤差大、穩健性差的問題[18-19]。

為了解決城市移動測量制圖的難點，本文在LOAM和LeGO-LOAM的基礎上，融合了差分GNSS軌跡、慣性測量單元位姿、激光序列幀數據開展高精度SLAM制圖。本文的主要貢獻可歸納為：①針對城市地物形狀特性，聯合桿狀和面狀特征點，采用先高程方向后水平方向的兩步優化策略實現激光序列幀的相對位姿估計，提高了局部地圖配準精度，降低了城市環境下SLAM的累積誤差。②針對GNSS信號不穩定的特點，在全局地圖構建及優化過程中增量式的加入GNSS角點構成的位置因子約束，實現了復雜城市場景下厘米級的高精度制圖。

1 總體框架設計

本文提出的緊耦合激光SLAM算法在LOAM和LeGO-LOAM的基礎上，融合了IMU預積分因子和GNSS因子，并在激光序列幀數據的初始相對位姿估計和全局地圖構建及優化中分別引入了桿狀和面狀特征點和多因子聯合優化的策略。如圖1所示，本文提出的緊耦合激光SLAM系統可分為7個模塊。

(1) IMU預積分。根據IMU運動方程對采樣間隔的觀測量進行積分，獲取激光點云序列幀時刻對應位姿。

(2) 激光運動補償和特征提取。利用IMU積分得到的相對運動位姿修正點云的運動畸變，隨后提取桿狀和面狀特征點。

(3) 特征匹配和相對位姿估計。匹配序列幀同名特征，利用Levenberg-Marquardt(L-M)方法求解獲取相鄰幀的相對運動位姿。

(4) 局部地圖構建及優化。聯合序列幀同名特征點、初始相對位姿、IMU預積分因子優化構建局部地圖。

(5) 閉環檢測。新的局部地圖構建完成后與歷史局部地圖進行特征匹配檢測閉環。

(6) GNSS位置因子構建。依據GNSS軌跡方位變化提取出GNSS角點構成GNSS位置因子。

(7) 全局地圖構建及優化。當前局部地圖與歷史地圖存在閉環，則聯合局部地圖相對位姿、GNSS位置因子、IMU預積分因子、閉環因子等優化構建全局地圖，不存在閉環則直接更新全局地圖。

圖1 系統框架Fig.1 Framework of the proposed method

2 基于特征點匹配的相對位姿估計

準確估計激光序列幀的相對位姿是構建局部地圖的必要前提。本文采用IMU預積分的方式獲得激光序列幀對應時刻的相對運動[13-14]，基于此消除移動平臺位姿變換所引起的運動畸變。在實現各單幀點云的畸變糾正后，通過配準得到激光序列幀數據的初始相對位姿估計。在點云配準中，迭代最近點(iterative closest point,ICP)算法是目前最常用的配準方法，然而由于城市場景移動測量數據采集中易存在大量的移動目標，且往返區域遮擋現象嚴重，直接基于原始激光點數據的ICP配準失敗率高，因此本文采用基于特征點的配準方法，通過特征匹配得到同名特征點后，利用L-M法迭代求解激光序列幀間的相對位姿。

2.1 特征點提取

考慮到相對位姿估計中地面點對于垂直方向的約束較好，而桿狀點和面狀點則對水平方向的平移和旋轉的約束較好，本文先進行地面點和非地面點的區分，隨后進一步在非地面點中提取桿狀和面狀特征點[20-21]，具體處理流程如下：

(1) 首先將單幀掃描獲取激光點集ρt={ρ1,ρ2,…,ρn}生成深度圖像(c,r)，其中每個點ρi對應深度圖像的一個像素，c和r分別為圖像的列數和行數，圖像深度值為ρi到傳感器中心的距離。

(3) 獲取角度圖像后進一步利用Savitsky-Golay濾波算法進行平滑處理以削弱噪聲的影響。最后將最低行中角度小于45°的所有像素標記為初始地面，并利用廣度優先算法(breadth-first search,BFS)在角度圖像上搜索與地面像素間角度差值小于5°的像素，將其標記為地面像素[22]。

(4) 根據地面像素，將其對應的點云標記為地面特征點fg，其余為非地面點，并對各地面點計算其法向nfg。

(1)

(2)

2.2 特征匹配和相對位姿估計

本文通過不同幀間特征點的匹配得到同名特征點[25]，對于每一個特征點，本文在其相鄰幀的特征點集中利用KDtree搜索其最鄰近的同屬性特征點，并將法向和主方向夾角均小于5°，且距離小于1 m的特征點對視為同名特征點[26-27]。同名特征提取后，聯合點到線距離和點到面距離最小構建殘差方程，利用L-M方法求解獲取相鄰幀的相對運動位姿[28]?？紤]地面點對于垂直方向的約束較好，而桿狀點和面狀點則對水平方向的平移和旋轉的約束較好，與直接求解6自由度位姿不同，本文采用了兩步優化求解的策略，先利用地面點估算Z方向、橫滾角、俯仰角的變化量，再利用桿狀點和面狀點估算X方向、Y方向、方位角的變化量，最終聯合兩步優化獲取序列幀的相對位姿[12]。由于單幀點云稀疏且場景多樣，無法保證配準的穩健性，且序列幀配準及IMU測量信息存在累積誤差，因此本文選取1 s的點云數據建立局部地圖，在獲取激光序列幀相對位姿的基礎上，融合IMU預積分測量信息，完成局部地圖的構建，并將其與歷史局部地圖集合進行閉環檢測，實現累積誤差的削弱。

3 多因子聯合的全局地圖構建及優化

3.1 GNSS角點提取

全局地圖構建及優化是生成高精度的全局一致性地圖的關鍵步驟。GNSS位置因子可有效約束長距離軌跡(無閉環情況下)的漂移誤差，但在城市復雜環境中，GNSS存在著位置精度不穩定的現象。此外，由于GNSS位置信息和激光慣導SLAM軌跡間沒有初始的轉換關系，其僅能約束XYZ位置而不能約束姿態，且在直線分布的軌跡中易存在歧義解。

為了提高GNSS位置因子的穩定性，本文在基于GNSS定位標準差剔除誤差較大的GNSS點的基礎上，進一步結合相同時間段內SLAM的軌跡距離和GNSS點位移動距離的差值約束剔除了GNSS中的虛假高精度點。此外，針對GNSS位置因子僅能約束XYZ，特別是在直線分布的軌跡中易存在歧義解的問題，本文提出了基于GNSS角點的位置因子約束方法，與直接使用GNSS位置因子不同的是，本文將GNSS位置段方位變化超過30°的GNSS點視為角點，在隨后的全局優化中，增量式的加入GNSS角點，構成GNSS位置因子約束。

3.2 全局地圖構建及優化

(3)

(4)

(5)

Eglobal=arcmin(rloop+rM+rimu+rgnss)

(6)

4 試驗與分析

4.1 試驗設備及數據介紹

為驗證本文方法的有效性，采用了集成Velodyne VLP-16 LiDAR、Xsens MTI-100 IMU和Novatel 718D GNSS系統的背包式激光掃描平臺對4種典型城市場景(開放園區、地下車庫、城市公園、街區道路)進行了數據獲取(圖3)。

試驗區中，開放公園整體為大型回環，場景中包含大量的建筑立面和少量移動車輛，軌跡總長度約為2.7 km，數據采集過程中存在少量往返采集。地下車庫場景面積較小，軌跡總長度約為800 m，數據采集過程中場景內無移動目標干擾，但該場景GNSS信號較弱，存在GNSS信號丟失現象。城市公園場景內地物以樹木為主，由于樹木枝葉對激光掃描的遮擋，該場景內存在大量往返掃描。街區道路場景為筆直的城市主干道，掃描過程中存在大量移動車輛和行人的干擾，行走軌跡不存在閉環。各場景復雜情況和數據采集信息見表1。

表1 數據集概況

在上述4個場景中，研究將本文所提方法與目前主流的LOAM、LeGO-LOAM、LIO-SAM方法進行了對比，通過計算SLAM軌跡的絕對位置誤差(absolute position error,APE)評定各方法的定位精度。同時，本文研究還對僅利用激光慣導進行SLAM制圖(our-odom)和增加GNSS角點位置約束后的SLAM制圖(our-method)結果進行了對比，以表明GNSS位置因子約束的有效性。

除定位精度的評定外，本文還在開放園區場景中，利用RTK(real-time kinematic)采集了20個控制點，用于評定點云制圖的絕對坐標精度。

4.2 僅利用激光慣導的本文方法與不同SLAM方法的精度對比

不同場景下各SLAM方法的軌跡平面位置對比結果如圖4所示。在開放園區場景中，局部放大圖顯示LeGO-LOAM方法的軌跡存在明顯斷裂(圖4(a))。地下車庫場景的局部放大圖分別展示了車庫入口和出口處軌跡結果，同樣顯示出LeGO-LOAM方法存在明顯的軌跡跳變(圖4(b))。城市公園場景中，由于LOAM和LeGO-LOAM方法精度較差，圖中僅展示了LIO-SAM和本文方法的對比結果(圖4(c))。街區道路場景中，結合表2的APE統計結果和圖4(d)所示，可發現LIO-SAM方法APE誤差主要出現在了Z方向上。

各場景下4種SLAM方法的軌跡APE隨時間的變化情況如圖5所示。在開放園區場景地下車庫場景中，LeGO-LOAM方法出現點云配準失敗情況后，誤差直線上升(圖5(a)和5(b))。城市公園場景中,LeGO-LOAM、LIO-SAM和本文方法的軌跡APE均隨時間變化存在起伏(圖5(c))，說明3種方法的閉環約束均削弱了累積誤差的影響，且本文方法的累積誤差最小。街區道路場景中，LIO-SAM存在較為嚴重的退化現象，APE隨時間的增加顯著上升。

精度評定結果表明(表2)，LOAM方法在唯一成功的城市公園場景中，軌跡APE的RMSE高達32.406 m。LeGO-LOAM方法雖較LOAM方法有所提升，但其RMSE均在數十米量級。LIO-SAM方法在4個場景下均成功實現了制圖，但最優精度出現在地下車庫場景中，RMSEAPE為0.505 m。在開放園區、地下車庫、城市公園、街區道路場景中，本文僅利用激光慣導的方法較LIO-SAM方法分別提升0.772 m、0.014 m、0.126 m和71.117 m。本文僅利用激光慣導的方法與LIO-SAM在街區道路和開放園區場景下的差異較大，其主要原因是兩個場景中均存在較多的移動車輛，致使LIO-SAM方法點云配準發生退化現象，結果均表現為Z方向出現大的偏差。而本文方法提取了更趨近于靜態目標的桿狀和面狀特征，并結合兩步優化，有效地提高了點云配準的精度，削弱了累積誤差。

4.3 GNSS角點位置約束的影響分析

如圖5所示，加入GNSS角點位置約束有效降低了本文方法在4個場景下的累積誤差。精度評價結果表明，4個場景的平均RMSEAPE由13.449 m提升至0.062 m(表3)，其中在街區道路場景中的提升最為顯著，主要原因在于該場景存在大量移動目標且移動軌跡不存在閉環，致使SLAM累積誤差較大，此外，近似直線分布的GNSS點無法有效約束相應時刻的姿態，本文方法利用提出的GNSS角點約束有效提高了城市場景制圖的穩健性和精度。

圖2 全局地圖構建及優化Fig.2 Global map construction and optimization

圖3 試驗區概況Fig.3 Overview of the study area

圖4 SLAM軌跡平面位置對比Fig.4 Comparison between plane positions of SLAM trajectories

圖5 SLAM軌跡APE對比Fig.5 Comparison between APE of SLAM trajectories

4.4 本文方法的點云制圖精度分析

本文以開放園區為例，進一步展開本文方法點云制圖結果的精度驗證工作。試驗采集了均勻分布于開放園區場景測區的20個有效的檢查點(圖6(a))，用于點云制圖結果的絕對坐標精度驗證。驗證結果表明，本文所提方法制圖結果的平面中誤差為0.041 m，高程中誤差為0.024 m，整體中誤差為0.047 m(表4)，檢查點的平面和高程誤差分布均為殘留的隨機誤差(圖6(b))，誤差分布相對較大的點基本位于移動軌跡出現急劇方位變化之處，原因在于該處相對于平滑運動的軌跡而言仍有部分姿態誤差殘留，本文方法可有效提升SLAM制圖的絕對坐標精度，滿足復雜城市場景下的高精度制圖要求。

圖6 檢查點及誤差分布Fig.6 Distribution of check points in the accuracy analysis

表2 SLAM軌跡APE統計結果

表3 有無GNSS角點位置因子約束的軌跡APE統計結果對比

表4 檢查點精度統計結果

5 結 論

本文面向復雜城市場景的高精度制圖要求，提出了一種LiDAR、IMU、GNSS緊耦合SLAM方法，該方法利用桿狀和面狀特征進行序列幀間、激光掃描幀和局部地圖間及局部地圖間的點云配準，并采用GNSS角點位置約束SLAM，有效降低了城市環境下SLAM的累積誤差，提高了全局地圖構建的準確性。試驗結果表明，LOAM和LeGO-LOAM方法穩健性及精度均較差，本文方法僅利用激光慣導進行SLAM制圖時，在開放園區、地下車庫、城市公園和街區道路場景中的精度(RMSEAPE=13.449 m)顯著高于目前主流的緊耦合LIO-SAM(RMSEAPE=31.456 m)方法，結合GNSS位置因子約束后，精度提升至厘米級(RMSEAPE=0.062 m)，其中開放園區中基于控制點的點云絕對坐標精度評價結果表明，本文方法(RMSE=0.047 m)可有效支持厘米級高精度城市制圖。