激光雷達里程計作為在復雜的沒有GNSS環(huán)境中實現(xiàn)魯棒定位方法引起了廣泛關注。然而，由于自主機器人所需的機載計算計和內存資源的限制，在大規(guī)模環(huán)境中的異構平臺上實現(xiàn)可靠和高效的性能仍然是一個開放的挑戰(zhàn)。在這項工作中，我們介紹了LOCUS 2.0，這是一種用于實時地下3D地圖繪制的穩(wěn)健且計算效率高的激光雷達里程計系統(tǒng)，LOCUS 2.0包括一種新的基于法線的廣義迭代最近點（GICP）公式，該公式減少了點云對齊的計算時間，一種自適應體素網格濾波器，無論環(huán)境的幾何結構如何，都能保持所需的計算負荷，以及一種滑動窗口建圖方法，該方法限制了內存消耗。所提出的方法被證明適用于在嚴重的計算和內存限制下參與大規(guī)模探索的異構機器人平臺。我們演示了LOCUS 2.0，這是CoSTAR團隊參與DARPA地下挑戰(zhàn)的關鍵要素，涵蓋了各種地下場景，我們發(fā)布了LOCUS 2.0作為一個開放源代碼庫，還發(fā)布了一個基于激光雷達的里程計數(shù)據(jù)集，用于具有挑戰(zhàn)性的大規(guī)模地下環(huán)境，該數(shù)據(jù)集的特點是在多個環(huán)境中，包括霧、灰塵、黑暗和幾何退化環(huán)境中的平臺，總共運行11小時，行駛16公里。

之前的發(fā)布了LOCUS 1.0，這是一種以多傳感器激光雷達為中心的解決方案，用于實時高精度里程測量和3D建圖，具有多級掃描匹配模塊，配備了魯棒的感知傳感器集成，以松耦合的方案融合了其他傳感模式。雖然在感知退化的環(huán)境中實現(xiàn)了顯著的準確性和魯棒性，但先前版本的LOCUS 1.0：

• 具有更大的計算負荷，

• 在存儲器中保持全局地圖，

• 對更廣泛的傳感器故障（例如，一個激光雷達傳感器故障）的魯棒性較差。

LOCUS 2.0提供了算法和系統(tǒng)級改進，以減少計算負載和內存需求，使系統(tǒng)能夠在嚴重的計算和內存限制下，在大規(guī)模探索中，在具有挑戰(zhàn)性的感知條件下實現(xiàn)準確和實時的自身運動估計。這項工作的新特點和貢獻包括

• 來自法線的GICP：廣義迭代最近點（GICP）的新公式，利用點云法線近似點協(xié)方差計算。

• 自適應體素網格濾波器，其獨立于周圍環(huán)境和激光雷達確保確定性和接近恒定的運行時間。

• 改進和評估兩種滑動窗口地圖存儲數(shù)據(jù)結構：多線程八叉樹、ikd樹。

• 數(shù)據(jù)集發(fā)布，包括在具有挑戰(zhàn)性的真實世界地下環(huán)境（城市、隧道、洞穴）中.

如圖所示1，由異構機器人平臺收集。所有這些特性都改進了計算和存儲操作，同時保持了相同水平的準確性LOCUS 2.0的源代碼已開源：https://github.com/NeBula-Autonomy/LOCUS.git。

圖1：基于大型地下激光雷達的SLAM數(shù)據(jù)集的四個示例，包括超過16公里的路程和11小時在不同環(huán)境中的運行：（a）大型石灰石礦（肯塔基州地下），（b）具有大型開放空間和狹窄通道的三級城市環(huán)境（洛杉磯地鐵），（c）垂直變化較大的熔巖管，（d）具有狹窄通道的熔巖管。LOCUS 2.0在計算受限的機器人上成功地在所有這些環(huán)境中運行。

LOCUS 2.0提供了一種精確的基于廣義迭代最近點（GICP）算法，實現(xiàn)多級掃描匹配單元和一個魯棒感知傳感器集成模塊，用于在松耦合方案中穩(wěn)健融合附加傳感模態(tài)。

如圖2 LOCUS 2.0架構，該架構包含三個主要組件：i）點云預處理器，ii）掃描匹配單元，iii）傳感器集成模塊。

