平台

Ubuntu 16.04
ROS Kinetic Kame

建图算法概述

在机器人领域中，比较常见的研究问题包括：建图Mapping、定位Localization和路径规划Path Planning。而同时定位与地图构建Simultaneous Localization and Mapping属于位于定位和建图的交集部分。
机器人领域关系图

建图算法分类

SLAM的具体含义为：搭建特定传感器的主体，在没有环境先验信息的情况下，与运动过程中建立环境的模型，同时估计自己的运动。由此可见，SLAM主要解决两个问题：1.定位：估计传感器自身的位置；2.地图构建：建立周围环境的模型。目前SLAM按照传感器类型可以分为基于视觉传感器的视觉SLAM和基于激光雷达传感器的激光SLAM。由于视觉传感器很受光照的影响，目前主流的建图算法还是基于激光雷达传感器。但不管是哪种传感器，其基本框架大致相同：
SLAM框架
由图可见，在传感器信息读取、前端、回环检测和建图这几个模块，目前主流的建图算法都大致相同，而针对后端模块的处理大致可以分为两类：一类是基于滤波器的算法，例如高斯滤波器、直方图滤波器和粒子滤波器等，其基本原理为：通过估计后验概率，递推贝叶斯状态估计来求解机器人路径和地图。另一类为基于图优化的算法，例如非线性最小二乘法、随机梯度下降法等，其基本原理为：将数学模型转化为因子图的方式来求解，通过将位姿描述成节点和将约束关系描述成边以使问题可视化。本博客所探讨的Gmapping建图算法属于基于滤波器的粒子滤波器算法。另一种比较主流的建图算法Cartographer属于基于图优化的算法。
建图算法分类

地图

由上图可见，SLAM中的地图有很多种，一般2D的建图算法生成的均为栅格地图。
栅格地图很好理解，就是一张普通的灰度图像，通常为pgm格式。这张图像上的黑色像素表示障碍物，白色像素表示可行区域，灰色是未探索的区域。

在SLAM建图的过程中，你可以在RViz里看到一张地图被逐渐建立起来的过程，类似于一块块拼图被拼接成一张完整的地图。这张地图对于我们定位、路径规划都是不可缺少的信息。事实上，地图在ROS中是以Topic的形式维护和呈现的，这个Topic名称就叫做/map，它的消息类型是nav_msgs/OccupancyGrid。
由于/map中实际上存储的是一张图片，为了减少不必要的开销，这个Topic往往采用锁存(latched)的方式来发布。什么是锁存？其实就是：地图如果没有更新，就维持着上次发布的内容不变，此时如果有新的订阅者订阅消息，这时只会收到一个/map的消息，也就是上次发布的消息；只有地图更新了(比如SLAM又建出来新的地图)，这时/map才会发布新的内容。锁存器的作用就是，将发布者最后一次发布的消息保存下来，然后把它自动发送给后来的订阅者。这种方式非常适合变动较慢、相对固定的数据(例如地图)，然后只发布一次，相比于同样的消息不定的发布，锁存的方式既可以减少通信中对带宽的占用，也可以减少消息资源维护的开销。
注：参考中国大学MOOC—-《机器人操作系统入门》课程讲义，下载地址。

Gmapping算法原理理解

Gmapping算法概述

论文下载地址
Gmapping是基于滤波SLAM框架的常用开源SLAM算法。其基本原理基于RBpf粒子滤波算法，即将定位和建图过程分离，先进行定位再进行建图。粒子滤波算法是早期的一种建图算法，其基本原理为机器人不断地通过运动、观测的方式，获取周围环境信息，逐步降低自身位置的不确定度，最终得到准确的定位结果。用上一时刻的地图和运动模型预测当前时刻的位姿，然后计算权重，重采样，更新粒子的地图，如此往复。
Gmapping可以实时构建室内地图，在构建小场景地图所需的计算量较小且精度较高。相比Cartographer在构建小场景地图时，Gmapping不需要太多的粒子并且没有回环检测因此计算量小于Cartographer而精度并没有差太多。Gmapping有效利用了车轮里程计信息，这也是Gmapping对激光雷达频率要求低的原因：里程计可以提供机器人的位姿先验。
随着场景增大所需的粒子增加，因为每个粒子都携带一幅地图，因此在构建大地图时所需内存和计算量都会增加。因此不适合构建大场景地图。并且没有回环检测，因此在回环闭合时可能会造成地图错位，虽然增加粒子数目可以使地图闭合但是以增加计算量和内存为代价。所以不能像Cartographer那样构建大的地图。Gmapping和Cartographer一个是基于滤波框架SLAM另一个是基于优化框架的SLAM，两种算法都涉及到时间复杂度和空间复杂度的权衡。Gmapping牺牲空间复杂度保证时间复杂度，这就造成Gmapping不适合构建大场景地图。

