移动机器人激光SLAM导航（四）：GMapping SLAM 篇

参考引用

• GMapping ROS-Wiki
• 从零开始搭二维激光SLAM
• GMapping 漫谈
• 小白学移动机器人
• 机器人工匠阿杰
• wpr_simulation

移动机器人激光SLAM导航（文章链接汇总）

1. GMapping

1.1 FastSLAM 问题分解

概率论相关公式

• 1. 条件概率公式：$p(x,y)=p(x|y)p(y)=p(y|x)p(x)$
• 2. 贝叶斯公式：$p(x|y)=\frac{p(y|x)p(x)}{p(y)}=\eta p(y|x)p(x),\eta 为归一化常数$
• 3. 条件贝叶斯公式：$p(x|y,z)=\frac{p(y|x,z)p(x|z)}{p(y|z)}=\eta p(y|x,z)p(x|z)$
• 4. 条件联合概率公式：$p(x,y|z)=p(x|y,z)p(y|z)$

• FastSLAM 算法采用 RBPF（Rao-Blackwellized Particle Filters） 方法，将 SLAM 分解成两个问题：机器人定位问题 & 已知机器人定位进行地图构建问题。在定位问题中，每一个粒子只需要表示一个可能的姿态，但 SLAM 中的姿态和地图都是变量，所以一个粒子需要同时保存所有历史时刻的机器人位姿和整个地图
  • $p(x_{1:t}\mid z_{1:t},u_{1:t-1})$ ：利用上一时刻里程计数据和当前时刻的传感器观测来估计机器人的轨迹/位姿（定位）
  • $p(m\mid x_{1:t},z_{1:t})$ ：利用当前时刻的机器人的轨迹/位姿和传感器观测来估计环境地图（建图）
  • 通过上一个时刻的地图和运动模型预测当前的位姿，然后计算权重，即在当前位姿下得到当前观测的可能性，再进行重采样，进而更新粒子地图，如此往复

$$\begin{array}{c}p(x_{1:t},m\mid z_{1:t},u_{1:t-1})=\\ p(m\mid x_{1:t},z_{1:t})\cdot p(x_{1:t}\mid z_{1:t},u_{1:t-1}).\end{array}$$

1.2 GMapping 改善方案

1.2.1 改善提议分布，降低粒子数量

问题：建图对机器人位姿要求较高，对任意一个粒子，仅仅依靠运动模型采样（里程计）的结果构建地图，误差会非常大，通过运动模型得到的提议分布（proposal）太过于分散，与真实分布相差过大，则需要较多的粒子才能更好的表示机器人位姿的估计，这样将会造成内存使用与计算量急剧上升

• 方案 1：采用极大似然估计

  • 里程计的概率模型比较平滑，范围较大，如果对整个范围采样将需要很多的粒子，如果能找到一个位姿优值，在其周围进行小范围采样，就可以降低粒子数量
  • 因此直接采用极大似然估计的方式，根据粒子的位姿预测分布与地图的匹配程度，通过扫描匹配找到粒子的最优位姿参数，就用该位姿参数直接当做新粒子的位姿

• 方案 2：激光雷达的观测模型

  • 粒子滤波采用里程计运动模型作为提议分布，导致大部分粒子都偏离真实位置，而与运动模型相反，激光雷达的观测模型则可以给出一个相对集中的分布
  • 通过激光雷达最近一帧的观测（scan），把 proposal 分布限制在一个狭小的有效区域，然后再对 proposal 分布进行采样，这样可以用更少的粒子来覆盖机器人位姿的概率分布，从下图可以看到激光雷达观测模型方差较小
    

• 总结如下

  • 1、首先按照里程计运动模型给出预测，图 1 中所有粒子通过里程计运动模型预测出新的位置如图 2 所示
  • 2、以图 2 粒子的预测位置为初始值，对每一个粒子进行一次扫描匹配，通过扫描匹配得到该粒子的最优位姿如图 3，使得当前观测与该粒子所携带的地图最为贴合
  • 3、得到每个粒子的最优位姿后，再在每个粒子的附近撒播 k 个粒子，用高斯函数来模拟提议分布如图 4 所示，根据这 k 个粒子的里程计和观测模型来计算高斯分布均值和方差
  • 4、新的粒子从高斯分布中采样得到，对于每个粒子，还需要赋予一个权值，以供后续的重采样步骤使用
    

