占据栅格地图（Occupancy Grid Map）

占用栅格地图基础概念

在这里插入图片描述
在这里插入图片描述

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

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

ROS的栅格地图使用

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

占据栅格地图的数学模型

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

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

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

对于一个点，新来了一个测量值,之后我们需要更新它的状态,假设测量值来之前，该点的状态为Odd(s)，
我们要更新它为：Odd(s|z)=\frac{p(s=1|z)}{p(s=0|z)}
这种表达方式类似于条件概率，表示在发生z的条件下s的状态。

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

带入$Odd(s|z)$中得到

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)

对两边取对数得：


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

这样，含有测量值的项就只剩下了log\frac{p(z|s=1)}{p(z|s=0)}称这个比值为测量值的模型,标记为lomeas
测量值的模型只有两种lofree=log\frac{p(z=0|s=1)}{p(z=0|s=0)}和looccu=log\frac{p(z=1|s=1)}{p(z=1|s=0)}

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

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

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

例如
假设我们设定looccu=0.9 lofree=-0.7
那么， 一个点状态的数值越大，就表示越肯定它是Occupied状态，相反数值越小，就表示越肯定它是Free状态。
在这里插入图片描述
上图就展示了用两个激光传感器的数据更新地图的过程。在结果中，一个点颜色越深表示越肯定它是Free的，颜色越浅表示越肯定它是Occupied的。

ROS中使用OccupancyGrid

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

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

header 消息头
info 地图元数据（地图的一些信息）
data 地图数据
地图数据解释：一维数组，数组中的数据为对应栅格的占用概率，范围[1,100],概率越大，说明该点越有可能是障碍点，-1表示位置区域，数组元素的序列号计算为：实际地图中某点坐标为(x,y)[单位m]，先转换为栅格右上角为原点的坐标，即mx=(x-map.info.origin.x)/map.info.resolution,my=(y-map.info.origin.y)/map.info.resolution对应栅格地图中坐标为[my*map.info.height+mx]。

nav_msgs/MapMetaData的数据格式如下：

time map_load_time
float32 resolution
uint32 width
uint32 height
geometry_msgs/Pose origin

map_load_time 地图加载时间
resolution 地图分辨率m/cell（一个格子代表多么米，一般为0.05）
width 地图宽度，像素数
height 地图高度，像素数
origin 地图的原点，即像素(0,0)在世界坐标系的坐标（地图左下角的格子对应的物理世界的坐标，[m,m,rad]）

测试代码

#include "ros/ros.h"
#include "sensor_msgs/LaserScan.h"
#include "nav_msgs/OccupancyGrid.h"

int main(int argc, char * argv[]) {

  ros::init(argc, argv, "gridMap");

  ros::NodeHandle nh;

  ros::Publisher pub = nh.advertise<nav_msgs::OccupancyGrid>("/gridMap1", 1);
  nav_msgs::OccupancyGrid map;

  map.header.frame_id="odom";
  map.header.stamp = ros::Time::now(); 
  map.info.resolution = 0.5;         // float32
  map.info.width      = 20;           // uint32
  map.info.height     = 20;           // uint32
  map.info.origin.position.x = -10.0;
  map.info.origin.position.y = 0.0;
  map.info.origin.position.z = 0.0;
  map.info.origin.orientation.x = 0.0;
  map.info.origin.orientation.y = 0.0;
  map.info.origin.orientation.z = 0.0;
  map.info.origin.orientation.w = 0.0;

  int p[map.info.width*map.info.height] = {-1};   // [0,100]
  p[10] = 100;
  std::vector<signed char> a(p, p+400);
  map.data = a;

  while (ros::ok())
  {
      pub.publish(map);
  }

  ros::shutdown();
  return 0;
}

rospy获取nav_msgs/OccupancyGrid的map话题并可视化

#!/usr/bin/env python

import rospy
import cv2

import numpy as np

#导入消息类型，OccupancyGrid是消息类型
from nav_msgs.msg import OccupancyGrid
import matplotlib.pyplot as plt

class Map(object):
  def __init__(self):
  #rospy订阅map话题，第二个是数据类型，第三个是回调函数
  #将订阅的数据传给回调函数，就是那个mapmsg变量
  #如果有话题来了，就直接调用callback函数
    self.map_sub = rospy.Subscriber("map",OccupancyGrid, self.callback)
    print "get map~"
    #下面输出的是地址，并不是数据
    print self.map_sub

#回调函数的定义，传了mapmsg
  def callback(self,mapmsg):
    try:
      print "into callback"
      #主要是想拿到data，这里存的是地图的信息
      map = mapmsg.data
      #下面是tuple类型
      print type(map)
      #变成可以画图的numpy格式
      map = np.array(map)
      #下面输出的是(368466,)，明显不能画图
      print map.shape
      #需要reshape，将上面的数字在线因数分解，然后算出了两个最大因数
      #于是就大概是这样：
      map = map.reshape((651,566))
      print map
      #可以看到大部分的值是-1，所以需要把值规整一下
      row,col = map.shape
      print row,col
      tem = np.zeros((row,col))
      for i in range(row):
        for j in range(col):
          if(map[i,j]==-1):
             tem[i,j]=255
          else:
             tem[i,j]=map[i,j]
      print map.shape
      cv2.imshow("map",tem)
      cv2.waitKey(0)
#      plt.imshow(map)
#      plt.show()
    except Exception,e:
      print e
      rospy.loginfo('convert rgb image error')

  def getImage():
    return self.rgb_image

def main(_):
  rospy.init_node('map',anonymous=True)
  v=Map()
  rospy.spin()

if __name__=='__main__':
  main('_')