建图 SLAM

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

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

编写gmapping节点相关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" args="-d $(find nav_demo)/config/nav_test.rviz"/>
</launch>

执行

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

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

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

rosrun teleop_twist_keyboard teleop_twist_keyboard.py

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

地图服务 map_server

map_server功能包中提供了两个节点： map_saver 和 map_server，前者用于将栅格地图保存到磁盘，后者读取磁盘的栅格地图并以服务的方式提供出去。

地图保存节点 map_server

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>

实现过程：

1. 参考上一节，依次启动仿真环境，键盘控制节点与SLAM节点；

2. 通过键盘控制机器人运动并绘图；

3. 通过上述地图保存方式保存地图。

结果： 在指定路径下会生成两个文件，xxx.pgm 与 xxx.yaml

xxx.pgm 本质是一张图片，直接使用图片查看程序即可打开。

xxx.yaml 保存的是地图的元数据信息，用于描述图片，内容格式如下:

image: /home/chenyikeng/URDF_ws/src/nav_demo/map/nav.pgm
resolution: 0.050000
origin: [-50.000000, -50.000000, 0.000000]
negate: 0
occupied_thresh: 0.65
free_thresh: 0.196

解释:

• image：被描述的图片资源路径，可以是绝对路径也可以是相对路径。

• resolution：图片分片率（单位: m/像素）。

• origin: 地图中左下像素的二维姿势，为（x，y，偏航），偏航为逆时针旋转（偏航= 0表示无旋转）。

• occupied_thresh: 占用概率大于此阈值的像素被视为完全占用。

• free_thresh: 占用率小于此阈值的像素被视为完全空闲。

• negate: 是否应该颠倒白色/黑色自由/占用的语义。

map_server 中障碍物计算规则:

地图中的每一个像素取值在 [0,255] 之间，白色为 255，黑色为 0，该值设为 x；

map_server 会将像素值作为判断是否是障碍物的依据，首先计算比例: p = (255 - x) / 255.0，白色为0，黑色为1(negate为true，则p = x / 255.0)；

根据步骤2计算的比例判断是否是障碍物，如果 p > occupied_thresh 那么视为障碍物，如果 p < free_thresh 那么视为无物。

地图服务 map_server

通过 map_server 的 map_server 节点可以读取栅格地图数据，编写 launch 文件如下:

<launch><!-- 设置地图的配置文件 --><arg name="map" default="nav.yaml" /><!-- 运行地图服务器，并且加载设置的地图--><node name="map_server" pkg="map_server" type="map_server" args="$(find nav_demo)/map/$(arg map)"/>
</launch>

其中参数是地图描述文件的资源路径，执行该launch文件，该节点会发布话题:map(nav_msgs/OccupancyGrid)

结果如下：

定位 amcl

所谓定位就是推算机器人自身在全局地图中的位置。当然，SLAM中也包含定位算法实现，不过SLAM的定位是用于构建全局地图的，是属于导航开始之前的阶段，而当前定位是用于导航中。

导航中，机器人需要按照设定的路线运动，通过定位可以判断机器人的实际轨迹是否符合预期。在ROS的导航功能包集navigation中提供了 amcl 功能包，用于实现导航中的机器人定位。

里程计本身也是可以协助机器人定位的，不过里程计存在累计误差且一些特殊情况时(车轮打滑)会出现定位错误的情况，amcl 则可以通过估算机器人在地图坐标系下的姿态，再结合里程计提高定位准确度。

• 里程计定位：只是通过里程计数据实现 /odom_frame 与 /base_frame 之间的坐标变换。
• amcl定位：可以提供 /map_frame 、/odom_frame 与 /base_frame 之间的坐标变换。

编写amcl节点相关的launch文件

