SLAM整理-3-占用栅格地图

• 对于一个栅格来说

  • 占用概率: $p(s=1)$
  • 空闲概率: $p(s=0)$
    其比值$odd(s)=\frac{p(s=1)}{p(s=0)}$

• 当有一个新的观测值出现,观测值可以知道这个栅格是否被占用,也就是$z\in [0,1]$, 此时这个栅格的概率状态将会改变为$odd(s/z)$

  • $odd(s/z)=\frac{p(s=1/z)}{p(s=0/z)}$
  • 由贝叶斯公式知道$P(A/B)=P(B/A)\ast P(A)/P(B)$
  • $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)$

• 两边取对数$log$

  • $log(odd(s/z))=log(\frac{p(z/s=1)}{p(z/s=0)}+log(odd(s))$

• 从这个公式可以看出, $log\frac{p(z/s=1)}{p(z/s=0)}$是唯一包含测量值的项, 这一项称为$lo{g}_{measure}$并且这个测量值只会有两种情况出现,分别叫做

  • $lo{g}_{free}=log\frac{p(z=0/s=1)}{p(z=0/s=0)}$
  • $lo{g}_{occu}=log\frac{p(z=1/s=1)}{p(z=1/s=0)}$

• 此时将$log(odd(s/z))$表示为$S^{+}$, 也就是该栅格更新之后的状态

• 此时将$log(odd(s))$表示为$S^{-}$, 也就是该栅格更新之前的状态,那么

  • $S^{+}=lo{g}_{measure}+S^{-}$

• 当地图进行初始化第一帧时,栅格是未知的,$p(s=0)=p(s=1)=0.5$, 所以$S^{-}=log(\frac{0.5}{0.5})=0$

• 之后按照这个循环更新gridmap即可

• 参考链接:robotics-learning