基于學(xué)習(xí)的激光雷達(dá)定位退化場景估計(jì)

摘要

基于激光雷達(dá)的定位與建圖系統(tǒng)是許多現(xiàn)代機(jī)器人系統(tǒng)中的核心組成部分。其直接繼承了環(huán)境的深度與幾何信息，從而允許精確的運(yùn)動(dòng)估計(jì)與實(shí)時(shí)生成高質(zhì)量的地圖。然而，不充分的環(huán)境約束會(huì)導(dǎo)致定位失敗，這種情況經(jīng)常發(fā)生在諸如隧道等對稱的場景中。本文的工作準(zhǔn)確地解決了這個(gè)問題，通過神經(jīng)網(wǎng)絡(luò)法來檢測機(jī)器人的周圍環(huán)境是否發(fā)生退化。我們特別關(guān)注于激光雷達(dá)的幀間匹配，因?yàn)槠涫窃S多激光雷達(dá)里程計(jì)的至關(guān)重要的部件。與之前的方法不同的是，我們的方法直接根據(jù)原始的點(diǎn)云測量來檢測定位失敗的可能，而不是在配準(zhǔn)過程中檢測。此外，之前的方法在泛化能力上有所局限，因?yàn)樾枰藶檎{(diào)節(jié)退化檢測的閾值。我們提出的方法通過從一系列不同的環(huán)境中學(xué)習(xí)來使得網(wǎng)絡(luò)在各個(gè)場景中都表現(xiàn)得更好，從而避免了這個(gè)問題。此外，該網(wǎng)絡(luò)專門針對模擬數(shù)據(jù)進(jìn)行訓(xùn)練，避免在具有挑戰(zhàn)性和退化且通常難以訪問的環(huán)境中進(jìn)行艱巨的數(shù)據(jù)收集。所提出的方法在未經(jīng)任何修改的情況下，在具有挑戰(zhàn)性的環(huán)境和兩種不同的傳感器類型上進(jìn)行的實(shí)地實(shí)驗(yàn)中進(jìn)行了測試。觀察到的檢測性能與最先進(jìn)方法在專門調(diào)整閾值后的性能相當(dāng)。

主要貢獻(xiàn)

提出一種基于學(xué)習(xí)的方法，來檢測單幀點(diǎn)云在六個(gè)自由度上是否發(fā)生退化。

提出一種驗(yàn)證方法，來驗(yàn)證四足機(jī)器人在挑戰(zhàn)性與退化場景下的定位能力。

所有部分都經(jīng)過完備的設(shè)計(jì)與實(shí)現(xiàn)，包括數(shù)據(jù)集的采集與生成。相關(guān)的部分均會(huì)在機(jī)器人社區(qū)開源。

方法論

1. 問題建模

本文想檢測機(jī)器人在某個(gè)時(shí)刻的定位能力：（localizability）。這可以通過一個(gè)6維的向量表示：

其中，x， y， z表示平移方向上的分量，Φ，θ，ψ表示旋轉(zhuǎn)方向上的分量（歐拉角表示）。向量d_k的每個(gè)分量都是二值的（0或者1，其中0表示該分量上的定位信息可靠，1則表示不可靠）。

估計(jì)d_k的過程可以構(gòu)建為一個(gè)多標(biāo)簽二值分類問題，并且通過一個(gè)神經(jīng)網(wǎng)絡(luò)分類器得到結(jié)果。

2. 訓(xùn)練數(shù)據(jù)生成

按照上述的問題建模方式，一個(gè)重要的點(diǎn)是如何生成帶有標(biāo)簽的訓(xùn)練數(shù)據(jù)？具體而言，對一個(gè)點(diǎn)云，如何評估它在六個(gè)自由度上的退化情況？

本文提出的方法是加擾動(dòng)然后看配準(zhǔn)誤差的方式。對于某個(gè)點(diǎn)云s以及采集到它時(shí)機(jī)器人的位姿T，生成M個(gè)子點(diǎn)云，生成一個(gè)子點(diǎn)云的方式為：

1. 對位姿進(jìn)行擾動(dòng)，擾動(dòng)的值通過對0均值高斯分布進(jìn)行采樣，每個(gè)維度上的高斯分布的δ為：

2. 根據(jù)擾動(dòng)后的位姿，在仿真環(huán)境中，通過光線投射來獲得一個(gè)新的子點(diǎn)云。

將這M個(gè)子點(diǎn)云與原始點(diǎn)云進(jìn)行配準(zhǔn)，配準(zhǔn)方法為點(diǎn)到面的ICP方法。計(jì)算每個(gè)子點(diǎn)云配準(zhǔn)得到的transform與原始擾動(dòng)的transform的差值e，分解到六個(gè)維度上取絕對值，并相加求平均值得到e_p。通過評估e_p在6個(gè)維度上是否超過特定的閾值，來判斷原始點(diǎn)云是否容易在特定的維度上發(fā)生退化。6個(gè)維度上的閾值為：

3. 網(wǎng)絡(luò)結(jié)構(gòu)

上一步獲得了帶有標(biāo)簽的點(diǎn)云，這一步則是通過網(wǎng)絡(luò)來學(xué)習(xí)并訓(xùn)練。本文所采取的網(wǎng)絡(luò)結(jié)構(gòu)基于3D ResUNet（見論文Fully convolutional geometric features），其具體網(wǎng)絡(luò)結(jié)構(gòu)為：

分類網(wǎng)絡(luò)則是一個(gè)5層的MLP，輸出是一個(gè)6維的向量。

該網(wǎng)絡(luò)的損失函數(shù)定義為：

其中，t表示第k個(gè)點(diǎn)云在第i個(gè)分量上的標(biāo)簽，而p則表示網(wǎng)絡(luò)預(yù)測的對應(yīng)的概率值。

由于網(wǎng)絡(luò)預(yù)測的是一個(gè)概率值，在使用時(shí)本文通過對各個(gè)維度設(shè)置不同的閾值來確定各個(gè)維度是否發(fā)生退化，其值為

方法論

1. 消融實(shí)驗(yàn)

在該實(shí)驗(yàn)中，將特征提取網(wǎng)絡(luò)換為Point-Net，并且比較了更換前后的各項(xiàng)分類性能：

從表格中可以得出，在大部分指標(biāo)中，ResUNet均高于PointNet。

2.實(shí)地實(shí)驗(yàn)

在三個(gè)場景中進(jìn)行了場地實(shí)驗(yàn)：隧道、開闊戶外與城鎮(zhèn)場景。采用的SLAM框架為CompSLAM（見論文Complementary multi–modal sensor fusion forresilient robot pose estimation in subterranean environments）。這個(gè)框架中，所用的評估退化的方式是Ji Zhang的論文“On Degeneracy of Optimization-based State Estimation Problems”中提到的方法（該論文在泡泡機(jī)器人-點(diǎn)云時(shí)空中亦有詳細(xì)翻譯，有興趣同學(xué)可以自行搜索）。CompSLAM在檢測到激光雷達(dá)出現(xiàn)退化后，切換為VIO的結(jié)果進(jìn)行遞推，而在沒有退化的場景中，用VIO的結(jié)果作為LOAM的先驗(yàn)進(jìn)行幀間配準(zhǔn)。本文將該方法替換為所提出的基于學(xué)習(xí)的方法，并與原始版本的CompSLAM進(jìn)行比較。

a. 隧道場景：

下圖給出了隧道場景中的建圖效果。其中左邊三幅圖為Ji zhang的方法采用不同閾值后的結(jié)果，最右則為本文提出的結(jié)果。而下面的條狀圖則表示各個(gè)維度出現(xiàn)退化的時(shí)間。可以看出，本文提出的方法與Ji Zhang的方法在閾值為110時(shí)的效果類似。

b. 開放戶外場景：

同隧道場景。

c. 城市場景：

這個(gè)場景中沒有做對比試驗(yàn)，而是僅僅測試了提出的方法。測試場景為咖啡店的內(nèi)部與外部。機(jī)器人從室內(nèi)走到室外并且回到室內(nèi)，可以看到建圖保持了較好的一致性，并且學(xué)習(xí)到的6維向量表示全程都沒有發(fā)生退化情況

3. 泛化實(shí)驗(yàn)

對于隧道場景，利用VLP-16與Ouster 128線的激光雷達(dá)進(jìn)行測試。從下圖中可以看出，利用Ji zhang的方法（即求一個(gè)特定矩陣的最小特征值），對于兩個(gè)傳感器，其最小特征值的差別是巨大的。然而本文所提出的方法仍能比較好的檢測到隧道的特定段發(fā)生了退化，并且與場地實(shí)驗(yàn)中的隧道實(shí)驗(yàn)保持一致。

評價(jià)與未來工作

1. 個(gè)人評價(jià)

這項(xiàng)工作的創(chuàng)新在于直接對點(diǎn)云進(jìn)行退化的評估，而不是根據(jù)配準(zhǔn)的結(jié)果。數(shù)據(jù)的生成與訓(xùn)練的方式都比較直接易懂，并且實(shí)驗(yàn)的效果也很好。

這個(gè)方法是應(yīng)用在類似于LOAM的系統(tǒng)上的，因此制作訓(xùn)練集的時(shí)候，只用單幀點(diǎn)云的匹配結(jié)果來生成退化label。對于類似于Fast-LIO這樣直接進(jìn)行scan-map配準(zhǔn)的方法，這種制作訓(xùn)練集并訓(xùn)練的方式不一定可靠：因?yàn)樵谝恍﹫鼍跋拢赡艹霈F(xiàn)scan-scan配準(zhǔn)退化，但scan-map不一定是退化的（local map有更豐富的信息）。解決這個(gè)問題的途徑之一，可以在訓(xùn)練的時(shí)候也采用scan-map的配準(zhǔn)方法來評價(jià)配準(zhǔn)誤差。

2. 未來工作

本文的作者提出的未來工作有：

1）網(wǎng)絡(luò)直接出6x6的位姿協(xié)方差矩陣。

2）根據(jù)退化信息來決定優(yōu)化的時(shí)候，僅僅優(yōu)化部分維度而不是所有維度。

審核編輯 ：李倩