cartographer概率栅格地图更新过程

1.占据栅格地图理论推导

1.1理论推导

​ 对于一个点，它要么有（Occupied状态，下面用1来表示）障碍物，要么没有（Free状态，下面用0来表示）障碍物。在占据栅格地图中，对于一个点，我们用$p(s=0)$来表示它是Free状态的概率，用$p(s=1)$来表示它是Occupied状态的概率，当然两者的和为1。两个值太多了，我们引入两者的比值来作为点的状态。
o d d s ( s ) = p ( s = 1 ) p ( s = 0 ) = p ( s = 1 ) 1 − p ( s = 1 ) (1) odds(s)=\frac{p(s=1)}{p(s=0)}=\frac{p(s=1)}{1-p(s=1)} \tag1 odds(s)=p(s=0)p(s=1)​=1−p(s=1)p(s=1)​(1)

如果当前时刻对该点进行了一次观测z，如何更新此点的栅格状态？
设对该点进行观测之前该点的状态表示为$Odd(s)$，观测后的状态表示为$Odd(s|z)$
根据推导可以得到该点进行了一次观测z后此点的栅格状态：
o d d s ( s ∣ z ) = p ( z ∣ s = 1 ) p ( z ∣ s = 0 ) o d d s ( s ) (2) odds(s|z)=\frac{p(z|s=1)}{p(z|s=0)}odds(s) \tag2 odds(s∣z)=p(z∣s=0)p(z∣s=1)​odds(s)(2)
公式（2）具体推导如下：
由定义
o d d s ( s ∣ z ) = p ( s = 1 ∣ z ) p ( s = 0 ∣ z ) (3) odds(s|z)=\frac{p(s=1|z)}{p(s=0|z)} \tag3 odds(s∣z)=p(s=0∣z)p(s=1∣z)​(3)
由贝叶斯公式
p ( s = 1 ∣ z ） = p ( z ∣ s = 1 ) p ( s = 1 ) p ( z ) (4) p(s=1|z）=\frac{p(z|s=1)p(s=1)}{p(z)} \tag4 p(s=1∣z）=p(z)p(z∣s=1)p(s=1)​(4)
p ( s = 0 ∣ z ） = p ( z ∣ s = 0 ) p ( s = 0 ) p ( z ) (5) p(s=0|z）=\frac{p(z|s=0)p(s=0)}{p(z)} \tag5 p(s=0∣z）=p(z)p(z∣s=0)p(s=0)​(5)

公式（4）（5）代入公式（3）得到
o d d s ( s ∣ z ) = p ( z ∣ s = 1 ) p ( s = 1 ) p ( z ∣ s = 0 ) p ( s = 0 ) = p ( z ∣ s = 1 ) p ( z ∣ s = 0 ) o d d s ( s ) odds(s|z)=\frac{p(z|s=1)p(s=1)}{p(z|s=0)p(s=0)}\\ = \frac{p(z|s=1)}{p(z|s=0)}odds(s) odds(s∣z)=p(z∣s=0)p(s=0)p(z∣s=1)p(s=1)​=p(z∣s=0)p(z∣s=1)​odds(s)

其中 p ( z ∣ s = 1 ) p ( z ∣ s = 0 ) \frac{p(z|s=1)}{p(z|s=0)} p(z∣s=0)p(z∣s=1)​称为更新系数，这里有疑问1？更新系数怎么设置？

某个栅格

hit的更新系数表示为： p ( z = 1 ∣ s = 1 ) p ( z = 1 ∣ s = 0 ) \frac{p(z=1|s=1)}{p(z=1|s=0)} p(z=1∣s=0)p(z=1∣s=1)​

miss的更新系数表示为： p ( z = 0 ∣ s = 1 ) p ( z = 0 ∣ s = 0 ) \frac{p(z=0|s=1)}{p(z=0|s=0)} p(z=0∣s=0)p(z=0∣s=1)​

可以分解定义：

$p(z=1|s=1)$表示栅格是Occupied状态的条件下这次观测是hit的概率

$p(z=0|s=1)$表示栅格是Occupied状态的条件下这次观测是miss的概率

$p(z=1|s=0)$表示栅格是Free状态的条件下这次观测是hit的概率

$p(z=0|s=0)$表示栅格是Free状态的条件下这次观测是miss的概率

这四项都是由传感器的模型确定的一个具体数值，在程序中一般是个固定的配置参数，他们的取值范围都为（0，1）。

注意，满足以下关系
$$\begin{gathered} p(z=1|s=1)+p(z=1|s=0)=1 \\ p(z=0|s=0)+p(z=0|s=0)=1 \end{gathered}$$
注意，由公式（2），已知odd(s)可以得到p(s=1),即栅格是Occupied状态的概率。
p ( s = 1 ) = o d d s ( s ) 1 + o d d s ( s ) (6) p(s=1)=\frac{odds(s)}{1+odds(s)}\tag6 p(s=1)=1+odds(s)odds(s)​(6)

1.2概率更新过程例子

例子1，假设我们设定：

$p(z=1|s=1)=0.55$栅格是Occupied状态的条件下这次观测是hit的概率

$p(z=0|s=1)=0.49$栅格是Occupied状态的条件下这次观测是miss的概率

$p(z=1|s=0)=0.45$栅格是Free状态的条件下这次观测是hit的概率

$p(z=0|s=0)=0.51$栅格是Free状态的条件下这次观测是miss的概率

那么

hit的更新系数为： p ( z = 1 ∣ s = 1 ) p ( z = 1 ∣ s = 0 ) = 0.55 0.45 = 1.22 \frac{p(z=1|s=1)}{p(z=1|s=0)}=\frac{0.55}{0.45}=1.22 p(z=1∣s=0)p(z=1∣s=1)​=0.450.55​=1.22

miss的更新系数表示为： p ( z = 0 ∣ s = 1 ) p ( z = 0 ∣ s = 0 ) = 0.49 0.51 = 0.96 \frac{p(z=0|s=1)}{p(z=0|s=0)}=\frac{0.49}{0.51}=0.96 p(z=0∣s=0)p(z=0∣s=1)​=0.510.49​=0.96

（2）推导过程

1.初始的时候该栅格的概率为p(s=1)=0.5,p(s=0)=0.5，由公式1，odds(s)=1。

2.进行了一次观测，且观测结果为z=1障碍物,这次观测后的odds由公式（2）

o d d s ( s ∣ z ) = p ( z ∣ s = 1 ) p ( z ∣ s = 0 ) o d d s ( s ) = 1.22 ∗ 1 = 0.22 odds(s|z)=\frac{p(z|s=1)}{p(z|s=0)}odds(s)=1.22*1=0.22 odds(s∣z)=p(z∣s=0)p(z∣s=1)​odds(s)=1.22∗1=0.22

有公式（6），将odds转换为p(s=1)的概率。

那么击中一次后更新后的概率
p ( s = 1 ) = 1.22 1 + 1.22 = 0.55 p(s=1)=\frac{1.22}{1+1.22}=0.55 p(s=1)=1+1.221.22​=0.55

扩展下，如果第二次是击中呢？

odds(s)=1.22*1.22=1.488, p(s=1)=0.598

可以发现越接近1增加幅度越小。

如果第二次是miss呢？

odds(s)=1.22*0.96=1.1715, p(s=1)=0.53

2.cartographer代码实现概率栅格地图更新

2.1cartographer代码和公式对于关系

cartographer对于每个概率栅格更新，没有用到公式(6)，而是用的是公式（2），只是用的别的方法优化更新

对应的就是公式（2）

o d d s ( s ∣ z ) = p ( z ∣ s = 1 ) p ( z ∣ s = 0 ) o d d s ( s ) (2) odds(s|z)=\frac{p(z|s=1)}{p(z|s=0)}odds(s) \tag2 odds(s∣z)=p(z∣s=0)p(z∣s=1)​odds(s)(2)

cartographer中概率地图更新相关公式和函数对应关系如下表

2.2cartographer建表和查表操作详解

cartographer为了提高效率，用查表操作直接替代加减法更新概率栅格，在初始化阶段一次调用公式2建立表。

Cartographer针对地图更新过程，采用以空间换时间的策略，主要是建立地图更新对照表，然后更新过程就直接变成了查表操作，连加减法都省去了，牺牲内存空间，节约时间。
栅格更新具体流程为：
（1）开始的时候一次性构建hit和miss表
（2）如果需要更新直接在hit和miss表中找更新后的值。

2.2.1建表过程

构建表cartographer的表存的是概率映射为整形的值。可以将（0-1）映射到（0-32768），但是实际情况下cartographer映射如下

[1, 32767] ->[lower_bound(0.1), upper_bound(0.9)]

如下为一个实际的hit表

按照p->odds->int16表示的hit表如下

hit表在程序中为一个数组。

cartographer中映射函数如下表

同样也有一个miss表。

2.2.2查表过程

下面来详细介绍查表操作,如例1中，如果两次都是hit，查表过程如下表

第一次是hit，概率p由0.5->0.55，直接取a[16384]数组值得到更新后的数组int值，再用映射函数由整形值映射到概率值。

第一次是hit，概率p由0.55->0.598，直接取a[18432]数组值得到更新后的数组int值，再用映射函数由整形值映射到概率值。

3.对数更新

如果不用查表操作来更新，可以用取对数的方式来更新也行

可以对公式（2）取对数，
l o g ( o d d s ( s ∣ z ) ) = l o g ( p ( z ∣ s = 1 ) p ( z ∣ s = 0 ) ) + l o g ( o d d s ( s ) ) (6) log( odds(s|z))=log(\frac{p(z|s=1)}{p(z|s=0)})+log(odds(s)) \tag6 log(odds(s∣z))=log(p(z∣s=0)p(z∣s=1)​)+log(odds(s))(6)

每次观测根据是hit还是miss来对log( odds(s|z))进行加减固定值。

参考文献
https://zhuanlan.zhihu.com/p/21738718
https://zhuanlan.zhihu.com/p/49030629
【图解 cartographer】 之地图概率更新过程

本文来自互联网用户投稿，文章观点仅代表作者本人，不代表本站立场，不承担相关法律责任。如若转载，请注明出处。 如若内容造成侵权/违法违规/事实不符，请点击【内容举报】进行投诉反馈！