RBpf粒子滤波算法——先定位再建图

SLAM需要解决的是定位与建图问题，而定位需要建图，建图也需要先定位，如何平衡这两者关系就是解决SLAM问题的关键。为此，基于RBpf(Rao-Blackwellized)粒子滤波算法提供了一个思路：先定位再建图，具体如下：

$P\left ( x_{1:t},m|z_{1:t},u_{1:t-1} \right )=P\left ( x_{1:t}|z_{1:t},u_{1:t-1} \right ) \cdot P\left ( m|x_{1:t} ,z_{1:t}\right )$

其中，$x{1:t}$表示1到t时刻的位姿，m表示地图，$z{1:t}$表示观测值，这里就是激光雷达的值，$u_{1:t-1}$表示运动值，这里就是里程计(IMU)的值。
一般的，SLAM的数学模型可以用上式表示。第一部分表示已知运动值和观测值求出地图和机器人位姿，第二部分表示已知运动值和观测值求出机器人位姿，第三部分为已知位姿和观测值求出地图信息。根据概率论的知识，上式是一个条件联合概率分布，即通过观测值和里程计的值得出位姿和地图，但如果把这2个值都作为条件去求概率会十分困难，而由概率论的知识可知，联合概率可以转换成条件概率，即$P\left ( x,y \right )=P\left ( y|x \right )\cdot p\left ( x \right )$。因此可以将上式转换为先根据观测值和运动值求出位姿，然后根据观测值和位姿求出地图。这就大大降低了求解的难度。
这看起好像很好，但基于粒子滤波器的算法会存在一些问题：1.精确的地图需要更多的粒子，但同时也会增加计算量和内存消耗；2.频繁执行重采样会造成粒子退化(正确的粒子被丢弃，粒子多样性减小)。
因此，Gmapping算法针对这两个问题，分别进行了改进：改进的提议分布和选择性重采样。

附：SIR(Samping Importance Resampling)粒子滤波算法流程

预测阶段：粒子滤波首先根据状态转移函数预测生成大量的采样，这些采样就称之为粒子，利用这些粒子的加权和来逼近后验概率密度。
校正阶段：随着观测值的依次到达，为每个粒子计算相应的重要性权值。这个权值代表了预测的位姿取第个粒子时获得观测的概率。如此这般下来，对所有粒子都进行这样一个评价，越有可能获得观测的粒子，获得的权重越高。
重采样阶段：根据权值的比例重新分布采样粒子。由于近似逼近连续分布的粒子数量有限，因此这个步骤非常重要。下一轮滤波中，再将重采样过后的粒子集输入到状态转移方程中，就能够获得新的预测粒子了。
地图估计：对于每个采样的粒子，通过其采样的轨迹与观测计算出相应的地图估计。

SIR算法需要在新的观测值到达时从头评估粒子的权重。当轨迹的长度随着时间的推移而增加时，这个过程的计算复杂度将越来越高。因此Doucet等学者通过式(2)限制重要性概率密度函数来获得递归公式去计算重要性权值。

改进的提议分布

参考博客

博客1
博客2
博客3

Gmapping源代码理解

下载完成后，进入src/slam_gmapping/gmapping/src文件夹，打开main.cpp，可以看到gmapping在ros下面定义的一个节点slam_gmapping，该节点也是上节中rosrun命令下的节点。
mian函数比较简单，只是声明了一个节点和一个SlamGMapping类，并调用了该类下面的startLiveSlam()函数，该函数的声明在该文件夹下的slam_gmapping.cpp文件中。下面则正式进入源代码的分析。
注：在正式进入前，需要有一定的ROS基础，具体可参考我的另一篇博客ROS功能包及其应用，也可以百度其他博客学习。

Gmapping源代码框架

gmapping整体框架

startLiveSlam()

该函数的功能为正式开始Slam进程。其代码如下：

