電子發燒友App
粒子濾波也是一個十分經典的算法，它與卡爾曼濾波的不同之處在于卡爾曼濾波假設概率分布是高斯分布，然后在計算后驗概率（pdf）時，利用正態分布的性質，可以計算出來；而粒子濾波的后驗概率分布是通過蒙特卡洛方法采樣得到的。蒙特卡洛方法很清楚的一點是采樣的粒子越多，概率分布越準確，但是計算速度會下降。也就是說如何分布你的有限個數的采樣粒子來得到更為準確的后驗概率分布是粒子濾波一直在做的事情。在本文中粒子濾波的改善一個是局部采樣，另一個是采樣時更好的概率分布來得到更精確的后驗概率。在閉環檢測這里則是應用了深度學習的方法。具體實現可以隨筆者一起看下面的文章。
An Enhanced LiDAR-Inertial SLAM System for Robotics Localization and Mapping
摘要——基于激光雷達和慣性傳感器的定位和建圖對于無人地面車輛相關應用具有重要意義。在這項工作中，我們為無人地面車輛開發了改進的 LiDAR 慣性定位和映射系統，適用于多功能搜索和救援應用。與現有的基于 LiDAR 的定位和建圖系統（如 LOAM）相比，我們有兩個主要貢獻：第一個是提高了基于粒子群濾波器的 LiDAR SLAM 的魯棒性，第二個是為全局開發的閉環方法優化以提高整個系統的定位精度。我們通過實驗證明，LiDAR SLAM 系統的準確性和魯棒性都得到了提高。最后，我們在香港科學園以及其他室內或室外真實復雜的測試環境中進行了系統的實驗測試，證明了我們的方法的有效性和效率。結果表明，我們的系統具有高精度、魯棒性和效率。我們的系統對于無人駕駛地面車輛在未知環境中的定位和建圖具有重要意義。一、引言及相關工作激光雷達(Light Detection And Ranging, LiDAR) 傳感器是一種典型的傳感器，可以獲取具有局部范圍的所有周圍物體的距離[1]。基于激光雷達的同步定位和建圖(SLAM) 對各種應用具有重要意義，例如自動駕駛、3D 機器人抓取、搜索和救援機器人以及機器人監視和檢查。在過去的幾年中，除了各種先進的控制技術 [2] 之外，基于深度學習的方法也引起了極大的關注，因為它們重新創建了視覺世界。最近，由于開發了基于大規模 LiDAR 的定位基準，例如用于自動駕駛的 KITTI [3]-[7]，已經提出了各種 LiDAR-SLAM 系統。基于 gmapping 的方法已經提出[8]，這是一個典型的基于粒子濾波器的LiDAR SLAM。谷歌提出的Cartographer [9]，[10]利用Ceres求解器求解非線性最小二乘來實現掃描中的匹配。結構通過子圖和子圖到掃描的匹配，實現閉環檢測和全局優化。這種方法包括Fast-LIO [11]，它在里程計的前端利用增量KD-Tree .后端使用迭代擴展卡爾曼濾波器。它具有持續時間短和計算速度快的優點。基于LOAM的LiDAR-SLAM利用點云的邊緣和平面特征作為前端特征。這些特征是存儲在地圖中，匹配時利用點到線和點到面的距離信息。高頻視覺信息和低頻LiDAR里程計信息都被用來實現機器人運動估計[ 12].[13]中提出了用于細粒度特征提取的LEGO-LOAM。并且該系統可以在簡單的環境中以可接受的精度實現實時定位和映射性能。最近，也提出了一些方法戰術解決了固態LiDAR視場受限的問題，例如Loam-Livox。最近提出了基于深度學習的特征提取方法。但是，主要問題是以前的方法都沒有包含循環因此，LiDAR SLAM系統存在漂移和精度低的問題。此外，上述所有方法都利用卡爾曼濾波器在LiDAR SLAM系統的前端計算里程計。但是，粒子濾波器-基于[8]、[14]的方法計算里程計尚未得到充分探索。總而言之，目前基于LiDAR的SLAM的各種研究大致可以概括為兩個方面：第一部分著重于提高精度第二部分側重于提高系統的魯棒性。已經提出了各種方法來提高各種室內或室外場景中的定位精度和魯棒性。但是，如何取得很好的效果這兩個因素之間的平衡需要更多的探索。
圖 1. 復雜環境下的大尺度測繪結果
圖 2. 推薦的 LiDAR SLAM 建圖結果的最終演示。可以看出，在 Mulran [15] 基準上，我們提出的基于學習的回環檢測方法可以達到令人滿意的精度，并將較大的誤差（子圖（a）是提出回環檢測方法之前的全局圖）糾正為全局一致地圖（子圖（b）是我們做閉環后得到的地圖。）子圖（c）和子圖（d）分別展示了我們在室外道路案例和室外港口案例上的測試結果。可以看出，使用機械和固態 LiDAR [16]-[18] 可以在室內和室外環境中實現全局一致映射。證明了我們提出的方法的穩健性。為了應對上述挑戰，在這項工作中，我們提出了一種基于粒子濾波的方法來解決未知環境下的激光雷達慣性定位和映射問題。首先，我們提出了一種適用于室內和室外定位和映射的自適應粒子群濾波器。我們在自適應粒子群濾波器的最優化中提出了較小的時間步長，以提高定位的成功率和穩健性。此外，我們還提出了使用重采樣策略來做出正確的抽樣分布。最后，我們利用基于擴展卡爾曼濾波器的粒子濾波器進行狀態估計。此外，我們還提出了一種基于輕量級學習的方法來尋找激光雷達SLAM中的閉環。我們利用帶有分類頭的深度神經網絡判斷兩個局部激光雷達掃描是否重疊。此外，我們為深度神經網絡設計了一個回歸頭來預測兩個激光雷達掃描之間的相對角度。總之，我們有以下突出貢獻：1) 我們針對定位問題提出了基于粒子群濾波器的自適應框架。適應的方法利用重采樣策略和適當的建議密度基于構建的局部地圖進行優化。2）我們設計了一種有效的閉環方法來提高定位精度和魯棒性。各種實驗證明了我們提出的閉環算法的有效性。3）對于效率問題，我們還利用各種最近鄰搜索算法來快速計算鄰居。在原有實現的基礎上，我們將最近鄰搜索的速度提高了 5.8 倍。4）我們已經將所提出的方法集成到我們的系統中，無論是Livox-Avia的固態激光雷達還是VLP-16等機械激光雷達。可以證明，我們的方法在有效性和效率上有明顯的優點。它在包括香港科技園在內的各種測試環境中取得了令人滿意的性能。此外，我們的方法可以與基于機器人學安全走廊的運動規劃無縫集成。我們方法的穩健性和準確率在公共數據集上以及真實復雜的室內和室外環境中都得到了證明
圖 3. 固態激光雷達的掃描模式
圖 4. 校園場景的機械 LiDAR 建圖結果
圖 5. 推薦的 LiDAR-慣性 SLAM 系統的最終軟件框架在接下來的工作中，第二部分介紹了我們的整體系統框架和我們的兩個主要貢獻。接下來，我們提出我們的兩個主要貢獻，自適應粒子群濾波器和基于學習的閉環方法。由于頁數限制，我們在每個小節中都包含了我們的實驗結果。最后，我們簡要介紹了我們在香港進行本地建筑檢查的綜合系統。二、推薦的方法A. 整體系統框架1）我們提出的激光雷達慣性定位和建圖系統的框架：提出的定位和建圖框架主要基于典型的激光雷達 SLAM 或激光雷達慣性系統，如 LEGO-LOAM [13]和 LIO-SAM [19]。與他們的方法相比，我們有兩個主要區別：首先是我們提出了第一個基于粒子濾波器的方法，與以前的方法如 LEGO-LOAM [13] 和 LIO-SAM [19] 相比，它獲得了更好的效率。第二個是我們提出了一種有效的閉環檢測網絡，稱為 LoopNet。結果表明，我們提出的方法可以非常有效地執行閉環，并且整個 LiDAR SLAM 系統的整體精度大大提高。
圖 6. 基于 FG-Conv 的 FG-Net 網絡架構的總體框架2) 推薦的 LiDAR-慣性 SLAM 系統的最終軟件框架：我們推薦的 LiDAR-慣性 SLAM 的編碼框架如圖 6 所示。我們軟件的運行過程總結如下：1.激光運動畸變校正。預積分是通過使用當前幀開始和結束之間的 IMU 數據和 IMU 里程計數據來計算的。獲取各時刻激光點的位姿，將其轉化為激光在初始時刻的坐標系，實現標定。2.提取特征。針對運動畸變校正后的當前幀激光點云，計算每個點的曲率，然后提取角點和平面點特征。3.掃描到地圖匹配。提取局部關鍵幀圖的特征點，與當前幀的特征點進行scan-to-map匹配，更新當前幀的位姿。4. 因子圖優化或基于擴展卡爾曼濾波器的優化。在圖中添加激光里程計因子、GPS因子、閉環因子并進行因子圖優化，更新所有關鍵幀位姿。5. 閉環檢測。在歷史關鍵幀中尋找候選閉環匹配幀，進行掃描到地圖匹配以獲得位姿變換。然后我們可以構建閉環因子，并將它們添加到因子圖中進行優化。應該提到的是，為了保持我們傳感器套裝的最低要求，我們在我們的軟件中包含了 GPS 融合模塊，但我們沒有在所有實驗中使用 GPS 信息。B.自適應粒子群濾波器1）粒子群濾波器的原理：基于粒子濾波器的SLAM的基本思想是使用m個隨機樣本 表示機器人在第k個時間步的置信度值 . Xr(k)表示機器人在第k步的位姿值。wk 表示 Xr(k) 對應的權值。d0,1,2,..,k表示從時間步長Zk = {Z(0), Z(1), .., Z(k)開始的數據，估計狀態為Xr(k)。Xr(k) 是移動機器人的位姿。Xr(k) 是移動機器人的位姿。Xr(k) 代表移動機器人的位姿。Xr(k) = (xr(k), yr(k), θr(k))。系統說明如下：
其中 u(k) 是運動輸入，例如來自里程計的數據。w(k) 是過程噪聲。測量函數可以表述如下：
Xn(k)代表路由信息，而v(k)代表測量噪聲。然后我們就可以獲得機器人的可信度。
根據貝葉斯定理，我們可以得到遞歸計算，可以表述為：
根據馬爾可夫假設，測量 Z(k) 與先前的測量條件獨立。將所有先前的測量表示為 Zk, Zk?1 = {Z(0), ..., Z(K ? 1)}，這僅取決于先前的測量。并且它只與姿勢 Xn(k) 有關。
給定 Xn(k ? 1) 和 u(k ? 1)，狀態 Xn(k) 獨立于之前的狀態 Xn(k ? 1) 和控制 u(k ? 1)，即：
這個方程是粒子群濾波器的基礎。其中 ?, p(Z(k)|Xr(k) 是傳感器模型對應的機器人。我們可以定義作為重要性函數。q的確定對機器人的影響很大基于粒子濾波的SLAM性能
圖 7. 推薦的 LiDAR SLAM 建圖結果的最終演示。可以看出，在 Mulran [15] 基準上，我們提出的基于學習的回環檢測方法可以達到令人滿意的精度并將大誤差（子圖（a）是提出回環檢測方法之前的全局圖）糾正為全局一致圖（子圖（b）是我們做閉環后得到的地圖。）子圖（c）和子圖（d）分別展示了我們在室外道路案例和室外港口案例上的測試結果。可以看出，無論是在室內還是室外環境下，機械和固態激光雷達都可以實現全局一致的建圖。證明了我們提出的方法的穩健性2）先前粒子群濾波器的缺點：我們提出了一種用于定位和建圖的自適應粒子群濾波器。傳統上，SLAM 中典型的粒子群濾波器 [8] 尋求達到全局收斂。在理想情況下，基于標準粒子群濾波器的 SLAM 系統將在無限次迭代優化后達到全局收斂。最后，我們可以獲得機器人系統的確定性姿態和環境的確定性映射。然而，在現實中，由不確定的機器人姿勢構建的映射并不是很準確。同時，從不確定的環境地圖中估計的機器人位姿不準確。隨著機器人的運動，機器人姿態的準確性將對建圖精度產生很大影響，反之亦然。這將導致機器人映射和定位的準確性和效率低下。有時，可能不滿足一些弱約束，最終定位不會收斂。同時，基于標準粒子濾波器的 SLAM 在整個探索環境中采用均勻分布的粒子。并且收斂速度也會降低。此外，粒子數量也會增加，這將增加計算需求并影響整個LiDAR-Inertial SLAM系統的實時性能[20]，[21]3）自適應粒子群濾波器優化的更小時間步長：我們提出了自適應粒子群濾波器。自適應粒子群濾波器針對機器人姿態的不確定性和建圖結果的不確定性這一事實。我們提出的方法在局部范圍內尋找機器人的最佳姿態 [11]、[22]、[23]。然后，利用機器人在局部范圍內的最佳位姿來構建局部范圍內的環境地圖。我們最終提出的具體方法可以總結如下：1）找出以計算出的機器人姿勢為中心的小方塊。對于室內案例，我們將矩形的邊長設置為 1cm，對于室外案例，我們將矩形的邊長設置為 10cm。2) 我們讓一定數量的粒子均勻分布在矩形中。機器人移動十個時間步后，定位完成。3）然后我們使用激光雷達重新掃描環境并執行地圖更新。這樣，利用更小時間步的簡單技巧，并在局部范圍內進行優化，我們可以提高最終 LiDAR SLAM 系統定位的成功率和魯棒性。總之，我們在短時間內基于構建的局部地圖執行基于粒子群的定位。另外，我們讓粒子相對于機器人在一個小范圍內，這可以大大減少所需的粒子4）解決退化問題的建議方法：基本粒子濾波器中存在的問題是退化問題。粒子權值的變化會增加迭代次數，退化不可避免。通過迭代，其他粒子的權重將達到一個可以忽略不計的小值。那么退化意味著大量的時間和計算資源將浪費在重量很小的粒子上。不僅會造成資源的浪費，還會影響最終的估算結果。在我們的工作中，我們提出了解決權重退化問題的方法：1）利用重采樣策略。重采樣策略的本質是增加粒子的多樣性。通過引入重采樣，多樣性會變大。在我們的工作中，我們利用 Gordon 等人提出的多項重采樣 [24]，它解決了粒子濾波器中的退化問題。2) 我們還利用了類似于 [25] 的適當的建議密度分布。基本粒子濾波器的假設是：基于重要性的重采樣可以從適當的后驗建議密度分布中采樣一組點。建議的密度分布函數指導重采樣以做出正確的樣本分布。因此，如果我們能夠找到一個最優的建議密度分布函數，那么最終的濾波質量就可以得到保證三、基于學習的閉環此外，我們還提出了一種基于深度學習的 SLAM 系統閉環算法。通過我們提出的回環檢測技術，可以有效地比較相似地方的特征，并且可以有效地檢測回環。此外，我們提出的網絡架構非常輕巧，適合實時機器人應用。我們還提出了基于學習的閉環檢測網絡 Loop-Net。該網絡嚴重依賴于我們之前的工作 FG-Net [26]-[28]。我們使用分類損失來判斷兩次 LiDAR 掃描之間是否存在重疊。我們使用回歸損失來預測兩次 LiDAR 掃描之間的相對角度四、香港本地驗樓綜合系統最后，我們成功利用上述LiDAR SLAM系統在香港本地進行建筑物檢查并獲得監控。該系統如圖 8 所示。實驗表明，我們提出的 LiDAR-Inertial 同步定位和建圖系統具有強大的生成高質量地圖的能力，這對于進行自主運動規劃以生成可行路徑。LiDAR SLAM 系統構建的 3D 地圖對于 3D 建筑物重建、3D 模型分析、建筑物目標或缺陷定位與檢測等進一步應用也具有重要意義五、結論在這項工作中，我們提出了一種用于無人駕駛地面車輛定位的集成改進型 LiDAR-慣性同步定位和建圖系統 [29]。我們提出了一種基于粒子濾波器的里程計的系統設計。通過進行局部優化，我們提高了以前的 LiDAR-Inertial SLAM 的魯棒性和效率。并且通過提出的基于學習的閉環檢測方法，我們可以提高最終全局地圖的準確性。可以證明，我們提出的 LiDAR-Inertial SLAM 系統在室內和室外環境中都顯示出出色的性能和魯棒性，機械 LiDAR 或固態 LiDAR 具有良好的準確性。在未來，我們希望探索使用語義 SLAM [30] 進行機器人場景解析。
圖 8. 基于我們提出的 LiDAR SLAM 系統的 UGV 檢查部署系統的詳細說明編輯：黃飛
工商網監
評論