ROS学习笔记6（仿真机器人导航）

7.2 导航实现

新建功能包
依赖

gmapping map_server amcl move_base

编译时可能存在问题：

解决方案：

cd /home/herrscher/urdf_demo
rm -rf build devel
catkin_make

7.2.1 SLAM建图

新建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="base_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" type="joint_state_publisher" name="joint_state_publisher" />
      <node pkg="robot_state_publisher" type="robot_state_publisher" name="robot_state_publisher" />
      <node pkg="rviz" type="rviz" name="rviz" />
</launch>

参数介绍

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发布的坐标变换

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

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

启动gazebo节点和launch文件
可以操作机器人运动一圈来补全地图

7.2.2 地图保存

新建launch文件

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

find后改成功能包名

7.2.3 地图读取

编写launch文件

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

直接启动，再启动rviz就可以订阅到

7.2.4 地图配置文件（.yaml）

#声明地图资源的路径
image: /home/herrscher/urdf_demo/src/nav_demo/map/nav.pgm
#地图的分辨率 单位 m/像素
resolution: 0.050000
#地图的原点坐标（按照右手坐标系，地图右下角相对于rviz中原点的位姿）
#值1：x坐标
#值2：y坐标
#值3：地图的偏航角 单位：弧度
origin: [-50.000000, -50.000000, 0.000000]
#取反
negate: 0

#地图中的障碍物判定
#白色为可通行区域，黑色为障碍物区域，蓝灰为未知区域
#判断规则
#地图的每个像素点都取值【0，255】白色为0，黑色为255，设像素值为x
#根据像素值计算比例p = (255-x)/255 白色为0，黑色为1 灰色介于0-1之间
#如果p>occupied_thresh 则认为该像素点为障碍物 p<free_thresh则认为该像素点为空闲区域

#地图的占用阈值
occupied_thresh: 0.65
#地图的空闲阈值
free_thresh: 0.196

7.2.5 amcl定位实现

通过shell找到示例文件，并复制其内容到新建的launch文件里（diff是差速机器人，omni是全方向机器人）
其中比较常用的参数有这几个

~odom_model_type(string, default:“diff”)
里程计模型选择: “diff”,“omni”,“diff-corrected”,“omni-corrected” (diff 差速、omni 全向轮)
~odom_frame_id(string, default:“odom”)
里程计坐标系。
~base_frame_id(string, default:“base_link”)
机器人极坐标系。
~global_frame_id(string, default:“map”)
地图坐标系。

amcl文件无法单独运行，需要将其编入测试文件中

<launch>
    <node name="joint_state_publisher" pkg="joint_state_publisher" type="joint_state_publisher"/>
    <node name="robot_state_publisher" pkg="robot_state_publisher" type="robot_state_publisher"/>
    <node name="rviz" pkg="rviz" type="rviz" />

    <!-- 加载地图服务 -->
    <include file="$(find nav_demo)/launch/map_server.launch" />

    <!-- 启动 amcl -->
    <include file="$(find nav_demo)/launch/nav_amcl.launch" />
</launch>

先启动gazebo，再启动rviz
配置如下

启动键盘控制节点操作运动，会发现位置越来越精确

7.3 路径规划（move_base）

7.3.1 move_base节点说明

动作

动作订阅

move_base/goal(move_base_msgs/MoveBaseActionGoal)

move_base 的运动规划目标。

move_base/cancel(actionlib_msgs/GoalID)

取消目标。

动作发布

move_base/feedback(move_base_msgs/MoveBaseActionFeedback)

连续反馈的信息，包含机器人底盘坐标。

move_base/status(actionlib_msgs/GoalStatusArray)

发送到move_base的目标状态信息。

move_base/result(move_base_msgs/MoveBaseActionResult)

操作结果(此处为空)。

2.2订阅的Topic

move_base_simple/goal(geometry_msgs/PoseStamped)

运动规划目标(与action相比，没有连续反馈，无法追踪机器人执行状态)。

2.3发布的Topic

cmd_vel(geometry_msgs/Twist)

输出到机器人底盘的运动控制消息。

2.4服务

~make_plan(nav_msgs/GetPlan)

请求该服务，可以获取给定目标的规划路径，但是并不执行该路径规划。

~clear_unknown_space(std_srvs/Empty)

允许用户直接清除机器人周围的未知空间。

~clear_costmaps(std_srvs/Empty)

允许清除代价地图中的障碍物，可能会导致机器人与障碍物碰撞，请慎用。

7.3.2 代价地图概述

概念

ros中，通过设置每个像素点代价值的方式来评估路径

例如：
路线1：直接撞开围墙，代价值计为200
路线2：绕围墙走，可能与围墙发生碰撞，代价值计为100
路线3：绕开走，代价值计为0
机器人导航(尤其是路径规划模块)是依赖于地图的，地图在SLAM时已经有所介绍了，ROS中的地图其实就是一张图片，这张图片有宽度、高度、分辨率等元数据，在图片中使用灰度值来表示障碍物存在的概率。不过SLAM构建的地图在导航中是不可以直接使用的，因为：

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

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

组成

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

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

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

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

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

Other Layers：自定义costmap。

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

算法

7.4 move_base使用流程

文件编写

新建launch文件

<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>

新建param文件夹如下

实际上是launch文件里的yaml文件
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}

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 # 局部地图宽度 单位是 m
  height: 3 # 局部地图高度 单位是 m
  resolution: 0.05 # 局部地图分辨率 单位是 m，一般与静态地图分辨率保持一致

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.

base_local_planner_params.yaml

TrajectoryPlannerROS:

# Robot Configuration Parameters
  max_vel_x: 0.5 # X 方向最大速度
  min_vel_x: 0.1 # X 方向最小速速

  max_vel_theta:  1.0 # 
  min_vel_theta: -1.0
  min_in_place_vel_theta: 1.0

  acc_lim_x: 1.0 # X 加速限制
  acc_lim_y: 0.0 # Y 加速限制
  acc_lim_theta: 0.6 # 角速度加速限制

# Goal Tolerance Parameters，目标公差
  xy_goal_tolerance: 0.10
  yaw_goal_tolerance: 0.05

# Differential-drive robot configuration
# 是否是全向移动机器人
  holonomic_robot: false

# Forward Simulation Parameters，前进模拟参数
  sim_time: 0.8
  vx_samples: 18
  vtheta_samples: 20
  sim_granularity: 0.05

参数需要参考注释进行修改

先启动gazebo
再启动rviz

rviz设置如下

点这个设置目的地

效果如下

rviz配置

添加时，下方可以命名
注意path的订阅节点

自动导航建图

launch文件编写如下

<launch>
    <include file="$(find nav_demo)/launch/nav1_slam.launch" />
    <include file="$(find nav_demo)/launch/nav_path.launch" />
</launch>

可以通过设置目标点实现自动导航
导航结束后用map_save保存

7.5导航相关消息

参考链接