一起自学SLAM算法：8.1 Gmapping算法

连载文章，长期更新，欢迎关注：

写在前面

第1章-ROS入门必备知识

第2章-C++编程范式

第3章-OpenCV图像处理

第4章-机器人传感器

第5章-机器人主机

第6章-机器人底盘

第7章-SLAM中的数学基础

第8章-激光SLAM系统
    8.1 Gmapping算法

    8.2 Cartographer算法

    8.3 LOAM算法
第9章-视觉SLAM系统

第10章-其他SLAM系统

第11章-自主导航中的数学基础

第12章-典型自主导航系统

第13章-机器人SLAM导航综合实战

下面将从原理分析、源码解读和安装与运行这3个方面展开讲解Gmapping 算法。

8.1.1 Gmapping原理分析

首先要知道，Gmapping是一种基于粒子滤波的算法。在7.7.2节中已经提到过用RBPF（Rao-Blackwellization Particle Filter）这种粒子滤波器来求解SLAM问题，Fast-SLAM算法就是其典型实现之一。其中也有人基于RBPF来研究构建栅格地图（Grid Map）的SLAM算法，它就是ROS中大名鼎鼎的Gmapping算法。不过在Gmapping算法中，对RBPF的建议分布（proposal distribution）和重采样（resampling）进行了改进。下面就首先介绍RBPF的滤波过程，然后介绍对RBPF建议分布和重采样的改进，最后介绍使用改进RBPF滤波的过程，这些内容主要参考Gmapping算法的论文[1]。

1.RBPF的滤波过程

其实RBPF的思想就是将SLAM中的定位和建图问题分开来处理，如式（8-1）所示。也就是利用 $P(x_{1:t}|z_{1:t},u_{1:t-1})$ 首先估计出机器人的轨迹 $x_{1:k}$ ，然后在轨迹 $x_{1:t}$ 已知的情况下很容易估计出地图 $m$ 。

在给定机器人位姿的情况下，利用 $P(m|x_{1:t},z_{1:t})$ 进行建图很简单，可以参考文献[2]。所以，RBPF讨论的重点其实就是 $P(x_{1:t}|z_{1:t},u_{1:t-1})$ 定位问题的具体求解过程，一种流行的粒子滤波算法是SIR（sampling importance resampling）滤波器。那么，下面就来介绍基于SIR的RBPF滤波过程。

（1）采样

新的粒子点集 $\left \{ x_{t}^{(i)} \right \}$ 由上个时刻粒子点集 $\left \{ x_{t-1}^{(i)} \right \}$ 在建议分布 $\pi$ 里采样得到。通常把机器人的概率运动模型做为建议分布 $\pi$ ，这样新的粒子点集 $\left \{ x_{t}^{(i)} \right \}$ 的生成过程就可以表示成 $x_{t}^{(i)}\sim P(x_{t}|x_{t-1}^{(i)},u_{t-1})$ 。

（2）重要性权重

上面只是介绍了生成当前时刻粒子点集 $\left \{ x_{t}^{(i)} \right \}$ 的过程，考虑整个运动过程，机器人每条可能的轨迹都可以用一个粒子点 $x_{1:t}^{(i)}$ 表示，那么每条轨迹对应粒子点 $x_{1:t}^{(i)}$ 的重要性权重可以定义成式（8-2）所示的形式。其中分子是目标分布，分母是建议分布，重要性权重反映了建议分布与目标分布的差异性。

（3）重采样

新生成的粒子点需要利用重要性权重进行替换，这就是重采样。由于粒子点总量保持不变，当权重比较小的粒子点被删除后，权重大的粒子点需要进行复制以保持粒子点总量不变。经过重采样后粒子点的权重都变成一样，接着进行下一轮的采样和重采样。

（4）地图估计

在每条轨迹对应粒子点 $x_{1:t}^{(i)}$ 条件下，都可以用 $P(m^{(i)}|x_{1:t}^{(i)},z_{1:t})$ 计算出一幅地图 $m^{(i)}$ ，然后将每个轨迹计算出的地图整合就得到最终的地图 $m$ 。

从式（8-2）中可以发现一个明显的问题，不管当前获取到的观测 $z_{t}$ 是否有效，都要计算一遍整个轨迹对应的权重。随着时间的推移，轨迹将变得很长，这样每次还是计算一遍整个轨迹对应的权重，计算量将越来越大。可以将式（8-2）进行适当变形，推导出权重的递归计算方法，如式（8-3）所示。其实就是用贝叶斯准则和全概率公式将分子展开，用全概率公式将分母展开，然后利用贝叶斯网络中的条件独立性进一步化简，最后就得到了权重的递归计算形式。

值得注意的是，式（8-2）中的建议分布 $\pi$ 以及利用权重重采样的策略还是一个开放性话题。其实，Gmapping算法主要就是对该RBPF的建议分布和重采样策略进行了改进，下面就来具体讨论这两个改进。

2.RBPF的建议分布改进

