Occupancy Grid Map(OGM:占用栅格地图)

参考：占用栅格地图原理分析：https://blog.csdn.net/weixin_40863346/article/details/92575794

基本概率

TSDF: Truncated Signed Distance Function（截断的符号距离函数） Occupancy Grid Map(OGM:占用栅格地图): “free space”：用于planning 使用SLAM输出的位姿创建occupancy grid map OGM可用于路径规划(planning)和定位(localization) 又叫做根据已知的Pose建图(mapping with know poses) Cell的值：表示此Cell有障碍物的概率（被占据） 采用Bayesian滤波更新OGM Cell的值 OGM的特征 把真实世界离散化为cells 栅格结构是刚体 每个cell有占用、空闲、未知三种状态 非参数化模型 大的map需要大量的内存 不依赖特征 每个cell是一个随机变量（模型化占用情况） Cell的占用概率 Cell被占用：p(mi)=1

p(mi​)=1 Cell未被占用：p(mi)=0 p(mi​)=0 Cell未知状态：p(mi)=0.5

p(mi​)=0.5

每个Cell有一个对应的随机变量，这些随机变量相对独立

随机变量表示cell被占用的概率

根据已知数据估计一个map

给定Sensor数据z1:t

z1:t​和Sensor的位姿x1:tx1:t​，估计map:

p(m∣z1:t,x1:t)=∏ip(mi∣z1:t,x1:t)p(m∣z1:t​,x1:t​)=i∏​p(mi​∣z1:t​,x1:t​)

1.1 Log-odd概念

Log odd的概念：计算odd(赔率)的对数 为什么需要Log-Odd? 使用log-odds的形式，cell的值更新只使用加法(即把乘法变成了加法) 避免在0或１附近，概率被截断问题 l(x)∈(−∞,∞) l(x)∈(−∞,∞)

Odd定义：事件发生的概率与事件不发生的概率的比值 odd(x)=p(x)1−p(x) odd(x)=1−p(x)p(x)​ Log-odd定义：　l(x)=log(odd(x))=logp(x)1−p(x) l(x)=log(odd(x))=log1−p(x)p(x)​ 反向计算p(x)：p(x)=1−11+expl(x) p(x)=1−1+expl(x)1​ p(x)=odd(x)1+odd(x)

p(x)=1+odd(x)odd(x)​

1.2 Log-Odd update (对数赔率更新法) 1.2.1 OGM概率更新原理

在这里插入图片描述

每个Cell的值为Cell被占据的概率值 在Bayes’ Rule下，根据传感器的测量数据更新map中的cell被占据概率 Bayes规则：p(mx,y∣z)=p(z∣mx,y)p(mx,y)p(z))

p(mx,y​∣z)=p(z)p(z∣mx,y​)p(mx,y​)​) 直接计算概率是困难的，为了简化计算，不使用占据概率本身，而使用log-odd Unexpected text node: ’  ’

Odd((mx,y​=1)givenz)=p(mx,y​=0∣z)p(mx,y​=1∣z)​

1.2.2 Log-Odd更新推导过程

p(mx,y=1∣z)=p(z∣mx,y=1)p(mx,y=1)p(z) p(mx,y​=1∣z)=p(z)p(z∣mx,y​=1)p(mx,y​=1)​ Odd(mx,y=1∣z)=p(mx,y=1∣z)p(mx,y=0∣z)=p(z∣mx,y=1)p(mx,y=1)/p(z)p(mx,y=0∣z)

Odd(mx,y​=1∣z)=p(mx,y​=0∣z)p(mx,y​=1∣z)​=p(mx,y​=0∣z)p(z∣mx,y​=1)p(mx,y​=1)/p(z)​

同理，p(mx,y=0∣z)=p(z∣mx,y=0)p(mx,y=0)p(z)

