基于正態(tài)分布變換(NDT)的實時三維SLAM方法

01簡介

礦井救援機(jī)器人在執(zhí)行災(zāi)后探測救援任務(wù)時，巷道空間的準(zhǔn)確描述和自定位是一個關(guān)鍵問題。在復(fù)雜的巷道環(huán)境中，特別是災(zāi)害發(fā)生后，三維同時定位與地圖構(gòu)建(SLAM)是一項有效但時間緊迫且極具挑戰(zhàn)性的任務(wù)。本文提出了一種新的基于正態(tài)分布變換(NDT)的實時三維SLAM方法，該方法利用姿態(tài)圖優(yōu)化和回環(huán)檢測來進(jìn)一步提高mapping的一致性。我們創(chuàng)新地提取巷道中的地板和墻壁作為平面節(jié)點來構(gòu)建地標(biāo)約束，此外還通過無損檢測應(yīng)用激光雷達(dá)里程計中的姿態(tài)節(jié)點。

配備多傳感器的煤礦救援機(jī)器人(CMRR)可以檢查事故現(xiàn)場并重建與災(zāi)區(qū)的通信。聲音、圖像、溫度、氣體濃度等數(shù)據(jù)也可以傳輸?shù)街笓]中心，以便制定救援策略。但是，煤礦巷道環(huán)境復(fù)雜，場景不可預(yù)測，現(xiàn)有的機(jī)器人定位方法有限。煤礦災(zāi)害導(dǎo)致先驗的路標(biāo)和地圖不再可靠，GPS也無法到達(dá)現(xiàn)場，在災(zāi)后電力中斷的情況下，其他依賴基站的無線定位方法也無法使用?；诤轿煌扑愕臋C(jī)器人姿態(tài)估計由于在粗糙地形上、特別是在旋轉(zhuǎn)情況下的累積誤差，無法提供精確的長期估計。目前，大多數(shù)煤礦救援機(jī)器人（CMRR）都是基于視頻進(jìn)行遠(yuǎn)程操作和定位，但這限制了機(jī)器人的運(yùn)動范圍和安全性。

本文利用里程計和回環(huán)檢測的方法，建立了一種輕量級、有效的環(huán)路檢測方法，以構(gòu)建全局一致的地圖。在公共數(shù)據(jù)集上對該方法進(jìn)行了評估，并在礦井下進(jìn)行了現(xiàn)場試驗。結(jié)果表明，該算法具有較低的計算復(fù)雜度和漂移，可為煤礦救援機(jī)器人（CMRR）提供姿態(tài)估計和環(huán)境描述，實現(xiàn)煤礦救援任務(wù)中的遙控輔助和自動導(dǎo)航。

02方法

圖1 煤礦井下典型環(huán)境(a)主巷；(b)采煤工作面附近半構(gòu)造環(huán)境區(qū)域；(c)混亂場景

我們的工作是基于無損檢測方法來構(gòu)建激光雷達(dá)里程計。與尋找點、線、面等相關(guān)特征的ICP及其變體不同，無損檢測使用范圍掃描來描述環(huán)境模型。將空間劃分為單元，并分配正態(tài)分布來表示測量空間的概率模型。數(shù)據(jù)掃描中的點與模型掃描中的一系列分布相匹配。所提出的算法框架如圖2所示。

圖2 系統(tǒng)概述，顯示所有步驟執(zhí)行的預(yù)濾波器，激光雷達(dá)里程計，約束，并與姿態(tài)圖形優(yōu)化線程閉環(huán)

為了處理一個數(shù)據(jù)集中的多個模型，在迭代RANSAC之前，利用點云表面的法線方向進(jìn)行特征濾波。RANSAC用于檢測平面的目標(biāo)點云僅限于法線方向距離垂直法線或水平法線在一定閾值范圍內(nèi)的目標(biāo)點云。如果濾波后有足夠的點，則進(jìn)一步使用迭代RANSAC進(jìn)行平面檢測。在真實環(huán)境中，特別是結(jié)構(gòu)化礦井中，我們要檢測的平面地標(biāo)通常是側(cè)壁或地面，它們通常是相互垂直的。特征濾波可以減少RANSAC中處理的點，加快平面地標(biāo)的提取速度。圖3給出了使用上述方法對平面點分段的直觀解釋。

圖3利用迭代RANSAC對平面點進(jìn)行掃描分割(a)將藍(lán)、綠、青色點建模為平面特征，并將不同顏色的地基準(zhǔn)點建模為平面特征，紅色的點被特征過濾器過濾(b)掃描的原始點云

03實驗及驗證

初始測試采用在SLAM社區(qū)廣泛用于算法評估的KITTI odometry基準(zhǔn)。圖4描繪了基準(zhǔn)測試使用的平臺和數(shù)據(jù)集中的典型場景。該平臺包含Velodyne HDL-64E 3D激光掃描儀和OXTS RT3003慣性和GPS導(dǎo)航系統(tǒng)等?，F(xiàn)實世界的場景包括有建筑物的城市環(huán)境、有周圍植被的鄉(xiāng)村環(huán)境和有開放場景的高速公路。由于KITTI測程基準(zhǔn)已經(jīng)糾正了激光雷達(dá)數(shù)據(jù)收集中的點畸變，因此在實驗中使用了原始數(shù)據(jù)集。該算法在CPU: i7-3555LE (2.5 GHz, 4核)和8G內(nèi)存的IPC上運(yùn)行。

