-
对于一个栅格来说
- 占用概率: p ( s = 1 ) p(s=1) p(s=1)
- 空闲概率:
p
(
s
=
0
)
p(s=0)
p(s=0)
其比值 o d d ( s ) = p ( s = 1 ) p ( s = 0 ) odd(s)=frac{p(s=1)}{p(s=0)} odd(s)=p(s=0)p(s=1)
-
当有一个新的观测值出现,观测值可以知道这个栅格是否被占用,也就是 z ∈ [ 0 , 1 ] zin[0,1] z∈[0,1], 此时这个栅格的概率状态将会改变为 o d d ( s / z ) odd(s/z) odd(s/z)
- o d d ( s / z ) = p ( s = 1 / z ) p ( s = 0 / z ) odd(s/z)=frac{p(s=1/z)}{p(s=0/z)} odd(s/z)=p(s=0/z)p(s=1/z)
- 由贝叶斯公式知道 P ( A / B ) = P ( B / A ) ∗ P ( A ) / P ( B ) P(A/B)= P(B/A)*P(A)/P(B) P(A/B)=P(B/A)∗P(A)/P(B)
- o d d ( s / z ) = p ( s = 1 / z ) p ( s = 0 / z ) = p ( z / s = 1 ) p ( s = 1 ) / p ( z ) p ( z / s = 0 ) p ( s = 0 ) / p ( z ) = p ( z / s = 1 ) p ( z / s = 0 ) o d d ( s ) odd(s/z)=frac{p(s=1/z)}{p(s=0/z)}=frac{p(z/s=1)p(s=1)/p(z)}{p(z/s=0)p(s=0)/p(z)}=frac{p(z/s=1)}{p(z/s=0)}odd(s) odd(s/z)=p(s=0/z)p(s=1/z)=p(z/s=0)p(s=0)/p(z)p(z/s=1)p(s=1)/p(z)=p(z/s=0)p(z/s=1)odd(s)
-
两边取对数 l o g log log
- l o g ( o d d ( s / z ) ) = l o g ( p ( z / s = 1 ) p ( z / s = 0 ) + l o g ( o d d ( s ) ) log(odd(s/z))= log(frac{p(z/s=1)}{p(z/s=0)} + log(odd(s)) log(odd(s/z))=log(p(z/s=0)p(z/s=1)+log(odd(s))
-
从这个公式可以看出, l o g p ( z / s = 1 ) p ( z / s = 0 ) logfrac{p(z/s=1)}{p(z/s=0)} logp(z/s=0)p(z/s=1)是唯一包含测量值的项, 这一项称为 l o g m e a s u r e log_{measure} logmeasure并且这个测量值只会有两种情况出现,分别叫做
- l o g f r e e = l o g p ( z = 0 / s = 1 ) p ( z = 0 / s = 0 ) log_{free} = logfrac{p(z=0/s=1)}{p(z=0/s=0)} logfree=logp(z=0/s=0)p(z=0/s=1)
- l o g o c c u = l o g p ( z = 1 / s = 1 ) p ( z = 1 / s = 0 ) log_{occu} =logfrac{p(z=1/s=1)}{p(z=1/s=0)} logoccu=logp(z=1/s=0)p(z=1/s=1)
-
此时将 l o g ( o d d ( s / z ) ) log(odd(s/z)) log(odd(s/z))表示为 S + S^{+} S+, 也就是该栅格更新之后的状态
-
此时将 l o g ( o d d ( s ) ) log(odd(s)) log(odd(s))表示为 S − S^{-} S−, 也就是该栅格更新之前的状态,那么
- S + = l o g m e a s u r e + S − S^{+}= log_{measure}+S^{-} S+=logmeasure+S−
-
当地图进行初始化第一帧时,栅格是未知的, p ( s = 0 ) = p ( s = 1 ) = 0.5 p(s=0)= p(s=1)= 0.5 p(s=0)=p(s=1)=0.5, 所以 S − = l o g ( 0.5 0.5 ) = 0 S^{-}= log(frac{0.5}{0.5})= 0 S−=log(0.50.5)=0
-
之后按照这个循环更新gridmap即可
-
参考链接:robotics-learning
