【里程计在激光雷达SLAM中的作用】【gmapping算法hector_mapping算法】

提示：详情可参考index.ros.org

一、初识odom（odometry 里程计）

在建图过程中map和base_footprint（机器人地盘到地面的投影正中心）中间隔了一个odom，其作用是什么？

启动ros
roscore

启动wpr_simulation的建图仿真程序
roslaunch wpr_simulation wpb_corridor_hector.launch
机器人的运动环境变成了平直的长廊；
发现rviz里的机器人走到一半停下了，TF定位出现了问题

换个建图方式(gmapping方法)
roslaunch wpr_simulation wpb_corridor_gmapping.launch
建图任务近似完成；

机器人两侧只有平滑的墙面，缺少可以作为位移参照物的特征，无法估算自己的位移；
通过里程变化得到走过的距离，实际中通过电机转速计算机器人位移的方法就是激光雷达SLAM中常用的里程计；
里程计是一种软件算法而不是一种硬件设备，这个算法会运行在机器人的驱动节点中，根据电机的转动数据计算机器人的位移信息，该信息会以TF消息包的形式发布到话题/tf中；

激光雷达SLAM输出的是map到base_footprint的TF；
里程计的输出是odom到bse_foootprint；
odom即单词odometry（里程计）的简写

通过里程计的信息就能推断出机器人当前的位置，不会受到参照物特征的误导；
SLAM通过雷达点云和参照物配准实现定位功能；
那么可以用里程计来取代SLAM的上述定位功能吗？
实际上，里程计存在误差，比如机器人轮胎打滑，里程计的定位信息就会和实际上的定位信息不相符，此外这种误差会不断累积，使得定位误差越来越大，此时需要使用障碍物点云配准算法来修正里程计的误差；

gmapping的核心算法：
先使用里程计推算机器人的位移，然后通过雷达点云贴合障碍物轮廓，修正里程计误差的方法；

让base_footprint的位置和scanmatcher的位置保持一致；

二、gmapping与hector mapping两种SLAM算法的最大区别

在gmapping中，机器人的位移主要由里程计推算，激光雷达点云障碍物配准算法只是为了修正里程计出现的误差；（map->odom->base_footprint）

而hector mapping在定位过程中不会用到里程计里的数据,只使用雷达点云和障碍物配准的方法来进行定位；（map->base_footprint）
为了rviz里能显示地图和机器人的模型，输出了一段map->odom的TF去抵消不断增长的里程计TF，以此让机器人的base_footprint和scanmatcher_frame的位置始终保持一致；

通过仿真分析hector mapping的建图

运行hector mapping建图
roslaunch wpr_simulation wpb_corridor_hector.launch

在rviz中添加TF显示，显示比例Marker Scale调节为5，坐标系只保留map和scanmatcher_frame;

给机器人一个初始速度；

刚开始的时候，障碍物点云配准的算法还能正常定位，当机器人进入走廊形状的道路时，激光雷达只能扫描到两侧的墙壁，rviz中机器人只获取到了左右两排平行的点云，导致障碍物和点云配准的结果认为机器人没有往前移动，误以为是雷达数据的噪声导致定位在一个小范围内抖动；

将rviz中的TF显示的Frames选项切换到odom和map；
给机器人一个初始速度；

仿真机器人向正前方行驶，里程计不断增加，而rviz里的机器人并未向前移动，此时rviz里输出了一段与机器人运动方向相反的TF；
结果分析：
里程计的这段TF（odom到bse_foootprint）是在不断变长的，而hector mapping会输出一段反向增长的TF来抵消里程计的变化，目的是为了让base_footprint（机器人模型的底盘投影中心）的位置始终和scanmatcher_frame保持一致，这样就可以让rviz里的机器人模型显示在scanmatcher_frame的位置了，由此可知hector mapping对里程计数据的处理只考虑了机器人在rviz里的显示，对于定位的问题没有考虑；

通过仿真分析gmapping建图

运行gmapping建图
roslaunch wpr_simulation wpb_corridor_gmapping.launch

在rviz里添加TF显示，调整TF显示大小（Mark Scale设置为5），坐标系只保留base_footprint、odom、map；

给机器人一个初始速度；

