栅格地图(occupancy grid map)构建

Reference:

1. 占据栅格地图构建（Occupancy Grid Map）

1. 地图的分类

尺度地图：具有真实的物理尺寸，如 栅格地图、特征地图、点云地图，常用于地图构建、定位、SLAM、小规模路径规划。
拓扑地图：不具备真实的物理尺寸，只表示不同地点的连通关系和距离，如铁路网，常用于大规模的机器人路径规划。
语义地图：加标签的尺度地图，SLAM和深度学习的结合，常用于人机交互。

其中对尺度地图进行补充说明，如下图所示：

2. 占据栅格地图构建算法

2.1 为什么使用占据栅格地图构建算法构建地图？

构建栅格地图的传感器(如相机、激光雷达、毫米波雷达传感器)是有噪声存在的，通俗的说，不一定准。

举个例子，机器人在同一位姿的不同时刻，通过激光雷达对一个固定的障碍物探测距离不一致，一帧为 $5m$，一帧为 $5.1m$，难道我们要把 $5m$ 和 $5.1m$ 的位置都标记为障碍物？这也就是使用占据栅格地图构建算法的原因。

2.2 什么是占据栅格地图构建算法？

为了解决这一问题，我们引入 占据栅格地图(Occupancy Grid Map) 的概念。我们将地图栅格化，对于每一个栅格的状态要么占用，要么空闲，要么未知(即初始化状态)。

关于占据栅格地图构建算法的引出、推导、演化，可从下面公式得出：
对于每一个栅格，我们用 $p(s=1)$ 来表示 Free 状态的概率，我们用 $p(s=0)$ 来表示 Occupied 状态的概率，两者之和为 $1$。
用两个值表示一个栅格的状态，有点不妥，所以我们引入两者的比值表示栅格的状态：
$$\mathrm{Odd}(s)=\frac{p(s=1)}{p(s=0)}$$

当激光雷达测量值的观测结果(z~{0,1})到来之后，相关的栅格就要更新栅格状态 $Odd(s)$。
假设激光雷达测量值来之前，该栅格的状态为 $Odd(s)$，则来之后，更新栅格的状态为：
$$Odd(s\mid z)=\frac{p(s=1\mid z)}{p(s=0\mid z)}$$

这种表达方式类似于条件概率，表示在 $z$ 发生条件下栅格的状态。
根据贝叶斯公式得出以下两个式子：
$$\begin{array}{l}p(s=1\mid z)=\frac{p(z\mid s=1)p(s=1)}{p(z)}\\ p(s=0\mid z)=\frac{p(z\mid s=0)p(s=0)}{p(z)}\end{array}$$

将上述两个式子代入 $Odd(s)$ 之后，我们得出：
$$\begin{array}{l}\mathrm{Odd}(s\mid z)=\frac{p(s=1\mid z)}{p(s=0\mid z)}\\ \mathrm{Odd}(s\mid z)=\frac{p(z\mid s=1)p(s=1)}{p(z\mid s=0)p(s=0)}\\ \mathrm{Odd}(s\mid z)=\frac{p(z\mid s=1)}{p(z\mid s=0)}\ast \frac{p(s=1)}{p(s=0)}\\ \mathrm{Odd}(s\mid z)=\frac{p(z\mid s=1)}{p(z\mid s=0)}\ast \mathrm{Odd}(s)\end{array}$$

此时，我们上式两边同时取对数得出：栅格状态表示方法再一次发生改变，即用 $\log \mathrm{Odd}(s)$ 表示一个栅格的状态。
$$\log \mathrm{Odd}(s\mid z)=\log \frac{p(z\mid s=1)}{p(z\mid s=0)}+\log \mathrm{Odd}(s)$$

这时含有测量值的项就只有 $\log \frac{p(z\mid s=1)}{p(z\mid s=0)}$ 了。我们把这个比值称为测量值的模型。

$\log \frac{p(z\mid s=1)}{p(z\mid s=0)}$ 的选项只有两种 $lofree$ 和 $looccu$，因为激光雷达的对一个栅格观测结果只有两种：占据和空闲。
$$\begin{array}{l}\text{ lofree }=\log \frac{p(z=0\mid s=1)}{p(z=0\mid s=0)}\\ \text{ looccu }=\log \frac{p(z=1\mid s=1)}{p(z=1\mid s=0)}\end{array}$$

此时，如果我们用 $\log \mathrm{Odd}(s)$ 来表示栅格 $s$ 的状态 $S$ 的话，我们的更新规则进一步化简为：
$$S^{+}=S^{-}+\log \frac{p(z\mid s=1)}{p(z\mid s=0)}$$