1.2.2 限制重采样次数，缓解粒子耗散

问题：GMapping 采用粒子滤波算法对移动机器人轨迹进行估计，必然少不了粒子重采样过程，但随着重采样次数的增多，会出现所有粒子都从一个粒子复制而来，粒子的多样性会消失

• 解决方案：自适应重采样
  • 重采样的目的是抛弃那些明显远离真实值的粒子，增强那些离真实值近的粒子
    • 如果所有的粒子都在真实值附近且分布均匀，那么就没有理由执行重采样
    • 在回环发生之前，即使有些粒子已经远离了真实值，但现有的观测不足以区分正确的粒子和错误的粒子，也没有理由执行重采样
    • 只有在回环发生之后，新的观测彻底拉开正确粒子与错误粒子权重差距，此时重采样才能有效
  • 根据所有粒子自身权重的离散程度（也就是权重方差）来决定是否进行粒子重采样的操作
    • Neff 越大，粒子的权重差距越小，当 Neff 小于某个阈值，说明粒子的权重差距较大，即粒子的分布与真实分布差距很大，在粒子层面表现为某些粒子离真实值很近，而很多粒子离真实值较远。这正是回环发生时经常出现的情况，此时就应该进行重采样，否则不进行采样

$$N_{\mathrm{eff}}=\frac{1}{{\sum }_{i=1}^N{\left({\tilde{w}}^{(i)}\right)}^2}$$

1.3 GMapping 解析

1.3.1 简介

• GMapping 是一个基于 2D 激光雷达使用 RBPF 算法完成二维栅格地图构建的 SLAM 算法
  • 通过里程计数据获取粒子群的先验位姿，再通过雷达数据与地图的匹配程度对所有粒子进行打分，通过分数高的粒子群来近似机器人的真实位姿
  • 作为 ROS 节点称为 slam_gmapping
• 硬件要求
  • 需要一个提供里程计数据并配备一个水平安装的、固定的 2D 激光雷达的移动机器人，slam_gmapping 节点将尝试将每次传入的扫描（scan）转化为 odom (里程计) tf 坐标系
• 优缺点
  • 可实时构建室内环境地图，在小场景中计算量少且地图精度较高，对激光雷达扫描频率要求较低
  • 不适合大场景建图

  对于 200 x 200 米的范围，如果栅格分辨率是 5cm，每个栅格占用一个字节内存，那么每个粒子携带的地图都要 16M 的内存，如果是 100 粒子就是 1.6G 内存

1.3.2 话题节点

• 订阅的话题（Topic）
  • /tf (tf/tfMessage)：订阅激光雷达坐标系、基坐标系和里程计坐标系之间的变换
  • /scan (sensor_msgs/LaserScan)：订阅 2D 激光雷达扫描数据
• 发布的话题（Topic）
  • map_metadata (nav_msgs/MapMetaData)：发布 Meta（元）地图数据
  • map (nav_msgs/OccupancyGrid)：发布占据栅格地图数据
  • ~entropy (std_msgs/Float64)：发布机器人定位的置信度（越大则定位错误的可能性越大）
• Service
  • dynamic_map (nav_msgs/GetMap)：获取地图数据

1.3.3 TF 变换

有关 TF 基础请查看往期博客：ROS学习5：ROS常用组件

• 必要的 TF 变换

  • → base_link：激光雷达坐标系 与 基坐标系 之间的变换，一般由 robot_state_publisher 或 static_transform_publisher 发布
  • base_link → odom：基坐标系 与 里程计坐标系 之间的变换，一般由里程计节点发布

• 发布的 TF 变换

  • map → odom：地图坐标系 与 里程计坐标系 之间的变换，估计机器人在地图 map 坐标系中的位姿

1.4 建图测试

本小节使用 移动机器人激光SLAM导航（二）：运动控制与传感器篇 中安装的测试环境 wpr_simulation（GMapping 已在此环境中通过脚本安装）

