基于LeGO-LOAM的實際場景下的同步定位與建圖方法

崔 洋，顧恒之，徐 震

（長安大學 汽車學院，陜西 西安 710064）

隨著現代汽車產業的發展，無人駕駛技術成為當前汽車行業的高新前沿研究方向之一。同步定位與地圖重建（Simultaneous Localization And Mapping, SLAM）技術是無人駕駛技術中實現無人化的關鍵技術之一[1]。

國內外學者對基于視覺和激光雷達的 SLAM算法進行了大量的研究[2]。由于激光雷達具有測距精度高、不容易受照明以及視角變化的影響，以及在全天候條件下的工作能力，因此廣泛應用于無人駕駛領域，更適合在復雜多變的汽車行駛環境中進行 SLAM。本文基于輕量化和地面優化的激光雷達里程計和地圖（Lightweight and Groud Optimized Lidar Odometry and Mapping, LeGOLOAM）算法，并采用迭代最近點（Iterative Closest Point, ICP）算法對回環獲取的全局地圖進行優化，以提高地圖繪制的準確性和魯棒性，并在長安大學汽車學院周圍進行實際建圖，以檢測算法優化的可行性，從而更好地為無人車感知技術提供技術支持。

1 LeGO-LOAM算法

1.1 LOAM算法以及存在的問題

使用高精度激光雷達進行建圖，有時候不能滿足無人車對環境的實時測繪與定位的要求，且成本較高。研究者基于規則和廣義的 ICP的輕量級環路檢測和優化算法，提出了基于廣義迭代最近點算法（Generalized ICP, GICP）的3D點云配準的SLAM優化方法，但定位和映射精度仍然需要提高。激光雷達里程計和建圖[3]（LOAM）是目前最具有代表性的基于特征匹配的實時三維激光SLAM 算法。他的計算量和運動補償量較小，但是沒有環路檢測，導致漂移誤差隨時間累計，映射精度差，并且在開放環境中存在退化問題。LeGO-LOAM 以在減少計算資源的情況下達到類似甚至更好的精度。

1.2 LeGO-LOAM算法

LeGO-LOAM是SHAN T等人[4]于2018年在LOAM算法的基礎上提出的一種基于 3D激光雷達的輕型實時定位和建圖算法。它主要由點云分割、特征提取、雷達里程計、雷達建圖和變換融合組成，如圖1所示。

圖1 LeGO-LOAM的系統結構圖

1.3 具體過程

為了提高處理效率和特征提取的準確性，通過點云分割模塊，將點云劃分為不同的簇，并標記為地面點云或分割點[5]。同時，為后續模塊獲取每個點的三個特征即點的標簽（地面點云或分割點）、深度圖中的行和列索引以及和傳感器的距離值。首先，將點云投影到深度圖上，并將點云中的點映射到深度圖上的像素點上。在分割之前，估計深度圖的地平面以提取地面特征，并將代表地面的點標記為不參與點云分割的點。特征提取模塊從地面點或者分割點提取邊緣和平面特征，提取過程如下：

（1）計算每個點的平滑度c

式中，S為同一行的連續點Pi的點集，S中有一半位于Pi的兩側，ri和rj分別是從集合S中的點Pi和Pj到激光雷達的歐氏距離。

（2）將深度圖水平分割成幾個相等的子圖像，以均勻地提取特征。

（3）根據設定的閾值cth來分割不同類型的特征。將平滑度c大于cth的點分割成邊緣特征點組成集合Fme。將平滑度c小于cth的點分割成平面特征點組成集合Fmp。

（4）再進行一次篩選，得到精煉的邊緣點Fe和平面點集Fp，其中Fe∈Fme，Fp∈Fmp。

激光雷達里程計模塊在兩個連續的幀中估計車的運動，使用從特征提取模塊提取的特征在連續掃描幀中找到車的位置變換。

激光雷達建圖模塊將特征點集{Fe,Fp}中的特征與周圍的點云地圖匹配，進一步細化姿態變換，然后使用L-M優化得到的最終變換姿態在點云地圖的新節點和歷史選擇的節點之間添加空間約束，并且通過循環檢測添加新的約束，然后將姿態地圖發送給基于因子圖的C++庫（Georgia Tech Smoothing And Mapping, GTSAM）進行圖形優化，并通過傳感器更新估計的姿態。轉換模塊融合來自激光雷達里程計模塊和激光雷達建圖模塊的姿態估計結果，并輸出最終的姿態估計。