圖 4 (a) KITTI基準(zhǔn)使用的平臺(b)真實場景及其對應(yīng)的點云。

圖5是基于無損檢測的圖SLAM的建圖結(jié)果。可以看到，點云是一致的，高建圖質(zhì)量。得益于環(huán)路檢測和圖形優(yōu)化，該地圖與真實場景極為吻合。地圖在保證地圖精度和細(xì)節(jié)的同時節(jié)省了點云的存儲空間

圖5（a)(b)表示序列00上構(gòu)建的點云圖(b)(c)不同點的高程(c)城市場景中一個十字路口的近景

從圖6的軌跡可以看出，在序列00、05和07中，LeGO-LOAM在準(zhǔn)確性和一致性方面表現(xiàn)最好。這些序列都在居民區(qū)和城市道路中，包含了許多環(huán)境中的線和平面特征。在所有序列上，該算法在與地面真值的一致性和符合性方面都優(yōu)于ICP-graph-SLAM和NDT-Odometry方法。與NDT-Odometry相比，得益于環(huán)路檢測和優(yōu)化過程，該方法的姿態(tài)估計不會沿路徑發(fā)散。

圖6 NDT-graph-SLAM（藍(lán)色），NDT-Odometry（綠色），ICP-graph-SLAM（黑色），（a)有環(huán)00序列，(b)無環(huán)01序列，(c)有環(huán)05序列，(d)有環(huán)07序列

在之前的工作中，設(shè)計了一種多驅(qū)動履帶式行走機(jī)構(gòu)，以保證CMRR在處理復(fù)雜地形時的高機(jī)動性。圖8為新一代CMRR系統(tǒng)，一個具有多個感知傳感器的防爆移動平臺。該系統(tǒng)配備了獨立的傳感器套件，包括3D掃描激光雷達(dá)(rslidar-16,12V/9W)、電機(jī)驅(qū)動旋轉(zhuǎn)2D激光雷達(dá)(UTM-30LX, 12V/8.4W)、IMU (Xsens Mti-G-710，在套件中)和RGB-D攝像頭(Xtion Pro Live，由USB2.0接口供電)。所有傳感器通過電源和網(wǎng)線的物理隔離連接到移動平臺。該套件采用阻燃抗靜電材料制成，滿足煤礦應(yīng)用要求。在這項工作中，只使用3D激光雷達(dá)進(jìn)行SLAM工作。

圖8 新CMRR系統(tǒng)

在充分模擬地下礦山環(huán)境的中國礦業(yè)大學(xué)瓦斯爆炸實驗室進(jìn)行了現(xiàn)場試驗。圖9的俯視圖是通過我們的SLAM算法生成的。導(dǎo)線從A點開始，繼續(xù)到B、C、d點，然后機(jī)器人從C、B、E點返回，在A點形成一個小環(huán)路。最后，機(jī)器人穿過F、G點，在h點停止。有四個連接點(A、B、F、G)，只有從A點到C點的區(qū)域被照亮。它構(gòu)成了一條長約200米，寬約2.5米的狹長的巷道，從C到D，有輕微的曲率，沒有照明。F-G部分為變寬巷道，其中一條巷道寬度4.75 m，長度15 m；另一個寬2.5米，長55米。從G到H區(qū)域為向下傾斜的巷道。圖10是圖13中從B到C、A點、F到G、G點分別生成的四張局部地圖和照片。

圖10 NDT-SLAM生成的地圖俯視圖，包含多個掃描姿勢。機(jī)器人從A點開始，按照這個順序A- b - c - b - d - c - b - e -A- f - g - h。在A、C、D、F、G交叉處設(shè)置固定反射標(biāo)記

圖11現(xiàn)場測試生成的地圖及其實際位置

04結(jié)論

這篇論文提出了一種具有閉環(huán)和圖優(yōu)化功能的三維激光SLAM算法，通過采用3D-NDT構(gòu)建激光雷達(dá)測距約束和迭代RANSAC提取地標(biāo)平面，提高了建圖的一致性和定位的魯棒性。同時，通過檢測關(guān)鍵幀之間的外觀相似度和距離實現(xiàn)輕量閉環(huán)，并采用多線程加速提高計算速度以滿足實時應(yīng)用。

這篇論文對于撰寫建造機(jī)器人的SLAM應(yīng)用綜述有以下幾點啟示：

（1）了解不同的SLAM算法：通過研究不同的SLAM算法，特別是具有閉環(huán)和圖優(yōu)化功能的算法，可以深入了解SLAM技術(shù)的發(fā)展和應(yīng)用。

（2）關(guān)注傳感器融合：考慮多傳感器的融合，如將IMU和紅外視覺等傳感器結(jié)合起來，可以提高在退化環(huán)境中的精度性能。

（3）實地測試和評估：通過在實際環(huán)境中進(jìn)行測試和評估，可以驗證SLAM算法在特定應(yīng)用場景中的有效性和適用性。

（4）解決特定環(huán)境中的問題：針對特定環(huán)境中定位方法有限的問題，可以探索基于無損檢測的多傳感器集成定位方法，以提高精確定位的能力。

責(zé)任編輯：彭菁