参考：占用栅格地图原理分析：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
