基於粒子濾波的SLAM(GMapping)算法分析
博主連接：
https://blog.csdn.net/tiancailx/article/details/78590809
本文是《Improved Techniques for Grid Mapping_with Rao-Blackwellized Particle Filters》的大致翻譯，難免有不通順與錯誤的地方，如有錯誤請指出，謝謝！
設想一個機器人在一個未知環境中移動，其目的是獲得當前環境的地圖。地圖可以用一個儲存每個網格單元顏色的矩陣表示，單元格的顏色只能爲黑色或白色。由於傳感器與電機都存在誤差，運動很可能偏離目標方向，因此機器人很容易“迷路”。所以同步定位與地圖構建(SLAM)問題總是被稱作“雞和蛋”的問題：機器人首先需要知道自身的位置去獲得一個準確的地圖，但是獲取地圖同樣也需要一個準確的定位。定位與地圖估計之間的相互依賴使SLAM問題變得非常困難，並且通常需要在高維空間中搜索解決方案。
很多學者對SLAM問題進行了廣泛的研究。在2000年，Murphy應用Rao-Blackwellized粒子濾波算法去解決SLAM問題。Rao-Blackwellized的主要方式是用許多粒子去獲取準確的地圖。因此減少粒子的數量是優化這一算法的主要挑戰。
1 地圖構建中的Rao-Blackwellized粒子濾波算法
SLAM的核心思想是根據其觀測值和其里程計測量信息去估計聯合後驗概率密度函數（代表地圖中的點、代表機器人的軌跡）。可以看出，軌跡和地圖需要同時計算出來，這樣的計算很複雜而且計算的結果可能不收斂。而RBPF(Rao-Blackwellized Particle Filter)算法利用公式(1)對聯合概率密度函數進行因式分解。
因此RBPF可以先估計機器人的軌跡而後再去根據已知的軌跡計算地圖。由地圖的概率密度函數可以看出，地圖強烈依賴於機器人的位姿，所以這個方法是可行的。
地圖的概率密度函數可以通過已知位姿的建圖方法“mapping with known poses”來計算。後驗概率密度函數應用粒子濾波來估計。
粒子濾波算法的核心思想是利用一系列隨機樣本的加權和近似後驗概率密度函數，通過求和來近似積分操作。該算法源於Monte Carlo思想，即以某事件出現的頻率來指代該事件的概率。因此在濾波過程中，需要用到概率的地方，一概對變量採樣，以大量採樣及其相應的權值來近似表示概率密度函數。
其中最普遍的粒子濾波算法爲SIR(Samping Importance Resampling)濾波器。該算法通過以下四步完成：
1）預測階段：粒子濾波首先根據狀態轉移函數預測生成大量的採樣，這些採樣就稱之爲粒子，利用這些粒子的加權和來逼近後驗概率密度。
2）校正階段：隨着觀測值的依次到達，爲每個粒子計算相應的重要性權值。這個權值代表了預測的位姿取第個粒子時獲得觀測的概率。如此這般下來，對所有粒子都進行這樣一個評價，越有可能獲得觀測的粒子，獲得的權重越高。
3）重採樣階段：根據權值的比例重新分佈採樣粒子。由於近似逼近連續分佈的粒子數量有限，因此這個步驟非常重要。下一輪濾波中，再將重採樣過後的粒子集輸入到狀態轉移方程中，就能夠獲得新的預測粒子了。
4）地圖估計：對於每個採樣的粒子，通過其採樣的軌跡與觀測計算出相應的地圖估計。
SIR算法需要在新的觀測值到達時從頭評估粒子的權重。當軌跡的長度隨着時間的推移而增加時，這個過程的計算複雜度將越來越高。因此Doucet等學者通過式(2)限制重要性概率密度函數來獲得遞歸公式去計算重要性權值。
根據等式2，權值可通過以下公式計算
2 GMapping中的優化算法
GMapping爲2007年在ROS中開源的SLAM軟件包，是目前使用最廣泛的軟件包。它可用於室內和室外，應用改進的自適應RBPF算法來進行定位與建圖。
Doucet等學者基於RBPF算法提出了改進的重要性概率密度函數並且增加了自適應重採樣技術。如上一節所述，爲了獲得下一迭代步驟的粒子採樣我們需要在預測階段從重要性概率密度函數中抽取樣本。顯然，重要性概率密度函數越接近目標分佈，濾波器的效果越好。
2.1 最優重要性概率密度函數
典型的粒子濾波器應用里程計運動模型作爲重要性概率密度函數。這種運動模型的計算非常簡單，並且權值只根據觀測模型即可算出。然而，這種模型並不是最理想的。當機器人裝備激光雷達（如SICK，Hokuyo等）時，激光測得的數據比里程計精確的多，因此使用觀測模型作爲重要性概率密度函數將要準確的多。圖1展示了觀測模型的分佈明顯小於運動模型的分佈。由於觀測模型的分佈區域很小，樣本處在觀測的分佈（圖中的區域）的機率很小，在保證充分覆蓋觀測的分佈情況下所需要的粒子數就會變得很多，這將會導致使用運動模型作爲重要性概率密度函數類似的問題：需要大量的樣本來充分覆蓋分佈的區域。
爲了克服這個問題，可以在生成下一次採用時將最近的觀測考慮進去。通過將整合到概率分佈中，可以將抽樣集中在觀測似然的有意義的區域。爲此Doucet等提出了最優重要性概率密度函數，式4爲粒子權重方差的最優分佈。
所以式(4)在機器人裝備激光測距儀的時候非常適用。
現在的RBPF算法過程是這樣的，首先根據運動模型對機器人下一時刻位姿進行預測，得到預測的狀態值並且對其進行採樣。第二步是通過最優概率密度函數（4）對各個粒子進行權值的計算。之後進行重採樣，根據粒子的權重重新分佈粒子，爲下次預測提供輸入。最後，根據粒子的軌跡計算地圖的後驗概率密度函數。
圖2中展示了在不同場景下的粒子分佈情況。（a）爲在開放的走廊中，粒子沿着走廊分佈。（b）爲在死衚衕中，粒子分佈的不確定性很小，分佈的很集中。（c）爲根據里程計運動模型預測生成的粒子分佈，分佈的很分散。
因此，該算法將最近的里程計信息與觀測信息同時併入重要性概率密度函數中，使用匹配掃描過程來確定觀察似然函數的分佈區域，這樣就把採樣的重點集中在可能性更高的區域。當由於觀察不佳或者當前掃描與先前計算的地圖重疊區域太小而失敗時，將會用圖2中（c）所示的里程計運動模型作爲重要性概率密度函數。
2.2 自適應重採樣
對粒子濾波的性能具有重要影響的另一個因素是重採樣步驟。在重採樣期間，低權值的粒子通常由高權值的採樣代替。由於用來逼近目標分佈使用的粒子數量是有限的，所以重採樣步驟非常重要。重採樣步驟也可能把一些好的粒子濾去，隨着的進行，粒子的數目會逐漸減少，最後導致粒子耗盡使該算法失效。通常採用有效粒子數來衡量粒子權值的退化程度，即
這裏的爲粒子的歸一化權值。
Doucet等爲了減少進行重採樣步驟的次數，提出了一種理論判定方法來判定是否需要進行重採樣。只有當下降到閾值（，爲粒子數）以下時，才進行一次重採樣。由於重採樣只在需要時進行，進行重採樣的次數將大大減少。多次的實驗證明了這種方法大大降低了將好粒子濾去的風險。
3 實驗
圖3的(a)圖是通過配有SICK激光傳感器的先鋒2(Pioneer 2)機器人在長寬均爲28m的室內環境中構建的地圖。圖3的(b)圖顯示了該地圖的放大細節，展示了該算法建圖的準確性。
圖4顯示了包含幾個嵌套閉環的室內環境。在這種環境下地圖構建非常困難，閉環增加了Rao-Blackwellized粒子濾波器粒子耗盡的機率。圖4展示了RBPF算法在使用60個粒子時可以生成一致並且準確的地圖，然而，此時產生的地圖有時會產生雙層牆壁。
4總結
本文介紹了一種由Doucet等提出的改進Rao-Blackwellized粒子濾波器構建柵格地圖的算法。該方法基於最近的傳感器信息，測距和掃描匹配過程的觀測似然函數來計算高精度的後驗概率密度函數。改進算法比原始算法應用了更少的粒子來構建地圖，並且使用了一種更準確的方式分佈採樣粒子。此外，該算法使用了基於有效樣本量的自適應重採樣策略。該方法減少了粒子濾波器中不必要的重採樣過程的次數，從而大大降低了粒子耗盡的風險。
該算法通過應用不同機器人裝備激光測距儀進行了多次實驗。在這些實驗中，該算法所需的粒子數量通常比原始RBPF算法所需的粒子數小一個數量級，證明了其魯棒性、優越性。
REFERENCES
http://blog.csdn.net/u010545732/article/details/17462941 粒子濾波(Particle filter)matlab實現
http://blog.csdn.net/heyijia0327/article/details/40899819 Particle Filter Tutorial 粒子濾波：從推導到應用
http://ishare.iask.sina.com.cn/f/24615049.html 粒子濾波理論