式（8-3）中建议分布 $\pi$ 最直观的形式就是采用运动模型来计算，那么当前时刻粒子点集 $\left \{ x_{t}^{(i)} \right \}$ 的生成及对应权重的计算方式就变为式（8-4）所示。

不过直接采用运动模型做为建议分布，显然有问题。如图8-1所示，当观测数据可靠性比较低时（即观测分布的区间 $L^{(i)}$ 比较大），利用运动模型 $x_{t}^{(i)}\sim P(x_{t}|x_{t-1}^{(i)},u_{t-1})$ 采样生成的新粒子落在区间 $L^{(i)}$ 内的数量比较多；而当观测数据可靠性比较高时（即观测分布的区间 $L^{(i)}$ 比较小），利用运动模型 $x_{t}^{(i)}\sim P(x_{t}|x_{t-1}^{(i)},u_{t-1})$ 采样生成的新粒子落在区间 $L^{(i)}$ 内的数量比较少。由于粒子滤波是采用有限个粒子点近似表示连续空间的分布情况，所以观测分布的区间 $L^{(i)}$ 内粒子点较少时，会降低观测更新过程的精度。

图8-1 观测的可靠性

也就是说观测更新过程可以分2种情况来处理，当观测可靠性低时，采用式（8-3）所示默认的运动模型生成新粒子点集 $\left \{ x_{t}^{(i)} \right \}$ 及对应权重；当观测可靠性高时，就直接从观测分布的区间 $L^{(i)}$ 内采样，并将采样点集 $\left \{ x_{k} \right \}$ 的分布近似为高斯分布，利用点集 $\left \{ x_{k} \right \}$ 可以计算出该高斯分布的参数 $\mu _{t}^{(i)}$ 和 $\Sigma _{t}^{(i)}$ ，最后采用该高斯分布 $x_{t}^{(i)}\sim N(\mu _{t}^{(i)},\Sigma _{t}^{(i)})$ 采样生成新粒子点集 $\left \{ x_{t}^{(i)} \right \}$ 及对应权重。判断观测更新过程采用哪种方式很简单，首先利用运动模型推算出粒子点的新位姿 ${x'} _{t}^{(i)}$ ，然后在 ${x'} _{t}^{(i)}$ 附近区域搜索，计算观测 $z_{t}$ 与已有地图 $m_{t-1}^{(i)}$ 的匹配度，当搜索区域存在 $\hat{x}_{t}^{(i)}$ 使得匹配度很高时，就可以认为观测可靠性高，具体过程如式（8-5）所示。

那么，下面就具体讨论一下观测可靠性高的情况。观测分布的区间 $L^{(i)}$ 的范围可以定义成 $L^{(i)}=\left \{ x|P(z_{t}|m_{t-1}^{(i)},x)>\varepsilon \right \}$ ，搜索出的匹配度最高的位姿点 $\hat{x}_{t}^{(i)}$ 其实就是区间 $L^{(i)}$ 概率峰值的地方。首先以 $\hat{x}_{t}^{(i)}$ 为中心的区域内随机采固定数量的 $K$ 个点 $\left \{ x_{k} \right \}$ ，其中每个点的采样如式（8-6）所示。

将采样点集 $\left \{ x_{k} \right \}$ 的分布近似为高斯分布，并将运动和观测信息都考虑进来，就可以通过点集 $\left \{ x_{k} \right \}$ 计算该高斯分布的参数 $\mu _{t}^{(i)}$ 和 $\Sigma _{t}^{(i)}$ ，如式（8-7）所示。

因此，新粒子点集 $\left \{ x_{t}^{(i)} \right \}$ 将通过从高斯分布 $x_{t}^{(i)}\sim N(\mu _{t}^{(i)},\Sigma _{t}^{(i)})$ 中采样生成，而式（8-3）中建议分布 $\pi$ 采用改进建议分布 $P(x_{t}^{(i)}|m_{t-1}^{(i)},x_{t-1}^{(i)},z_{t},u _{t-1})$ 来计算，那么当前时刻粒子点集 $\left \{ x_{t}^{(i)} \right \}$ 的生成及对应权重的计算方式就变为式（8-8）所示。在原论文[1]的推导中，变量 $x_{t}^{(i)}$ 书写中存在缺少上标 $(i)$ 的错误，这里都予以了更正。

3.RBPF的重采样改进

生成新的粒子点集 $\left \{ x_{t}^{(i)} \right \}$ 及对应权重后，就可以进行重采样了。如果每更新一次粒子点集 $\left \{ x_{t}^{(i)} \right \}$ ，都要利用权重进行重采样的话，当粒子点权重在更新过程中变化不是特别大，或者由于噪声使得某些坏粒子点比好粒子点的权重还要大时，此时执行重采样就会导致好粒子点的丢失。所以在执行重采样前，必须要确保其有效性，改进的重采样策略通过式（8-9）所示参数来衡量有效性。其中 $\widetilde{w}^{(i)}$ 是粒子的归一化权重，当建议分布与目标分布之间的近似度高时，各个粒子点的权重都很相近；而当建议分布与目标分布之间的近似度低时，各个粒子点的权重差异较大。也就是说可以用某个阈值来判断参数 $N_{eff}$ 的有效性，当 $N_{eff}$ 小于阈值时就执行重采样，否则跳过重采样。