<launch><node pkg="amcl" type="amcl" name="amcl" output="screen"><!-- Publish scans from best pose at a max of 10 Hz --><param name="odom_model_type" value="diff"/><!-- 里程计模式为差分 --><param name="odom_alpha5" value="0.1"/><param name="transform_tolerance" value="0.2" /><param name="gui_publish_rate" value="10.0"/><param name="laser_max_beams" value="30"/><param name="min_particles" value="500"/><param name="max_particles" value="5000"/><param name="kld_err" value="0.05"/><param name="kld_z" value="0.99"/><param name="odom_alpha1" value="0.2"/><param name="odom_alpha2" value="0.2"/><!-- translation std dev, m --><param name="odom_alpha3" value="0.8"/><param name="odom_alpha4" value="0.2"/><param name="laser_z_hit" value="0.5"/><param name="laser_z_short" value="0.05"/><param name="laser_z_max" value="0.05"/><param name="laser_z_rand" value="0.5"/><param name="laser_sigma_hit" value="0.2"/><param name="laser_lambda_short" value="0.1"/><param name="laser_lambda_short" value="0.1"/><param name="laser_model_type" value="likelihood_field"/><!-- <param name="laser_model_type" value="beam"/> --><param name="laser_likelihood_max_dist" value="2.0"/><param name="update_min_d" value="0.2"/><param name="update_min_a" value="0.5"/><param name="odom_frame_id" value="odom"/><!-- 里程计坐标系 --><param name="base_frame_id" value="base_footprint"/><!-- 添加机器人基坐标系 --><param name="global_frame_id" value="map"/><!-- 添加地图坐标系 --><param name="resample_interval" value="1"/><param name="transform_tolerance" value="0.1"/><param name="recovery_alpha_slow" value="0.0"/><param name="recovery_alpha_fast" value="0.0"/></node>
</launch>

编写测试launch文件

amcl节点是不可以单独运行的，运行 amcl 节点之前，需要先加载全局地图，然后启动 rviz 显示定位结果，上述节点可以集成进launch文件

<launch><!-- 设置地图的配置文件 --><arg name="map" default="nav.yaml" /><!-- 运行地图服务器，并且加载设置的地图--><node name="map_server" pkg="map_server" type="map_server" args="$(find nav_demo)/map/$(arg map)"/><!-- 启动AMCL节点 --><include file="$(find nav_demo)/launch/nav04_amcl.launch" /><!-- 运行rviz --><node pkg="rviz" type="rviz" name="rviz"/><node pkg="joint_state_publisher" type="joint_state_publisher" name="joint_state_publisher" output="screen" /><node pkg="robot_state_publisher" type="robot_state_publisher" name="robot_state_publisher" output="screen" />
</launch>

执行

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

2. 启动键盘控制节点：

rosrun teleop_twist_keyboard teleop_twist_keyboard.py

3. 启动上一步中集成地图服务、amcl 与 rviz 的 launch 文件；

4. 在启动的 rviz 中，添加RobotModel、Map组件，分别显示机器人模型与地图，添加 posearray 插件，设置topic为particlecloud来显示 amcl 预估的当前机器人的位姿，箭头越是密集，说明当前机器人处于此位置的概率越高；

5. 通过键盘控制机器人运动，会发现 posearray 也随之而改变。
   

路径规划 move_base

move_base 功能包提供了基于动作(action)的路径规划实现，move_base 可以根据给定的目标点，控制机器人底盘运动至目标位置，并且在运动过程中会连续反馈机器人自身的姿态与目标点的状态信息。如前所述，move_base主要由全局路径规划与本地路径规划组成。

move_base与代价地图

SLAM构建的地图在导航中是不可以直接使用的，因为：

1. SLAM构建的地图是静态地图，而导航过程中，障碍物信息是可变的，可能障碍物被移走了，也可能添加了新的障碍物，导航中需要时时的获取障碍物信息；
2. 在靠近障碍物边缘时，虽然此处是空闲区域，但是机器人在进入该区域后可能由于其他一些因素，比如：惯性、或者不规则形体的机器人转弯时可能会与障碍物产生碰撞，安全起见，最好在地图的障碍物边缘设置警戒区，尽量禁止机器人进入…