机器人刚出发时，map和odom的坐标系是基本重合的，说明此时里程计的误差比较小；
当机器人进入两侧墙体平滑的走廊时，map和odom的位置开始分离，并且出现了明显的跳动，这里不是里程计的误差变大导致的，而是激光雷达点云和障碍物配准导致的定位跳动；
之后激光雷达和障碍物配准的定位功能基本失效（类似hector mapping），此时由于里程计还在运转，尽管误差还在累积，但是机器人的定位还是在继续推进的；
由此可见，在里程计的作用下激光雷达SLAM有效克服了建图过程中位移特征缺失的问题，有效避免了建图过程中的中断，最终的建图结果和实际场地基本保持一致，建图任务基本完成；

三、实际系统中的里程计

实际的机器人系统中，里程计的形式可能是多种多样的，SLAM输出的定位信息也可以另一个SLAM系统所需的里程计，无论是什么样的形式，里程计的思想即：利用不同形式的定位方法去克服某种单一SLAM算法的缺陷，减少误差或者增加稳定性；

四、gmapping的作用

gmapping运行的需求列表：
雷达坐标系=>base_link
base_lingk=>odom
/scan话题

启动模拟场景(模拟RoboCup@Home家庭机器人比赛的一个场景)
roslaunch wpr_simulation wpb_stage_robocup.launch

rostopic list
rostopic echo /scan --noarr
rosrun rqt_tf_tree rqt_tf_tree

运行gmapping
rosrun gmapping slam_gmapping

启动rviz
rosrun rviz rviz

设置rviz界面
添加机器人模型显示；
添加激光雷达LaserScan测距点显示，话题名称为/scan；
添加地图显示，话题为/map；

使用键盘控制控制机器人移动，完成建图:
rosrun wpr_simulation keyboard_vel_ctrl
键盘控制WPR机器人：
w - 向前加速
s - 向后加速
a - 向左加速
d - 向右加速
q - 左旋加速
e - 右旋加速
空格 - 刹车
x - 退出

五、launch文件的创建与编写

新建软件包装载launch文件：
cd catkin_ws/src/
新建slam_pkg包（之前已经新建过了就不必执行下列指令）
catkin_create_pkg slam_pkg roscpp rospy std_msgs
在vscode中找到slam_pkg,新建launch文件夹;

在上述launch文件夹中新建gmapping.launch文件
将下列指令编写到gmapping.launch文件
roslaunch wpr_simulation wpb_stage_robocup.launch
rosrun gmapping slam_gmapping
rosrun rviz rviz
rosrun wpr_simulation keyboard_vel_ctrl

<launch>
    <include file="$(fine wpr_simulation)/launch/wpb_stage_robocup.launch/">

    <node pkg="gmapping" type="slam_gmapping" name="slam_gmapping"/>

    <node pkg="rviz" type="rviz" name="rviz"/>   

    <node pkg="wpr_simulation" type="keyboard_vel_ctrl" name="keyboard_vel_ctrl"/>
    
</launch>

保存，编译slam_pkg软件包
cd ~/catking_ws/
catking_make

运行刚刚编写的gmapping.launch文件
roslaunch slam_pkg gmapping.launch

rviz界面按照前面的配置
设置rviz界面
添加机器人模型显示；
添加激光雷达LaserScan测距点显示，话题名称为/scan；
添加地图显示，话题为/map；

将rviz设置保存成配置文件：
File=>Save Config As
将配置文件保存到新建的slam_pkg目录下的rviz子目录下，命名为gmapping.rviz

推出当前ros程序，可以查看当前配置是否生效：
在gmapping.launch文件里新增rviz相关参数

<launch>
    <include file="$(fine 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/gmapping.rviz"/>   

    <node pkg="wpr_simulation" type="keyboard_vel_ctrl" name="keyboard_vel_ctrl"/>

</launch>

在终端执行：
roslaunch slam_pkg gmapping.launch

六、gmapping参数设置

gmapping参数分类

接口相关（与仿真或者实体机器人对接的相关参数）
base_frame（默认base_link）底盘坐标系名称
map_frame(默认map) 地图坐标系名称
odom_frame(默认odom_link) 里程计坐标系名称

性能相关
maxRange 激光雷达射线的最大采纳距离
throttle_scans (默认 1) 激光雷达数据跳帧处理
map_update_interval (默认5秒) 地图更新的时间间隔