** 4.改进RBPF的滤波过程**

介绍完建议分布和重采样的改进后，这里就可以引出用改进RBPF实现Gmapping算法的整个流程了，见代码清单8-1所示的改进RBPF算法伪代码。

代码清单8-1 改进RBPF算法伪代码

第1行，算法的输入是 $S_{t-1}$ ， $z_{t}$ ， $u_{t-1}$ ，算法的输出是 $S_{t}$ 。

第8~10行，是观测可靠性低时，生成新粒子点及权重的过程。

第11~31行，是观测可靠性高时，生成新粒子点及权重的过程。

第33行，为每个粒子点计算其对应的一幅地图。

第37~40行，是重采样。

8.1.2 Gmapping源码解读

上面讨论完Gmapping的原理，现在就来解读Gmapping的源码。Gmapping是ROS中非常著名的开源功能包，本书以melodic版本的Gmapping代码进行讲解，其代码框架如图8-2所示。可以看出Gmapping算法用了2个ROS功能包来组织代码，分别为slam_gmapping功能包和openslam_gmapping功能包。其中slam_gmapping功能包用于实现算法的ROS相关接口，其实slam_gmapping本身没有实质性的内容是一个元功能包，具体实现被放在其所包含的gmapping功能包中。单线激光雷达数据通过/scan话题输入gmapping功能包，里程计数据通过/tf关系输入gmapping功能包，gmapping功能包通过调用openslam_gmapping功能包中的建图算法，将构建好的地图发布到/map等话题。而openslam_gmapping功能包用于实现建图核心算法，也就是8.1.1中提到的粒子滤波的具体过程实现。

图8-2 Gmapping代码框架

在解读具体代码之前，先介绍一下程序运行过程中的调用流程，便于大家从整体上认识代码。程序调用主要流程如图8-3所示，其实主要就是涉及到SlamGMapping和GridSlamProcessor这2个类。其中SlamGMapping类在gmapping功能包中实现，GridSlamProcessor类在openslam_gmapping功能包中实现，而GridSlamProcessor类以成员变量的形式被SlamGMapping类调用。程序main()函数很简单，就是创建了一个SlamGMapping类的对象gn。然后，SlamGMapping类的构造函数会自动调用init()函数执行初始化，包括创建GridSlamProcessor类的对象gsp_和设置Gmapping算法参数。接着，调用SlamGMapping类的startLiveSlam()函数，就可以进行在线SLAM建图了。startLiveSlam()函数首先对建图过程所需要的ROS订阅和发布话题进行了创建，然后开启双线程进行工作。其中laserCallback线程在激光雷达数据的驱动下，对雷达数据进行处理并更新地图，其中调用到的GridSlamProcessor类的processScan函数就是代码清单8-1所示改进RBPF算法伪代码的具体实现；而publishLoop线程负责维护map->odom之间的tf关系。

图8-3 Gmapping程序调用流程

由于篇幅限制，下面就以代码的主要调用为线索，摘录关键代码进行解读，为了便于阅读，摘录出的代码保持原有的行号不变。首先来看gmapping功能包中src/main.cpp里面的mian()函数，见代码清单8-2所示。

代码清单8-2 main()函数

32 #include <ros/ros.h>
33 
34 #include "slam_gmapping.h"
35 
36 int
37 main(int argc, char** argv)
38 {
39   ros::init(argc, argv, "slam_gmapping");
40 
41   SlamGMapping gn;
42   gn.startLiveSlam();
43   ros::spin();
44 
45   return(0);
46 }

从main()函数可以看出，其实就是创建了SlamGMapping类的对象gn，SlamGMapping类的构造函数会自动调用init()函数执行初始化，包括创建GridSlamProcessor类的对象gsp_和设置Gmapping算法参数。接着，调用SlamGMapping类的startLiveSlam()函数，就可以进行在线SLAM建图。

而SlamGMapping类在gmapping功能包中src/slam_gmapping.h和slam_gmapping.cpp中实现，下面就来分析该类的init()函数和startLiveSlam()函数。先来看init()函数，见代码清单8-3所示。

代码清单8-3 init()函数

