基于LeGO-LOAM的實(shí)際場(chǎng)景下的同步定位與建圖方法

崔 洋，顧恒之，徐 震

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

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

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

1 LeGO-LOAM算法

1.1 LOAM算法以及存在的問題

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

1.2 LeGO-LOAM算法

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

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

1.3 具體過程

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

（1）計(jì)算每個(gè)點(diǎn)的平滑度c

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

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

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

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

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

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

2 算法改進(jìn)

2.1 回環(huán)檢測(cè)

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

2.2 掃描上下文算法

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

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

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

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

3 實(shí)車建圖

3.1 建圖場(chǎng)景

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

圖2 實(shí)驗(yàn)采用的無人駕駛平臺(tái)

3.2 實(shí)驗(yàn)過程

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

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

3.3 實(shí)驗(yàn)結(jié)果

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

圖4 算法的建圖運(yùn)行過程

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

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

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

4 結(jié)論

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