算法相关
sigma (默认0.05) 粒子滤波器的噪声标准差
iterations (默认5) 粒子滤波器的优化迭代次数
occ_thresh(默认0.25) 占据栅格的阈值参数

gmapping相关luanch文件的修改

接口参数修改的目的：为了让gmapping兼容机器人的TF树；

性能参数修改的目的：为了让gmapping在略微损失一些精度的情况下在低性能的系统上流畅运行起来；
性能相关参数可以划分为四组类型：
1.地图尺寸
xmin（默认-100.0米）地图X轴的负向边界
xmax（默认100.0米）地图X轴的正向边界
ymin（默认-100.0米）地图Y轴的负向边界
ymax（默认100.0米）地图的Y轴正向边界
delta（默认0.05米/格）栅格地图的分辨率
其中地图尺寸越大，占用内存越大；
delta越小，同样大小的地图划分的栅格数越多，占用的内存越多；

2.激光雷达
maxUrange 激光雷达射线的最大有效距离
maxRange 激光雷达射线的最大采纳距离
lskip（默认0）激光雷达扫描的跳线处理
throttle_scans（默认1）激光雷达数据跳帧处理
其中超出maxUrange半径范围外的扫描区域会被裁剪掉；
长度超过maxRange的距离会被抛弃，不能参与建图运算；
lskip=1表示跳过一个雷达扫描数据数据，进行等间隔采样，这样可以有效减少测距值的处理数量，从而降低算力消耗；
跳帧处理表示gmapping接收到一定数据后只对最后一帧数据进行处理，例如throttle_scans=5表示5帧数据只取第5帧数据（最后到来的一帧数据）进行处理，其它数据则会被抛弃，循环往复；

3.地图更新
map_update_interval（默认5秒）地图更新的时间间隔
linearUpdate（默认1.0米）地图更新的移动距离阈值
angularUpdate（默认0.5弧度）地图更新的旋转角度阈值
其中只有当机器人移动超过linearUpdate的阈值的时候地图才会更新，更新会受到地图更新时间间隔的限制 （该参数的优先级最高）；

4.定位相关
particles（默认30）滤波器的粒子数
resampleThreshold（默认0.5）粒子重采样阈值
其中particles、resampleThreshold数值越大对算力消耗越大，建图过程中遇到卡顿可以降低该值；

在gmapping.launch文件中修改如下：

<node pkg="gmapping" type="slam_gmapping" name="slam_gmapping">
	<param name="maxUrange" value="3.0"/>
</node>

roslaunch slam_pkg gmapping.launch

七、地图的保存与加载

ROS中有个map_server的软件包

rosrun map_server map_saver
默认保存为map.pgm与map.yaml文件

地图保存

先完成slam建图(执行前面编写好的gmapping.launch文件)
roslaunch slam_pkg gmapping.launch

把地图保存在主文件夹里
rosrun map_server map_saver -f 文件名字
例如rosrun map_server map_saver -f map
主目录下会生成 map.pgm和map.yaml文件

地图文件内容简介

map.pgm是地图的图片；
map.yaml文件记录了地图的参数，包含：
1.image：map.pgm 即地图图片名称描述；

2.resolution:0.050000 即地图分辨率，单位是米/像素；

3.origin:[-100.000000,-100.000000,0.000000] 地图左下角像素的坐标、map.pgm地图相对于map坐标系的偏转角度；

4.negate:0 Whether the white/black free/occupied semantics should be reversed(interpretation of thresholds is unaffected)

5.occupied_thresh:0.65 Pixels with occupancy probability greater than this threshold are considered completely occupied.

6.free_thresh:0.196 Pixels with occupancy probability less than this threshold are considered completely free.

地图的加载

map_server是一个ros节点，专门负责将磁盘里储存的地图文件加载到ROS中；
加载地图指令示例：rosrun 包名称 节点名称 地图文件
rosrun map_server map_server mymap.yaml

启动ros
roscore

加载地图
rosrun map_server map_server map.yaml

使用rviz查看地图
rosrun rviz rviz
在rviz中Add-Map，topic默认填写/map

总结

简单描述了里程计在激光雷达SLAM中的作用，比较了gmapping算法&hector_mapping算法的区别，了解了ROS中的地图是如何创建、保存、加载的。