• 沿用 移动机器人激光SLAM导航（三）：Hector SLAM 篇 中 2.4 小节创建的功能包 slam_pkg，在 slam_pkg 中的 launch 文件夹中新建 gmapping.launch 文件

  <launch>
      <include file="$(find wpr_simulation)/launch/wpb_stage_robocup.launch" />
      <node pkg="gmapping" type="slam_gmapping" name="slam_gmapping" />
      <node pkg="rviz" type="rviz" name="rviz" args="-d $(find slam_pkg)/rviz/slam.rviz" />
      <node pkg="wpr_simulation" type="keyboard_vel_ctrl" name="keyboard_vel_ctrl" />
  launch>
  

• 编译并启动 gmapping.launch 建图

  $ cd ~/wpr_ws
  $ catkin_make
  $ source devel/setup.bash
  $ roslaunch slam_pkg gmapping.launch
  

• 机器人运动控制节点 keyboard_vel_ctrl

  #include 
  #include 
  #include 
  #include 
  
  static float linear_vel = 0.1;
  static float angular_vel = 0.1;
  static int k_vel = 3;
  
  int GetCh() {
      static struct termios oldt, newt;
      tcgetattr(STDIN_FILENO, &oldt);
      newt = oldt;
      newt.c_lflag &= ~(ICANON);
      tcsetattr(STDIN_FILENO, TCSANOW, &newt);
      int c = getchar();
      tcsetattr(STDIN_FILENO, TCSANOW, &oldt);
      return c;
  }
  
  int main(int argc, char** argv) {
      ros::init(argc, argv, "keyboard_vel_cmd");
  
      printf("键盘控制 WPR 机器人: \n");
      printf("w - 向前加速 \n");
      printf("s - 向后加速 \n");
      printf("a - 向左加速 \n");
      printf("d - 向右加速 \n");
      printf("q - 左旋加速 \n");
      printf("e - 右旋加速 \n");
      printf("空格 - 刹车 \n");
      printf("x - 退出 \n");
      printf("------------- \n");
  
      ros::NodeHandle n;
      ros::Publisher cmd_vel_pub = n.advertise<geometry_msgs::Twist>("/cmd_vel", 10);
  
      geometry_msgs::Twist base_cmd;
      base_cmd.linear.x = 0;
      base_cmd.linear.y = 0;
      base_cmd.angular.z = 0;
  
      while(n.ok()) {
        int cKey = GetCh();
        if (cKey == 'w') {
          base_cmd.linear.x += linear_vel;
          if (base_cmd.linear.x > linear_vel*k_vel)
              base_cmd.linear.x = linear_vel*k_vel;
          cmd_vel_pub.publish(base_cmd);
          printf(" - linear.x= %.2f linear.y= %.2f angular.z= %.2f \n",base_cmd.linear.x,base_cmd.linear.y,base_cmd.angular.z);
        } else if (cKey == 's') {
          base_cmd.linear.x += -linear_vel;
          if (base_cmd.linear.x < -linear_vel*k_vel)
              base_cmd.linear.x = -linear_vel*k_vel;
          cmd_vel_pub.publish(base_cmd);
          printf(" - linear.x= %.2f linear.y= %.2f angular.z= %.2f \n",base_cmd.linear.x,base_cmd.linear.y,base_cmd.angular.z);
        } else if (cKey == 'a') {
          base_cmd.linear.y += linear_vel;
          if (base_cmd.linear.y > linear_vel*k_vel)
              base_cmd.linear.y = linear_vel*k_vel;
          cmd_vel_pub.publish(base_cmd);
          printf(" - linear.x= %.2f linear.y= %.2f angular.z= %.2f \n",base_cmd.linear.x,base_cmd.linear.y,base_cmd.angular.z);
        } else if (cKey == 'd') {
          base_cmd.linear.y += -linear_vel;
          if (base_cmd.linear.y < -linear_vel*k_vel)
              base_cmd.linear.y = -linear_vel*k_vel;
          cmd_vel_pub.publish(base_cmd);
          printf(" - linear.x= %.2f linear.y= %.2f angular.z= %.2f \n",base_cmd.linear.x,base_cmd.linear.y,base_cmd.angular.z);
        }  else if (cKey == 'q') {
          base_cmd.angular.z += angular_vel;
          if (base_cmd.angular.z > angular_vel*k_vel)
              base_cmd.angular.z = angular_vel*k_vel;
          cmd_vel_pub.publish(base_cmd);
          printf(" - linear.x= %.2f linear.y= %.2f angular.z= %.2f \n",base_cmd.linear.x,base_cmd.linear.y,base_cmd.angular.z);
        } else if (cKey == 'e') {
            base_cmd.angular.z += -angular_vel;
            if (base_cmd.angular.z < -angular_vel*k_vel)
                base_cmd.angular.z = -angular_vel*k_vel;
            cmd_vel_pub.publish(base_cmd);
            printf(" - linear.x= %.2f linear.y= %.2f angular.z= %.2f \n",base_cmd.linear.x,base_cmd.linear.y,base_cmd.angular.z);
        } else if (cKey == ' ') {
            base_cmd.linear.x = 0;
            base_cmd.linear.y = 0;
            base_cmd.angular.z = 0;
            cmd_vel_pub.publish(base_cmd);
            printf(" - linear.x= %.2f linear.y= %.2f angular.z= %.2f \n",base_cmd.linear.x,base_cmd.linear.y,base_cmd.angular.z);
        } else if (cKey == 'x') {
            base_cmd.linear.x = 0;
            base_cmd.linear.y = 0;
            base_cmd.angular.z = 0;
            cmd_vel_pub.publish(base_cmd);
            printf(" - linear.x= %.2f linear.y= %.2f angular.z= %.2f \n",base_cmd.linear.x,base_cmd.linear.y,base_cmd.angular.z);
            printf("退出！ \n");
            return 0;
        } else {
            printf(" - 未定义指令 \n");
        }
        
      }
      
      return 0;
  }
  

