Gmapping筆記

本文轉載自查看原文 2018-11-09 09:47 4301 SLAM

2D-slam 激光slam: 開源代碼的比較HectorSLAM Gmapping KartoSLAM CoreSLAM LagoSLAM

作者：kint_zhao
原文：https://blog.csdn.net/zyh821351004/article/details/47381135

最近找到一篇論文比較了一下目前ros下2D激光slam的開源代碼效果比較:

詳細參見論文: An evaluation of 2D SLAM techniques available in robot operating system

1. 算法介紹
A . HectorSLAM
scan-matching(Gaussian-Newton equation) + 　傳感器的要求高
要求: 高更新頻率小測量噪聲的激光掃描儀. 不需要里程計,使空中無人機與地面小車在不平坦區域運行存在運用的可能性.

利用已經獲得的地圖對激光束點陣進行優化, 估計激光點在地圖的表示,和占據網格的概率.

其中掃描匹配利用的是高斯牛頓的方法進行求解. 找到激光點集映射到已有地圖的剛體轉換(x,y,theta).

( 接觸的匹配的方法還有最近鄰匹配的方法(ICP) ,gmapping代碼中的scanmatcher部分有兩種方法選擇. )

為避免局部最小而非全局最優的(類似於多峰值模型的,局部梯度最小了,但非全局最優)出現,地圖采用多分辨率的形式.

　　導航中的狀態估計可以加入慣性測量，進行ＥＫＦ濾波．

B. Gmapping: tutorial
proposed by Grisetti et al. and is a Rao-Blackwellized PF SLAM approach.
adaptive resampling technique
　目前激光2Dslam用得最廣的方法，gmapping采用的是RBPF的方法. 必須得了解粒子濾波(利用統計特性描述物理表達式下的結果)的方法.

粒子濾波的方法一般需要大量的粒子來獲取好的結果,但這必會引入計算的復雜度;粒子是一個依據過程的觀測逐漸更新權重與收斂的過程,這種重采樣的過程必然會代入粒子耗散問題(depletion problem), 大權重粒子顯著,小權重粒子會消失(有可能正確的粒子模擬可能在中間的階段表現權重小而消失).

自適應重采樣技術引入減少了粒子耗散問題 , 計算粒子分布的時候不單單僅依靠機器人的運動(里程計),同時將當前觀測考慮進去, 減少了機器人位置在粒子濾波步驟中的不確定性. (FAST-SLAM 2.0 的思想，可以適當減少粒子數)

C. KartoSLAM
   graph-based SLAM approach developed bySRI International’s Karto Robotics
   highly-optimized and non iterative Cholesky matrix decomposition for sparse linear systems as its solver
   the Sparse Pose Adjustment (SPA) is responsible for both scan matching and loop-closure procedures
    Karto Open Libraries 2.0 SDK(Karto Open Libraries 2.0 is available under the LGPL open source license. You can try the full Karto SDK 2.1 for 30 days.)   后面在詳細研究下(比較下ＭRPT )

圖優化的核心思想我認為主要就是矩陣的稀疏化與最小二乘..參見graphslam學習

　　KartoSLAM是基於圖優化的方法，用高度優化和非迭代 cholesky矩陣進行稀疏系統解耦作為解．　圖優化方法利用圖的均值表示地圖，每個節點表示機器人軌跡的一個位置點和傳感器測量數據集，箭頭的指向的連接表示連續機器人位置點的運動，每個新節點加入，地圖就會依據空間中的節點箭頭的約束進行計算更新．

　　KartoSLAM的ROS版本，其中采用的稀疏點調整（the Spare Pose Adjustment(SPA)）與掃描匹配和閉環檢測相關．landmark越多,內存需求越大,然而圖優化方式相比其他方法在大環境下制圖優勢更大.在某些情況下KartoSLAM更有效,因為他僅包含點的圖(robot pose),求得位置后再求map.

