• 概述

在SLAM中，機(jī)器人位姿和地圖都是狀態(tài)變量，我們需要同時(shí)對(duì)這兩個(gè)狀態(tài)變量進(jìn)行估計(jì)，即機(jī)器人獲得一張環(huán)境地圖的同時(shí)確定自己相對(duì)于該地圖的位置。我們用x表示機(jī)器人狀態(tài)，m表示環(huán)境地圖，z表示傳感器觀測(cè)情況，u表示輸入控制，下標(biāo)表示時(shí)刻，則對(duì)

進(jìn)行估計(jì)。而由條件貝葉斯法則，可以得到

這一分解相當(dāng)于把SLAM分離為定位和構(gòu)建地圖兩步，大大降低的SLAM問(wèn)題的復(fù)雜度。基于此，Gmaping算法的大致過(guò)程為用上一時(shí)刻的地圖和運(yùn)動(dòng)模型預(yù)測(cè)當(dāng)前時(shí)刻的位姿，然后根據(jù)傳感器觀測(cè)值計(jì)算權(quán)重，重采樣，更新粒子的地圖，如此往復(fù)。ROS中實(shí)現(xiàn)的Gmapping算法框架大致如下，后面講述原理時(shí)將說(shuō)明對(duì)應(yīng)的代碼模塊：

• 定位

Gmapping算法基于粒子濾波，因此定位部分和粒子濾波大致相同：粒子狀態(tài)預(yù)測(cè)，測(cè)量，更新，重采樣。接下來(lái)分別說(shuō)明：

1、狀態(tài)預(yù)測(cè)（draw from motion）

??當(dāng)前時(shí)刻粒子的狀態(tài)首先由運(yùn)動(dòng)模型進(jìn)行更新，在初始值上增加高斯采樣的噪聲，進(jìn)行一個(gè)粗略狀態(tài)估計(jì)。

??在Gmapping算法中，則采用以下算法對(duì)運(yùn)動(dòng)進(jìn)行采樣：

2、測(cè)量（scan match）

??? 這一步是在粗略估計(jì)的基礎(chǔ)上做一次掃描匹配，找到一個(gè)使當(dāng)前觀測(cè)最貼合地圖的位姿，以改進(jìn)基于里程計(jì)模型的提議分布?；舅悸肥窃诨谶\(yùn)動(dòng)模型預(yù)測(cè)的位姿，向負(fù)x，正x，負(fù)y，正y，左旋轉(zhuǎn)，右旋轉(zhuǎn)一共六個(gè)狀態(tài)移動(dòng)預(yù)測(cè)位姿，計(jì)算每個(gè)狀態(tài)下的匹配得分，選擇最高得分對(duì)應(yīng)的位姿為最優(yōu)位姿。

掃描匹配的重點(diǎn)就在于如何計(jì)算匹配得分。所謂匹配，是將當(dāng)前采集的激光數(shù)據(jù)與環(huán)境地圖進(jìn)行對(duì)準(zhǔn)：1）激光點(diǎn)的坐標(biāo)轉(zhuǎn)換至網(wǎng)格地圖坐標(biāo)；2）分別處理六個(gè)狀態(tài)：當(dāng)確定激光點(diǎn)網(wǎng)格坐標(biāo)的地圖值為障礙物時(shí)，進(jìn)行打分（原理與NDT類似，距離越小，分?jǐn)?shù)越大）；3）得分最高的位姿為最優(yōu)位姿。

?? 獲得最優(yōu)粒子位姿后，可以把粒子采樣范圍從又扁又寬的區(qū)域更改到激光雷達(dá)觀測(cè)模型所代表的尖峰區(qū)域L，新的粒子分布就可以更貼近于真實(shí)分布。

???? 掃描匹配之后，我們就找到了L所代表的尖峰區(qū)域，接下來(lái)的任務(wù)是確定