167 void SlamGMapping::init()
168 {
...
173   gsp_ = new GMapping::GridSlamProcessor();
...
187   // Parameters used by our GMapping wrapper
188     if(!private_nh_.getParam("throttle_scans", throttle_scans_))
189       throttle_scans_ = 1;
190     if(!private_nh_.getParam("base_frame", base_frame_))
191       base_frame_ = "base_link";
192     if(!private_nh_.getParam("map_frame", map_frame_))
193       map_frame_ = "map";
194     if(!private_nh_.getParam("odom_frame", odom_frame_))
195       odom_frame_ = "odom";
196   
197     private_nh_.param("transform_publish_period", transform_publish_period_, 0.05);
198   
199     double tmp;
200     if(!private_nh_.getParam("map_update_interval", tmp))
201       tmp = 5.0;
202     map_update_interval_.fromSec(tmp);
203     
204     // Parameters used by GMapping itself
205     maxUrange_ = 0.0;  maxRange_ = 0.0; // preliminary default, will be set in initMapper()
206     if(!private_nh_.getParam("minimumScore", minimum_score_))
207       minimum_score_ = 0;
208     if(!private_nh_.getParam("sigma", sigma_))
209       sigma_ = 0.05;
210     if(!private_nh_.getParam("kernelSize", kernelSize_))
211       kernelSize_ = 1;
212     if(!private_nh_.getParam("lstep", lstep_))
213       lstep_ = 0.05;
214     if(!private_nh_.getParam("astep", astep_))
215       astep_ = 0.05;
216     if(!private_nh_.getParam("iterations", iterations_))
217       iterations_ = 5;
218     if(!private_nh_.getParam("lsigma", lsigma_))
219       lsigma_ = 0.075;
220     if(!private_nh_.getParam("ogain", ogain_))
221       ogain_ = 3.0;
222     if(!private_nh_.getParam("lskip", lskip_))
223       lskip_ = 0;
224     if(!private_nh_.getParam("srr", srr_))
225       srr_ = 0.1;
226     if(!private_nh_.getParam("srt", srt_))
227       srt_ = 0.2;
228     if(!private_nh_.getParam("str", str_))
229       str_ = 0.1;
230     if(!private_nh_.getParam("stt", stt_))
231       stt_ = 0.2;
232     if(!private_nh_.getParam("linearUpdate", linearUpdate_))
233       linearUpdate_ = 1.0;
234     if(!private_nh_.getParam("angularUpdate", angularUpdate_))
235       angularUpdate_ = 0.5;
236     if(!private_nh_.getParam("temporalUpdate", temporalUpdate_))
237       temporalUpdate_ = -1.0;
238     if(!private_nh_.getParam("resampleThreshold", resampleThreshold_))
239       resampleThreshold_ = 0.5;
240     if(!private_nh_.getParam("particles", particles_))
241       particles_ = 30;
242     if(!private_nh_.getParam("xmin", xmin_))
243       xmin_ = -100.0;
244     if(!private_nh_.getParam("ymin", ymin_))
245       ymin_ = -100.0;
246     if(!private_nh_.getParam("xmax", xmax_))
247       xmax_ = 100.0;
248     if(!private_nh_.getParam("ymax", ymax_))
249       ymax_ = 100.0;
250     if(!private_nh_.getParam("delta", delta_))
251       delta_ = 0.05;
252     if(!private_nh_.getParam("occ_thresh", occ_thresh_))
253       occ_thresh_ = 0.25;
254     if(!private_nh_.getParam("llsamplerange", llsamplerange_))
255       llsamplerange_ = 0.01;
256     if(!private_nh_.getParam("llsamplestep", llsamplestep_))
257       llsamplestep_ = 0.01;
258     if(!private_nh_.getParam("lasamplerange", lasamplerange_))
259       lasamplerange_ = 0.005;
260     if(!private_nh_.getParam("lasamplestep", lasamplestep_))
261       lasamplestep_ = 0.005;
262       
263     if(!private_nh_.getParam("tf_delay", tf_delay_))
264       tf_delay_ = transform_publish_period_;
265   
266   }

第173行，创建GridSlamProcessor类的对象gsp_，该对象的processScan()函数将在laserCallback线程中的addScan()函数中被调用。

第187~202行，设置Gmapping算法ROS接口参数，主要传感器数据的frame_id名称，在解析tf关系中的数据时要用到。

第204~264行，设置Gmapping算法参数，这些参数直接跟粒子滤波过程相关。开发者可以结合自己的实际应用场景，调节这些参数以改善算法运行性能。

在SlamGMapping类的构造函数自动调用init()函数执行初始化后，通过调用SlamGMapping类的startLiveSlam()函数，就可以进行在线SLAM建图。下面来看startLiveSlam()函数，见代码清单8-4所示。

代码清单8-4 startLiveSlam()函数

269 void SlamGMapping::startLiveSlam()
270 {
271   entropy_publisher_ = private_nh_.advertise<std_msgs::Float64>("entropy", 1, true);
272   sst_ = node_.advertise<nav_msgs::OccupancyGrid>("map", 1, true);
273   sstm_ = node_.advertise<nav_msgs::MapMetaData>("map_metadata", 1, true);
274   ss_ = node_.advertiseService("dynamic_map", &SlamGMapping::mapCallback, this);
275   scan_filter_sub_ = new message_filters::Subscriber<sensor_msgs::LaserScan>(node_, "scan", 5);
276   scan_filter_ = new tf::MessageFilter<sensor_msgs::LaserScan>(*scan_filter_sub_, tf_, odom_frame_, 5);
277   scan_filter_->registerCallback(boost::bind(&SlamGMapping::laserCallback, this, _1));
278 
279   transform_thread_ = new boost::thread(boost::bind(&SlamGMapping::publishLoop, this, transform_publish_period_));
280 }