void SlamGMapping::startLiveSlam()
{
  entropy_publisher_ = private_nh_.advertise<std_msgs::Float64>("entropy", 1, true);
  sst_ = node_.advertise<nav_msgs::OccupancyGrid>("map", 1, true);
  sstm_ = node_.advertise<nav_msgs::MapMetaData>("map_metadata", 1, true);
  ss_ = node_.advertiseService("dynamic_map", &SlamGMapping::mapCallback, this);

  scan_filter_sub_ = new message_filters::Subscriber<sensor_msgs::LaserScan>(node_, "scan", 5);
  scan_filter_ = new tf::MessageFilter<sensor_msgs::LaserScan>(*scan_filter_sub_, tf_, odom_frame_, 5);
  scan_filter_->registerCallback(boost::bind(&SlamGMapping::laserCallback, this, _1));

  transform_thread_ = new boost::thread(boost::bind(&SlamGMapping::publishLoop, this, transform_publish_period_));
}

该函数整体上为ros中的通讯部分，大致可以分为三部分，第一部分为消息的发布，主要有<std_msgs::Float64>：激光雷达数据序号，<nav_msgs::OccupancyGrid> ：2D栅格图信息， <nav_msgs::MapMetaData>：栅格图特征信息这3个topic信息(具体可以参考官方文档：nav_msgs)和一个服务器信息：dynamic_map：不断更新的动态地图。
第二部分为消息的订阅，这里都是对激光雷达消息<sensor_msgs::LaserScan>的订阅，其中message_filters表示消息过滤器，类似一个消息缓存，当消息到达消息过滤器的时候，可能并不会立即输出，而是在稍后的时间点里满足一定条件下输出。而tf::MessageFilter可以订阅任何的ROS消息，然后将其缓存，直到这些消息可以转换到目标坐标系，然后进行相应的处理(进入回调函数)。tf::MessageFilter的初始化需要message_filters::Subscriber，tf转换，目标坐标系，等待时间。
第三部分为回调函数和线程的启动，其中处理激光雷达和坐标变换的回调函数为laserCallback()，处理完之后会启动发布转换关系的线程publishLoop()。
总而言之，就是发布地图消息，订阅激光雷达数据，并通过回调函数对其进行坐标转换，并启动线程发布该坐标转换，更新地图。因此比较重要的有2个函数：laserCallback()和publishLoop()。

laserCallback()

void
SlamGMapping::laserCallback(const sensor_msgs::LaserScan::ConstPtr& scan)
{
  laser_count_++;
  if ((laser_count_ % throttle_scans_) != 0)  // 判断是否降频
    return;

  static ros::Time last_map_update(0,0);

  // We can't initialize the mapper until we've got the first scan
  if(!got_first_scan_)  // 判断是否为第一帧，是则进入初始化地图
  {
    if(!initMapper(*scan))
      return;
    got_first_scan_ = true;
  }

  GMapping::OrientedPoint odom_pose;

  if(addScan(*scan, odom_pose))  // 第二帧及以后的数据
  {
    ROS_DEBUG("scan processed");

    GMapping::OrientedPoint mpose = gsp_->getParticles()[gsp_->getBestParticleIndex()].pose;
    ROS_DEBUG("new best pose: %.3f %.3f %.3f", mpose.x, mpose.y, mpose.theta);
    ROS_DEBUG("odom pose: %.3f %.3f %.3f", odom_pose.x, odom_pose.y, odom_pose.theta);
    ROS_DEBUG("correction: %.3f %.3f %.3f", mpose.x - odom_pose.x, mpose.y - odom_pose.y, mpose.theta - odom_pose.theta);

    // Transform坐标变换
    tf::Transform laser_to_map = tf::Transform(tf::createQuaternionFromRPY(0, 0, mpose.theta), tf::Vector3(mpose.x, mpose.y, 0.0)).inverse();
    tf::Transform odom_to_laser = tf::Transform(tf::createQuaternionFromRPY(0, 0, odom_pose.theta), tf::Vector3(odom_pose.x, odom_pose.y, 0.0));

    map_to_odom_mutex_.lock();
    map_to_odom_ = (odom_to_laser * laser_to_map).inverse();
    map_to_odom_mutex_.unlock();

    // 如果没有地图则直接更新地图，如果有地图则需要到时间了，才更新地图
    if(!got_map_ || (scan->header.stamp - last_map_update) > map_update_interval_)
    {
      updateMap(*scan);
      last_map_update = scan->header.stamp;
      ROS_DEBUG("Updated the map");
    }
  } else
    ROS_DEBUG("cannot process scan");
}