該尖峰區(qū)域所代表的高斯分布的均值和方差。作者的方法是，在L中隨機(jī)采樣K個(gè)點(diǎn)，根據(jù)這K個(gè)點(diǎn)的里程計(jì)和觀測(cè)模型計(jì)算均值和方差，如下式所示。

3、計(jì)算權(quán)重

???? 然后，對(duì)于每個(gè)粒子，我們需要計(jì)算它的權(quán)重，以供后續(xù)的重采樣步驟使用。由于在前面我們利用激光數(shù)據(jù)對(duì)提議分布進(jìn)行了優(yōu)化：

???? 那么粒子的權(quán)重公式變成了：

??? 這里還有一個(gè)問(wèn)題就是權(quán)重計(jì)算，權(quán)重描述的是目標(biāo)分布和提議分布之間的差別。因此我們?cè)谟?jì)算權(quán)重時(shí)就是計(jì)算我們模擬出的提議分布和目標(biāo)分布的不同。而這種不同體現(xiàn)在我們是由有限的采樣模擬出目標(biāo)分布，因此權(quán)重的計(jì)算公式為：

4、重采樣（update Tree Weights）

??? 在執(zhí)行重采樣之前計(jì)算了每個(gè)粒子的權(quán)重，有時(shí)會(huì)因?yàn)榄h(huán)境相似度高或是由于測(cè)量噪聲的影響會(huì)使接近正確狀態(tài)的粒子數(shù)權(quán)重較小而錯(cuò)誤狀態(tài)的粒子的權(quán)重反而會(huì)大。重采樣是依據(jù)粒子權(quán)重來(lái)重新采粒子的，這樣正確的粒子就很有可能會(huì)被丟棄，頻繁的重采樣更加劇了正確但權(quán)重較小粒子被丟棄的可能性。

Gmapping算法中，作者采用權(quán)重值離差的量度進(jìn)行重采樣的判定。

Neff越大，粒子權(quán)重差距越小。想象極端情況，當(dāng)所有粒子權(quán)重都一樣的時(shí)候（比如重采樣之后），這些粒子恰好可以表示真實(shí)分布（類似于按照某個(gè)分布隨機(jī)采樣的結(jié)果）。當(dāng)Neff降低到某個(gè)閾值以下，說(shuō)明粒子的分布與真實(shí)分布差距很大，在粒子層面表現(xiàn)為某些粒子離真實(shí)值很近，而很多粒子離真實(shí)值較遠(yuǎn)，這時(shí)候恰好進(jìn)行重采樣。

• 建圖

Gmapping算法會(huì)構(gòu)建一個(gè)柵格地圖，對(duì)二維環(huán)境進(jìn)行了柵格尺度劃分，而假設(shè)每一個(gè)柵格的狀態(tài)是獨(dú)立的。

對(duì)于環(huán)境中的一個(gè)點(diǎn)，我們用 來(lái)表示它是Free狀態(tài)的概率，用 來(lái)表示它是Occupied狀態(tài)的概率，當(dāng)然兩者的和為1。為了更方便的表示，我們用 作為該點(diǎn)的狀態(tài)，比值越大說(shuō)明該點(diǎn)約可能是障礙物。

對(duì)于一個(gè)點(diǎn)，對(duì)于一個(gè)點(diǎn)，新來(lái)了一個(gè)測(cè)量值z(mì)之后我們需要更新它的狀態(tài)。假設(shè)測(cè)量值來(lái)之前，該點(diǎn)的狀態(tài)為 ，我們要更新它為：

。

由貝葉斯公式計(jì)算可得：

為了方便計(jì)算，我們對(duì)兩邊取對(duì)數(shù)：

在沒(méi)有任何測(cè)量值的初始狀態(tài)下，一個(gè)點(diǎn)的初始狀態(tài)為0，而這一部分關(guān)鍵的地方在于 的計(jì)算，我們稱這個(gè)比值為測(cè)量值的模型，標(biāo)記為lomeas。實(shí)際上測(cè)量值的模型只有兩種： 和 ?，而且都是定值。這樣每獲得一次測(cè)量值，我們都能用加減法對(duì)點(diǎn)狀態(tài)進(jìn)行更新。從而完成更新地圖的工作。以下圖為例：

