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