D. CoreSLAM
tinySLAM algorithm: two different steps(distance calculation and update of the map
simple and easy
為了簡單和容易理解最小化性能損失的一種slam算法.將算法簡化為距離計算與地圖更新的兩個過程, 第一步,每次掃描輸入,基於簡單的粒子濾波算法計算距離,粒子濾波的匹配器用於激光與地圖的匹配,每個濾波器粒子代表機器人可能的位置和相應的概率權重,這些都依賴於之前的迭代計算. 選擇好最好的假設分布,即低權重粒子消失,新粒子生成..在更新步驟,掃描得到的線加入地圖中,當障礙出現時,圍繞障礙點繪制調整點集,而非僅一個孤立點.

E. LagoSLAM
Linear Approximation for Graph Optimization
the optimization process requires no initial guess
基本的圖優化slam的方法就是利用最小化非線性非凸代價函數.每次迭代, 解決局部凸近似的初始問題來更新圖配置,過程迭代一定次數直到局部最小代價函數達到. (假設起始點經過多次迭代使得局部代價函數最小). LagoSLAM 是線性近似圖優化,不需要初始假設. 優化器的方法可以有三種選擇 Tree-based netORK Optimizer(TORO), g2o,LAGO

2. 實驗結果比較
分別從大小仿真環境和實際環境以及cpu消耗的情況下對算法進行了比較. CartoSLAM 與gampping占很大優勢

說明:能力有限,講得有問題歡迎指正,暫且粗解到這,后面再具體看看對應算法的詳細論文介紹,有問題再改...

slam算法的論文幾篇
下載：http://download.csdn.net/detail/zyh821351004/8986339

--------------------------------------------------------------------

補：源碼工程整理： https://github.com/kintzhao/laser_slam_openSources
---------------------

GMapping原理分析

作者：豆子愛玩
原文：https://blog.csdn.net/liuyanpeng12333/article/details/81946841

概念：
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中的很多概念。這里問題主要有：

為什么RBpf可以將定位和建圖分離；
Gmapping是如何在RBpf的基礎改進提議分布的；
為什么要執行選擇性重采樣；
什么是粒子退化及如何防止粒子退化；
為什么Gmapping嚴重依賴里程計；
什么是提議分布；
什么是目標分布；
為什么需要提議分布和目標分布；
算法中是如何計算權重；
粒子濾波粒子數和傳感器精度的關系；
為什么在有大回環的環境中增加粒子數可以使建出的地圖正確閉合；
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個數據后我們就可以模擬出一個高斯函數作為提議分布：

有了模擬好的提議分布我們就可以采樣出下一時刻機器人的位姿信息。

這里還有一個問題就是權重計算，我們知道權重描述的是目標分布和提議分布之間的差別。因此我們在計算權重時就是計算我們模擬出的提議分布和目標分布的不同。而這種不同體現在我們是由有限的采樣模擬出目標分布，因此權重的計算公式為：

到此改進提議分布就完成了，接下來是選擇性重采樣。這部分比較簡單，就是設定一個閾值，當粒子的權重變化大於我們設定的閾值時就會執行重采樣，這樣減少了采樣的次數，也就減緩了粒子退化。至此理論部分就講完了！
---------------------

[SLAM] GMapping SLAM源碼閱讀（草稿）

作者：太一吾魚水
原文：http://www.cnblogs.com/yhlx125/p/5634128.html

目前可以從很多地方得到RBPF的代碼，主要看的是Cyrill Stachniss的代碼，據此進行理解。

Author：Giorgio Grisetti; Cyrill Stachniss http://openslam.org/

https://github.com/Allopart/rbpf-gmapping 和文獻[1]上結合的比較好，方法都可以找到對應的原理。

https://github.com/MRPT/mrpt MRPT中可以采用多種掃描匹配的方式，可以通過配置文件進行配置。

雙線程和程序的基本執行流程

GMapping采用GridSlamProcessorThread后台線程進行數據的讀取和運算，在gsp_thread.h和相應的實現文件gsp_thread.cpp中可以看到初始化，參數配置，掃描數據讀取等方法。

最關鍵的流程是在后台線程調用的方法void * GridSlamProcessorThread::fastslamthread(GridSlamProcessorThread* gpt)

而這個方法中最主要的處理掃描數據的過程在428行，bool processed=gpt->processScan(*rr);同時可以看到GMapping支持在線和離線兩種模式。

注意到struct GridSlamProcessorThread : public GridSlamProcessor ，這個后台線程執行類GridSlamProcessorThread 繼承自GridSlamProcessor，所以執行的是GridSlamProcessor的processScan方法。

GridSlamProcessor::processScan

可以依次看到下面介紹的1-7部分的內容。

1.運動模型

t時刻粒子的位姿最初由運動模型進行更新。在初始值的基礎上增加高斯采樣的noisypoint，參考MotionModel::drawFromMotion()方法。原理參考文獻[1]5.4.1節，sample_motion_model_odometry算法。

p是粒子的t-1時刻的位姿，pnew是當前t時刻的里程計讀數，pold是t-1時刻的里程計讀數。參考博客：小豆包的學習之旅：里程計運動模型

 1 OrientedPoint MotionModel::drawFromMotion(const OrientedPoint& p, const OrientedPoint& pnew, const OrientedPoint& pold) const{
 2     double sxy=0.3*srr;
 3     OrientedPoint delta=absoluteDifference(pnew, pold);
 4     OrientedPoint noisypoint(delta);
 5     noisypoint.x+=sampleGaussian(srr*fabs(delta.x)+str*fabs(delta.theta)+sxy*fabs(delta.y));
 6     noisypoint.y+=sampleGaussian(srr*fabs(delta.y)+str*fabs(delta.theta)+sxy*fabs(delta.x));
 7     noisypoint.theta+=sampleGaussian(stt*fabs(delta.theta)+srt*sqrt(delta.x*delta.x+delta.y*delta.y));
 8     noisypoint.theta=fmod(noisypoint.theta, 2*M_PI);
 9     if (noisypoint.theta>M_PI)
10         noisypoint.theta-=2*M_PI;
11     return absoluteSum(p,noisypoint);
12 }

2.掃描匹配

掃描匹配獲取最優的采樣粒子。GMapping默認采用30個采樣粒子。

GridSlamProcessor::scanMatch

注意ScanMatcher::score()函數的原理是likehood_field_range_finder_model方法，參考《概率機器人》手稿P143頁。

ScanMatcher::optimize()方法獲得了一個最優的粒子，基本流程是按照預先設定的步長不斷移動粒子的位置，根據提議分布計算s，得到score最小的那個作為粒子的新的位姿。

View Code

ScanMatcher::likelihoodAndScore()和ScanMatcher::score()方法基本一致，但是是將新獲得的粒子位姿進行計算q，為后續的權重計算做了准備。

ScanMatcher::optimize()方法——粒子的運動+score()中激光掃描觀測數據。

其他的掃描匹配方法也是可以使用的：比如ICP算法(rbpf-gmapping的使用的是ICP方法，先更新了旋轉角度，然后采用提議分布再次優化粒子)、Cross Correlation、線特征等等。

3.提議分布（Proposal distribution）

注意是混合了運動模型和觀測的提議分布，將掃描觀測值整合到了提議分布中^{[2]（公式9）}。因此均值和方差的計算與單純使用運動模型作為提議分布的有所區別。提議分布的作用是獲得一個最優的粒子，同時用來計算權重，這個體現在ScanMatcher::likelihoodAndScore()和ScanMatcher::score()方法中，score方法中采用的是服從0均值的高斯分布近似提議分布（文獻[1] III.B節)。關於為什么采用混合的提議分布，L(i)區域小的原理在文獻[1]中III.A節有具體介紹。