其中，$S^{+}$ 和 $S^{-}$ 分别表示测量值之后和之前栅格 $s$ 的状态。
此外，一个栅格的初始状态 $S_{init}$ 为：默认栅格空闲和栅格占用的概率都为 $0.5$。
$$S_{\text{init }}=\log 0dd(s)=\log \frac{p(s=1)}{p(s=0)}=\log \frac{0.5}{0.5}=0$$

此时，经过这样的建模，更新一个栅格的状态只需要做简单的加法即可：
$$\begin{array}{c}{S}^{+}=S^{-}+\text{lofree }\\ \text{ 或 }\\ S^{+}=S^{-}+\text{looccu }\end{array}$$

2.3 举个例子验证占据栅格地图构建算法

首先，我们假设 $looccu=0.9$，$lofree=-0.7$。那么，显而易见，一个栅格状态的数值越大，就越表示该栅格为占据状态；相反数值越小，就表示该栅格为空闲状态。（这也就解决了此前文中提出的激光雷达观测值"不一定准"的问题）

上图是用两次激光扫描数据更新地图的过程。在结果中，颜色越深越表示栅格是空闲的，颜色越浅越表示是占据的。这里要区分常用的激光SLAM算法中的地图，只是表述方式的不同，没有对错之分。

3. 如何通过激光雷达数据构建栅格地图？

3.1 算法核心依据

整篇文章得出的一个结论就是下图所示，这里假设 $lofree$ 和 $looccu$ 为确定的数值，一般情况下一正一负。
$$\begin{array}{c}{S}^{+}=S^{-}+\text{lofree }\\ \text{ 或 }\\ S^{+}=S^{-}+\text{looccu }\end{array}$$

然后，我们通过激光雷达数据栅格进行判断：如果判定栅格是空闲，就执行上面公式；如果判定栅格是占据，就执行下面的公式。在经过许多帧激光雷达数据的洗礼后，每一个栅格都存储了一个值，此时我们可以自己设定阈值与该值比较，来做栅格最终状态的判定。

3.2 算法输入数据

激光雷达数据包（每个扫描点包含角度（逆时针为正方向）和距离，每帧激光数据包含若干扫描点，激光雷达数据包包含若干帧激光雷达数据）

机器人位姿数据包（每一个位姿包含世界坐标系下的机器人位置和航向角，初始航向角与世界坐标系X轴正方向重合，逆时针为正方向）

地图参数（需要构建地图的高度和宽度，构建地图的起始点，$lofree$ 和 $looccu$ 的设定值，地图的分辨率）

假设激光雷达坐标系和机器人坐标系重合。

3.3 算法步骤

这里以处理第一帧激光雷达为例，从上向下依次介绍：

1. 读取一帧激光雷达数据和该帧对应的机器人位姿

//获取每一帧的激光雷达、机器人位姿数据
GeneralLaserScan scan = scans[i];
Eigen::Vector3d robotPose = robot_poses[i];

2. 计算该帧机器人位置的栅格序号

//获取该帧机器人位姿的栅格序号
GridIndex robotIndex = ConvertWorld2GridIndex(robotPose(0),robotPose(1));

即从世界坐标系转入栅格坐标系，每个栅格序号有 $x$，$y$ 两个数字。这里与地图分辨率有关，比如说：地图分辨率为 $0.05$，也就是 $1m$ 用 $20$ 个栅格表示。

例如：世界坐标系下机器人位置（1,1）对应栅格坐标系的（20,20）。

注意：世界坐标系与像素坐标系区分开来，他们之间的y轴方向相反，其他都一致，地图的显示使用的像素坐标系（栅格坐标系）。

3. 遍历该帧激光雷达数据的所有扫描点执行以下操作

计算每一个激光点击中栅格在像素坐标系下的栅格序号:

//明确这里的世界坐标系world_x，不是真实的世界坐标系，而是像素坐标系，y轴与真实的世界坐标系相反，这样是laser_y加负号的原因
double laser_x =   dist * cos(theta + angle);
double laser_y =  -dist * sin(theta + angle);

//得到该激光扫描点，在世界坐标系下（像素坐标系下）的位置
double world_x = laser_x + robotPose(0);
double world_y = laser_y + robotPose(1);

//将该激光扫描点在世界坐标系下的位置，转化为栅格序号
GridIndex mapIndex = ConvertWorld2GridIndex(world_x,world_y);

从当前机器人位姿的栅格序号到该激光扫描点的栅格序号划线，找出所有空闲的栅格序号:

//从机器人的栅格序号到该激光扫描点的栅格序号划线
//目的：找到两点之间途径的空闲栅格，将栅格序号存入std::vector中
std::vector<GridIndex> freeIndex = TraceLine(robotIndex.x,robotIndex.y,mapIndex.x,mapIndex.y);

• 遍历所有空闲的栅格更新空闲栅格状态:

data += mapParams.log_free;//log_free=-1，data将变小

• 更新该激光扫描点击中的栅格状态:

