崔 洋,顧恒之,徐 震
(長(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ù)支持。
使用高精度激光雷達(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á)到類似甚至更好的精度。
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)圖
為了提高處理效率和特征提取的準(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ì)。
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)景的建圖中仍存在很大的誤差。
掃描上下文算法是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è)。
在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)行全局建圖,可以提高建圖精度。
實(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)
本次實(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)連接圖
實(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á)建圖
本文針對(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)景下建圖的可行性。