该函数输入参数为之前订阅的消息<sensor_msgs::LaserScan>，没有返回值。
该函数为处理激光雷达数据的回调函数，首先当接收到一帧激光雷达数据时，判断是否为第一帧数据，如果是则调用initMapper()初始化地图，否则调用addScan()将其数据插入到当前的地图中，如果到了地图更新的时间，则会调用updateMap()进行地图更新。

initMapper()

该函数为gmapping算法的初始化，该函数在收到的第一帧激光雷达数据的时候会被调用一次，之后就再也不会被调用了。该函数的功能主要就是对gmapping算法中需要的一些参数进行赋值，即：

判断激光雷达是否是水平放置的，如果不是则报错。
设激光雷达数据的角度是对称的&递增的，为每个激光束分配角度。
为gmapping算法设置各种需要的参数。

这里代码较长，且大部分为判断激光雷达的初始设置，有兴趣可以阅读参考博客[1-2]。

addScan()

bool
SlamGMapping::addScan(const sensor_msgs::LaserScan& scan, GMapping::OrientedPoint& gmap_pose)
{
  // 得到与激光雷达时间戳相对应的机器人里程计位姿
  if(!getOdomPose(gmap_pose, scan.header.stamp))
     return false;

  if(scan.ranges.size() != gsp_laser_beam_count_)
    return false;

  // GMapping wants an array of doubles...
  double* ranges_double = new double[scan.ranges.size()];
  // If the angle increment is negative, we have to invert the order of the readings.
  // 如果激光是反着装的，则激光的顺序需要反过来
  if (do_reverse_range_)
  {
    ROS_DEBUG("Inverting scan");
    int num_ranges = scan.ranges.size();
    for(int i=0; i < num_ranges; i++)
    {
      // Must filter out short readings, because the mapper won't
      if(scan.ranges[num_ranges - i - 1] < scan.range_min)
        ranges_double[i] = (double)scan.range_max;
      else
        ranges_double[i] = (double)scan.ranges[num_ranges - i - 1];
    }
  } else 
  {
    for(unsigned int i=0; i < scan.ranges.size(); i++)
    {
      // Must filter out short readings, because the mapper won't
      if(scan.ranges[i] < scan.range_min)
        ranges_double[i] = (double)scan.range_max;
      else
        ranges_double[i] = (double)scan.ranges[i];
    }
  }

  // 将ROS的激光雷达的数据信息转换为GMapping算法中的数据格式
  GMapping::RangeReading reading(scan.ranges.size(),
                                 ranges_double,
                                 gsp_laser_,
                                 scan.header.stamp.toSec());

  // ...but it deep copies them in RangeReading constructor, so we don't
  // need to keep our array around.
  delete[] ranges_double;

  // 设置和激光雷达数据时间戳相匹配的机器人位姿
  reading.setPose(gmap_pose);

  /*
  ROS_DEBUG("scanpose (%.3f): %.3f %.3f %.3f\n",
            scan.header.stamp.toSec(),
            gmap_pose.x,
            gmap_pose.y,
            gmap_pose.theta);
            */
  ROS_DEBUG("processing scan");

  // 调用GMapping算法进行处理
  return gsp_->processScan(reading);
}

该函数输入参数为之前订阅的消息<sensor_msgs::LaserScan>和自定义的数据类型OrientedPoint，具体定义在openslam_gmapping/include/gmapping/utils/point.h中，表示的是(x,y,$\theta$)，即当前点的x,y和偏航角。
首先通过getOdomPose()得到与激光雷达时间戳相对应的机器人里程计的位姿，然后将ROS中的激光雷达数据信息转换为Gmapping算法的数据格式，并调用setPose()设置其位姿，最后通过processScan()函数进行处理。
这里除了getOdomPose()是该.cpp文件中的函数，其余均是底层算法openslam_gmapping中的函数。
reading.setPose(gmap_pose);中的reading是GMapping::RangeReading类的实例化，该类的定义位于openslam_gmapping/include/gmapping/sensor/sensor_range/rangereading.h中。
gsp_->processScan(reading)中的gsp_的声明在slam_gmapping.h中，是GMapping::GridSlamProcessor类的实例化，该类的定义位于openslam_gmapping/include/gmapping/gridfastslam/gridslamprocessor.h。
这里最重要的当然是processScan()这个函数了，这也是整个Gmapping算法最核心的一个函数，这里先暂时跳过该函数，后面再详细介绍。

UpdateMap()

processScan()

参考博客

博客1
博客2