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