所以，静态地图无法直接应用于导航，其基础之上需要添加一些辅助信息的地图，比如时时获取的障碍物数据，基于静态地图添加的膨胀区等数据。因此有了代价地图。

代价地图有两张：global_costmap(全局代价地图) 和 local_costmap(本地代价地图)，前者用于全局路径规划，后者用于本地路径规划。

两张代价地图都可以多层叠加,一般有以下层级:

• Static Map Layer：静态地图层，SLAM构建的静态地图。

• Obstacle Map Layer：障碍地图层，传感器感知的障碍物信息。

• Inflation Layer：膨胀层，在以上两层地图上进行膨胀（向外扩张），以避免机器人的外壳会撞上障碍物。

• Other Layers：自定义costmap。

多个layer可以按需自由搭配。

碰撞算法

在ROS中计算代价值的方法如下：

上图中，横轴是距离机器人中心的距离，纵轴是代价地图中栅格的灰度值。

• 致命障碍：栅格值为254，此时障碍物与机器人中心重叠，必然发生碰撞；
• 内切障碍：栅格值为253，此时障碍物处于机器人的内切圆内，必然发生碰撞；
• 外切障碍：栅格值为[128,252]，此时障碍物处于其机器人的外切圆内，处于碰撞临界，不一定发生碰撞；
• 非自由空间：栅格值为(0,127]，此时机器人处于障碍物附近，属于危险警戒区，进入此区域，将来可能会发生碰撞；
• 自由区域：栅格值为0，此处机器人可以自由通过；
• 未知区域：栅格值为255，还没探明是否有障碍物。

膨胀空间的设置可以参考非自由空间。

move_base使用

路径规划算法在move_base功能包的move_base节点中已经封装完毕了，但是还不可以直接调用，因为算法虽然已经封装了，但是该功能包面向的是各种类型支持ROS的机器人，不同类型机器人可能大小尺寸不同，传感器不同，速度不同，应用场景不同…最后可能会导致不同的路径规划结果，那么在调用路径规划节点之前，我们还需要配置机器人参数。具体实现如下:

1. 先编写launch文件模板
2. 编写配置文件
3. 集成导航相关的launch文件
4. 测试

launch文件

调用move_base节点：

<launch><node pkg="move_base" type="move_base" respawn="false" name="move_base" output="screen" clear_params="true"><rosparam file="$(find nav_demo)/param/costmap_common_params.yaml" command="load" ns="global_costmap" /><rosparam file="$(find nav_demo)/param/costmap_common_params.yaml" command="load" ns="local_costmap" /><rosparam file="$(find nav_demo)/param/local_costmap_params.yaml" command="load" /><rosparam file="$(find nav_demo)/param/global_costmap_params.yaml" command="load" /><rosparam file="$(find nav_demo)/param/base_local_planner_params.yaml" command="load" /></node>
</launch>

启动了 move_base 功能包下的 move_base 节点，respawn 为 false，意味着该节点关闭后，不会被重启；clear_params 为 true，意味着每次启动该节点都要清空私有参数然后重新载入；通过 rosparam 会载入若干 yaml 文件用于配置参数

配置文件

costmap_common_params.yaml

#机器人几何参，如果机器人是圆形，设置 robot_radius,如果是其他形状设置 footprint
robot_radius: 0.12 #圆形
# footprint: [[-0.12, -0.12], [-0.12, 0.12], [0.12, 0.12], [0.12, -0.12]] #其他形状obstacle_range: 3.0 # 用于障碍物探测，比如: 值为 3.0，意味着检测到距离小于 3 米的障碍物时，就会引入代价地图
raytrace_range: 3.5 # 用于清除障碍物，比如：值为 3.5，意味着清除代价地图中 3.5 米以外的障碍物#膨胀半径，扩展在碰撞区域以外的代价区域，使得机器人规划路径避开障碍物
inflation_radius: 0.2
#代价比例系数，越大则代价值越小
cost_scaling_factor: 3.0#地图类型
map_type: costmap
#导航包所需要的传感器
observation_sources: scan
#对传感器的坐标系和数据进行配置。这个也会用于代价地图添加和清除障碍物。例如，你可以用激光雷达传感器用于在代价地图添加障碍物，再添加kinect用于导航和清除障碍物。
scan: {sensor_frame: laser, data_type: LaserScan, topic: scan, marking: true, clearing: true}