第271~274行，创建发布器，包括话题entropy、map、map_metadata和服务dynamic_map。

第275~276行，是使用message_filters同步机制订阅激光雷达话题/scan和里程计tf，保证获取到的这2种话题数据时间上同步。

第277行，创建laserCallback线程，该线程由激光雷达数据和里程计tf同步数据驱动。也就是每到来一帧传感器数据，laserCallback线程中的逻辑被执行一次。

第279行，创建publishLoop线程，该线程负责维护map->odom之间的tf关系。

其实算法的核心部分在laserCallback线程中实现，所以下面详细介绍一下laserCallback()线程函数，见代码清单8-5所示。

代码清单8-5 laserCallback()线程函数

609 void
610 SlamGMapping::laserCallback(const sensor_msgs::LaserScan::ConstPtr& scan)
611 {
...
618   // We can't initialize the mapper until we've got the first scan
619   if(!got_first_scan_)
620   {
621     if(!initMapper(*scan))
622       return;
623     got_first_scan_ = true;
624   }
625 
626   GMapping::OrientedPoint odom_pose;
627 
628   if(addScan(*scan, odom_pose))
629   {
...
644     if(!got_map_ || (scan->header.stamp - last_map_update) > map_update_interval_)
645     {
646       updateMap(*scan);
647       last_map_update = scan->header.stamp;
648       ROS_DEBUG("Updated the map");
649     }
650   } else
651     ROS_DEBUG("cannot process scan");
652 }

第621行，当laserCallback()线程函数第一次被执行，在第一帧数据到来时调用initMapper()函数对建图算法进行初始化，包括算法参数初始化、地图初始化、机器人位姿粒子点初始化等。

第628行，调用addScan()函数对激光雷达数据进行处理，addScan()函数中调用了GridSlamProcessor类的processScan()函数。而processScan()函数就是代码清单8-1所示改进RBPF算法伪代码的具体实现，可以说processScan()函数实现了粒子滤波的具体过程，包括drawFromMotion、scanMatch和resample这三个主要步骤。

第646行，调用updateMap()函数，利用当前雷达扫描数据对地图进行更新。

最后，publishLoop线程就比较简单，该线程负责维护map->odom之间的tf关系，通过循环调用publishTransform()函数发布map->odom之间的tf关系。publishLoop()线程函数，见代码清单8-6所示。

代码清单8-6 publishLoop()线程函数

352 void SlamGMapping::publishLoop(double transform_publish_period){
353   if(transform_publish_period == 0)
354     return;
355 
356   ros::Rate r(1.0 / transform_publish_period);
357   while(ros::ok()){
358     publishTransform();
359     r.sleep();
360   }
361 }

第357~360行，按照指定的频率循环执行publishTransform()函数，其实publishTransform()函数的功能就是发布map->odom之间的tf关系。

关于Gmapping源码中的更多内容，感兴趣的读者可以自行阅读，这里就不在详细展开讲解了。

8.1.3 Gmapping安装与运行

学习完Gmapping算法的原理及源码之后，大家肯定迫不及待想亲自安装运行一下Gmapping体验一下真实效果。在第1章中已经声明过，本书在Ubuntu18.04和ROS melodic环境下进行讨论。不管是使用X86主机、X86主机虚拟机还是ARM主机，一旦装好Ubuntu18.04系统后，就可以在该系统上安装ROS melodic发行版了。如果你只是想利用数据集离线跑算法，可以选择在X86主机或X86主机虚拟机上运行Ubuntu18.04和ROS melodic，关于这一部分的环境搭建请参考1.2.1节的内容。如果需要在实际机器人上在线跑算法，可以选择在ARM主机上运行Ubuntu18.04和ROS melodic，关于这一部分的环境搭建请参考第5章的内容。所以，下面的讨论假设Ubuntu18.04和ROS melodic环境已经准备妥当了。

1.Gmapping安装

在上面Gmapping源码解读中已经提过，Gmapping算法用了2个ROS功能包来组织代码，分别为slam_gmapping功能包和openslam_gmapping功能包。而大多数受ROS系统默认支持的功能包可以用2种方式进行安装，一种是直接用像安装系统程序一样的apt install的方式安装指定的ROS功能包，这种方式安装的程序直接以可执行文件的方式存在；而学习开发算法为目的的话，就需要以另一种方式来安装该ROS功能包，也就是直接下载该ROS功能包的源码到用户的ROS工作空间，然后手动编译安装，这种方式允许开发者随时修改源码并编译执行。

