1. 基本概率
- 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 ( m i ) = 1 p(m_i)=1 p(mi)=1
- Cell未被占用: p ( m i ) = 0 p(m_i)=0 p(mi)=0
- Cell未知状态: p ( m i ) = 0.5 p(m_i)=0.5 p(mi)=0.5
- 每个Cell有一个对应的随机变量,这些随机变量相对独立
- 随机变量表示cell被占用的概率
- 根据已知数据估计一个map
- 给定Sensor数据
z
1
:
t
z_{1:t}
z1:t和Sensor的位姿
x
1
:
t
x_{1:t}
x1:t,估计map:
p ( m ∣ z 1 : t , x 1 : t ) = ∏ i p ( m i ∣ z 1 : t , x 1 : t ) p(m|z_{1:t}, x_{1:t})=\prod_i p(m_i|z_{1:t}, x_{1:t}) p(m∣z1:t,x1:t)=i∏p(mi∣z1:t,x1:t)
- 给定Sensor数据
z
1
:
t
z_{1:t}
z1:t和Sensor的位姿
x
1
:
t
x_{1:t}
x1:t,估计map:
1.1 Log-odd概念
- Log odd的概念:计算odd(赔率)的对数
- 为什么需要Log-Odd?
- Odd定义:事件发生的概率与事件不发生的概率的比值
o d d ( x ) = p ( x ) 1 − p ( x ) odd(x) = \frac{p(x)}{1-p(x)} odd(x)=1−p(x)p(x) - Log-odd定义: l ( x ) = l o g ( o d d ( x ) ) = l o g p ( x ) 1 − p ( x ) l(x) =log(odd(x))= log\frac{p(x)}{1-p(x)} l(x)=log(odd(x))=log1−p(x)p(x)
- 反向计算p(x): p ( x ) = 1 − 1 1 + e x p l ( x ) p(x) = 1- \frac {1}{1+expl(x)} p(x)=1−1+expl(x)1 p ( x ) = o d d ( x ) 1 + o d d ( x ) p(x)=\frac{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 ( m x , y ∣ z ) = p ( z ∣ m x , y ) p ( m x , y ) p ( z ) ) p(m_{x,y}|z) = \frac {p(z|m_{x,y})p(m_{x,y})}{p(z)}) p(mx,y∣z)=p(z)p(z∣mx,y)p(mx,y))
- 直接计算概率是困难的,为了简化计算,不使用占据概率本身,而使用log-odd
- O d d ( ( m x , y = 1 )    g i v e n    z ) = p ( m x , y = 1 ∣ z ) p ( m x , y = 0 ∣ z ) Odd((m_{x,y}=1)\; given \; z) = \frac{p(m_{x,y}=1|z)}{p(m_{x,y}=0|z)} Odd((mx,y=1)givenz)=p(mx,y=0∣z)p(mx,y=1∣z)
1.2.2 Log-Odd更新推导过程
p
(
m
x
,
y
=
1
∣
z
)
=
p
(
z
∣
m
x
,
y
=
1
)
p
(
m
x
,
y
=
1
)
p
(
z
)
p(m_{x,y}=1|z) = \frac {p(z|m_{x,y}=1)p(m_{x,y}=1)}{p(z)}
p(mx,y=1∣z)=p(z)p(z∣mx,y=1)p(mx,y=1)
O
d
d
(
m
x
,
y
=
1
∣
z
)
=
p
(
m
x
,
y
=
1
∣
z
)
p
(
m
x
,
y
=
0
∣
z
)
=
p
(
z
∣
m
x
,
y
=
1
)
p
(
m
x
,
y
=
1
)
/
p
(
z
)
p
(
m
x
,
y
=
0
∣
z
)
Odd(m_{x,y}=1|z) = \frac {p(m_{x,y}=1|z)}{p(m_{x,y}=0|z)}=\frac {p(z|m_{x,y}=1)p(m_{x,y}=1)/p(z)}{p(m_{x,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
(
m
x
,
y
=
0
∣
z
)
=
p
(
z
∣
m
x
,
y
=
0
)
p
(
m
x
,
y
=
0
)
p
(
z
)
p(m_{x,y}=0|z) = \frac {p(z|m_{x,y}=0)p(m_{x,y}=0)}{p(z)}
p(mx,y=0∣z)=p(z)p(z∣mx,y=0)p(mx,y=0)
O d d ( m x , y = 1 ∣ z ) = p ( m x , y = 1 ∣ z ) p ( m x , y = 0 ∣ z ) = p ( z ∣ m x , y = 1 ) p ( m x , y = 1 ) p ( z ∣ m x , y = 0 ) p ( m x , y = 0 ) Odd(m_{x,y}=1|z) = \frac {p(m_{x,y}=1|z)}{p(m_{x,y}=0|z)}= \frac {{p(z|m_{x,y}=1)p(m_{x,y}=1)}}{{p(z|m_{x,y}=0)p(m_{x,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) - 取对数:
l o g ( O d d ( m x , y = 1 ∣ z ) ) = l o g p ( m x , y = 1 ∣ z ) p ( m x , y = 0 ∣ z ) = l o g p ( z ∣ m x , y = 1 ) p ( m x , y = 1 ) p ( z ∣ m x , y = 0 ) p ( m x , y = 0 ) log(Odd(m_{x,y}=1|z)) =log \frac {p(m_{x,y}=1|z)}{p(m_{x,y}=0|z)}=log \frac {{p(z|m_{x,y}=1)p(m_{x,y}=1)}}{{p(z|m_{x,y}=0)p(m_{x,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)
= l o g p ( z ∣ m x , y = 1 ) p ( z ∣ m x , y = 0 ) + l o g p ( m x , y = 1 ) p ( m x , y = 0 ) = log \frac{p(z|m_{x,y}=1)}{p(z|m_{x,y}=0)} + log \frac {p(m_{x,y}=1)}{p(m_{x,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更新公式:
l o g    o d d + = l o g    o d d    m e a s + l o g    o d d − log \; odd^+ = log \; odd \; meas + log \; odd^- logodd+=logoddmeas+logodd−
1.2.3 OGM Log-Odd 更新原理
- 每个Cell的值为Log-Odd值
- Log-Odd更新公式:
l
o
g
  
o
d
d
+
=
l
o
g
  
o
d
d
  
m
e
a
s
+
l
o
g
  
o
d
d
−
log \; odd^+ = log \; odd \; meas + log \; odd^-
logodd+=logoddmeas+logodd−
- 更新规则(
l
o
g
  
o
d
d
  
+
=
l
o
g
  
o
d
d
  
m
e
a
s
log\;odd\; += log\;odd\;meas
logodd+=logoddmeas):
- 只更新被观察到的Cell
- 被更新之后的值变成了先验值
1.2.4 Log-Odd形式的测量模型(Measurement Model)
- 测量项: l o g p ( z ∣ m x , y = 1 ) p ( z ∣ m x , y = 0 ) log \frac {p(z|m_{x,y}=1)}{p(z|m_{x,y}=0)} logp(z∣mx,y=0)p(z∣mx,y=1)
- 情况1:Cell (z=1)
l o g    o d d    o c c = l o g p ( z = 1 ∣ m x , y = 1 ) p ( z = 1 ∣ m x , y = 0 ) log \; odd \; occ = log \frac {p(z=1|m_{x,y}=1)}{p(z=1|m_{x,y}=0)} logoddocc=logp(z=1∣mx,y=0)p(z=1∣mx,y=1) - 情况2:Cell (z=0)
l o g    o d d    f r e e = l o g p ( z = 0 ∣ m x , y = 0 ) p ( z = 0 ∣ m x , y = 1 ) log \; odd \; free = log \frac {p(z=0|m_{x,y}=0)}{p(z=0|m_{x,y}=1)} logoddfree=logp(z=0∣mx,y=1)p(z=0∣mx,y=0) - 如果log-odd的值为0,则Odd=1,即 p ( m x , y = 1 ) = p ( m x , y = 0 ) = 0.5 p(m_{x,y}=1)=p(m_{x,y}=0) = 0.5 p(mx,y=1)=p(mx,y=0)=0.5
- 测量模型: p ( z ∣ m x , y ) p(z|m_{x,y}) p(z∣mx,y)
- p ( z = 1 ∣ m x , y = 1 ) p(z=1|m_{x,y}=1) p(z=1∣mx,y=1):True occupied measurement
- p ( z = 0 ∣ m x , y = 1 ) p(z=0|m_{x,y}=1) p(z=0∣mx,y=1):False free measurement
- p ( z = 1 ∣ m x , y = 0 ) p(z=1|m_{x,y}=0) p(z=1∣mx,y=0):False occupied measurement
- p ( z = 0 ∣ m x , y = 0 ) p(z=0|m_{x,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从未被更新,则分配 p h i t 或 p m i s s 给 它 p_{hit}或p_{miss}给它 phit或pmiss给它,当它分别位于hits集合或misses集合
- 如果一个Grid Point已经被更新,则更新公式如下:
o d d s ( p ) = p 1 − p odds(p)=\frac {p}{1-p} odds(p)=1−pp - 为hit更新:
M n e w ( x ) = c l a m p ( o d d s − 1 ( o d d s ( M o l d ( x ) ) ∙ o d d s ( p h i t ) ) ) M_{new}(x) = clamp(odds^{-1}(odds(M_{old}(x)) \bullet odds(p_{hit}))) Mnew(x)=clamp(odds−1(odds(Mold(x))∙odds(phit))) - 为miss更新:
M n e w ( x ) = c l a m p ( o d d s − 1 ( o d d s ( M o l d ( x ) ) ∙ o d d s ( p m i s s ) ) ) M_{new}(x) = clamp(odds^{-1}(odds(M_{old}(x)) \bullet odds(p_{miss}))) Mnew(x)=clamp(odds−1(odds(Mold(x))∙odds(pmiss))) - 因为
p
h
i
t
>
0.5
p_{hit} > 0.5
phit>0.5且
p
m
i
s
s
<
05
p_{miss} < 05
pmiss<05所以有:
o d d s ( p h i t ) > 1 o d d s ( p m i s s ) < 1 odds(p_{hit}) > 1 \quad odds(p_{miss}) < 1 odds(phit)>1odds(pmiss)<1