「0公式系列」之粒子濾波演算法 逃離迷宮

2021-09-01 23:59:56 字數 3062 閱讀 7790

你醒來在乙個迷宮內,不知道自己在這個大迷宮的哪乙個位置,你要出去,但是你也不能亂走吧,正當你迷茫的時候,你發現自己手裡有一張紙質的迷宮地圖,該地圖不僅包含了迷宮的形狀,還包括了迷宮的尺寸。於是,希望來了,你現在要做的就是用這個地圖,應用神奇的「粒子濾波」演算法(particle filtering,以下簡稱pf),完成自身的定位,然後走出去(走出去這個環節與pf無關)。

圖1.手中的迷宮地圖

pf第一步:假設你在迷宮的任何乙個位置(這可能聽的有點扯),周圍的牆體高大,你爬不上去,如圖2。你假設的你所處的位置就叫粒子(紅點),現在你前方不遠處有一堵牆(等下我再解釋看不到牆的情況),這個所謂的「牆」,它可能是個轉角處,也可能是個死胡同。你能用你的眼睛,估計你面前這堵牆到你的距離假設是是20m左右吧,注意這個地方,你眼睛看到的叫觀測值,觀測值是有偏差的。好了,我們有了第乙個觀測值,重複一下,我們到我們面前這堵牆的距離有大概20m遠,你不用管你面朝東南西北,你也不用管你的後面有沒有牆。總之啊,你距離一堵眼前的牆有大概20m遠,那麼就是說,你可以把距離轉角處或者死胡同不在20m範圍內的粒子(假設的位置點)去掉,這樣的話,你可以排除一些最開始假設的位置點。如圖3。

圖2.紅色的點是你最開始假設的位置,我畫的一點也不密集,實際上假設的位置是鋪滿整個迷宮的

圖3.雖然去除了一些點,但是還是有不少假設的位置

pf第二步:現在你要往前走20m,就走到你面前的牆的那個位置。分三種情況:

(1)是個拐角處,那還好,拐過去就行了。

(2)是個「t」型岔口也沒關係,規定遇到岔口向左轉吧。

(3)是個死胡同就悲劇了,那就回到出發點向後轉吧,看看你在出發點的時候後面有沒有牆。

pf第三步:假設你的運氣不錯,不是個死胡同,那麼你到了轉角處後,轉向,然後停!你再看看你的面前沒有新的牆。這分兩種情況:

(1)有牆,這跟你的第一步一樣,估計你目前的位置到這個新牆的距離。不管它是個轉角處還是個死胡同都不重要。假設你距離新牆的位置是10m。現在重點來了啊!你捋捋思路,你看到前面有堵牆,起身,往前走了大概20m,轉彎後前面又有一堵牆,距離大概10m。也就是說,你轉彎後的視野內,必須有一堵牆在你面前10m左右遠(定義為條件1)。不滿足這個條件的之前的粒子(假設的位置)都要去掉。好了現在我們有了兩幅圖,一幅是所有粒子往前走20m到了牆跟頭,見圖4。你得到了很多藍點,根據條件1,除去不符合條件藍色點以及之前所有假設的紅點。見圖5。

(2)沒有牆,是個無盡的大道。那就有點悲劇了,只能往前走了。直到看到牆位置,而且你還得記得你走了大概多少公尺,專業點這叫里程計讀到的資料。假設你走了大概100m,終於看到了乙個新的牆在你前面50m處,那麼這條道就有100+50m左右的距離。接下來跟上面(1)的過程一樣,看到牆就好了。

圖4.藍色是紅點(也就是你醒來的位置)向前移動大概20m的後的位置

圖5.現在我只保留了符合條件1的藍點,紅色箭頭表示你最開始前進的方向,黃色箭頭表示你到了牆角轉頭後的方向

現在很舒服,你已經定位到地圖3個可能的位置了,比之前全地圖都是假設位置的情況好多了。但是你有沒有發現,你在轉角處看到新牆的位置,是大概10m,有可能是6m,也可能是14m,所以說,這個3個假設位置也是有不同的可信度。根據圖5。例如,對於最上面這個假設位置,看到新牆的距離就比10m小一些,右下角這個位置,看到新牆的距離就很接近10m,左下角的這個位置,看到新牆的距離就比10m大一些。但他們三個都是大概10m, 為了區分,我們就給右下角的可信度大一些,其他兩個位置(最上面和左下角)的可信度就小一些。

話說我定義的可信度有什麼用呢?

pf第四步:我們基於上面三個點重新假設位置,注意,根據可信度的大小,假設位置的點的數量不同,見圖6,就是說我們更相信我們在右下角的位置,但是我們不排除在其他那兩個可信度不高的位置的情況。

然後,重複第二步到第四步,重複幾次,你就可以定位到你的位置了,很神奇吧。這個重複的圖我就不畫了,大家自己想想吧。重點就是,目前的位置跟上一時刻的位置是相關的

整個粒子濾波演算法的流程我就說完了。這裡沒有公式(貝葉斯公式巴拉巴拉),專業術語(重取樣巴拉巴拉),網上有很多原理的介紹,感興趣的,可以一邊看正規的原理和公式,一邊思考我這個迷宮定位的方法,找一找他們之間的聯絡。

另外推薦一本書《粒子濾波演算法及其應用》(朱志宇),上面還有粒子濾波的衍生演算法,以及在目標檢測,地圖定位,fpga,dsp的應用。

CSS回顧系列0之雜談

css不需要編譯,屬於瀏覽器解釋型語言,可以直接由瀏覽器執行。之前看的w3school教程入門css,這段時間我看了看doye的手冊,遇到不太理解的地方的話就去查閱一些資料並在codeopen上面做一些實踐,下面我來講講我認為一些比較重要的收穫。position 可以取值static absolut...

併發系列之 sleep 0

作業系統前置知識 在講兩者的區別前,先回顧一下作業系統的知識。作業系統中,cpu競爭有很多種策略 在時間片演算法中,所有的程序排成乙個佇列。作業系統按照他們的順序,給每個程序分配一段時間,即該程序允許執行的時間。如果在時間片結束時程序還在執行,則cpu將被剝奪並分配給另乙個程序。如果程序在時間片結束...

OpenGL ES系列之0 建立Xcode專案

為iphone建立乙個opengl es的xcode專案是很容易的事,特別是在蘋果公司的sdk發布時引入了模板的概念後。我們所需要做的只是在適當的地方快速而簡單的加入 這就是我們今天的主題。好,我們假定你肯能以前已經在哪看過這個模板或執行過用這個模板建立的專案。我們說要做的就是移除那個旋轉的彩色正方...