rbpf-gmapping的使用的是運動模型作為提議分布。

4.權重計算

在重采樣之前進行了一次權重計算updateTreeWeights(false);

重采樣之后又進行了一次。代碼在gridslamprocessor_tree.cpp文件中。

1 void  GridSlamProcessor::updateTreeWeights(bool weightsAlreadyNormalized)
2 {
3   if (!weightsAlreadyNormalized)
4   {
5     normalize();
6   }
7   resetTree();
8   propagateWeights();
9 }

5.重采樣

原理：粒子集對目標分布的近似越差，則權重的方差越大。

因此用N_eff作為權重值離差的量度。

GridSlamProcessor::resample

6.占用概率柵格地圖

此處的方法感覺有點奇怪，在resample方法中執行ScanMatcher::registerScan()方法，計算占用概率柵格地圖。采樣兩種方式，采用信息熵的方式和文獻[1] 9.2節的計算方法不一樣。

ScanMatcher::registerScan

rbpf-gmapping的使用的是文獻[1] 9.2節的計算方法，在Occupancy_Grid_Mapping.m文件中實現，同時調用Inverse_Range_Sensor_Model方法。

gridlineTraversal實現了beam轉成柵格的線。對每一束激光束，設start為該激光束起點，end為激光束端點（障礙物位置），使用Bresenham划線算法確定激光束經過的格網。

小豆包的學習之旅：占用概率柵格地圖和cost-map

7.計算有效區域

第一次掃描，count==0時，如果激光觀測數據超出了范圍，更新柵格地圖的范圍。同時確定有效區域。

computeActiveArea

每次掃描匹配獲取t時刻的最優粒子后會計算有效區域。

重采樣之后，調用ScanMatcher::registerScan() 方法，也會重新計算有效區域。

參考文獻：

[1]Sebastian Thrun et al. "Probabilistic Robotics（手稿）."

[2]Grisetti, G. and C. Stachniss "Improved Techniques for Grid Mapping with Rao-Blackwellized Particle Filters."

---------------------

[SLAM]2D激光掃描匹配方法

作者：太一吾魚水
原文：http://www.cnblogs.com/yhlx125/p/5586499.html

1.Beam Model