點云預處理器負責多個輸入激光雷達流的管理，以產生可由掃描匹配單元有效處理的統(tǒng)一3D數(shù)據(jù)，預處理器模塊由點云的運動失真校正（MDC）組成。該模塊使用IMU測量值校正掃描期間由于機器人移動引起的傳感器旋轉引起的點云失真。點云合并通過使用已知的外部變換將來自機器人機身中不同激光雷達傳感器的點云組合在一起，擴大了機器人的FOV。為了實現(xiàn)多個激光雷達源的彈性合并，我們引入了一個基于外部超時的監(jiān)測器，該監(jiān)測器可動態(tài)更新點云合并中應組合哪些激光雷達（即，如果激光雷達的消息延遲太久則忽略）。監(jiān)測使子模塊對單個激光雷達的滯后和故障具有魯棒性，從而始終向下游管道中提供輸出數(shù)據(jù)。然后，濾波器移除屬于機器人自身的三維點。接下來，自適應體素網格過濾器保持固定數(shù)量的體素化點，以管理CPU負載并確保確定性行為。它允許機器人具有一致的計算負載，而不管環(huán)境的大小或激光雷達的數(shù)量（或潛在的激光雷達故障）。與LOCUS 1.0相比，自適應體素網格濾波器將點云縮減策略從具有固定葉子大小和隨機濾波器的體素化策略更改為自適應系統(tǒng)。

法線計算模塊從體素化點云計算法線。掃描匹配單元執(zhí)行GICP掃描以掃描并掃描到子地圖配準以估計機器人的6自由度運動，與前一代相比，LOCUS 2.0沒有重新計算協(xié)方差，而是利用了一種新的GICP公式來使用法線，法線只需要計算一次并存儲在地圖中。在具有多模態(tài)傳感的機器人中，如果可用，LOCUS 2.0使用來自非激光雷達源（來自傳感器集成模塊）的初始估計，通過使用接近最優(yōu)的種子初始化優(yōu)化，來簡化掃描到掃描匹配階段的GICP收斂，從而提高精度并減少計算，增強實時性能。

LOCUS 2.0還包括更有效的地圖存儲技術，該系統(tǒng)使用滑動窗口方法，因為在內存中維護大規(guī)模區(qū)域是不可行的。例如，在這里展示的一個洞穴數(shù)據(jù)集中，1厘米分辨率全局地圖需要50 GB的內存，遠遠超過了小型移動機器人通?？捎玫膬却?，這種方法需要插入、刪除和搜索的高效計算解決方案。

A、 基于法線的GICP

LOCUS 2.0使用GICP進行掃描數(shù)據(jù)和掃描數(shù)據(jù)到子地圖匹配，GICP通過使用配準問題的概率模型概括了點對點和點對面ICP配準，為了做到這一點，GICP要求點云中每個點的協(xié)方差可用性，協(xié)方差通?；诮o定點周圍相鄰點的分布來計算，Segal等人提出了平面對平面應用，假設真實世界表面至少是局部平面的，在該公式中，曲面上的點由協(xié)方差矩陣局部表示，其中已知該點屬于具有高置信度的平面，但其在平面中的精確位置具有較高的不確定性。這里展示了平面到平面的協(xié)方差計算如何等同于從預先計算的法線計算協(xié)方差，僅需要法線這一事實對于掃描數(shù)據(jù)到子地圖的對齊尤其重要，因為否則地圖將需要重新計算點協(xié)方差，這是一項昂貴的操作，涉及創(chuàng)建kd樹和最近鄰居搜索，相反，通過使用法線，協(xié)方差計算只執(zhí)行一次（因為它不受附加點的影響），其結果可以存儲和重用。

B、 自適應體素網格濾波器

為了管理激光雷達里程計的計算負荷，無論環(huán)境和激光雷達配置如何（根據(jù)激光雷達的數(shù)量和類型），我們提出了一種自適應體素網格濾波器，在這種方法中，目標是將體素化的點的數(shù)量保持在固定水平，而不是指定體素葉子大小并使系統(tǒng)暴露于輸入點云的變化，這些變化源于不同的傳感器配置和環(huán)境的橫截面幾何形狀。這個設計目標來自這樣一個事實，即配準階段中的幾乎所有計算都依賴于點云的數(shù)量。因此，想法是保持3D點的體素化數(shù)量固定，以便每次掃描具有近似固定的計算時間。這種簡單的技術將點數(shù)保持在固定的水平上，同時避免點數(shù)的任何大的跳躍、點數(shù)太少（例如掃描錯誤）或點數(shù)太多，結果是提高了效率并減少了系統(tǒng)的計算負荷。

