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

01簡(jiǎn)介

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

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

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

02方法

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

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

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

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

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

03實(shí)驗(yàn)及驗(yàn)證

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

圖 4 (a) KITTI基準(zhǔn)使用的平臺(tái)(b)真實(shí)場(chǎng)景及其對(duì)應(yīng)的點(diǎn)云。

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

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

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

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

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

圖8 新CMRR系統(tǒng)

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

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

圖11現(xiàn)場(chǎng)測(cè)試生成的地圖及其實(shí)際位置

04結(jié)論

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

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

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

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

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

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

責(zé)任編輯：彭菁