p(mx,y​=0∣z)=p(z)p(z∣mx,y​=0)p(mx,y​=0)​ Odd(mx,y=1∣z)=p(mx,y=1∣z)p(mx,y=0∣z)=p(z∣mx,y=1)p(mx,y=1)p(z∣mx,y=0)p(mx,y=0) Odd(mx,y​=1∣z)=p(mx,y​=0∣z)p(mx,y​=1∣z)​=p(z∣mx,y​=0)p(mx,y​=0)p(z∣mx,y​=1)p(mx,y​=1)​ 取对数： log(Odd(mx,y=1∣z))=logp(mx,y=1∣z)p(mx,y=0∣z)=logp(z∣mx,y=1)p(mx,y=1)p(z∣mx,y=0)p(mx,y=0) log(Odd(mx,y​=1∣z))=logp(mx,y​=0∣z)p(mx,y​=1∣z)​=logp(z∣mx,y​=0)p(mx,y​=0)p(z∣mx,y​=1)p(mx,y​=1)​ =logp(z∣mx,y=1)p(z∣mx,y=0)+logp(mx,y=1)p(mx,y=0) =logp(z∣mx,y​=0)p(z∣mx,y​=1)​+logp(mx,y​=0)p(mx,y​=1)​ 上式左边为后验概率的Log-Odd，右边为Sensor model和先验概率，通过取对数，把右边两项相乘变成了两项相加 可得OGM的log-odds更新公式： Unexpected text node: ’  ’

logodd+=logoddmeas+logodd−

1.2.3 OGM Log-Odd 更新原理

每个Cell的值为Log-Odd值 Log-Odd更新公式：Unexpected text node: '  '

logodd+=logoddmeas+logodd− 在这里插入图片描述 更新规则(Unexpected text node: ’  ’

logodd+=logoddmeas)： 只更新被观察到的Cell 被更新之后的值变成了先验值

1.2.4 Log-Odd形式的测量模型(Measurement Model)

测量项：logp(z∣mx,y=1)p(z∣mx,y=0)

logp(z∣mx,y​=0)p(z∣mx,y​=1)​ 情况1：Cell (z=1) Unexpected text node: ’  ’ logoddocc=logp(z=1∣mx,y​=0)p(z=1∣mx,y​=1)​ 情况２：Cell (z=0) Unexpected text node: ’  ’ logoddfree=logp(z=0∣mx,y​=1)p(z=0∣mx,y​=0)​ 如果log-odd的值为０，则Odd＝１，即p(mx,y=1)=p(mx,y=0)=0.5 p(mx,y​=1)=p(mx,y​=0)=0.5 测量模型：p(z∣mx,y) p(z∣mx,y​) p(z=1∣mx,y=1) p(z=1∣mx,y​=1)：True occupied measurement p(z=0∣mx,y=1) p(z=0∣mx,y​=1)：False free measurement p(z=1∣mx,y=0) p(z=1∣mx,y​=0)：False occupied measurement p(z=0∣mx,y=0)

p(z=0∣mx,y​=0)：True free measurement

1.3 Cartographer Submap的Cell更新过程

当一个Scan插入Submap时，通过以下步骤进行： 为hits计算一个栅格点的集合，且为misses计算一个与hits不相交的集合 对于每一个hit,插入一个最接近的grid point进入hits集合 对于每一个miss,把与线段(origin,scan point)交叉的Grid Point（且不在hits集合中的）插入misses集合 如果一个Grid Point从未被更新，则分配phit或pmiss给它

phit​或pmiss​给它，当它分别位于hits集合或misses集合 如果一个Grid Point已经被更新，则更新公式如下： odds§=p1−p odds§=1−pp​ 为hit更新： Mnew(x)=clamp(odds−1(odds(Mold(x))∙odds(phit))) Mnew​(x)=clamp(odds−1(odds(Mold​(x))∙odds(phit​))) 为miss更新： Mnew(x)=clamp(odds−1(odds(Mold(x))∙odds(pmiss))) Mnew​(x)=clamp(odds−1(odds(Mold​(x))∙odds(pmiss​))) 因为phit>0.5 phit​>0.5且pmiss<05pmiss​<05所以有： odds(phit)>1odds(pmiss)<1odds(phit​)>1odds(pmiss​)<1

原文来自于：https://blog.csdn.net/MyArrow/article/details/83185200