机器人导航必备的栅格地图数学模型及使用

• 占据栅格地图（Occupancy Grid Map）
• • 占用栅格地图基础概念
  • 占据栅格地图的数学模型
  • ROS中使用OccupancyGrid

占据栅格地图（Occupancy Grid Map）

占用栅格地图基础概念

上图就是一个ROS中的占据栅格地图显示

栅格地图定义 ：栅格地图就是用一个个栅格组成的网格来代表地图. 栅格里可以存储不同的数值, 代表这个栅格的不同含义.

ROS的栅格地图使用

• 白色代表空闲，也就是可通过区域，其存储的值为 0；
• 黑色代表占用，也就是不可通过区域，其存储的值为 100；
• 灰色代表未知，就是说目前还不清楚这个栅格是否可以通过，其存储的值为 -1．

占据栅格地图的数学模型

占据率（Occupancy）
在通常的尺度地图中，对于一个点，它要么有（Occupied状态）障碍物，要么没有（Free状态）障碍物
在占据栅格地图中，对于一个点，用p(s=0)p(s=0)p(s=0)来表示它是Free状态的概率;

用p(s=1)p(s=1)p(s=1)来表示它是Occupied状态的概率.两者的和为1

引入两者的比值来作为点的状态：Odd(s)=p(s=1)p(s=0)Odd(s)=\frac{p(s=1)}{p(s=0)}Odd(s)=p(s=0)p(s=1)​

对于一个点，新来了一个测量值,之后我们需要更新它的状态,假设测量值来之前，该点的状态为Odd(s)Odd(s)Odd(s)，
我们要更新它为：Odd(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)​
这种表达方式类似于条件概率，表示在发生z的条件下s的状态。

根据贝叶斯公式，我们有：
p(s=1∣z)=p(z∣s=1)p(s=1)p(z)p(s=1|z)=\frac{p(z|s=1)p(s=1)}{p(z)}p(s=1∣z)=p(z)p(z∣s=1)p(s=1)​
p(s=0∣z)=p(z∣s=0)p(s=0)p(z)p(s=0|z)=\frac{p(z|s=0)p(s=0)}{p(z)}p(s=0∣z)=p(z)p(z∣s=0)p(s=0)​

带入Odd(s∣z)Odd(s|z)Odd(s∣z)中得到
Odd(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)Odd(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)

对两边取对数得：

logOdd(s∣z)=logp(z∣s=1)p(z∣s=0)+logOdd(s)logOdd(s|z)=log\frac{p(z|s=1)}{p(z|s=0)}+logOdd(s) logOdd(s∣z)=logp(z∣s=0)p(z∣s=1)​+logOdd(s)

这样，含有测量值的项就只剩下了logp(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)​称这个比值为测量值的模型,标记为lomeaslomeaslomeas
测量值的模型只有两种lofree=logp(z=0∣s=1)p(z=0∣s=0)lofree=log\frac{p(z=0|s=1)}{p(z=0|s=0)}lofree=logp(z=0∣s=0)p(z=0∣s=1)​和looccu=logp(z=1∣s=1)p(z=1∣s=0)looccu=log\frac{p(z=1|s=1)}{p(z=1|s=0)}looccu=logp(z=1∣s=0)p(z=1∣s=1)​

这样，如果我们用Odd(s)Odd(s)Odd(s)来表示位置s的状态S的话，我们的更新规则就进一步简化成了：S+=S−+lomeasS^+=S^-+lomeasS+=S−+lomeas
其中S+S^+S+和S−S^-S−分别表示测量值之后和之前的状态。

另外，在没有任何测量值的初始状态下，一个点的初始状态Sinit=logOdd(s)=logp(s=1)p(s=0)=log0.50.5=0S_{init}=logOdd(s)=log\frac{p(s=1)}{p(s=0)}=log\frac{0.5}{0.5}=0Sinit​=logOdd(s)=logp(s=0)p(s=1)​=log0.50.5​=0

经过这样的建模，更新一个点的状态就只需要做简单的加减法了。

例如
假设我们设定looccu=0.9looccu=0.9looccu=0.9 lofree=−0.7lofree=-0.7lofree=−0.7
那么， 一个点状态的数值越大，就表示越肯定它是Occupied状态，相反数值越小，就表示越肯定它是Free状态。

上图就展示了用两个激光传感器的数据更新地图的过程。在结果中，一个点颜色越深表示越肯定它是Free的，颜色越浅表示越肯定它是Occupied的。

ROS中使用OccupancyGrid

在ros中的数据格式定义如下：（官网链接）

std_msgs/Header header
nav_msgs/MapMetaData info
int8[] data

后续
详情参考 古月居