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

崔 洋，顧恒之，徐 震

（長安大學(xué) 汽車學(xué)院，陜西 西安 710064）

隨著現(xiàn)代汽車產(chǎn)業(yè)的發(fā)展，無人駕駛技術(shù)成為當(dāng)前汽車行業(yè)的高新前沿研究方向之一。同步定位與地圖重建（Simultaneous Localization And Mapping, SLAM）技術(shù)是無人駕駛技術(shù)中實現(xiàn)無人化的關(guān)鍵技術(shù)之一[1]。

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

1 LeGO-LOAM算法

1.1 LOAM算法以及存在的問題

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

1.2 LeGO-LOAM算法

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

圖1 LeGO-LOAM的系統(tǒng)結(jié)構(gòu)圖

1.3 具體過程

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

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

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

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

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

（4）再進(jìn)行一次篩選，得到精煉的邊緣點Fe和平面點集Fp，其中Fe∈Fme，F(xiàn)p∈Fmp。

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

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

2 算法改進(jìn)

2.1 回環(huán)檢測

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

2.2 掃描上下文算法

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

掃描上下文算法使用一次掃描的點云數(shù)據(jù)構(gòu)造矩形圖像掃描上下文，并構(gòu)造 KD樹，執(zhí)行最鄰近搜索，找到可能與當(dāng)前幀回環(huán)的多個相似幀；然后，分成扇形區(qū)域計算最小偏移量和相似性分?jǐn)?shù)，選擇相似性分?jǐn)?shù)最高的幀作為回環(huán)幀，求解當(dāng)前幀和回環(huán)幀之間的姿態(tài)關(guān)系，實現(xiàn)回環(huán)檢測。

2.3 回環(huán)改進(jìn)

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

3 實車建圖

3.1 建圖場景

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

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

3.2 實驗過程

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

圖3 無人駕駛平臺慣性組合導(dǎo)航系統(tǒng)連接圖

3.3 實驗結(jié)果

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

圖4 算法的建圖運行過程

圖5 改進(jìn)前的鐳神16線激光雷達(dá)建圖

圖6 改進(jìn)后的鐳神16線激光雷達(dá)建圖

圖7 改進(jìn)后的鐳神32線激光雷達(dá)建圖

4 結(jié)論

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