机器人导航(仿真)(一)——SLAM建图

导航实现01_SLAM建图

参考视频：【奥特学园】ROS机器人入门课程《ROS理论与实践》零基础教程_哔哩哔哩_bilibili

参考文档：http://www.autolabor.com.cn/book/ROSTutorials/

SLAM算法有多种，当前我们选用gmapping，后续会再介绍其他几种常用的SLAM实现。

1.gmapping简介

gmapping 是ROS开源社区中较为常用且比较成熟的SLAM算法之一，gmapping可以根据移动机器人里程计数据和激光雷达数据来绘制二维的栅格地图，对应的，gmapping对硬件也有一定的要求:

• 该移动机器人可以发布里程计消息
• 机器人需要发布雷达消息(该消息可以通过水平固定安装的雷达发布，或者也可以将深度相机消息转换成雷达消息)

关于里程计与雷达数据，仿真环境中可以正常获取的，不再赘述，栅格地图如案例所示。

gmapping 安装前面也有介绍，命令如下:

sudo apt install ros-<ROS版本>-gmapping

2.gmapping节点说明

gmapping 功能包中的核心节点是:slam_gmapping。为了方便调用，需要先了解该节点订阅的话题、发布的话题、服务以及相关参数。

2.1订阅的Topic

tf (tf/tfMessage)

• 用于雷达、底盘与里程计之间的坐标变换消息。

scan(sensor_msgs/LaserScan)

• SLAM所需的雷达信息。

2.2发布的Topic

map_metadata(nav_msgs/MapMetaData)

• 地图元数据，包括地图的宽度、高度、分辨率等，该消息会固定更新。

map(nav_msgs/OccupancyGrid)

• 地图栅格数据，一般会在rviz中以图形化的方式显示。

~entropy(std_msgs/Float64)

• 机器人姿态分布熵估计(值越大，不确定性越大)。

2.3服务

dynamic_map(nav_msgs/GetMap)

• 用于获取地图数据。

2.4参数

~base_frame(string, default:"base_link")

• 机器人基坐标系。

~map_frame(string, default:"map")

• 地图坐标系。

~odom_frame(string, default:"odom")

• 里程计坐标系。

~map_update_interval(float, default: 5.0)

• 地图更新频率，根据指定的值设计更新间隔。

~maxUrange(float, default: 80.0)

• 激光探测的最大可用范围(超出此阈值，被截断)。

~maxRange(float)

• 激光探测的最大范围。

.... 参数较多，上述是几个较为常用的参数，其他参数介绍可参考官网。

2.5所需的坐标变换

雷达坐标系→基坐标系

• 一般由 robot_state_publisher 或 static_transform_publisher 发布。

基坐标系→里程计坐标系

• 一般由里程计节点发布。

2.6发布的坐标变换

地图坐标系→里程计坐标系

• 地图到里程计坐标系之间的变换。

3.gmapping使用

3.1编写gmapping节点相关launch文件

新建功能包

名字为 nav_demo ，输入依赖包如下：

gmapping  map_server  amcl  move_base

新建 launch 文件夹，再新建 nav01_slam.launch 文件

launch文件编写可以参考 github 的演示 launch文件：https://github.com/ros-perception/slam_gmapping/blob/melodic-devel/gmapping/launch/slam_gmapping_pr2.launch

复制并修改如下:

<launch>
    <!--仿真环境下，将该参数设置为 true-->
    <param name="use_sim_time" value="true"/>
    <!--gmapping 节点-->
    <node pkg="gmapping" type="slam_gmapping" name="slam_gmapping" output="screen">
      <!--设置雷达话题-->
      <remap from="scan" to="scan"/>
      <!-- 关键参数：坐标系-->
      <param name="base_frame" value ="base_footprint" />
      <param name="map_frame" value ="map" />
      <param name="odom_frame" value ="odom" />
 
 
      <param name="map_update_interval" value="5.0"/>
      <param name="maxUrange" value="16.0"/>
      <param name="sigma" value="0.05"/>
      <param name="kernelSize" value="1"/>
      <param name="lstep" value="0.05"/>
      <param name="astep" value="0.05"/>
      <param name="iterations" value="5"/>
      <param name="lsigma" value="0.075"/>
      <param name="ogain" value="3.0"/>
      <param name="lskip" value="0"/>
      <param name="srr" value="0.1"/>
      <param name="srt" value="0.2"/>
      <param name="str" value="0.1"/>
      <param name="stt" value="0.2"/>
      <param name="linearUpdate" value="1.0"/>
      <param name="angularUpdate" value="0.5"/>
      <param name="temporalUpdate" value="3.0"/>
      <param name="resampleThreshold" value="0.5"/>
      <param name="particles" value="30"/>
      <param name="xmin" value="-50.0"/>
      <param name="ymin" value="-50.0"/>
      <param name="xmax" value="50.0"/>
      <param name="ymax" value="50.0"/>
      <param name="delta" value="0.05"/>
      <param name="llsamplerange" value="0.01"/>
      <param name="llsamplestep" value="0.01"/>
      <param name="lasamplerange" value="0.005"/>
      <param name="lasamplestep" value="0.005"/>
    </node>
    <node pkg="joint_state_publisher" name="joint_state_publisher" type="joint_state_publisher" />
    <node pkg="robot_state_publisher" name="robot_state_publisher" type="robot_state_publisher" />
    <node pkg="rviz" type="rviz" name="rviz"  />