C、 滑動窗口地圖

LOCUS 1.0通過八叉樹數(shù)據(jù)結構將全局地圖存儲在存儲器中，原始八叉樹實現(xiàn)沒有一種有效的方法來修剪數(shù)據(jù)。雖然一種可能的解決方法是過濾機器人周圍的點云，并相應地重建八叉樹，但這可能在計算上很昂貴，并導致長時間刷新。為了解決這些挑戰(zhàn)，并在內存限制下實現(xiàn)大規(guī)模探索，LOCUS 2.0提供了兩種地圖滑動窗口方法：i）多線程八叉樹，ii）增量k-dtree。

圖3：我們的滑動地圖方法的圖解，保持所有點云，直到機器人到達原始窗口的邊界。然后，將設置一個新窗口，并刪除該窗口之外的點云

多線程八叉樹方法只在內存中維護環(huán)境的以機器人為中心的子映射。兩個分別處理專用數(shù)據(jù)結構（mapa/octreea和mapb/octreeb）的并行線程（threada和threadb）負責通過盒過濾器動態(tài)過濾當前機器人位置周圍的點云圖，并根據(jù)更新后的圖重建八叉樹，同時考慮并行工作進程之間的機器人運動。Ikd樹是一種通過合并新掃描動態(tài)存儲3D點的二叉搜索樹。Ikd樹不僅在葉節(jié)點中維護3D點：它們在內部節(jié)點中也有點。這種結構允許動態(tài)插入和刪除功能，并依賴于整個數(shù)據(jù)結構中的惰性標簽存儲。ikd樹的初始構建類似于kd樹，其中空間在最長維度的中點處遞歸拆分。

A、 數(shù)據(jù)集

在過去的3年中，CoSTAR團隊在洞穴、隧道和廢棄工廠等真實環(huán)境中對我們的激光雷達里程表系統(tǒng)進行了深入測試，選擇每個數(shù)據(jù)集（表一），以包含激光雷達里程表具有挑戰(zhàn)性的組件，該數(shù)據(jù)集提供激光雷達掃描、IMU和輪式慣性里程表（WIO）測量以及相機流。所有數(shù)據(jù)集都記錄在不同的機器人平臺上，例如Husky和Spot（圖4），具有振動和大加速度，這是滑動轉向輪式機器人穿越崎嶇地形和在崎嶇地面上滑動并動態(tài)動作的腿式機器人的特點。該機器人配備了3個機載VLP16激光雷達傳感器，進行了外部校準。Spot機器人配備了一個外部校準的機載激光雷達傳感器，指出開箱即用的工具（運動慣性里程計）KIO和（視覺慣性里程計）V IO，因此數(shù)據(jù)也記錄了這些讀數(shù)，激光雷達掃描以10 Hz記錄。WIO和IMU以50 Hz記錄。為了確定機器人在環(huán)境中的地面實況，使用了勘測級3D地圖。地面實況軌跡是通過對照測量坡度圖運行LOCUS 1.0生成的（即掃描到地圖即掃描到測量地圖）。在這種模式下，LOCUS 1.0以計算效率為代價進行調整，以達到最大精度，因為它不需要實時運行。

圖4：DARPA地下挑戰(zhàn)星云框架中異構機器人系統(tǒng)的機器人類型

B、 指標

對于CPU和內存配置，使用跨平臺庫來檢索有關運行進程和系統(tǒng)利用率的信息，該庫用于系統(tǒng)監(jiān)視和分析，CPU表示當前系統(tǒng)范圍CPU利用率的百分比值，其中100%表示使用1個內核。內存通過根據(jù)平臺對不同的內存值求和來表示統(tǒng)計信息。里程計延遲測量里程表消息創(chuàng)建與系統(tǒng)當前時間戳之間的差異。該系統(tǒng)在機器人操作系統(tǒng)（ROS）框架中實現(xiàn)。這項工作考慮了最大延遲和平均延遲，因為這兩個度量更直接地影響使用里程測量結果的模塊的性能，例如控制器和路徑規(guī)劃器。

C、 計算時間