global_costmap_params.yaml

global_costmap:global_frame: map #地图坐标系robot_base_frame: base_footprint #机器人坐标系# 以此实现坐标变换update_frequency: 1.0 #代价地图更新频率publish_frequency: 1.0 #代价地图的发布频率transform_tolerance: 0.5 #等待坐标变换发布信息的超时时间static_map: true # 是否使用一个地图或者地图服务器来初始化全局代价地图，如果不使用静态地图，这个参数为false.

local_costmap_params.yaml

local_costmap:global_frame: odom #里程计坐标系robot_base_frame: base_footprint #机器人坐标系update_frequency: 10.0 #代价地图更新频率publish_frequency: 10.0 #代价地图的发布频率transform_tolerance: 0.5 #等待坐标变换发布信息的超时时间static_map: false  #不需要静态地图，可以提升导航效果rolling_window: true #是否使用动态窗口，默认为false，在静态的全局地图中，地图不会变化width: 3 # 局部地图宽度 单位是 mheight: 3 # 局部地图高度 单位是 mresolution: 0.05 # 局部地图分辨率 单位是 m，一般与静态地图分辨率保持一致

base_local_planner_params

TrajectoryPlannerROS:# Robot Configuration Parametersmax_vel_x: 0.5 # X 方向最大速度min_vel_x: 0.1 # X 方向最小速速max_vel_theta:  1.0 # min_vel_theta: -1.0min_in_place_vel_theta: 1.0acc_lim_x: 1.0 # X 加速限制acc_lim_y: 0.0 # Y 加速限制acc_lim_theta: 0.6 # 角速度加速限制# Goal Tolerance Parameters，目标公差xy_goal_tolerance: 0.10yaw_goal_tolerance: 0.05# Differential-drive robot configuration
# 是否是全向移动机器人holonomic_robot: false# Forward Simulation Parameters，前进模拟参数sim_time: 0.8vx_samples: 18vtheta_samples: 20sim_granularity: 0.05

launch文件集成

如果要实现导航，需要集成地图服务、amcl 、move_base 与 Rviz 等

<launch><include file="$(find nav_demo)/launch/nav03_map_server.launch"/><include file="$(find nav_demo)/launch/nav04_amcl.launch"/><include file="$(find nav_demo)/launch/nav05_path.launch"/><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>

测试

全局代价地图与本地代价地图组件配置如下：

全局路径规划与本地路径规划组件配置如下:

通过Rviz工具栏的 2D Nav Goal设置目的地实现导航

导航与SLAM建图

导航时需要地图信息，之前导航实现时，是通过 map_server 包的 map_server 节点来发布地图信息的，如果不先通过SLAM建图，那么如何发布地图信息呢？SLAM建图过程中本身就会时时发布地图信息，所以无需再使用map_server，SLAM已经发布了话题为 /map 的地图消息了，且导航需要定位模块，SLAM本身也是可以实现定位的。

该过程实现比较简单，步骤如下:

1. 编写launch文件，集成SLAM与move_base相关节点；
2. 执行launch文件并测试。

编写launch文件

当前launch文件实现，无需调用map_server的相关节点，只需要启动SLAM节点与move_base节点

<launch><!-- 启动SLAM节点 --><include file="$(find nav_demo)/launch/nav01_slam.launch" /><!-- 运行move_base节点 --><include file="$(find nav_demo)/launch/nav05_path.launch" />
</launch>

测试