</launch>

关键代码解释：

<remap from="scan" to="scan"/><!-- 雷达话题 -->
<param name="base_frame" value="base_footprint"/><!--底盘坐标系-->
<param name="odom_frame" value="odom"/> <!--里程计坐标系-->

执行

1.先启动 Gazebo 仿真环境(此过程略)

2.然后再启动地图绘制的 launch 文件:

roslaunch 包名 launch文件名

3.启动键盘键盘控制节点，用于控制机器人运动建图

rosrun teleop_twist_keyboard teleop_twist_keyboard.py

4.在 rviz 中添加组件，显示栅格地图

具体如下：

新建两个窗口，启动 gazebo

cys@ubuntu:~/demo05_ws$ source ./devel/setup.bash
cys@ubuntu:~/demo05_ws$ roslaunch urdf02_gazebo demo03_env.launch

再输入如下命令启动rviz

cys@ubuntu:~/demo05_ws$ source ./devel/setup.bash
cys@ubuntu:~/demo05_ws$ roslaunch nav_demo nav01_slam.launch

新建命令行，输入命令，让机器人运动

rosrun teleop_twist_keyboard teleop_twist_keyboard.py

最后，就可以通过键盘控制gazebo中的机器人运动，同时，在rviz中可以显示gmapping发布的栅格地图数据了，下一步，还需要将地图单独保存。

 4.地图保存

在 launch 里新建 nav02_map_save.launch

<launch>
    <arg name="filename" value="$(find nav_demo)/map/nav" />
    <node name="map_save" pkg="map_server" type="map_saver" args="-f $(arg filename)" />
</launch>

在功能包 nav_demo下新建 文件夹 map，运行 

cys@ubuntu:~/demo05_ws$ source ./devel/setup.bash
cys@ubuntu:~/demo05_ws$ roslaunch nav_demo nav02_map_save.launch 

 在指定路径下会生成两个文件，xxx.pgm 与 xxx.yaml，打开 nav.pgm 查看图

  

 4.地图读取

在 launch 里新建 nav03_map_server.launch

<launch>
 
    <arg name="map" default="nav.yaml" />
    <node pkg="map_server" type="map_server" name="map_server" args="$(find nav_demo)/map/$(arg map)" />
 
 </launch>

运行，就将地图信息发不出去了

cys@ubuntu:~/demo05_ws$ source ./devel/setup.bash
cys@ubuntu:~/demo05_ws$ roslaunch nav_demo nav03_map_server.launch 

再运行 rviz ，在命令行输入 rviz ，增加 Map， Topic 为 /map

 地图 yaml 参数详解

#1.声明地图图片资源的路径
image: /home/cys/demo05_ws/src/nav_demo/map/nav.pgm
#2.地图刻度尺，单位是 米/像素
resolution: 0.050000
#3.地图的相对位姿(按照右手坐标系，地图右下角相对于rviz中的原点的位姿)
#值1：x方向上的偏移量
#值2：y方向上的偏移量
#值3：地图的偏航角度(单位是弧度)
origin: [-50.000000, -50.000000, 0.000000]
 
# 地图中的障碍物判断：
# 最终地图结果：白色是可通行区域，黑色是障碍物，蓝灰是未知区域
# 判断规则：
# 1.地图中的每个像素都有取值[0,255] 白色：255  黑色：0
# 2.根据像素值计算一个比例：p = (255-x)/255 白色：0  黑色1
# 3.判断是否是障碍物，p>occupied_thresh 就是障碍物，p<free_thresh 就是无物，可以自由通行
 
#4.去反
negate: 0
#5.占用阈值
occupied_thresh: 0.65
#6.空闲阈值
free_thresh: 0.196