首先，需要准备好ROS工作空间，关于ROS工作空间的构建，在1.2.2节中已经讨论过了，因此这里不再赘述。

然后，安装Gmapping的依赖库，网上介绍了很多装依赖库的方法，但后续过程往往还是会出现缺少依赖的错误，这里介绍一种彻底解决依赖问题的巧妙方法。先用apt install的方式将slam_gmapping和openslam_gmapping装上，这样系统在安装过程中会自动装好相应的依赖。然后用apt remove将slam_gmapping和openslam_gmapping卸载但保留其依赖，这样就巧妙的将所需依赖都装好了。

#安装openslam_gmapping和slam_gmapping功能包及其依赖
sudo apt install ros-melodic-openslam-gmapping ros-melodic-gmapping
#卸载openslam_gmapping和slam_gmapping功能包但保留其依赖
sudo apt remove ros-melodic-openslam-gmapping ros-melodic-gmapping

接下来，就可以下载slam_gmapping和openslam_gmapping功能包的源码到工作空间编译安装了。

#切换到工作空间目录
cd ~/catkin_ws/src/
#下载slam_gmapping功能包源码
git clone https://github.com/ros-perception/slam_gmapping.git
cd slam_gmapping
#查看代码版本是否为molodic，如果不是请使用git checkout命令切换到对应版本
git branch
#下载openslam_gmapping功能包源码
git clone https://github.com/ros-perception/openslam_gmapping.git
cd  openslam_gmapping
#同样查看代码版本是否为molodic，如果不是请使用git checkout命令切换到对应版本
git branch
#编译
cd ~/catkin_ws/
catkin_make -DCATKIN_WHITELIST_PACKAGES="openslam_gmapping"
catkin_make -DCATKIN_WHITELIST_PACKAGES="gmapping"

安装上面的方法就完成了Gmapping的安装了，可以看出slam_gmapping元功能包中默认就包含了gmaping功能包，而我们手动再将openslam_gmapping功能包也下载到了slam_gmapping元功能包中。由于slam_gmapping属于元功能包不需要编译，只需要分别对其中包含的openslam_gmapping功能包和gmaping功能包进行编译就行了。

在完成Gmapping安装后，可以先用Gmapping官方数据数据集测试一下安装是否成功。这里使用basic_localization_stage_indexed.bag这个数据集进行测试，下载地址如下。将该数据集下载到本地目录，然后启动gmapping并播放数据集就行了。

#用默认launch文件启动gmapping
roslaunch gmapping slam_gmapping_pr2.launch

再打开一个命令行终端，播放basic_localization_stage_indexed.bag数据集。

#切换到数据集存放目录
cd ~/Downloads/
#播放数据集
rosbag play basic_localization_stage_indexed.bag

再打开一个命令行终端，启动rviz可视化工具。

#启动rviz
rviz

在rviz中订阅地图话题/map，如果能看到如图8-4所示的地图，那么就说明Gmapping安装成功了，到这里可以关闭所有命令行终端的程序了。

图8-4 Gmapping数据集测试效果

2.Gmapping在线运行

如果想要深入研究算法，并把算法应用到实际项目中，推荐将算法安装到机器人上在线运行。因此，你首先需要拥有一台能做实验的机器人底盘，机器人底盘由底盘运动学模型、传感器、主机、软硬件系统架构等构成。如果是机器人初学者，建议直接购买市场上成熟的底盘来学习，等掌握了底盘的软硬件各项功能原理后，再根据自己的能力和需求设计自己的底盘。

由于本书讨论的内容具有非常高的广度和深度，一方面要从硬件原理、硬件驱动、核心算法、应用层多个维度系统地讨论整个SLAM导航机器人的架构；另一方面还要结合SLAM导航数学理论对各种开源算法进行解读和实战。由于市场上购买到的底盘普遍存在软硬件接口不完全开放、算法兼容性等问题，所以为了迎合本书的整个写作思路，我自己从底盘运动学模型、传感器、主机、软硬件系统架构设计入手搭建了一台完全开放的机器人底盘，为了后续表述方便，我给它取了个名字叫“xiihoo机器人”。自己搭建的机器人使用起来非常便利，硬件接口可以根据需要轻松修改，传感器驱动程序可以随时优化升级，主机操作系统通过配置可以很方便地优化各项性能，移植各种开源SLAM算法非常友好。如果读者也想要按照本书一样搭建自己的底盘，可以参考第4~6章的内容，已经对整个底盘搭建过程进行了详细讨论。

因此，下面的讨论将假设已经搭建好了“xiihoo机器人”的前提下展开。通过图8-2可以知道，运行Gmapping需要机器人提供激光雷达数据和传感器之间的tf关系。在“xiihoo机器人”中，激光雷达数据通过ydlidar驱动包发布，雷达数据这里发布在话题/scan中，雷达数据帧中的frame_id这里设置为了base_laser_link，通过下面的命令启动“xiihoo机器人”中的激光雷达。

