机器人学习--粒子滤波及其在定位中的应用
前提基礎(chǔ)，先看一下 貝葉斯濾波? 和 蒙特卡洛方法
一、什么是粒子濾波？
這里有個(gè)基于粒子濾波的物體跟蹤?案例說(shuō)明：
參考：基于粒子濾波的物體跟蹤 - yangyangcv - 博客園
如果還是看不懂， 知乎上有個(gè) 通俗地解釋粒子濾波器 的案例說(shuō)明，感覺(jué)工程師做過(guò)實(shí)際項(xiàng)目有經(jīng)驗(yàn)的說(shuō)的要通透一點(diǎn)。
參考：通俗地解釋粒子濾波器 - 知乎
先定義一下基本場(chǎng)景，以及理清楚基礎(chǔ)概念。
我們有一個(gè)小車 ，可以在2D平面上移動(dòng)。
我們離散化整個(gè)過(guò)程，意思是，我們?cè)诿總€(gè)k*T（T是周期）的時(shí)候用粒子濾波定位小車的位置。比如，出發(fā)后第一秒1s，第二秒2s，第三秒3s。
*************************以下也可以先忽略*******************************
我們認(rèn)為這個(gè)過(guò)程可以用馬爾可夫鏈簡(jiǎn)化，也就是說(shuō)，這一步k的狀態(tài)由上一步的狀態(tài)+轉(zhuǎn)移過(guò)程就可以確定。如果想詳細(xì)了解可以看之前的文章，因?yàn)楸举|(zhì)上這也是個(gè)用蒙特卡洛（瞎試）方法得到概率分布的貝葉斯濾波器。當(dāng)然如果你不清楚上面這些名詞，不重要。因?yàn)槲抑蟮闹v解用不著！
風(fēng)光霽月游俠兒：通俗地解釋卡爾曼濾波器（一）——從貝葉斯濾波器說(shuō)起?zhuanlan.zhihu.com?
*************************以上也可以先忽略*******************************
粒子濾波定位小車的過(guò)程分為三步，需要三個(gè)信息（就是我們知道三件事）。
哪三個(gè)信息呢？
1.車上一秒在哪
2.車的運(yùn)動(dòng)是什么
3.測(cè)量結(jié)果，比如GPS
哪三步呢？
1.Prediction Step
2.Innovation Step
3. Re-sampling Step
這三步是干嘛呢？這三個(gè)信息在這三步里怎么用呢？
一步一步來(lái),
第一步，Prediction Step（預(yù)測(cè)）
用到上一步的結(jié)果，和運(yùn)動(dòng)。請(qǐng)看下圖
我們用“粒子”點(diǎn)來(lái)表示車的位置，也就是上圖的×。
上一步的位置，我們用紅色的x來(lái)表示，他們都是車的可能的位置。比如上圖的(1,5),(3,5),(3,7),(5,5)
那你說(shuō)這些位置聽(tīng)誰(shuí)的呢？車在哪呢？我們可以給每個(gè)位置定個(gè)權(quán)重，然后加權(quán)平均一下。在預(yù)測(cè)這步權(quán)重都是一樣的，我們有四個(gè)粒子，那么每個(gè)權(quán)重就是0.25。
也就是我們有四個(gè)粒子x1，x2，x3，x4，x[(橫坐標(biāo)x，縱坐標(biāo)y)，權(quán)重w]? ? ? ? ? ?? //注釋： 這部分粒子集合權(quán)重，參考《概率機(jī)器人》中文版 P74頁(yè) 程序4.3粒子濾波和P191頁(yè) 粒子濾波定位的內(nèi)容
x1[(1,5),0.25]
x2[(3,5),0.25]
x3[(3,7),0.25]
x4[(5,5),0.25]
然后我們有個(gè)運(yùn)動(dòng)，這個(gè)運(yùn)動(dòng)可能是輪速傳感器得來(lái)的，或者是比如IMU，復(fù)雜一點(diǎn)對(duì)于無(wú)人車來(lái)講，7自由度整車模型的預(yù)測(cè)結(jié)果等等（這就專業(yè)一點(diǎn)了）。不管怎么來(lái)的吧。
假設(shè)我們現(xiàn)在知道了運(yùn)動(dòng)是(5,4)。但是還有個(gè)誤差。
預(yù)測(cè)這步就是把所有權(quán)重不變，在狀態(tài)（運(yùn)動(dòng)）加上這個(gè)運(yùn)動(dòng)的結(jié)果。
x1[(6,9),0.25]
x2[(8,9),0.25]
x3[(8,11),0.25]
x4[(10,9),0.25]
如果你知道噪聲的大小，可以在結(jié)果上加上一個(gè)隨機(jī)數(shù)。結(jié)果如下：
x1[(6.1,9.2),0.25]
x2[(7.9,9),0.25]
x3[(8.3,10.8),0.25]
x4[(10.1,8.9),0.25]
預(yù)測(cè)這步就完啦。? ? ? ? ? //注釋： 以上這部分內(nèi)容參考 《概率機(jī)器人》這本書中文版的P103頁(yè)程序5.6， 采樣里程計(jì)運(yùn)動(dòng)模型
第二步，Innovation Step（這個(gè)咋翻譯，更新？）
這步的作用是，嘿我不是還有個(gè)GPS嗎（或者其他的測(cè)量，圖像定位之類的）？
我們說(shuō)測(cè)量值是下圖那個(gè)綠色的點(diǎn)，然后測(cè)量是不準(zhǔn)的嘛，假設(shè)他遵循高斯分布。
*************************以下也可以先忽略*******************************
一般來(lái)講測(cè)量結(jié)果是可以用隨機(jī)變量描述的。（根據(jù)最新的ISO和德國(guó)DIN，并不建議使用測(cè)量值加error的方式表述，而是使用隨機(jī)變量描述）
*************************以上也可以先忽略*******************************
我們?cè)趺窗堰@個(gè)測(cè)量和那一堆現(xiàn)在有的，經(jīng)過(guò)prediction的粒子的信息融合在一起呢？
是通過(guò)之前那個(gè)權(quán)重值。
按照測(cè)量的概率分布來(lái)重新分配權(quán)重。高斯分布的話當(dāng)然是離中心點(diǎn)越近權(quán)重越大啦。上圖舉例x2<x3<x1<x4
x1[(6.1,9.2),0.4]
x2[(7.9,9),0.5]
x3[(8.3,10.8),0.35]
x4[(10.1,8.9),0.25]
好現(xiàn)在權(quán)重更新了，但是還有個(gè)問(wèn)題！我們稍后希望用概率的方式來(lái)使用權(quán)重，但是權(quán)重加在一起大于1了怎么辦？歸一化一下就好了嘛。每個(gè)新權(quán)重=舊權(quán)重/求和（舊權(quán)重）
x1[(6.1,9.2),0.26]
x2[(7.9,9),0.33]
x3[(8.3,10.8),0.23]
x4[(10.1,8.9),0.16]
然后這步就完了，簡(jiǎn)單吧
第三步，Resampling step
這一步是干嘛呢？
你上一步得到的那些權(quán)重沒(méi)用上對(duì)不對(duì)，這步用上了。
我們之前那些粒子不用了，重新?lián)u色子搖幾個(gè)粒子出來(lái)。不過(guò)一般來(lái)講，粒子數(shù)量是不變的，都是能足夠多就足夠多。
比如之前的
x1[(6.1,9.2),0.26]
x2[(7.9,9),0.33]
x3[(8.3,10.8),0.23]
x4[(10.1,8.9),0.16]
比如x2的概率是0.33，那么新?lián)u號(hào)搖出來(lái)的粒子可能有兩個(gè)x2，因?yàn)樗怕首畲?#xff0c;x4的概率比較小可能一個(gè)沒(méi)有。
然后我們認(rèn)為搖出來(lái)的新的粒子權(quán)重相等。
x1[(6.1,9.2),0.25]
x2[(7.9,9),0.25]
x3[(8.3,10.8),0.25]
x4[(7.9,9),0.25]
最終的估計(jì)值可以是四個(gè)粒子的平均。
二、粒子濾波的案例代碼
參考：開(kāi)發(fā)者說(shuō)｜手把手教你用粒子濾波實(shí)現(xiàn)無(wú)人車定位? 感覺(jué)更新階段的代碼好像和預(yù)測(cè)階段的重復(fù)了。
粒子濾波的思想基于蒙特卡洛方法，利用粒子集來(lái)表示概率，可以用在任何形式的狀態(tài)空間模型上。相比于其他濾波算法，粒子濾波在解決非線性、非高斯的問(wèn)題上，有著較大的優(yōu)越性。在無(wú)人駕駛領(lǐng)域，粒子濾波常被用于解決無(wú)人車的定位問(wèn)題。
本文將以優(yōu)達(dá)學(xué)城（Udacity）無(wú)人駕駛工程師學(xué)位中的定位課程為例講解粒子濾波如何解決無(wú)人駕駛中的定位問(wèn)題。
參考：?opencv實(shí)現(xiàn)粒子濾波目標(biāo)跟蹤??OpenCV3學(xué)習(xí)（12.5） opencv實(shí)現(xiàn)粒子濾波目標(biāo)跟蹤_知識(shí)搬運(yùn)工的博客-CSDN博客
參考：機(jī)器人粒子濾波定位（蒙特卡羅定位）_weixin_34253539的博客-CSDN博客?蒙特卡洛定位
參考：particle filtering---粒子濾波（講的很通俗易懂）??particle filtering---粒子濾波（講的很通俗易懂）_上善若水-CSDN博客_粒子濾波
三、動(dòng)畫演示
國(guó)外網(wǎng)站參考：https://www.cs.utexas.edu/~teammco/misc/particle_filter/
四、基于2D柵格地圖的移動(dòng)機(jī)器人全局定位參考和代碼
[PR-2] PF 粒子濾波/蒙特卡羅定位 - 知乎
總結(jié)
以上是生活随笔為你收集整理的机器人学习--粒子滤波及其在定位中的应用的全部?jī)?nèi)容，希望文章能夠幫你解決所遇到的問(wèn)題。
- 上一篇： 人工智能学习--文本识别实践-tesse
- 下一篇： 机器人学习--Turtelbot3学习-