1）來自法線的GICP：給出的實驗旨在顯示來自法線GICP的GICP優(yōu)于GICP的優(yōu)勢，并支持這樣的說法，即這種重新計算在不犧牲精度的情況下產生更好的計算性能。圖5.a-e顯示了法線GICP和數(shù)據(jù)集GICP之間的比較結果，而圖5.f顯示了關于GICP方法的每個度量的所有數(shù)據(jù)集的平均百分比變化，來自法線的GICP減少了LOCUS 2.0中的所有計算指標：平均和最大CPU使用率、平均和最大里程表延遲、稀疏掃描、掃描到子地圖、激光雷達回調持續(xù)時間及其最大時間。

圖5:LOCUS 2.0中正常和GICP比較的GICP結果。

2） 自適應體素網格濾波器：實驗顯示了LOCUS 2.0自適應行為，實驗在所有數(shù)據(jù)集上進行，GICP來自法線，使用ikd樹數(shù)據(jù)結構進行地圖維護，框大小為50 m，Ndesired范圍為1000至10000，盡管如此，數(shù)據(jù)集的計算時間仍有一些變化。

圖8：自適應體素化的計算時間與3000點和恒定葉子大小0.25的一致性比較。a） 使用2臺激光雷達的城市數(shù)據(jù)集。b） 使用3個激光雷達的洞穴數(shù)據(jù)集。

如圖8所示，該方法在不同的環(huán)境和傳感器配置中產生了一致的平均計算時間，而沒有任何大的計算時間峰值。這種性能提供了更可預測的計算負載，無論是機器人還是環(huán)境。

如圖7所示，在相同的自適應體素化設置下，所有數(shù)據(jù)集的平均回調時間和CPU負載都相似。

D、 內存

1） 地圖維護：第三個實驗展示了與經典的靜態(tài)八叉樹結構相比，滑動窗口地圖在實時系統(tǒng)中的優(yōu)勢。在這些實驗中，LOCUS 2.0使用：ikd樹和多線程八叉樹（mto）。

圖9顯示了F和I數(shù)據(jù)集的最大內存使用情況以及內存占用隨時間的變化。最大的內存占用是八叉樹和mto版本的0.001m葉大小。ikd樹在內存和CPU使用方面的性能與葉大小為0.01m的mto相似。

2） 地圖大?。哼@些實驗表明，在權衡計算、內存負載和結果準確性的同時，滑動地圖的大小是一個需要考慮的重要參數(shù)。根據(jù)實時操作系統(tǒng)要求的范例，滑動窗口映射允許系統(tǒng)被機器人分配的最大內存限制。

圖10顯示了ikd樹和mto在映射大小方面的最大APE、CPU和內存度量。

E、 與最先進技術的比較

表III顯示了針對不同環(huán)境領域（城市、隧道、洞穴（A、C、F、H、I、J））的LOCUS 2.0與最先進方法FAST-LIO和LINS的比較研究。該表顯示，LOCUS 2.0在最大和平均APE誤差度量方面表現(xiàn)出最先進的性能，并在6個數(shù)據(jù)集中的5個數(shù)據(jù)集中實現(xiàn)了最小的誤差。此外，LOCUS 2.0是唯一一種不會在發(fā)生激光雷達滑動的隧道型環(huán)境（數(shù)據(jù)集F）中失敗的方法。在計算方面，LOCUS 2.0的性能與FAST-LIO相當，LOCUS 2.0的內存使用量稍大，但這可能與所有系統(tǒng)中默認選擇的地圖分辨率有關。

表II顯示了與LOCUS 1.0中的參考方法相比，滑動窗口映射方法如何減少內存使用，同時增加CPU使用。

這項工作介紹了LOCUS 2.0，一個強大且計算效率高的激光雷達里程計系統(tǒng)，用于在嚴重的計算和內存限制下進行實時、大規(guī)模探測，適合部署在異構機器人平臺上。這項工作從預計算的法線重新計算GICP協(xié)方差計算，提高了GICP的計算性能，LOCUS 2.0使用自適應體素網格濾波器，使計算負荷獨立于環(huán)境和傳感器配置，自適應行為使激光雷達的點數(shù)保持一致，同時保持環(huán)境的體素化結構，這穩(wěn)定并提高了計算負載。評估了兩種減少內存使用的滑動建圖策略：多線程八叉樹和ikd樹，并顯示了它們的計算成本和內存使用的改進，開源了LOCUS 2.0和數(shù)據(jù)集，用于具有挑戰(zhàn)性和大規(guī)模的地下環(huán)境，這些環(huán)境具有各種現(xiàn)實條件，如霧、灰塵、黑暗和限制流動性的幾何退化環(huán)境。

更多詳細內容請加入知識星球查看原文。