1.5 建图参数设置 Parameters

<launch>
    <include file="$(find wpr_simulation)/launch/wpb_stage_robocup.launch" />
    <node pkg="gmapping" type="slam_gmapping" name="slam_gmapping">
        <param name="maxUrange" value="3.0" />
        <param name="map_update_interval" value="0.5" />
        <param name="linearUpdate" value="0.1" />
    node>
    <node pkg="rviz" type="rviz" name="rviz" args="-d $(find slam_pkg)/rviz/slam.rviz" />
    <node pkg="wpr_simulation" type="keyboard_vel_ctrl" name="keyboard_vel_ctrl" />
launch>

2. 地图的保存与加载

使用 map_server 软件包实现占据栅格地图的保存与加载

2.1 保存地图

• GMapping 建图

  $ cd ~/wpr_ws
  $ source devel/setup.bash
  $ roslaunch slam_pkg gmapping.launch
  

• 保存地图

  $ rosrun map_server map_saver -f mymap
  

  通过上述保存地图指令会在 /home 目录生成 mymap.pgm（地图图片）和 mymap.yaml（地图信息） 两个文件

• mymap.yaml

  image: mymap.pgm  # 到包含占用数据的图像文件的路径；可以是绝对的，也可以是相对于 YAML 文件的位置
  resolution: 0.050000   # 地图分辨率，单位：米/像素
  origin: [-100.000000, -100.000000, 0.000000]  # 地图中左下方像素的二维姿态为 (x, y, 偏航)，偏航为逆时针旋转(偏航 = 0表示无转动)
  negate: 0    # 白/黑的空闲/被占用 语义是否应该颠倒 (阈值的解释不受影响)
  occupied_thresh: 0.65  # 占据概率大于该阈值的像素被认为是完全占据的
  free_thresh: 0.196     # 占有率小于这个阈值的像素被认为是完全自由的
  

2.2 加载地图

• 加载地图

  $ roscore
  

  $ cd ~
  $ rosrun map_server map_server mymap.yaml
  

  $ rosrun rviz rviz
  

3. 2D 激光雷达运动畸变去除

• 详解2D激光雷达运动畸变去除