x是真實(shí)世界中的坐標(biāo)， 為柵格地圖中的坐標(biāo)，r為一格的長(zhǎng)度，1/r表示分辨率，則 。則二維情況下： 。假設(shè)圖中機(jī)器人的位姿為（x，y， ），我們可以很容易計(jì)算障礙物的位置：

其中，d為測(cè)量得到的距離， 為激光線與機(jī)器人位姿角的夾角。我們得到兩個(gè)坐標(biāo)后能計(jì)算出兩點(diǎn)在柵格地圖的位置（i，j ）與（ ）。

然后，我們利用bresenham算法(compute active area)來(lái)計(jì)算非障礙物格點(diǎn)的集合。然后利用上文所述結(jié)論，更新柵格地圖即可。

Bresenham算法基本思想是采用遞推步進(jìn)的辦法，令每次最大變化方向的坐標(biāo)步進(jìn)一個(gè)像素，同時(shí)另一個(gè)方向的坐標(biāo)依據(jù)誤差判別式的符號(hào)來(lái)決定是否也要步進(jìn)一個(gè)像素。舉例說(shuō)明：

由于顯示直線的象素點(diǎn)只能取整數(shù)值坐標(biāo)，可以假設(shè)直線上第i個(gè)像素點(diǎn)坐標(biāo)為(xi，yi)，它是直線上點(diǎn)(xi，yi)的最佳近似，并且xi=xi(假設(shè)直線斜率小于1）。那么，直線上下一個(gè)像素點(diǎn)的可能位置是(xi+1，yi)或(xi+1，yi+1)。由圖中可以知道，在x=xi+1處，直線上點(diǎn)的y值是y=m(xi+1)+b，該點(diǎn)離像素點(diǎn)(xi+1，yi)和像素點(diǎn)(xi+1，yi+1)的距離分別是d1和d2：?

　這兩個(gè)距離差是

分析d1-d2，有以下三種情況：

當(dāng)此值為正時(shí)，d1>d2，說(shuō)明直線上理論點(diǎn)離(xi+1，yi+1) 像素較近，下一個(gè)像素點(diǎn)應(yīng)取(xi+1，yi+1)。

當(dāng)此值為負(fù)時(shí)，d1<d2，說(shuō)明直線上理論點(diǎn)離(xi+1，yi) 像素較近，則下一個(gè)像素點(diǎn)應(yīng)取(xi+1，yi)。

當(dāng)此值為零時(shí)，說(shuō)明直線上理論點(diǎn)離上、下兩個(gè)像素點(diǎn)的距離相等，取哪個(gè)點(diǎn)都行，算法規(guī)定這種情況下取(xi+1，yi+1)作為下一個(gè)像素點(diǎn)。

因此只要利用(d1-d2)的符號(hào)就可以決定下一個(gè)像素點(diǎn)的選擇。

參考文獻(xiàn)

https://blog.csdn.net/qq_36355662/article/details/90301219

https://blog.csdn.net/shixiaolu63/article/details/93739379

https://www.jianshu.com/p/f044da681454

https://blog.csdn.net/liuyanpeng12333/article/details/81946841

https://www.cnblogs.com/yhlx125/p/5634128.html

概率機(jī)器人

粒子濾波：從推導(dǎo)到應(yīng)用

Improved Techniques for Grid Mapping with Rao-Blackwellized Particle Filters

總結(jié)

以上是生活随笔為你收集整理的Gmapping——从原理到实践的全部?jī)?nèi)容，希望文章能夠幫你解決所遇到的問(wèn)題。

如果覺(jué)得生活随笔網(wǎng)站內(nèi)容還不錯(cuò)，歡迎將生活随笔推薦給好友。