IROS 2023|CSIRO與昆士蘭科技大學釋出自然環境中的深度魯棒多機器人重定位
【Deep Robust Multi-Robot Re-localisation in Natural Environments】
文章連結：[2307.13950] Deep Robust Multi-Robot Re-localisati...
自然環境中的深度魯棒多機器人重定位
Deep Robust Multi-Robot Re-localisation in Natural Environments
摘要：重新定位的成功對於在先前地圖內操作或在現實場景中相互關聯的機器人的實際部署具有至關重要的影響。使用單一模式，在森林等具有挑戰性的環境中，地點識別和定位可能會受到影響。為了解決這個問題，我們提出了一種使用鐳射雷達影像跨模態來防止基於鐳射雷達的重新定位失敗的策略。我們的解決方案依靠自我監督的 2D-3D 特徵匹配來預測對齊和錯位。利用深度網路進行鐳射雷達特徵提取和點雲之間的相對姿態估計，我們訓練一個模型來評估估計的變換。透過分析嵌入空間中的影像-鐳射雷達相似性以及歐幾里得空間中兩種模態所見區域內可用的幾何約束，學習預測是否存在未對準的模型。使用真實資料集（離線和線上模式）的實驗結果證明了所提出的pipeline在非結構化自然環境中穩健重新定位的有效性。
I. 簡介
機器人技術中的（重新）定位是指在先前繪製的已知環境中確定機器人當前姿態（位置和方向）的過程。這項任務對於機器人無縫執行操作至關重要，即使它們在跟蹤其位置時遇到暫時困難。例如，“喚醒”問題涉及機器人在關閉或斷電後需要確定其位置。儘管依賴視覺[1]-[4]或鐳射雷達資料[5]-[8]的基於學習的重新定位方法取得了重大進展，但設計強大且可靠的重新定位技術仍然是一個挑戰，特別是在非結構化領域。 ，自然環境。這種環境缺乏鮮明的特徵，並且由於植被生長和天氣條件而隨著時間的推移而變化，影響了重新定位的穩健性[9]。
由於鐳射雷達和影像的固有侷限性，依靠單一模態進行重新定位很難提取適當的特徵（在複雜的自然場景中）。為了解決這個問題，我們建議整合一個自監督的影像到鐳射雷達特徵匹配過程，以預測由位置識別、姿態估計和假設驗證三個模組組成的pipeline中的重新定位失敗，每個模組都利用學習方法。對於位置識別和姿態估計模組，我們使用 EgoNN [10]，一個端到端深度重定位網路。藉助我們的鐳射雷達 SLAM 系統，即 Wildcat [11]，我們生成鐳射雷達子圖和姿態圖，其中包含機器人節點和幾何資訊，並將它們儲存在資料庫中。 EgoNN 在 Wildcat 子圖上進行離線訓練。在推理時，透過將查詢子圖與資料庫中的子圖進行比較，使用預訓練網路進行重新定位。一旦估計了查詢子圖和頂部候選子圖之間的相對位姿（圖1），所提出的假設驗證模組就會透過與查詢子圖和頂部同時捕獲的影像之間的跨模態比較來評估變換的正確性-候選子圖。實驗結果證明了所提出的pipeline在實現準確重新定位方面的有效性。
這項工作的主要貢獻可概括如下：
• 我們建議整合自監督影像到鐳射雷達特徵匹配過程來預測重新定位失敗。
• 我們提出了深度重新定位方法（R3Loc）的完整流程來解決多機器人重新定位問題。
• 我們在離線的大規模自然資料集和線上真實機器人的類似森林的環境中展示了我們的pipeline的有效性。
二.相關工作
本節回顧現有的鐳射雷達位置識別（LPR）演算法並討論現有的重新定位研究。最後，回顧了跨模態 PR 和配準中與影像鐳射雷達模態感知相關的工作。
A. 基於鐳射雷達的定位
人們已經為 LPR 提出了一系列演算法。傳統方法[12]-[16]將點雲編碼為表示整個點雲的全域性描述符或透過將點雲分割成補丁來將點雲編碼為多個區域性描述符。然而，這些手工方法通常依賴於旋轉，並且不能有效地為非結構化環境生成判別描述符。
Deep LPR在過去幾年中表現出了出色的成果。這些方法透過深度神經網路處理點雲以提取區域性特徵。特徵要麼直接用於地點識別，例如[17]、[18]中的工作，要麼使用一階池化技術進行聚合，例如GeM [19]、NetVLAD [20]或[7]中使用的二階池化]，[21]，生成點雲的全域性描述符[5]-[8]，[22]。 EgoNN [10] 和 LCDNet [23] 等方法在位置識別時估計兩個點雲之間的相對姿態。 EgoNN 計算關鍵點座標、區域性描述符和區域性頭部的顯著性。隨後，它透過匹配關鍵點並使用 RANSAC 刪除異常值來估計查詢點雲和頂級候選點雲之間的 6DoF 相對變換。 LCDNet 利用最佳傳輸 (OT) 理論端到端地訓練區域性特徵來匹配特徵，並最終使用奇異值分解 (SVD) 估計相對姿態，從而使整個pipeline可微分，因此可學習。然而，在測試時，LCDNet 採用 RANSAC 進行相對姿態估計，在自然環境中容易出現發散。 SpectralGV [24] 專注於對 top-k 檢索候選者進行重新排序，引入了一種計算高效的光譜重新排序方法來改進定位。
B. 跨模態定位
有一些與 PR 相關的工作，旨在透過利用鐳射雷達掃描和在同一地點捕獲的影像來增強地點識別。 [25]-[27]等作品在多模態融合的早期階段整合了鐳射雷達和視覺測量，使用投影技術將它們編碼成全域性描述符；然而，以尺寸損失為代價。相比之下，[28]-[30]等作品將鐳射雷達和視覺資料分別編碼（後期融合）到影像和點雲嵌入中，然後將它們聚合以建立雙峰全域性描述符。為了處理光照條件（影響影像特徵的質量），AdaFusion [31] 採用了一種注意機制，避免在影像質量難以識別時將兩種模式視為同等重要，反之亦然。
在計算機視覺領域，I2P [32] 和 2D3DMatchNet [33] 等工作已被提出，重點關注影像到鐳射雷達的配準。 I2P 訓練網路透過分類和逆相機投影兩個步驟來估計一對影像和點雲之間的位姿。 I2P 使用注意力機制對相機視錐體內外的鐳射雷達點進行分類。它使用逆相機投影和分類預測來最佳化鐳射雷達框架中的姿態。 2D3D-MatchNet 在三元組損失（錨定影像、正點雲和負點雲）中學習 2D 影像和 3D 點雲描述符，因為相似的影像鐳射雷達描述符被推近，而負對被推開。最近，SLidR [34]提出基於 2D 影像上的區域性相似區域及其相應的 3D 補丁獲得知識蒸餾來查詢點雲和影像對之間的相似性。
三． （R3LOC）：深度穩健的多機器人重新定位
我們的目標是根據車隊在初始會話中生成的先前（參考）地圖，提高重訪會話中機器人（重新）定位的魯棒性和可靠性機器人在非結構化的自然環境中的應用。
我們之前的地圖是由 Wildcat SLAM [11] 建立的，是一個姿態圖 G = (V, E)，由機器人的姿態（節點）V ∈ R6 和其間的邊 E ∈ SE(3) 組成。簡而言之，Wildcat 將鐳射雷達和慣性測量整合到滑動視窗定位和繪圖模組中。該模組使用連續時間軌跡表示來減少運動引起的地圖失真。未失真的子圖進一步用於位姿圖最佳化，以消除環路閉合時的漂移。生成的子圖 Si, i ∈ {1, ..., n} 也儲存在先驗圖中。更多詳細資訊可以在 Wildcat 論文 [11] 及其參考文獻中找到。
在從重訪會話生成新的子地圖（即查詢點雲 Sq）後，將形成一個深度鐳射雷達 PR 網路，如第 2 節所述。 III-A，用於使用相似性度量將 Sq 與先前圖的所有子圖 Si 進行比較，以找到頂部候選 St1。子圖 Sq 和 St1 之間的初始相對位姿 Tt1,q ∈ SE(3) 透過 RANSAC [35] 使用相應的關鍵點（參見第 III-A 節）進一步估計。這個最初的猜測後來透過 ICP（一種 3D 形狀配準的迭代演算法）進行了改進 [36]。然而，在使用之前需要對其進行評估，以將新節點合併到位姿圖中。誤報邊緣可能會導致生成較差的軌跡或 SLAM 中的最佳化失敗。
為了健全性檢查細化的相對位姿，我們提出了查詢影像 Iq ∈ R3×W ×H（W 和 H 是影像寬度和高度）之間的比較，即與點雲 Sq 同時獲得的影像，和使用估計的相對位姿的點雲 St1。為此，我們訓練一個自監督網路來檢測 2D 和 3D 對應特徵並研究 PR 輸出的正確性。此外，我們使用相對姿態將 St1 的 3D 關鍵點投影到影像 Iq 上，以檢查影像-鐳射雷達對應關係是否落在影像的同一區域。如果是這樣，相對位姿將傳遞給 SLAM 系統將新邊 Et1,q 合併到位姿圖（先前圖）中。否則，我們拒絕相對姿勢。秒。 III-B 詳細說明了假設驗證。圖 2 概述了我們的 R3Loc pipeline、其元件以及它們之間的關係。
圖 2：所提出的深度魯棒多機器人重定位系統 (R3Loc) 的框圖。
A. 深度重定位模組
我們的深度重定位模組基於 EgoNN [10]。使用輕型 3D CNN 網路，EgoNN 訓練全域性描述符 dG ∈ R256 和幾個區域性嵌入 dLt ∈ R128，其中 t ∈ {1, ..., M } 是 USIP [37] 檢測到的每個點中的關鍵點數量雲。全域性描述符是利用 GeM 池化 [19] 的全域性頭中特徵圖 FG ∈ RK×128 元素的聚合。 K是全域性頭部中區域性特徵的數量。透過處理區域性特徵圖 FL ∈ RM×64 的元素，在區域性頭中生成關鍵點描述符。使用兩層多層感知器（MLP）和 tanh 函式模組來計算每個點雲中的區域性嵌入座標。全域性描述符用於 PR，而區域性描述符用於本地化。
B.深度假設驗證
為了接受或拒絕重定位模組的輸出，我們利用跨模態感知來比較在查詢點雲Sq時捕獲的影像Iq和重定位模組估計的頂級候選點雲St1 。為此，需要使用本地分支估計的相對姿態 Tt1,q 將頂部候選投影到查詢影像上。如果姿態估計正確，則投影點必須與其相應的影像畫素重疊。為了評估這一點，必須提取並匹配相應的 2D 和 3D 特徵。
然而，由於其稀疏性，諸如[38]之類的手工方法不適合鐳射雷達點雲上的特徵提取，也不適合檢測影像上的相似特徵以建立精確的點到畫素匹配。逐點深度特徵描述符，例如[32]、[33]，儘管優於傳統技術，但在存在遮擋或運動模糊的情況下可能會受到影響，這在機器人技術中是不可避免的。因此，我們利用一種稱為超畫素驅動鐳射雷達表示（SLidR）[34]的深度影像到鐳射雷達自監督蒸餾方法，它將一組畫素與一組點聯絡起來。
SLidR 使用視覺特徵訓練 3D 點表示，以進行語義分割和物件檢測。跨模態表示學習的動機是帶註釋的 3D 點資料的稀缺性和影像標籤的豐富性。 SLidR 將特徵知識從超畫素（即具有視覺相似性的影像區域）傳輸到超點（即透過超畫素反投影分割的點組）。使用 SLIC [39] 將影像 Iq 分割為最多 250 個超畫素。重要的是，SLidR 不需要資料標籤來預訓練 3D 網路。給定同步鐳射雷達和相機資料流以及校準引數，SLidR 提取超畫素及其相應超點的特徵。從使用 [40] 訓練的預訓練 ResNet-50 主幹中提取的 2D 特徵，用作訓練 3D 稀疏殘差 U-Net 主幹 [41] 的監督訊號，使用對比損失來對齊池化 3D 點和 2D 畫素特徵。
使用 SLidR，我們的方法使用餘弦相似度比較超畫素 spIq i 的提取特徵（其中 i 是影像 Iq 中的超畫素數量）與超點 spSt1 j 的提取特徵（其中 j 是點雲 St1 中的超點數量）：
這裡f和g分別表示平均池化後的超畫素和超點特徵。符號⟨.、.⟩表示內積，∥.∥ L2範數。
現在，我們定義兩個度量，一個在特徵空間中，一個在歐幾里得空間中，以接受或拒絕重新定位。首先，我們使用相應的超畫素和超點特徵的平均餘弦相似度（MCS），即 1 L P i=j csij 來判斷點雲 Sq 和 St1 是否代表同一個地方。 L 是根據等式計算的相似性矩陣主對角線上的超畫素-超點對的總數。 (1).低 MCS 值是我們的重新定位模組出現誤報案例的指標。
其次，為了評估 EgoNN 估計的相對姿勢的準確性，我們為每個超畫素 spIq 識別前 5 個候選超點，表示為 spSt1 @5。我們將這些前 5 個超級點 spSt1 @5 中每一個的質心投影到影像 Iq 上。我們找到投影質心最接近 spIq 質心的超點 spSt1 c ，並選擇它作為 spIq 的對。我們檢查是否spSt1 c 的投影質心落在 spIq 範圍內，如果落在 spIq 範圍內，我們將其視為匹配，否則視為不匹配。我們計算整個對集中超畫素-超點不匹配對的百分比，以確定是拒絕還是接受重新定位。然後我們定義對齊比如下：
其中 n 是根據上述過程計算出的超畫素-超點不匹配對的數量。定義兩個相似性和對齊度量，我們訓練一個簡單的多類支援向量分類器（SVC），yi = K(MCSi, νi)，來預測對 i 是否屬於匹配、不匹配或不匹配類別，其中 yi ∈ {matched ，不匹配，不匹配}。
四．實驗結果
在本節中，我們提出以下結果：在大規模自然資料集 Wild-Places [9]（由 Venman 和 Karawatha 序列組成）上評估重定位模組及其與 Scan Context [12] 的比較（作為一種與鐳射雷達 SLAM 廣泛整合的手工 PR 方法），在同一資料集上評估跨模式定位。最後，我們在機器人系統的喚醒問題場景中評估整個提議的 R3Loc 流程。 EgoNN 和 SLidR 均在 Wild-Places 資料集上進行訓練。對於 EgoNN，我們遵循[9]中描述的訓練分割。然而，為了進行測試，我們在相反方向收集的兩個 Venman 序列上評估了模型。當在重訪會話中操作的機器人在相反方向生成的先前地圖內行進時，這種序列間 PR 評估模擬了喚醒問題。按照預設設定，使用相同的序列來評估掃描上下文。為了進行評估，當預測與肯定的基本事實相差 3 m 以內時，我們定義為真正的肯定重訪。
對於 SLidR，我們使用來自 Venman 的一個序列上約 1750 個匹配的鐳射雷達影像對（同時捕獲的影像對）來訓練和驗證網路。我們從驗證部分建立了三個測試集，透過增強影像和點雲之間的相對變換來建立匹配和不匹配的對，並透過隨機配對在不同位置捕獲的影像和點雲來獲得不匹配的對。這允許針對三種最常見的 EgoNN 輸出情況測試 SLidR。我們還在澳大利亞布里斯班昆士蘭先進技術中心 (QCAT) 非結構化區域收集的新資料集上測試了擬議的驗證pipeline。
A. EgoNN 離線評估
圖 3 顯示了 EgoNN 和 Scan Context 之間的 top-K Recall 曲線。如圖所示，EgoNN 的效能幾乎比 Scan Context 高出兩倍，這表明 Scan Context 在森林等雜亂環境中生成獨特且旋轉不變的描述符的侷限性。為了評估重定位精度，我們將估計的相對變換與地面實況進行比較，並計算旋轉和平移誤差分別在 5° 和 2 m 以內的成功率。由於該方法無法僅根據全域性描述符來估計 6DoF 旋轉和平移，因此未針對掃描上下文執行此評估。當僅使用關鍵點和透過 RANSAC 估計相對變換時，EgoNN 的成功率約為 40%。然而，在使用 ICP 改進估計變換後（我們將點雲下采樣到 40 釐米空間解析度以進行線上配準），成功率增加到 78%。這表明，儘管 EgoNN 在地點識別方面取得了高效能，但提取的關鍵點在非結構化環境中並不能很好地重複以實現準確的重新定位。
圖 3：EgoNN 和 Scan Context 之間的 Recall@K 效能。
B.SLidR 離線評估
圖4 分別示出了頂行、中行和底行中匹配、不匹配和不匹配對的示例。如圖所示，相似性矩陣（第二列）和投影向量（第三列）是從第 2 節中描述的過程獲得的。 III-B 是區分匹配、不匹配和不匹配對的良好措施。圖 5 顯示了驗證集上大約 250 個匹配對和 230 個不匹配和不匹配對（總共超過 700 對）計算的 MCS 和 ν 的箱線圖。不匹配和匹配/不匹配對之間 MCS 的顯著差異允許以高置信度對不匹配的進行分類。此外，匹配對的大 ν 有助於將它們與其他對進行分類。然而，我們觀察到如果 MSC 和 ν 一起使用，當訓練和測試環境不同時，它可以提高泛化能力。因此，我們訓練了一個多類五次多項式 SVC 模型 K(MCS, ν) 來預測一對是否屬於匹配、不匹配或不匹配的類別。
圖 4：匹配（上）、不匹配（中）和不匹配（下）對的示例。從左到右：影像上的投影點、超點-超畫素相似度矩陣、基於相似度矩陣中頂部候選的投影誤差向量。當影像和點雲在同一位置捕獲時，我們提出的驗證指標，即平均餘弦相似度（MCS）和對齊比（ν）（透過第III-B節中描述的過程計算）用於識別真實的/假陽性 PR 並相應地預測重新定位成功或失敗。
C. 整個pipeline的線上評估
為了評估我們的pipeline，配備了鐳射雷達感測器和四個攝像頭的履帶式機器人（如圖 2 所示）在非結構化區域中進行了遠端操作，一次作為初始會話，一次作為重訪QCAT 會議。兩個會話之間的時間差被合理地選擇得較大，使我們能夠評估在各種照明條件下的驗證效能。對於跨模態感知，我們僅使用前置攝像頭的相機幀。子地圖是由Wildcat生成，機器人作業系統（ROS）用於不同元件之間的通訊。我們的重新定位pipeline是透過 rosservice 命令觸發的。在請求重新定位時，查詢子圖和先前地圖中存在的子圖被輸入到已經訓練好的 EgoNN 模型中。透過使用權重執行前向傳遞並受益於 kdtree，選擇了最佳候選並估計了相對姿態。由於 PR 僅基於根節點執行，因此初始會話的先前地圖中最多有 20 個子地圖。為了徹底測試pipeline，回放重訪會話的記錄資料，併為生成的每個根節點請求重新定位，導致測試整個pipeline 20 次（即 20 個“喚醒”位置）。經過這個過程，EgoNN 的平均 Recall@1 為 100%。然而，重定位的成功率為70%，證明了假設驗證的必要性。
位姿估計不會轉移到我們的鐳射雷達慣性SLAM除非透過假設驗證。為此，頂級候選子圖和查詢影像（已經糾正）被輸入到我們預先訓練的驗證模型中。對於 QCAT 資料集，經過 20 次試驗，所提出的假設驗證檢測到了 EgoNN 無法生成準確姿態估計的所有不匹配對。圖 6 顯示了匹配（頂部）和不匹配（底部）場景的示例。所提出的驗證管道，包括預訓練的特徵匹配和 SVC 模型 K，成功地分離了這些情況並檢測到重新定位失敗。
圖 6：QCAT 資料集中重新定位成功（頂部）和失敗（底部）的情況。
經過驗證的重新定位後，重訪會話生成的位姿圖將安全地合併到現有地圖中，如圖 7 所示。圖 8 顯示了將重訪會話機器人合併到初始會話生成的地圖中後的定性結果，證明多智慧體重新定位的擬議管道可行性。
圖 7：重新定位示例。當機器人（來自重訪會話）在已知環境（從初始會話生成的地圖）（左）中移動時，成功重新定位（中）後，當前姿態圖將合併到先前的地圖中（右） ，允許機器人根據先前的地圖進行操作，以恢復未完成的任務。
圖 8：多智慧體重新定位。我們的系統可用於合併由在環境中同時執行的各個代理（以紅色和綠色顯示的路徑）建立的姿勢圖。
D. 執行時分析
為了證明我們提出的系統可以線上執行，我們評估了每個元件的計算時間。計時結果是透過在單個 NVIDIA Quadro T2000 GPU 上執行預訓練模型以及在具有 Intel Xeon W-10885M CPU 的單元上執行其餘管道來收集的。表 IV-D 報告了我們管道中各個模組的執行時間細目。總的執行時間（對於圖1所示規模的QCAT實驗）不到一秒，允許系統執行線上操作。
五、結論
這項工作介紹了一個強大的多機器人重新定位系統。我們的重新定位流程受益於深度鐳射雷達表示的位置識別和姿態估計。使用自監督影像到鐳射雷達知識蒸餾來推理查詢點雲與頂級候選點雲同時捕獲的影像之間的對齊情況。該系統的模組在大規模公共資料集上進行了單獨測試，並且與我們的鐳射雷達 SLAM 系統整合的整個管道已在喚醒案例場景中進行了線上測試。未來，我們將進一步研究如何透過端到端的表示學習來提高跨模態感知，包括在非結構化環境中用於超畫素建立的影像分割和驗證模型。