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

11 篇文章 0 订阅
7 篇文章 0 订阅
  • 对于一个栅格来说

    • 占用概率: 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 ] z\in[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 ) log\frac{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} = log\frac{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} =log\frac{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

  • 0
    点赞
  • 5
    收藏
    觉得还不错? 一键收藏
  • 0
    评论

“相关推荐”对你有帮助么?

  • 非常没帮助
  • 没帮助
  • 一般
  • 有帮助
  • 非常有帮助
提交
评论
添加红包

请填写红包祝福语或标题

红包个数最小为10个

红包金额最低5元

当前余额3.43前往充值 >
需支付:10.00
成就一亿技术人!
领取后你会自动成为博主和红包主的粉丝 规则
hope_wisdom
发出的红包
实付
使用余额支付
点击重新获取
扫码支付
钱包余额 0

抵扣说明:

1.余额是钱包充值的虚拟货币,按照1:1的比例进行支付金额的抵扣。
2.余额无法直接购买下载,可以购买VIP、付费专栏及课程。

余额充值