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中的很多概念。這裏問題主要有：
1.爲什麼RBpf可以將定位和建圖分離；
2.Gmapping是如何在RBpf的基礎改進提議分佈的；
3.爲什麼要執行選擇性重採樣；
4.什麼是粒子退化及如何防止粒子退化；
5.爲什麼Gmapping嚴重依賴里程計；
6.什麼是提議分佈；
7.什麼是目標分佈；
8.爲什麼需要提議分佈和目標分佈；
9.算法中是如何計算權重；
10.粒子濾波粒子數和傳感器精度的關係；
11.爲什麼在有大回環的環境中增加粒子數可以使建出的地圖正確閉合；
12.Gmapping是基於濾波框架的SLAM方法，爲什麼建圖過程中界面上顯示的地圖在不斷調整。
讀者如果可以很好地回答這些問題的話就已經明白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提出了選擇性重採樣。現在問題到了如何改進提議分佈了，先簡單說一下後面會有詳細介紹。就以下圖爲例，圖中虛線爲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源碼進行講解，歡迎有需要的同學繼續關注。
作者：豆子愛玩
來源：CSDN
原文：https://blog.csdn.net/liuyanpeng12333/article/details/81946841
版權聲明：本文爲博主原創文章，轉載請附上博文鏈接！
