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