data += mapParams.log_occ;//log_occ=2，data将变大

4. 占据栅格地图构建算法c++实现

这里分享核心代码，具体算法功能包，可在公众号：小白学移动机器人，发送：栅格地图，即可获得。

//占据栅格地图构建算法
//输入激光雷达数据和机器人位姿数据
//目的：通过遍历所有帧数据，为pMap[]中的每个穿过的空闲栅格或者击中栅格赋新值，中间有个计算方法，也就是占用栅格地图构建的理论实现
void OccupanyMapping(std::vector<GeneralLaserScan>& scans,std::vector<Eigen::Vector3d>& robot_poses)
{
  std::cout <<"Scans Size:"<<scans.size()<<std::endl;
  std::cout <<"Poses Size:"<<robot_poses.size()<<std::endl;

  //遍历所有帧激光雷达数据
  for(int i = 0; i < scans.size();i++)
  {
    //获取每一帧的激光雷达、机器人位姿数据
    GeneralLaserScan scan = scans[i];
    Eigen::Vector3d robotPose = robot_poses[i];

    //获取该帧机器人位姿的栅格序号
    GridIndex robotIndex = ConvertWorld2GridIndex(robotPose(0),robotPose(1));

    //判断该帧机器人位姿的栅格序号，是否在自己设定的栅格地图范围内
    if(isValidGridIndex(robotIndex) == false) continue;

    //遍历该帧激光雷达数据所有扫描点
    for(int id = 0; id < scan.range_readings.size();id++)
    {
      //取出该激光雷达扫描点的距离和角度
      double dist = scan.range_readings[id];
      double angle = scan.angle_readings[id];
      //剔除异常数据，跳过该次循环，不作处理
      if(std::isinf(dist) || std::isnan(dist)) continue;
      //机器人航向角，机器人x轴与世界坐标系x轴夹角
      double theta = robotPose(2);

      //在旋转过后（与世界坐标系（像素坐标系下）平行）的激光雷达坐标系下的坐标x,y
      //该开始一直不理解这个为啥laser_y要加一个负号
      //明确激光雷达数据的角度逆时针变化
      //明确机器人航向角与世界坐标系x轴呈逆时针变化
      //明确这里的世界坐标系world_x，不是真实的世界坐标系，而是像素坐标系，y轴与真实的世界坐标系相反，这样是laser_y加负号的原因
      double laser_x =   dist * cos(theta + angle);
      double laser_y =  -dist * sin(theta + angle);

      //得到该激光扫描点，在世界坐标系下（像素坐标系下）的位置
      double world_x = laser_x + robotPose(0);
      double world_y = laser_y + robotPose(1);

      //将该激光扫描点在世界坐标系下的位置，转化为栅格序号
      GridIndex mapIndex = ConvertWorld2GridIndex(world_x,world_y);

      //判断该激光扫描点的栅格序号，是否在自己设定的栅格地图900x900范围内，如果不在则跳过
      if(isValidGridIndex(mapIndex) == false)continue;

      //从机器人的栅格序号到该激光扫描点的栅格序号划线
      //目的：找到两点之间途径的空闲栅格，将栅格序号存入std::vector中
      std::vector<GridIndex> freeIndex = TraceLine(robotIndex.x,robotIndex.y,mapIndex.x,mapIndex.y);

      //遍历该扫描激光点通过的所有空闲栅格
      for(int k = 0; k < freeIndex.size();k++)
      {
        GridIndex tmpIndex = freeIndex[k];
        //将空闲栅格的栅格序号，转化到数组序号,该数组用于存储每一个栅格的数据
        int linearIndex = GridIndexToLinearIndex(tmpIndex);
        //取出该栅格代表的数据
        int data = pMap[linearIndex];
        //根据栅格空闲规则，执行data += mapParams.log_free;
        if(data > 0)//默认data=50
          data += mapParams.log_free;//log_free=-1，data将变小
        else
          data = 0;
        //给该空闲栅格赋新值，最小为0
        pMap[linearIndex] = data;
      }
      //更新该激光扫描点集中的栅格，
      int tmpIndex = GridIndexToLinearIndex(mapIndex);
      int data = pMap[tmpIndex];
      //根据栅格击中规则，执行data += mapParams.log_occ;
      if(data < 100)//默认data=50
        data += mapParams.log_occ;//log_occ=2，data将变大
      else
        data = 100;
      //给击中的栅格赋新值，最大100
      pMap[tmpIndex] = data;
      //到这里，对一个位姿下的一个激光扫描数据经过的空闲栅格和击中栅格的pMap进行了重新赋值
    }
    //到这里，对一个位姿下的一帧激光扫描数据经过的空闲栅格和击中栅格进行了重新赋值
  }
  //到这里，对所有帧激光扫描数据经过的空闲栅格和击中栅格进行了重新赋值
}