Beam Model我將它叫做測量光束模型。個人理解，它是一種完全的物理模型，只針對激光發出的測量光束建模。將一次測量誤差分解為四個誤差。

...

2.Likehood field

似然場模型，和測量光束模型相比，考慮了地圖的因素。不再是對激光的掃描線物理建模，而是考慮測量到的物體的因素。

似然比模型本身是一個傳感器觀測模型，之所以可以實現掃描匹配，是通過划分柵格，步進的方式求的最大的Score，將此作為最佳的位姿。

 
                  for 
                  k=1: 
                  size 
                  (zt,1) 
                 
                  if 
                  zt(k,2)>0 
                 
                  d = -grid_dim/2; 
                 
                  else 
                 
                  d = grid_dim/2; 
                 
                  end 
                 
                  phi = pi_to_pi(zt(k,2) + x(3)); 
                 
                  if 
                  zt(k,1) ~= Z_max 
                 
                  ppx = [x(1),x(1) + zt(k,1)* 
                  cos 
                  (phi) + d]; 
                 
                  ppy = [x(2),x(2) + zt(k,1)* 
                  sin 
                  (phi) + d]; 
                 
                  end_points = [end_points;ppx(2),ppy(2)]; 
                 
                  wm = likelihood_field_range_finder_model(X( 
                  j 
                  ,:)',xsensor,... 
                 
                  zt(k,:)',nearest_wall, grid_dim, std_hit,Z_weights,Z_max); 
                 
                  W( 
                  j 
                  ) = W( 
                  j 
                  ) * wm; 
                 
                  else 
                 
                  dist = Z_max + std_hit* 
                  randn 
                  (1); 
                 
                  ppx = [x(1),x(1) + dist* 
                  cos 
                  (phi) + d]; 
                 
                  ppy = [x(2),x(2) + dist* 
                  sin 
                  (phi) + d]; 
                 
                  missed_points = [missed_points;ppx(2),ppy(2)];                 
                 
                  end 
                 
                  set 
                  (handle_sensor_ray(k), 
                  'XData' 
                  , ppx,  
                  'YData' 
                  , ppy) 
                 
                  end

 
                  function 
                  q = likelihood_field_range_finder_model(X,x_sensor,zt,N,dim,std_hit,Zw,z_max) 
                 
                  % retorna probabilidad de medida range finder :) 
                 
                  % X col, zt col, xsen col 
                 
                  [n,m] =  
                  size 
                  (N); 
                 
                  % Robot global position and orientation 
                 
                  theta = X(3); 
                 
                  % Beam global angle 
                 
                  theta_sen = zt(2); 
                 
                  phi = pi_to_pi(theta + theta_sen); 
                 
                  %Tranf matrix in case sensor has relative position respecto to robot's CG 
                 
                  rotS = [ 
                  cos 
                  (theta),- 
                  sin 
                  (theta); 
                  sin 
                  (theta), 
                  cos 
                  (theta)]; 
                 
                  % Prob. distros parameters 
                 
                  sigmaR = std_hit; 
                 
                  zhit  = Zw(1); 
                 
                  zrand = Zw(2); 
                 
                  zmax  = Zw(3); 
                 
                  % Actual algo 
                 
                  q = 1; 
                 
                  if 
                  zt(1) ~= z_max 
                 
                  % get global pos of end point of measument 
                 
                  xz = X(1:2) + rotS*x_sensor + zt(1)*[ 
                  cos 
                  (phi); 
                 
                  sin 
                  (phi)]; 
                 
                  xi =  
                  floor 
                  (xz(1)/dim) + 1; 
                 
                  yi =  
                  floor 
                  (xz(2)/dim) + 1; 
                 
                  % if end point doesn't lay inside map: unknown 
                 
                  if 
                  xi<1 || xi>n || yi<1 || yi>m 
                 
                  q = 1.0/z_max;  
                  % all measurements equally likely, uniform in range [0-zmax] 
                 
                  return 
                 
                  end 
                 
                  dist2 = N(xi,yi); 
                 
                  gd = gauss_1D(0,sigmaR,dist2); 
                 
                  q = zhit*gd + zrand/zmax; 
                 
                  end 
                 
                  end

3.Correlation based sensor models相關分析模型

XX提出了一種用相關函數表達馬爾科夫過程的掃描匹配方法。

互相關方法Cross-Correlation，另外相關分析在進行匹配時也可以應用，比如對角度直方圖進行互相關分析，計算變換矩陣。

參考文獻：A Map Based On Laser scans without geometric interpretation

