9 概率機器人 Probabilistic Robotics 二值貝葉斯濾波 佔據柵格地圖 occupancy grid mapping
阿新 • • 發佈：2020-10-17
文章目錄
1 前言
- 如果通過感測器對一個環境中固定狀態進行評估，該狀態為二值狀態（例如判斷一扇門的開關狀態），那麼就需要用到二值貝葉斯濾波
- 二值貝葉斯濾波的一個重要應用就是通過鐳射雷達建立佔據柵格地圖，這會在下文中做介紹
2 二值貝葉斯濾波
2.1 理論基礎
- 對於一個靜態狀態，也就是說該狀態不隨時間變化且沒有動作作用於該狀態，那麼置信度函式可以寫為：
b e l t ( x ) = p ( x ∣ z 1 : t , u 1 : t ) = p ( x ∣ z 1 : t ) bel_t(x) = p(x|z_{1:t},u_{1:t}) = p(x|z_{1:t})belt(x)=p(x∣z1:t,u1:t)=p(x∣z1:t) - 二值狀態分別為
x
,
x
ˉ
x, \bar{x}
x,xˉ,那麼相應的置信度滿足
b e l t ( x ) = 1 − b e l t ( x ˉ ) bel_t(x) = 1 - bel_t(\bar{x}) belt(x)=1−belt(xˉ) - 二值貝葉斯濾波可以應用離散貝葉斯濾波計算，分別比較兩種情況的概率，概率較大的狀態即為當前狀態值
- 但是對於這種只有兩種狀態的二值狀態，有一個更加巧妙和便捷的方法來計算，那就是對這兩個狀態的概率作比後取對數，通過判斷這個對數值的大小，也可以比較出這兩種狀態的概率大小
- 這樣做的好處還有
l
(
x
)
l(x)
l(x)的值域是 − ∞ , + ∞ -\infty, +\infty −∞,+∞, 避免了概率值在 0 , 1 0,1 0,1附近的截斷問題
- 置信度的更新也更加方便，具體的看下文演算法
p ( x ) p ( x ˉ ) = p ( x ) 1 − p ( x ) ↓ 取對數 l ( x ) : = log ( p ( x ) 1 − p ( x ) ) \frac{p(x)}{p(\bar{x})} = \frac{p(x)}{1 - p(x)} \\ \downarrow \text{取對數}\\ l(x) := \log{( \frac{p(x)}{1 - p(x)} )} p(xˉ)p(x)=1−p(x)p(x)↓取對數l(x):=log(1−p(x)p(x))
- 這樣做的好處還有
l
(
x
)
l(x)
2.2 演算法流程
- 下面是二值貝葉斯濾波方法的更新過程：
- 可以看出演算法十分簡單，僅一條公式
- 具體公式如何得來的見下文推導
- 這裡用到了一個概率
p
(
x
∣
z
t
)
p(x|z_t)
p(x∣zt),被稱為逆測量模型(inverse measurement model)；相反
p
(
z
t
∣
x
)
p(z_t|x)
p(zt∣x),被稱為測量模型或感測器模型
- 逆測量模型是測量 z t z_t zt函式的概率分佈
- 逆測量模型一般用在感測器模型比較複雜的情況
- 例如：從圖片中判斷一扇門是開啟還是關閉狀態
- 這個狀態十分簡單，但是測量空間很大
- 已知測量圖片判斷門的狀態 p ( x ∣ z t ) p(x|z_t) p(x∣zt)這個通過適當演算法十分容易獲得
- 但是已知門的狀態求含有這扇門的圖片的概率分佈 p ( z t ∣ x ) p(z_t|x) p(zt∣x)就很難求得
- 已知
l
(
x
)
l(x)
l(x)，置信度
b
e
l
t
(
x
)
bel_t(x)
belt(x)十分容易求得：
l ( x ) : = log ( p ( x ) 1 − p ( x ) ) ↓ b e l t ( x ) = 1 − 1 1 + exp ( l t ) l(x) := \log{(\frac{p(x)}{1 - p(x)} )}\\ \downarrow\\ bel_t(x) = 1 - \frac{1}{1 + \exp{(l_t)}} l(x):=log(1−p(x)p(x))↓belt(x)=1−1+exp(lt)1
2.3 重要公式推導
- 從演算法流程圖中可以看出二值貝葉斯濾波僅有一條更新公式
l t = l t − 1 + log ( p ( x ∣ z t ) 1 − p ( x ∣ z t ) ) − log ( p ( x ) 1 − p ( x ) ) l_t = l_{t-1} + \log{(\frac{p(x|z_t)}{1 - p(x|z_t)} )} - \log{(\frac{p(x)}{1 - p(x)} )} lt=lt−1+log(1−p(x∣zt)p(x∣zt))−log(1−p(x)p(x)) - 這裡為大家推導這個公式的由來
b e l t ( x ) = p ( x ∣ z 1 : t ) = p ( x ∣ z t , z 1 : t − 1 ) = p ( z t ∣ x , z 1 : t − 1 ) p ( x ∣ z 1 : t − 1 ) p ( z t ∣ z 1 : t − 1 ) = p ( z t ∣ x ) p ( x ∣ z 1 : t − 1 ) p ( z t ∣ z 1 : t − 1 ) ↓ p ( z t ∣ x ) = p ( x ∣ z t ) p ( z t ) p ( x ) = p ( x ∣ z t ) p ( z t ) p ( x ∣ z 1 : t − 1 ) p ( x ) p ( z t ∣ z 1 : t − 1 ) ↓ 同理： x ↔ x ˉ b e l t ( x ˉ ) = p ( x ˉ ∣ z 1 : t ) = p ( x ˉ ∣ z t ) p ( z t ) p ( x ˉ ∣ z 1 : t − 1 ) p ( x ˉ ) p ( z t ∣ z 1 : t − 1 ) ↓ 上面兩個式子作比 p ( x ∣ z 1 : t ) p ( x ˉ ∣ z 1 : t ) = p ( x ∣ z t ) p ( x ˉ ∣ z t ) p ( x ∣ z 1 : t − 1 ) p ( x ˉ ∣ z 1 : t − 1 ) p ( x ˉ ) p ( x ) = p ( x ∣ z t ) 1 − p ( x ∣ z t ) p ( x ∣ z 1 : t − 1 ) 1 − p ( x ∣ z 1 : t − 1 ) 1 − p ( x ) p ( x ) ↓ 取對數 log p ( x ∣ z 1 : t ) 1 − p ( x ∣ z 1 : t ) = log p ( x ∣ z t ) 1 − p ( x ∣ z t ) + log p ( x ∣ z 1 : t − 1 ) 1 − p ( x ∣ z 1 : t − 1 ) + log 1 − p ( x ) p ( x ) l t ( x ) = log p ( x ∣ z t ) 1 − p ( x ∣ z t ) + l t − 1 ( x ) − log p ( x ) 1 − p ( x ) ↓ l 0 ( x ) = log p ( x ) 1 − p ( x ) l t ( x ) = l t − 1 ( x ) + log p ( x ∣ z t ) 1 − p ( x ∣ z t ) − l 0 ( x ) \begin{aligned} bel_t(x) = p(x|z_{1:t}) = & p(x|z_t, z_{1:t-1}) \\ = & \frac{p(z_t|x, z_{1:t-1}) p(x|z_{1:t-1}) }{p(z_t |z_{1:t-1}) }\\ = & \frac{p(z_t|x) p(x|z_{1:t-1}) }{p(z_t |z_{1:t-1}) }\\ & \downarrow p(z_t|x) = \frac{p(x|z_t)p(z_t)}{p(x)}\\ = & \frac{p(x|z_t)p(z_t) p(x|z_{1:t-1}) }{p(x) p(z_t |z_{1:t-1}) }\\ \end{aligned}\\ \downarrow \text{同理：} x \leftrightarrow \bar{x}\\ bel_t(\bar{x}) = p(\bar{x}|z_{1:t}) = \frac{p(\bar{x}|z_t)p(z_t) p(\bar{x}|z_{1:t-1}) }{p(\bar{x}) p(z_t |z_{1:t-1}) }\\ \downarrow \text{上面兩個式子作比}\\ \begin{aligned} \frac{ p(x|z_{1:t}) }{p(\bar{x}|z_{1:t}) } = &\frac{p(x|z_t)}{p(\bar{x}|z_t)} \frac{p(x|z_{1:t-1})}{p(\bar{x}|z_{1:t-1})} \frac{p(\bar{x})}{p(x) }\\ = & \frac{p(x|z_t)}{1 - p(x|z_t)} \frac{p(x|z_{1:t-1})}{ 1 - p(x|z_{1:t-1})} \frac{1 - p(x)}{p(x) }\\ \end{aligned}\\ \downarrow \text{取對數}\\ \begin{aligned} \log{\frac{ p(x|z_{1:t}) }{1 - p(x|z_{1:t}) } } = &\log{\frac{p(x|z_t)}{1 - p(x|z_t)}} + \log{\frac{p(x|z_{1:t-1})}{ 1 - p(x|z_{1:t-1})}} + \log{\frac{1 - p(x)}{p(x) }}\\ l_t(x) = &\log{\frac{p(x|z_t)}{1 - p(x|z_t)}} + l_{t-1}(x) - \log{\frac{ p(x)}{1 - p(x) }}\\ & \downarrow l_0(x) = \log{\frac{ p(x)}{1 - p(x) }}\\ l_t(x) = &l_{t-1}(x) + \log{\frac{p(x|z_t)}{1 - p(x|z_t)}} - l_0(x) \end{aligned} belt(x)=p(x∣z1:t)====p(x∣zt,z1:t−1)p(zt∣z1:t−1)p(zt∣x,z1:t−1)p(x∣z1:t−1)p(zt∣z1:t−1)p(zt∣x)p(x∣z1:t−1)↓p(zt∣x)=p(x)p(x∣zt)p(zt)p(x)p(zt∣z1:t−1)p(x∣zt)p(zt)p(x∣z1:t−1)↓同理：x↔xˉbelt(xˉ)=p(xˉ∣z1:t)=p(xˉ)p(zt∣z1:t−1)p(xˉ∣zt)p(zt)p(xˉ∣z1:t−1)↓上面兩個式子作比p(xˉ∣z1:t)p(x∣z1:t)==p(xˉ∣zt)p(x∣zt)p(xˉ∣z1:t−1)p(x∣z1:t−1)p(x)p(xˉ)1−p(x∣zt)p(x∣zt)1−p(x∣z1:t−1)p(x∣z1:t−1)p(x)1−p(x)↓取對數log1−p(x∣z1:t)p(x∣z1:t)=lt(x)=lt(x)=log1−p(x∣zt)p(x∣zt)+log1−p(x∣z1:t−1)p(x∣z1:t−1)+logp(x)1−p(x)log1−p(x∣zt)p(x∣zt)+lt−1(x)−log1−p(x)p(x)↓l0(x)=log1−p(x)p(x)lt−1(x)+log1−p(x∣zt)p(x∣zt)−l0(x) - 終於得出二值貝葉斯演算法中更新公式，那麼具體怎麼使用呢？ 我們用下一節的佔據柵格地圖（occupancy grid mapping）為例子給大家講解如何使用。
3 例項：佔據柵格地圖（occupancy grid mapping）
- 佔據柵格地圖應用在地圖構建上，它標識出哪地方有障礙物，哪地方是空閒的
- 這與二值貝葉斯濾波的應用條件十分吻合
- 地圖中各點的真實狀態是不變的
- 地圖中各點僅有兩種狀態（障礙物或者空閒）,符合二值性
- 這與二值貝葉斯濾波的應用條件十分吻合
- 佔據柵格地圖演算法是根據機器人位姿 x x x和感測器測量 z z z的資料，計算地圖 m m m的後驗概率 p ( m ∣ z 1 : t , x 1 : t ) p(m|z_{1:t}, x_{1:t}) p(m∣z1:t,x1:t)
- 一般把地圖平面分割成若干個柵格
m
=
{
m
i
}
m = \{m_i\}
m={mi},估計每個柵格的狀態
- 這裡約定 p ( m i = 1 ) = p ( m i ) p(m_i = 1) = p(m_i) p(mi=1)=p(mi)為柵格被佔據的概率（即該柵格存在障礙物）
- p ( m i = 0 ) p(m_i = 0) p(mi=0)為柵格是空閒的概率 （即該柵格不存在障礙物）
- 那麼計算整體地圖
m
m
m的後驗概率
p
(
m
∣
z
1
:
t
,
x
1
:
t
)
p(m|z_{1:t}, x_{1:t})
p(m∣z1:t,x1:t)就可以轉化為估計每個柵格的後驗概率
p
(
m
i
∣
z
1
:
t
,
x
1
:
t
)
p(m_i|z_{1:t}, x_{1:t})
p(mi∣z1:t,x1:t)
p ( m ∣ z 1 : t , x 1 : t ) = ∏ i p ( m i ∣ z 1 : t , x 1 : t ) p(m|z_{1:t}, x_{1:t}) = \prod_i p(m_i|z_{1:t}, x_{1:t}) p(m∣z1:t,x1:t)=i∏p(mi∣z1:t,x1:t) - 每個柵格都有二值性，可以使用二值貝葉斯濾波演算法
- 上述演算法是估計單一柵格被佔據的概率，但是柵格佔據地圖的例項中要估計多個柵格的概率，所以對上述演算法做一下簡單修改
- line 4: 對於感測器探測到的柵格，我們通過二值貝葉斯濾波演算法更新改狀態
- line 6: 對於感測器沒有探測到的柵格，該柵格的狀態保持不變
-
這裡給出上式演算法中的計算公式
l t , i = log p ( m i ∣ z 1 : t , x 1 : t ) 1 − p ( m i ∣ z 1 : t , x 1 : t ) l 0 = log p ( m i = 1 ) p ( m i = 0 ) = p ( m i ) 1 − p ( m i ) i n v e r s e _ s e n s o r _ m o d e l ( m i , x t , z t ) = log p ( m i ∣ z t , x t ) 1 − p ( m i ∣ z t , x t ) \begin{aligned} l_{t,i} = &\log { \frac{p(m_i|z_{1:t}, x_{1:t})}{1 - p(m_i|z_{1:t}, x_{1:t})} }\\ l_0 = &\log { \frac{p(m_i=1)}{p(m_i=0)} } = \frac{p(m_i)}{1 - p(m_i)}\\ inverse\_sensor\_model(m_i,x_t,z_t) = & \log { \frac{p(m_i|z_{t}, x_{t})}{1 - p(m_i|z_{t}, x_{t})} } \end{aligned} lt,i=l0=inverse_sensor_model(mi,xt,zt)=log1−p(mi∣z1:t,x1:t)p(mi∣z1:t,x1:t)logp(mi=0)p(mi=1)=1−p(mi)p(mi)log1−p(mi∣zt,xt)p(mi∣zt,xt) -
那麼二值貝葉斯濾波演算法是更新公式為
- 注意：這裡的 x t x_t xt是當前機器人的位姿，不是要估計的狀態
- 注意：這裡要估計的狀態的
m
i
m_i
mi，相當於二值貝葉斯演算法流程中的
x
t
x_t
xt
l t , i = l t − 1 , i + log p ( m i ∣ z t , x t ) 1 − p ( m i ∣ z t , x t ) − p ( m i ) 1 − p ( m i ) l_{t,i} = l_{t-1,i} + \log { \frac{p(m_i|z_{t}, x_{t})}{1 - p(m_i|z_{t}, x_{t})} } - \frac{p(m_i)}{1 - p(m_i)} lt,i=lt−1,i+log1−p(mi∣zt,xt)p(mi∣zt,xt)−1−p(mi)p(mi)
-
下面我們一起進行一個簡單的佔據柵格地圖的例項計算：這裡參考佔據柵格地圖
- 初始時刻機器人對外界一無所知
- 每次同過鐳射雷達探測外界環境，從探索資料中計算檢測到的柵格狀態的概率
- 對於探測到的柵格有障礙物和無障礙物的正確率均為 90 % 90\% 90%
-
-
t
=
0
t=0
t=0: 機器人對外界環境一無所知，所以
p
(
m
i
=
0
)
=
p
(
m
i
=
1
)
=
0.5
p(m_i = 0) = p(m_i = 1) = 0.5
p(mi=0)=p(mi=1)=0.5
l 0 = log p ( m i = 1 ) p ( m i = 0 ) = log 0.5 0.5 = 0 l_0 = \log { \frac{p(m_i=1)}{p(m_i=0)} } = \log \frac{0.5}{0.5} = 0 l0=logp(mi=0)p(mi=1)=log0.50.5=0
-
t
=
0
t=0
t=0: 機器人對外界環境一無所知，所以
p
(
m
i
=
0
)
=
p
(
m
i
=
1
)
=
0.5
p(m_i = 0) = p(m_i = 1) = 0.5
p(mi=0)=p(mi=1)=0.5
-
-
t
=
1
t=1
t=1: 鐳射雷達進行探測
- 圖中紅色線經過的柵格為探測到的柵格，也就是需要更新概率的柵格
- 圖中綠色方塊被檢查出有障礙物，將鐳射反射回去，其餘被檢查的柵格無障礙物
-
t
=
1
t=1
t=1: 鐳射雷達進行探測
-
-
t
=
1
t=1
t=1: 根據探測到的資料進行狀態更新
有障礙物的柵格： l 1 , i = l 0 , i + log p ( m i ∣ z 1 , x 1 ) 1 − p ( m i ∣ z 1 , x 1 ) − l 0 = 0 + log 0.9 1 − 0.9 − 0 ≈ 2.2 空閒的柵格： l 1 , i = l 0 , i + log p ( m i ∣ z 1 , x 1 ) 1 − p ( m i ∣ z 1 , x 1 ) − l 0 = 0 + log 0.1 1 − 0.1 − 0 ≈ − 2.2 沒探測到的柵格： l 1 , i = l 0 , i \begin{aligned} \text{有障礙物的柵格：} l_{1,i} = &l_{0,i} + \log { \frac{p(m_i|z_{1}, x_{1})}{1 - p(m_i|z_{1}, x_{1})} } - l_0\\ =& 0 + \log { \frac{0.9}{1 - 0.9} } - 0\\ \approx& 2.2 \end{aligned}\\ \begin{aligned} \text{空閒的柵格：} l_{1,i} = &l_{0,i} + \log { \frac{p(m_i|z_{1}, x_{1})}{1 - p(m_i|z_{1}, x_{1})} } - l_0\\ =& 0 + \log { \frac{0.1}{1 - 0.1} } - 0\\ \approx& -2.2 \end{aligned}\\ \text{沒探測到的柵格：} l_{1,i} = l_{0,i} 有障礙物的柵格：l1,i==≈l0,i+log1−p(mi∣z1,x1)p(mi∣z1,x1)−l00+log1−0.90.9−02.2空閒的柵格：l1,i==≈l0,i+log1−p(mi∣z1,x1)p(mi∣z1,x1)−l00+log1−0.10.1−0−2.2沒探測到的柵格：l1,i=l0,i
-
t
=
1
t=1
t=1: 根據探測到的資料進行狀態更新
-
- t = 2 t=2 t=2: 鐳射雷達進行探測
-
-
t
=
2
t=2
t=2: 根據探測到的資料進行狀態更新
有障礙物的柵格： l 2 , i = l 1 , i + log p ( m i ∣ z 2 , x 2 ) 1 − p ( m i ∣ z 2 , x 2 ) − l 0 = 0 + log 0.9 1 − 0.9 − 0 ≈ 2.2 空閒的柵格： l 2 , i = l 1 , i + log p ( m i ∣ z 2 , x 2 ) 1 − p ( m i ∣ z 2 , x 2 ) − l 0 = { 0 + log 0.1 1 − 0.1 − 0 ≈ − 2.2 − 2.2 + log 0.1 1 − 0.1 − 0 ≈ − 4.4 沒探測到的柵格： l 2 , i = l 1 , i \begin{aligned} \text{有障礙物的柵格：} l_{2,i} = &l_{1,i} + \log { \frac{p(m_i|z_{2}, x_{2})}{1 - p(m_i|z_{2}, x_{2})} } - l_0\\ =& 0 + \log { \frac{0.9}{1 - 0.9} } - 0\\ \approx& 2.2 \end{aligned}\\ \begin{aligned} \text{空閒的柵格：} l_{2,i} = &l_{1,i} + \log { \frac{p(m_i|z_{2}, x_{2})}{1 - p(m_i|z_{2}, x_{2})} } - l_0\\ =& \left\{\begin{matrix} 0 + \log { \frac{0.1}{1 - 0.1} } - 0\approx -2.2\\ -2.2 + \log { \frac{0.1}{1 - 0.1} } - 0\approx -4.4 \end{matrix}\right.\\ \end{aligned}\\ \text{沒探測到的柵格：} l_{2,i} = l_{1,i} 有障礙物的柵格：l2,i==≈l1,i+log1−p(mi∣z2,x2)p(mi∣z2,x2)−l00+log1−0.90.9−02.2空閒的柵格：l2,i==l1,i+log1−p(mi∣z2,x2)p(mi∣z2,x2)−l0{0+log1−0.10.1−0≈−2.2−2.2+log1−0.10.1−0≈−4.4沒探測到的柵格：l2,i=l1,i
-
t
=
2
t=2
t=2: 根據探測到的資料進行狀態更新
- t = 3 , 4... t=3,4... t=3,4...：同理
4 參考文獻
- Thrun, & Sebastian. (2005). Probabilistic robotics.
- 佔據柵格地圖