2D-slam 激光slam: 開源代碼的比較HectorSLAM Gmapping KartoSLAM CoreSLAM LagoSLAM
做者：kint_zhao
原文：https://blog.csdn.net/zyh821351004/article/details/47381135
html
最近找到一篇論文比較了一下 目前ros下2D激光slam的開源代碼效果比較:git
詳細參見論文: An evaluation of 2D SLAM techniques available in robot operating systemgithub
1. 算法介紹
A . HectorSLAM
scan-matching(Gaussian-Newton equation) + 傳感器的要求高
要求: 高更新頻率小測量噪聲的激光掃描儀. 不須要里程計,使空中無人機與地面小車在不平坦區域運行存在運用的可能性.算法
利用已經得到的地圖對激光束點陣進行優化, 估計激光點在地圖的表示,和佔據網格的機率.app
其中掃描匹配利用的是高斯牛頓的方法進行求解. 找到激光點集映射到已有地圖的剛體轉換(x,y,theta).框架
( 接觸的匹配的方法還有最近鄰匹配的方法(ICP) ,gmapping代碼中的scanmatcher部分有兩種方法選擇. )dom
爲避免局部最小而非全局最優的(相似於多峯值模型的,局部梯度最小了,但非全局最優)出現,地圖採用多分辨率的形式.ide
導航中的狀態估計能夠加入慣性測量，進行ＥＫＦ濾波．函數
B. Gmapping: tutorial
proposed by Grisetti et al. and is a Rao-Blackwellized PF SLAM approach.
adaptive resampling technique
目前激光2Dslam用得最廣的方法，gmapping採用的是RBPF的方法. 必須得了解粒子濾波(利用統計特性描述物理表達式下的結果)的方法.oop
粒子濾波的方法通常須要大量的粒子來獲取好的結果,但這必會引入計算的複雜度;粒子是一個依據過程的觀測逐漸更新權重與收斂的過程,這種重採樣的過程必然會代入粒子耗散問題(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
概念：
一、Gmapping是基於濾波SLAM框架的經常使用開源SLAM算法。
二、Gmapping基於RBpf粒子濾波算法，即將定位和建圖過程分離，先進行定位再進行建圖。
三、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個數據後咱們就能夠模擬出一個高斯函數做爲提議分佈：
有了模擬好的提議分佈咱們就能夠採樣出下一時刻機器人的位姿信息。
這裏還有一個問題就是權重計算，咱們知道權重描述的是目標分佈和提議分佈之間的差異。所以咱們在計算權重時就是計算咱們模擬出的提議分佈和目標分佈的不一樣。而這種不一樣體如今咱們是由有限的採樣模擬出目標分佈，所以權重的計算公式爲：
到此改進提議分佈就完成了，接下來是選擇性重採樣。這部分比較簡單，就是設定一個閾值，當粒子的權重變化大於咱們設定的閾值時就會執行重採樣，這樣減小了採樣的次數，也就減緩了粒子退化。至此理論部分就講完了！
---------------------
做者：太一吾魚水
原文：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.重採樣
原理：粒子集對目標分佈的近似越差，則權重的方差越大。
所以用Neff做爲權重值離差的量度。
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."
---------------------
做者：太一吾魚水
原文：http://www.cnblogs.com/yhlx125/p/5586499.html
1.Beam Model
Beam Model我將它叫作測量光束模型。我的理解，它是一種徹底的物理模型，只針對激光發出的測量光束建模。將一次測量偏差分解爲四個偏差。
ph hit phhit ，測量自己產生的偏差，符合高斯分佈。
ph xx phxx ，因爲存在運動物體產生的偏差。
...
2.Likehood field
似然場模型，和測量光束模型相比，考慮了地圖的因素。再也不是對激光的掃描線物理建模，而是考慮測量到的物體的因素。
似然比模型自己是一個傳感器觀測模型，之因此能夠實現掃描匹配，是經過劃分柵格，步進的方式求的最大的Score，將此做爲最佳的位姿。
|
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
|
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
|
|
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
|
function
q = likelihood_field_range_finder_model(X,x_sensor,zt,N,dim,std_hit,Zw,z_max)
[n,m] =
size
(N);
theta = X(3);
theta_sen = zt(2);
phi = pi_to_pi(theta + theta_sen);
rotS = [
cos
(theta),-
sin
(theta);
sin
(theta),
cos
(theta)];
sigmaR = std_hit;
zhit = Zw(1);
zrand = Zw(2);
zmax = Zw(3);
q = 1;
if
zt(1) ~= z_max
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
xi<1 || xi>n || yi<1 || yi>m
q = 1.0/z_max;
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 版權聲明：本文爲博主原創文章，轉載請附上博文連接！