circular Cross-Correlation的Matlab實現

 1 % Computes the circular cross-correlation between two sequences
 2 %
 3 % a,b             the two sequences
 4 % normalize       if true, normalize in [0,1]
 5 %
 6 function c = circularCrossCorrelation(a,b,normalize)
 7 
 8 for k=1:length(a)
 9     c(k)=a*b';
10     b=[b(end),b(1:end-1)]; % circular shift
11 end
12 
13 if normalize
14     minimum = min(c);
15     maximum = max(c);
16     c = (c - minimum) / (maximum-minimum);
17 end

4.MCL

蒙特卡洛方法

5.AngleHistogram

角度直方圖

6.ICP/PLICP/MBICP/IDL

屬於ICP系列，經典ICP方法，點到線距離ICP，

7.NDT

正態分布變換

8.pIC

結合概率的方法

9.線特征

目前應用線段進行匹配的試驗始終不理想：因為線對應容易產生錯誤，而且累積誤差似乎也很明顯！

---------------------

slam-gmapping之scanMatch算法原理

作者： Tomas_yuan
原文：https://blog.csdn.net/m0_37350344/article/details/81159413

Scan Matching

問題描述：給定Scan和map，或者給定scan和scan或者給定map和map,找到最匹配的變換（translation+rotation）
作用：提高提議分布
方法：
p(z|x,m)= beam sonsor model sensor full readings <-> map
p(z|x,m)= likelihood field model sensor beam endpoints <-> likelihood field
p(mlocal|x,m)=matching model local map<->global map

Beam sensor Model

噪聲原因：較小的測量噪聲、動態物體帶來的誤差、未探測到物體帶來的誤差（沒有探測到物體時將使用測距儀的最大測程作為測量數據，因此也有可能不正確）、隨機誤差。
（1）測量噪聲：由測距儀的精度、大氣對測量信號的影響等造成，其概率模型一般是以理想測量距離為均值的高斯模型
（2）動態物體誤差：由未預測到的物體（Unexpected objects）帶來的誤差。環境是動態的，而保存的地圖是靜態的，即不變的。沒有包含在地圖里的物體的出現會產生一個令人意外的比較小的測量數據。典型的動態物體就是行人。處理這些誤差的一種方法就是把它當成傳感器噪聲。測量到的距離越大，檢測到動態物體的概率越小，且小距離對應的測量到的是動態物體的概率應遠大於大距離的概率，隨意概率隨距離的增大呈指數下降趨勢，所以其概率模型為指數分布
（3）隨機誤差：測距儀偶爾會產生一些無法解釋的測量結果。
（4）測量失敗：有時候，傳感器探測不到障礙物，比如試圖探測一些吸收光線的物體，此時的探測數據將失效。典型的探測結果就是傳感器返回自身的最大探測距離。概率模型表示為：（應該就是兩點分布，即失敗與沒有失敗）

根據實際數據得到參數

Likelihood Field Model

+ 該方法的缺點
+ 沒有對人或者其他動態的障礙物建模
+ 沒有對激光束建模，認為傳感器可以穿過牆
+ 不能處理未知區域（改進若end_point 在未知區域P(z|x,m)=1/z max P(z|x,m)=1/zmaxP(z|x,m)=1/z_{max}）
+ P(z|x,m) P(z|x,m)P(z|x,m)對應於似然區域得分
+ slam工程中
激光傳感器的作用主要是感知周圍環境，獲取的掃描數據在SLAM過程中有兩個作用：一是構建地圖（占用概率柵格地圖）；另外一個是掃描匹配，優化里程計獲取的機器人位姿，掃描匹配是建立局部子圖和全局地圖位置關系的過程，常用到的就是傳感器觀測模型。

scanMatch
2D激光掃描匹配方法

ScanMatching 實現機器人位姿估計

matlab程序實現

transform = matchScans(currentScan, referenceScan)//計算相對位姿
transScan = transformScan(currentScan, transform);//轉為可視化結果
---
使用迭代算法建立柵格地圖1234

---------------------
作者：Tomas_yuan
來源：CSDN
原文：https://blog.csdn.net/m0_37350344/article/details/81159413
版權聲明：本文為博主原創文章，轉載請附上博文鏈接！

免責聲明！

本站轉載的文章為個人學習借鑒使用，本站對版權不負任何法律責任。如果侵犯了您的隱私權益，請聯系本站郵箱yoyou2525@163.com刪除。

猜您在找 gmapping cartographer之 gmapping gmapping 學習 hector與gmapping總結 gmapping 源碼詳解 ROS系列之初識gmapping 通過gmapping和偽造的odom，完成Kinect建圖 [SLAM] GMapping SLAM源碼閱讀（草稿） Ubuntu18.04環境下melodic安裝gmapping 筆記