#启动激光雷达
roslaunch ydlidar my_x4.launch

当然如果读者朋友使用自己搭建的机器人也是可以的，启动对应的雷达驱动节点就行了，不过要注意雷达数据所发布的话题名和雷达数据帧中的frame_id要与下面的设置保持一致。

而传感器之间的tf关系分为动态tf关系和静态tf关系。在“xiihoo机器人”中，xiihoo_bringup驱动包负责发布底盘里程计话题/odom以及相对于的odom->base_footprint之间的动态tf关系，同时还负责接收话题/cmd_vel的控制命令来驱动底盘电机运动。通过下面的命令启动“xiihoo机器人”中的xiihoo_bringup驱动包。

#启动底盘
roslaunch xiihoo_bringup minimal.launch

在“xiihoo机器人”中，xiihoo_description包通过urdf的方式发布静态tf关系。静态tf关系其实就是底盘上安装的激光雷达、IMU、底盘中心等之间的坐标相对关系，由于这些坐标关系在机器人运行中不会发生变化，所以就是静态tf关系。这里只关心底盘中心base_link、轮式编码器中心base_footprint、激光雷达中心base_laser_link等传感器之间的静态tf关系。安装在“xiihoo机器人”上的所有传感器都在xiihoo_description包中通过urdf设置好了其与底盘的静态tf关系，通过下面的命令启动“xiihoo机器人”中的xiihoo_description包就行了。

#启动底盘urdf描述
roslaunch xiihoo_description xiihoo_description.launch

这样，运行Gmapping所需要的输入数据就准备就绪了，接下来通过在launch文件中对Gmapping算法中的参数进行配置并启动建图。对于一个开源算法，开发者大多数情况下不会去直接修改算法源码，而是通过调整算法中的可配置参数使其能达到实际应用环境的性能指标，也就是常说的调参。

关于Gmapping参数配置的具体内容，请直接参考wiki官方教程。有些极少使用的参数并没有在wiki官方教程中给出，有需要的读者可以通过查阅源码了解这些未给出参数的使用方法。当然，大部分参数并不需要调整，所以往往只将需要调整的参数在launch文件中进行显式配置，而其他不必配置的参数使用默认值就行了。在目录slam_gmapping/gmapping/launch/中新建文件slam_gmapping_xiihoo.launch，文件内容见代码清单8-7所示。

代码清单8-7 slam_gmapping_xiihoo.launch文件内容

  1 <launch>
  2     <!--param name="use_sim_time" value="true"/-->
  3 
  4     <node pkg="gmapping" type="slam_gmapping" name="slam_gmapping" output="screen">
  5       <remap from="scan" to="/scan"/>
  6       <param name="base_frame" value="base_footprint"/>
  7       <param name="map_frame" value="map"/>
  8       <param name="odom_frame " value="odom"/>
  9 
 10       <param name="map_update_interval" value="5.0"/>
 11       <param name="maxUrange" value="16.0"/>
 12       <param name="sigma" value="0.05"/>
 13       <param name="kernelSize" value="1"/>
 14       <param name="lstep" value="0.05"/>
 15       <param name="astep" value="0.05"/>
 16       <param name="iterations" value="5"/>
 17       <param name="lsigma" value="0.075"/>
 18       <param name="ogain" value="3.0"/>
 19       <param name="lskip" value="0"/>
 20       <param name="srr" value="0.1"/>
 21       <param name="srt" value="0.2"/>
 22       <param name="str" value="0.1"/>
 23       <param name="stt" value="0.2"/>
 24       <param name="linearUpdate" value="1.0"/>
 25       <param name="angularUpdate" value="0.5"/>
 26       <param name="temporalUpdate" value="3.0"/>
 27       <param name="resampleThreshold" value="0.5"/>
 28       <param name="particles" value="30"/>
 29       <param name="xmin" value="-1.0"/>
 30       <param name="ymin" value="-1.0"/>
 31       <param name="xmax" value="1.0"/>
 32       <param name="ymax" value="1.0"/>
 33       <param name="delta" value="0.05"/>
 34       <param name="llsamplerange" value="0.01"/>
 35       <param name="llsamplestep" value="0.01"/>
 36       <param name="lasamplerange" value="0.005"/>
 37       <param name="lasamplestep" value="0.005"/>
 38     </node>
 39 </launch>

第2行，是用数据集离线运行算法时需要开启的参数，这里用不到，直接注释掉了。

第4行，是启动ROS节点的标准格式，每一个ROS节点都是通过pkg名称和type名称进行标识的。

第5行，是对算法订阅的话题名称进行重映射。当算法订阅的话题与传感器驱动发布的话题不一致时，这个重映射就能解决这种不一致问题。重映射其实就是对算法订阅的话题名进行重命名而已。

第6~8行，是对算法中用到的一些tf关系所涉及frame_id名称的设置。底盘通常以base_footprint为坐标系名称，地图通常以map为坐标系名称，轮式里程计通常以odom为坐标系名称。

