GMapping原理分析
概念：
? ? ? 1、Gmapping是基于濾波SLAM框架的常用開源SLAM算法。
? ? ? 2、Gmapping基于RBpf粒子濾波算法，即將定位和建圖過程分離，先進行定位再進行建圖。
? ? ? 3、Gmapping在RBpf算法上做了兩個主要的改進：改進提議分布和選擇性重采樣。
優缺點：
? ? ?優點：Gmapping可以實時構建室內地圖，在構建小場景地圖所需的計算量較小且精度較高。相比Hector SLAM對激光雷達頻率要求低、魯棒性高（Hector 在機器人快速轉向時很容易發生錯誤匹配，建出的地圖發生錯位，原因主要是優化算法容易陷入局部最小值）；而相比Cartographer在構建小場景地圖時，Gmapping不需要太多的粒子并且沒有回環檢測因此計算量小于Cartographer而精度并沒有差太多。Gmapping有效利用了車輪里程計信息，這也是Gmapping對激光雷達頻率要求低的原因：里程計可以提供機器人的位姿先驗。而Hector和Cartographer的設計初衷不是為了解決平面移動機器人定位和建圖，Hector主要用于救災等地面不平坦的情況，因此無法使用里程計。而Cartographer是用于手持激光雷達完成SLAM過程，也就沒有里程計可以用。
? ?缺點：隨著場景增大所需的粒子增加，因為每個粒子都攜帶一幅地圖，因此在構建大地圖時所需內存和計算量都會增加。因此不適合構建大場景地圖。并且沒有回環檢測，因此在回環閉合時可能會造成地圖錯位，雖然增加粒子數目可以使地圖閉合但是以增加計算量和內存為代價。所以不能像Cartographer那樣構建大的地圖，雖然論文生成幾萬平米的地圖，但實際我們使用中建的地圖沒有幾千平米時就會發生錯誤。Gmapping和Cartographer一個是基于濾波框架SLAM另一個是基于優化框架的SLAM，兩種算法都涉及到時間復雜度和空間復雜度的權衡。Gmapping犧牲空間復雜度保證時間復雜度，這就造成Gmapping不適合構建大場景地圖，試想一下你要構建200乘200米的環境地圖，柵格分辨率選擇5厘米，每個柵格占用一字節內存，那么一個粒子攜帶的地圖就需要16M內存，如果是100個粒子就需要1.6G內存。如果地圖變成500乘500米，粒子數為200個，可能電腦就要崩潰了。翻看Cartographer算法，優化相當于地圖中只用一個粒子，因此存儲空間比較Gmapping會小很多倍，但計算量大，一般的筆記本很難跑出來好的地圖，甚至根本就跑不動。優化圖需要復雜的矩陣運算，這也是谷歌為什么還有弄個ceres庫出來的原因。
問題：
?我希望讀者可以帶著問題去閱讀論文，這樣才可以真正理解Gmapping中的很多概念。這里問題主要有：
讀者如果可以很好地回答這些問題的話就已經明白Gmapping的算法了。? ??
論文：
? ? ? ?我先帶著讀者捋順論文的結構，在解析論文的過程中回答上面的幾個問題。
? ? ?摘要：
? ? ? ?這部分簡單解釋了Gmapping是基于RBpf。RBpf是一種有效解決同時定位和建圖的算法，它將定位和建圖分離；并且每一個粒子都攜帶一幅地圖（這也是粒子濾波不適合構建大地圖的原因之一）。但PBpf也存在缺點：所用粒子數多和頻繁執行重采樣（讀者可以思考一下什么造成了RBpf需要較多的粒子數，又為什么需要頻繁執行重采樣）。粒子數多會造成計算量和內存消耗變大；頻繁執行重采樣會造成粒子退化。因此Gmapping在RBpf的基礎上改進提議分布和選擇性重采樣，從而減少粒子個數和防止粒子退化。改進的提議分布不但考慮運動（里程計）信息還考慮最近的一次觀測（激光）信息這樣就可以使提議分布的更加精確從而更加接近目標分布。選擇性重采樣通過設定閾值，只有在粒子權重變化超過閾值時才執行重采樣從而大大減少重采樣的次數。
這里可以回答第一個問題了：為什么RBpf可以先定位后建圖？
? ? ? ?這里我們用公式來描述一下SLAM的過程：，這是一個條件聯合概率分布，我們有觀測和運動控制的數據來同時推測位姿和地圖。由概率論可知聯合概率可以轉換成條件概率即：P(x,y) = p(y|x)p(x)。 通俗點解釋就是我們在同時求兩個變量的聯合分布不好求時可以先求其中一個變量再將這個變量當做條件求解另一個變量。這就是解釋了Gmapping為什么要先定位再建圖：同時定位和建圖是比較困難的，因此我們可以先求解位姿，已知位姿的建圖是一件很容易的事情。
? ? ? 第一章 簡介：
? ? ? ? ?SLAM是一個雞生蛋、蛋生雞的問題。定位需要建圖，建圖需要先定位，這就造成SLAM問題的困難所在。因此RBpf被引入解決SLAM問題，即先定位再建圖。RBpf的主要問題在于其復雜度高，因為需要較多的粒子來構建地圖并頻繁執行重采樣。我們已知粒子數和計算量內存消耗息息相關，粒子數目較大會造成算法復雜度增高。因此減少粒子數是RBpf算法改進的方向之一；同時由于RBpf頻繁執行重采樣會造成粒子退化。因此減少重采樣次數是RBpf算法的另一個改進方向。
這里回答一下什么是粒子退化：
? ? ? ?粒子退化主要指正確的粒子被丟棄和粒子多樣性減小，而頻繁重采樣則加劇了正確的粒子被丟棄的可能性和粒子多樣性減小速率。這里先涉及一下重采樣的知識，我們知道在執行重采樣之前會計算每個粒子數的權重，有時會因為環境相似度高或是由于測量噪聲的影響會使接近正確狀態的粒子數權重較小而錯誤狀態的粒子的權重反而會大。重采樣是依據粒子權重來重新采粒子的，這樣正確的粒子就很有可能會被丟棄，頻繁的重采樣更加劇了正確但權重較小粒子被丟棄的可能性。這也就是粒子退化原因之一。
? ? ? ?另外一個原因就是頻繁重采樣導致的粒子多樣性減小的速率加大，什么是粒子多樣性呢？就是粒子的不同，就像最開始有十個粒子，如果發生重采樣后其中有五個粒子被丟棄，剩下的五個粒子復制出五個粒子，這時十個粒子中只有五個粒子是不同的也就是粒子多樣性減小。再通俗點解釋，比如兔子生兔子這個問題。我們的籠子只能裝十個兔子，所以在任意時刻我們只能有十只兔子，但兔子是會繁殖的，那么怎么辦呢？索性把長的不好看的兔子干掉（這里的好看就是粒子權重，好看的權重就高不好看的權重就低，哈哈作者就是這么任性）。讓好看的兔子多生一只補充干掉的兔子。我們假設兔子一月繁殖一回，這樣的話在多年后這些兔子可能就都是一個兔子的后代。就是說兔子們的DNA都是一樣的了，也就是兔子DNA的多樣性減小。為什么頻繁執行重采樣會使粒子多樣性減小呢，這就好比我兔子一月繁殖一會我可能五年后這些兔子的才會共有一個祖先。但如果讓兔子一天繁殖一會呢？可能一個月后這些兔子就全是最開始一只兔子的后代了，兔子們的DNA就成一樣了。因此為了防止粒子退化就要減少重采樣的次數。
? ? ? ?回到論文，為了減小粒子數Gmapping提出了改進提議分布，為了減少重采樣的次數Gmapping提出了選擇性重采樣?，F在問題到了如何改進提議分布了，先簡單說一下后面會有詳細介紹。就以下圖為例，圖中虛線為p(x|x’,u)的概率分布也就是我們里程計采樣的高斯分布，這里只是一維的情況。實線為p(z|x)的概率分布也就是使用激光進行觀測后獲得狀態的高斯分布。由圖可知觀測提供的信息的準確度（方差小）相比控制的準確度要高的很多。這就是Gmapping改進提議分布的動因。但問題是我們無法對觀測建模，這就造成了我們想用觀測但是呢觀測的模型又無法直接獲得，后面論文中改進提議分布就是圍繞如何利用最近的一次觀測來模擬目標分布。?
? ? ? ?第二章 使用RBpf建圖：
? ? ? ? ? 這節主要講RBpf建圖的過程，首先RBpf是個什么東西？SALM要解決的問題就是有控制數據u1:t和觀測數據z1:t來求位姿和 地圖的聯合分布。問題是這兩個東西在一起并不太好求，怎么辦使用條件概率把它倆給拆開先來求解位姿，我們知道有了位姿后建圖是一件很容易的事情。這就是RB要做的事情：先進行定位在進行建圖。公式就變成了下面的形式：
? ? ? ? 為了估計位姿，RBpf使用粒子濾波來估計機器人位姿，而粒子濾波中最常用的是重要性重采樣算法。這個算法通過不斷迭代來估計每一時刻機器人的位姿。算法總共包括四個步驟：采樣- 計算權重-重采樣-地圖估計。這些沒什么好講的看論文就會明白。
? ? ? ???這里讀者可能對論文中的權重計算的迭代公式不太清楚，這里我貼一張我注釋過的公式圖片
下面會用到提議分布和目標分布的知識，這里我先回答一下什么是提議分布和目標分布以及為什么需要這兩個概念？
? ? ? ? ?目標分布：什么是目標分布，就是我根據機器人攜帶的所有傳感器的數據能確定機器人狀態置信度的最大極限。我們知道機器人是不能直接進行測量的，它是靠自身攜帶的傳感器來獲得對自身狀態的估計。比如說我們想要估計機器人的位姿，而機器人只有車輪編碼器和激光雷達，兩者的數據結合就會形成機器人位姿估計，由于傳感器是有噪聲的，所以估計的機器人位姿就會有一個不確定度，而這個不確定度是機器人對當前位姿確定性的最大極限，因為我沒有數據信息來對機器人的狀態進行約束了。機器人位姿變量通常由高斯函數來表示，不確定度就對應變量的方差。
? ? ? ? ?提議分布：為什么要有提議分布？有人會說有了目標分布為什么還要有提議分布進行采樣來獲取下一時刻機器人位姿信息。答案是沒有辦法直接對目標分布建模進行采樣。知道里程計模型的都明白里程計模型是假設里程計三個參數是服從高斯分布的，因此我們可以從高斯分布中采樣出下一時刻即日起的位姿。但對于激光觀測是無法進行高斯建模的，這樣是激光SLAM使用粒子濾波而不用擴展卡爾曼濾波的原因之一。為什么呢？我們知道基于特征的SLAM算法經常會用擴展卡爾曼，因為基于特征的地圖進行觀測會返回機器人距離特征的 一個距離和角度值，這時很容易對觀測進行高斯建模然后使用擴展卡爾曼進行濾波。而激光的返回的數據是360點的位置信息，每個位置信息都包括一個距離和角度信息，要是對360個點進行高斯建模計算量不言而喻。?但問題是我們希望從一個分布中進行采樣來獲取對下一時刻機器人位姿的估計，而在計算機中能模擬出的分布也就是高斯分布、三角分布等有限的分布。因此提議分布被提出來代替目標分布來提取下一時刻機器人位姿信息。而提議分布畢竟不是目標分布因此使用粒子權重來表征提議分布和目標分布的不一致性。??
? ? ?第三章 在RBpf的基礎上改進提議分布和選擇性重采樣
? ? ? ? 主要是圍繞如何改進提議分布和選擇性重采樣展開的。
? ? ? ?我們知道我們需要從提議分布中采樣得到下一時刻機器人的位姿。那么提議分布與目標分布越接近的話我們用的粒子越少，如果粒子是直接從目標分布采樣的話只需要一個粒子就可以獲得機器人的位姿估計了。因此我們要做的是改進提議分布，如果我們只從里程計中采樣的話粒子的權重迭代公式就成了：
? ? ? ? 但是由第一幅圖片我們可知里程計提供位姿信息的不確定度要比激光大的多，我們知道激光的分布相比里程計分布更接近真正的目標分布，因此如果可以把激光的信息融入到提議分布中的話那樣提議分布就會更接近目標分布。文章中說激光的精度相比里程計準確的多，因此使用里程計作為提議分布是次優的。
? ? ? ?同時因為粒子要覆蓋里程計狀態的全部空間，而這其中只有一小部分粒子是正真符合目標分布的，因此在計算權重時粒子的權重變化就會很大。但我們只有有限的粒子來模擬狀態分布，因此我們需要把權重小的粒子丟棄，讓權重大的粒子復制以達到使粒子收斂到真實狀態附近。但這就造成需要頻繁重采樣，也就造成了RBpf的另一個弊端即：發生粒子退化。這里就解釋了RBpf需要大量粒子并執行頻繁重采樣。
? ? ? ?為了改進提議分布，論文中使用最近的一次觀測，因此提議分布變成了：
接下來粒子的權重公式變成了:
? ? ? ? 接下來就是到了如何高效計算提議分布，我們知道當用柵格地圖表征環境時，準確目標分布的近似形式是沒有辦法獲得的由于觀測的概率分布不可獲得（原因在前面講過就是激光360根線我們沒法直接建模）。但是我們可以采用采樣的思想，我們可以采樣來模擬出提議分布。
? ? ? ? 為了獲得改進的提議分布，我們可以第一步從運動模型采集粒子，第二步使用觀測對這些粒子加權以選出最好的粒子。然后用這些權重大粒子來模擬出改進后的提議分布。但是如果觀測概率比較尖銳則需要更多的粒子數目以能夠覆蓋觀測概率。這樣就導致了和從里程計中采樣相同的問題。計算量太大。
? ? ? ? 目標分布通常只有幾個峰值并在大多數情況下只有一個峰值。因此我們可以直接從峰值附近采樣的話就可以大大簡化計算量。因此論文中在峰值附近采K個值來模擬出提議分布。首先使用掃描匹配找出概率大的區域然后進行采樣。我們通常使用高斯函數來構建提議分布，因此有了K個數據后我們就可以模擬出一個高斯函數作為提議分布：
有了模擬好的提議分布我們就可以采樣出下一時刻機器人的位姿信息。
? ? ? ?這里還有一個問題就是權重計算，我們知道權重描述的是目標分布和提議分布之間的差別。因此我們在計算權重時就是計算我們模擬出的提議分布和目標分布的不同。而這種不同體現在我們是由有限的采樣模擬出目標分布，因此權重的計算公式為：
? ? ? ?到此改進提議分布就完成了，接下來是選擇性重采樣。這部分比較簡單，就是設定一個閾值，當粒子的權重變化大于我們設定的閾值時就會執行重采樣，這樣減少了采樣的次數，也就減緩了粒子退化。至此理論部分就講完了！
總結：
? ? ? ?理論部分就算講完了，論文中還有實驗效果的對比，讀者自己看就行了。在理論部分后我還對GMapping源碼進行講解，歡迎有需要的同學繼續關注。
??
?
?
總結
以上是生活随笔為你收集整理的GMapping原理分析的全部內容，希望文章能夠幫你解決所遇到的問題。
- 上一篇： mysql延迟关联为什么快_MySQL
- 下一篇： 微信小程序开发简易教程一