2 算法改進

2.1 回環檢測

LeGO-LOAM的回環檢測模塊采用 KD樹模型，根據歐幾里德距離找到與當前姿態相似的歷史姿態及其附近的點云，使用 ICP計算其匹配度并估計姿態，并使用最相似的歷史幀的車的姿態約束當前的姿態估計，更新點云地圖以獲取全局一致性地圖。算法計算量大，檢測效率低。考慮到實時性和準確性，采用了低頻環路檢測，在長距離、大場景的建圖中仍存在很大的誤差。

2.2 掃描上下文算法

掃描上下文算法是KAIST大學的KIM G和KIM A[6]提出的算法，該算法使用非直方圖的全局描述符來更快、更高效的搜索數據（當前的以前的數據）。掃描上下文，通過降維將3D點云轉化為 2.5D，并使用一種搜索算法匹配當前幀和歷史幀的點云數據，實現回環檢測。

掃描上下文算法使用一次掃描的點云數據構造矩形圖像掃描上下文，并構造 KD樹，執行最鄰近搜索，找到可能與當前幀回環的多個相似幀；然后，分成扇形區域計算最小偏移量和相似性分數，選擇相似性分數最高的幀作為回環幀，求解當前幀和回環幀之間的姿態關系，實現回環檢測。

2.3 回環改進

在LeGO-LOAM算法中，進行回環檢測環節采用的是根據歐幾里德距離找到與當前姿態相似的歷史姿態及其附近的點云，使用 ICP計算其匹配度并估計姿態，在此基礎上，加上掃描上下文算法，將計算后的姿態約束添加到 GTASM 中進行全局姿態優化，通過掃描上下文算法與 KD樹的結合，來對回環檢測環節進行改進，從而使得回環檢測精度得到提高。通過此方法進行全局建圖，可以提高建圖精度。

3 實車建圖

3.1 建圖場景

實際實驗場景位于長安大學汽車學院，通過安裝在無人車上的鐳神16線激光雷達進行圍繞汽車學院一周進行建圖。所用無人車如圖 2所示搭載車載慣性組合導航系統，平臺搭載導遠INS570D高精度 RTK_SINS組合導航。算法運行計算機為神舟戰神Z7M-CT7NA，系統為Ubuntu系統。

圖2 實驗采用的無人駕駛平臺

3.2 實驗過程

本次實驗通過采用無人駕駛平臺，通過環繞長安大學汽車學院的方法進行建圖，并且對改進后的算法的外參進行標定。并對所建地圖精準度進行對比。重新建圖過程如圖 3所示，可以看到圖中里程計坐標隨著車輛行駛而進行變化，這種組合慣導方式構建地圖可以為后來無人駕駛車輛更好提供信息。實驗過程中，采用鐳神16線激光雷達，分別運行改進前以及改進后的算法，通過兩種不同算法的建圖效果對比來驗證實驗的可行性，并且更換鐳神32線激光雷達，以此驗證不同型號雷達對算法的適用性。

圖3 無人駕駛平臺慣性組合導航系統連接圖

3.3 實驗結果

實驗通過環繞汽車學院行駛一周，在運行改進過后的 LeGO-LOAM算法后，運行過程如圖 4所示，運行結束后的建圖結果如圖 6所示，相比于圖 5可以很明顯看見建圖精度較之前的有所提高。圖7是改進后用鐳神32線激光雷達進行建圖。可以看出更換激光雷達算法的可行性依然適用。

圖4 算法的建圖運行過程

圖5 改進前的鐳神16線激光雷達建圖

圖6 改進后的鐳神16線激光雷達建圖

圖7 改進后的鐳神32線激光雷達建圖

4 結論

本文針對LeGO-LOAM算法建圖中精度低的問題，結合了掃描上下文算法，對回環檢測問題進行改進，并且通過對外參進行重新的標定，通過在實際場景下進行建圖，并且對建圖效果進行對比可知，改進后的LeGO-LOAM算法，可以提高了建圖的精度。并且通過更換為鐳神32線的激光雷達，通過采用不同型號的雷達的方法來驗證實際場景下建圖的可行性。