第10~37行，这些参数是与Gmapping算法粒子滤波过程直接相关的参数，要结合粒子滤波原理进行理解。由于wiki官方教程已经进行了详细介绍，这里就不展开了。

当然，还有极少数的参数并没有在上面的launch文件中进行配置，了解更多参数配置请参考wiki官方教程以及源码。到这里，只需要通过上面的launch文件就能轻松启动Gmapping进行地图构建了。

#启动建图
roslaunch gmapping slam_gmapping_xiihoo.launch

接下来，就可以遥控机器人在环境中移动，进行地图构建了。不同的机器人支持不同的遥控方法，比如手柄遥控、手机APP遥控、键盘遥控等。这里使用键盘遥控方式来遥控“xiihoo机器人”建图，键盘启动命令如下。

#首次使用键盘遥控，需要先安装对应功能包
sudo apt install ros-melodic-teleop-twist-keyboard
#启动键盘遥控
rosrun teleop_twist_keyboard teleop_twist_keyboard.py

在键盘遥控程序终端下，通过对应的按键就能控制底盘移动了。这里介绍一下按键的映射关系，前进（i）、后退（，）、左转（j）、右转（l），而增加和减小线速度对应按键w和x，增加和减小角速度对应按键e和c。

遥控底盘建图的过程中，可以打开rviz可视化工具查看所建地图的效果以及机器人实时估计位姿等信息。如图8-5所示，是“xiihoo机器人”在线建图的效果。

图8-5 Gmapping在线建图效果

其实到这里，Gmapping在线运行就全部讲完了。最后，回过头来再总结一下整个过程。可以借助rqt可视化工具，查看建图过程中ROS节点之间的数据流向以及tf状态。

#查看ROS节点数据流向
rosrun rqt_graph rqt_graph
#查看tf状态
rosrun rqt_tf_tree rqt_tf_tree

其中，Gmapping建图过程中ROS节点之间的数据流向，如图8-6所示。键盘遥控节点通过话题/cmd_vel与底盘控制节点通信，底盘控制节点将编码里程计解析后通过动态/tf输入给Gmapping建图节点，激光雷达节点通过话题/scan将数据输入给Gmapping建图节点，urdf解析节点将底盘各个传感器坐标系关系通过静态/tf_static输入给Gmapping建图节点。而Gmapping建图节点利用这些输入数据进行建图，并将地图发布到对应的话题，同时输出map->odom之间的动态tf关系到/tf。

图8-6 ROS节点数据流向

在图8-7中，可以更详细的看到整个建图过程的tf状态。其中激光雷达与底盘之间的静态tf关系为base_footprint->base_laser_link，由urdf解析节点维护；轮式里程计提供的动态tf关系为odom->base_footprint，由底盘控制节点维护；而地图与轮式里程计之间的动态tf关系map->odom，则由Gmapping建图节点维护。可以看出，Gmapping建图节点所维护的map->odom的tf关系，其实就是轮式里程计累积误差的动态修正量。至于其他的一些tf关系，与底盘上其他传感器有关，目前还用不到，故不用关心。

图8-7 tf状态

3.Gmapping离线运行

在调某个参数的时候，需要需要在复现场景下多次建图，这个时候将底盘上的数据录制成数据集，然后离线运行就很有用了。同时，对于一些刚接触机器人的初学者，在没有实体机器人做实验的情况下，用数据集在电脑上离线跑算法也是可以的。

先将上面Gmapping在线运行中的/scan、/tf和/tf_static录制成数据集，直接使用rosbag record命令录制就行了。假设录制好的数据集文件名叫gmapping_xiihoo.bag，需要这个数据集的读者朋友可在评论区留言。

#录制数据集
rosbag record /scan /tf /tf_static

只需要将代码清单8-7中第2行的注释打开，然后启动该launch文件，并播放数据集就行了。

#打开use_sim_time参数，启动建图
roslaunch gmapping slam_gmapping_xiihoo.launch
#播放数据集
rosbag play  gmapping_xiihoo.bag

源码仓库

Github下载：github.com/xiihoo/Books_Robot_SLAM_Navigation
Gitee下载（国内访问速度快）：gitee.com/xiihoo-robot/Books_Robot_SLAM_Navigation

参考文献

【1】张虎，机器人SLAM导航核心技术与实战[M]. 机械工业出版社，2022.

标签：算法人工智能机器人

本文转载自: https://blog.csdn.net/m0_68732180/article/details/128747383
版权归原作者 机器人研究猿 所有，如有侵权，请联系我们删除。

一起自学SLAM算法：8.1 Gmapping算法

8.1.1 Gmapping原理分析

8.1.2 Gmapping源码解读

8.1.3 Gmapping安装与运行

源码仓库

参考文献

发表评论

“一起自学SLAM算法：8.1 Gmapping算法”的评论:

关于作者

overfit同步小助手

相关阅读

文章导航