佔據柵格地圖(occupancy grid maps) -- 二值貝葉斯濾波應用

    其實,我想講的關鍵點是二值狀態的最優估計問題,而不僅僅是柵格地圖。

    Anyway,that is a good example.

    機器人的地圖表示方式有多種,如拓撲地圖、特徵地圖、直接表徵法、柵格地圖等。其中,柵格地圖應用廣泛,方便用於機器人的導航規劃中(我也比較喜歡)。以下地圖就是柵格圖表示(每一個像素可以認爲是一個格子),圖一是概率機器人書本中的例子,圖二是國內某品牌掃地機的清掃記錄(白色爲清掃軌跡)。


                                    圖一


                                    圖二

    接着,咱們來分析下柵格地圖是如何構建的(以二維激光雷達爲例)。

    二維激光雷達傳感器會掃描出空間中某一高度的平面,相當於給整個環境來一刀,得到一個切面(內側)。柵格圖中的線條對應環境中障礙物的內側邊界。

    雷達掃描過程中,光線撞到(hit)的地方所處座標位置就對應一種灰度值,沒有撞到(miss)的地方所處座標位置對應另外的灰度值。柵格圖中,一個格子某一時刻只有兩種狀態,要麼被光線撞到了要麼沒有;但是該格子可能多次被撞到或多次沒被撞到。如果某個格子多次被撞到了,說明它很有可能是被佔用了(有障礙物的意思)。因此,咱們可以用概率的方式來表示某個格子有多大的可能被佔用了。

    那麼問題來了,一方面,格子的概率如何表示?另一方面,格子多次被撞這種事情的概率要怎麼算?

    第一個問題,咱們可以利用圖像的灰度值來表達概率,只要有一張“灰度 -- 概率”對應表。如下圖所示,縱座標可以換算爲八位的灰度值,橫座標爲概率值。圖中曲線爲單調函數,這樣就能從灰度圖中直觀的看出概率大小(顏色越深代表可能性越大/小)。一般情況下,純黑色(灰度值爲0)概率爲0,代表佔用;灰色(灰度值爲127)概率爲0.5,代表未知;純白色(灰度值爲255)概率爲1,代表空閒。


                                                     圖三

    接着,咱們來討論第二個問題,以及圖三灰度 -- 概率”對應表的曲線由來。

    在實際環境中,柵格只有兩種狀態,要麼occupied,要麼free。假設已知格子x被hit一次,格子occupied的概率是Po(x) = 0.9,free的概率是Pf(x)= 1 - Po(x) = 0.1;格子x被miss一次,格子occupied的概率是Po(x) = 0.2,free的概率是Pf(x)= 1 - Po(x) = 0.8。如果格子x被連續hit三次,miss一次,那麼格子x的概率Po(x)該如何計算? 

    咱們對格子x的概率換一種表達方式,用概率對數的形式:

    p(x) / p(¬x) =p(x)/(1 − p(x)) // 該事件發生的概率除以該事件不發生的概率。(1)

    l(x) := log(p(x)/(1 − p(x))) //對上式取對數,以自然常數e爲底數。(2)

    換了這種表達方式後,格子x的置信度(對應原來的概率)取值範圍變成(−∞, ∞),同時避免了概率接近0或者1引起的截斷問題。

    圖三中的曲線就是(2)式的體現,橫座標表示概率值或置信度,縱座標表示圖像灰度值。

    從(2)式中可得,p(x) = 1 - 1 / (1 + exp{l(x)})  (3)

    回到原來的問題,如果格子x被連續hit三次,miss一次,置信度要如何計算? 

    首先給出結論,計算套路如下:


    也就是說,原來求概率的乘法運算,現在變成了加法。

    事實上,任何對測量做出反應的變量的遞增和遞減都可以解釋爲貝葉斯濾波的概率對數形式。

    以下引用《Probabilistic Robotics》中對二值貝葉斯濾波的推導過程。(太懶了,就不自己寫公式了)

   

    根據(4.20)式子,結合柵格地圖定義,可得:




    從貝葉斯定理推導出來的結論(4.20)式子很巧妙,如果想要知道當前時刻格子x被occupied的置信度,只需要知道上一時刻的置信度值(可以認爲是已知的,是迭代的過程量),當前時刻的觀察信息(格子x被hit或miss),以及初值(一般是常數)即可,避免了求格子x在occupied的條件下,格子被hit的概率值(這種概率就不容易求解了)。

    咱們來計算下格子x被hit三次,miss一次的occupied概率:

    初值L0 = log(0.5 / (1-0.5)) = 0.

    L1 = L0 + log(0.9 / (1-0.9)) - L0 = log9;

    L2 = L1 + log(0.9 / (1-0.9)) - L0 = log9 + log9 ;

    L3 = L2 + log(0.9 / (1-0.9)) - L0 = log9 + log9 + log9 ;

    L4 = L3 + log(0.2 / (1-0.2)) - L0 = log9 + log9 + log9 - log4 ;

    即L4 = log(9*9*9/4) ≈ 5.2。

    由於式子(3): p(x) = 1 - 1 / (1 + exp{l(x)})  

    因此,此時格子x被occupied的概率P(x) ≈ 0.9945,其對應的灰度值就是L4*某係數。

    從L4的計算過程可知,如果格子x被hit一次,加log9,被miss一次,減log4;看起來,求狀態的概率的過程就是加加減減。


    二值貝葉斯濾波方法除了能應用在佔據柵格地圖之外,還可以在其他二值狀態估計中應用。比如,掃地機器人的跌落傳感器檢測。Drop sensor觸發一次,機器人處於懸崖邊緣的概率是0.9(爲什麼不是1?一方面sensor有噪聲,另外一方面環境有干擾),drop sensor不觸發一次,機器人不處於懸崖邊緣的概率是0.8,那麼drop sensor連續觸發三次,第四次不觸發,此時機器人處於懸崖邊緣的概率是多少?這個問題跟格子x的問題是一樣的,答案也是一樣的。咱們可以規定,當機器人處於懸崖邊緣的概率達到99.9%時,機器人會執行後退動作,保護自己。通過這種估計方法,機器人能很好的過濾噪聲,估計出更加可信的狀態,以便更合理的執行動作。

    還有很多很多的例子能應用二值貝葉斯濾波方法,這裏就不一一贅述了。    

發表評論
所有評論
還沒有人評論,想成為第一個評論的人麼? 請在上方評論欄輸入並且點擊發布.
相關文章