ROS-基于已知地图的无人机路径规划算法仿真_使用cartographer_ros发布的地图直接用于导航实现动态地图路线规划

在上一篇文章中我们实现了基于PX4的无人机SLAM建图（Cartographer）仿真，最终得到了indoor3的地图。

现在，基于这张建立好的地图进行路径规划算法仿真。

一、创建功能包

在工作空间下创建功能包：

cd ~/catkin_ws/src
catkin_create_pkg my_nav geometry_msgs move_base_msgs roscpp rospy tf visualization_msgs

然后在功能包目录下创建几个文件夹，用于区分文件

 config——用于存放相关配置文件；

launch——用于存放launch文件；

map——用于存放地图；

rviz——用于存放rviz文件；

script——用于存放脚本文件；

二、文件配置

2.1 地图文件

将之前保存的地图文件（包括一个.pgm和一个.yaml文件）复制到map文件夹下，但要注意修改.yaml文件中的图片路径，

image: /home/xxx/catkin_ws/src/my_nav/map/indoor3_carto.pgm    # 注意修改路径
resolution: 0.050000
origin: [-12.806009, -9.646632, 0.000000]
negate: 0
occupied_thresh: 0.65
free_thresh: 0.196

2.2 代价地图和规划器配置

通用代价地图配置文件：costmap_common_params.yaml

# 设置机器人检测障碍物的最大范围
obstacle_range: 1
# 设置机器人检测自由空间的最大范围
raytrace_range: 2
 
# 设置机器人的外形半径
robot_radius: 0.17
# 设置障碍物的膨胀参数，也就是机器人应该与障碍物保持的最小安全距离
inflation_radius: 1.0
# 设置代价比例系数，越大则代价值越小，占用的计算量也越小
cost_scaling_factor: 2.0
# 设置地图类型为代价地图
map_type: costmap
 
# observation_sources列出了代价地图需要关注的所有传感器信息，每个传感器信息都会在后面列出详细内容（这里只有雷达）。
# sensor_frame表示传感器的参考系名称
# data_type表示激光数据或者点云数据使用的消息类型
# topic表示传感器发布的话题名称
# marking和clearing用来表示是否需要使用传感器的实时信息来添加或清除代价地图中的障碍物信息。
observation_sources: iris_0/scan
iris_0/scan: {sensor_frame: iris_0/laser_2d, data_type: LaserScan, topic: iris_0/scan, marking: true, clearing: true}

全局代价地图配置文件：global_costmap_params.yaml

global_costmap:
  # 设置全局代价地图运行的参考系
  global_frame: map
  # 设置代价地图可以参考的机器人本体的坐标系
  robot_base_frame: base_link
 
  # 设置全局地图信息更新频率，单位是Hz
  update_frequency: 10.0
  # 设置全局代价地图发布可视化信息的频率，单位是Hz
  publish_frequency: 10.0
  # 坐标系间的转换可以忍受的最大延时
  transform_tolerance: 0.5
 
  # 用来决定代价地图是否需要根据map_server提供的地图信息进行初始化
  static_map: true

本地代价地图配置文件：local_costmap_params.yaml

local_costmap:
  global_frame: map
  robot_base_frame: base_link
 
  update_frequency: 10.0
  publish_frequency: 10.0
  transform_tolerance: 0.5  
 
  static_map: false
  # 设置在机器人移动过程中是否需要滚动窗口，以保持机器人处于中心位置
  rolling_window: true
  # width、height和resolution用于设置代价地图的长（米）、高（米）和分辨率（米/格）
  width: 3
  height: 3
  resolution: 0.05

move_base配置文件：move_base_params.yaml

# 当move_base在不活动状态时，是否关掉costmap
shutdown_costmaps: false
# 向无人机发送话题命令的频率
controller_frequency: 2.0
# 在空间清理操作执行前，控制器花多长时间等有效控制下发
controller_patience: 15.0
# 全局规划操作的执行频率.
# 如果设置为0.0，则全局规划器仅在接收到新的目标点或者局部规划器报告路径堵塞时才会重新执行规划操作
planner_frequency: 5.0
# 在空间清理操作执行前，留给规划器多长时间来找出一条有效规划
planner_patience: 5.0
 
# 清除机制的参数，决定清除多远处的障碍
conservative_reset_dist: 3.0
 
# 震荡超时，超过则底层进行异常处理
oscillation_timeout: 10.0
# 震荡距离
oscillation_distance: 0.2
# 是否启动旋转的恢复，必须是recovery_behavior_enabled在使能的基础上才能生效
clearing_rotation_allowed: false

全局规划器配置文件：global_planner_params.yaml

GlobalPlanner:
  # 是否使用D*全局规划算法，默认为true，如果为false，则使用A*算法
  use_dijkstra: True
  # 是否沿着网格规划路径，默认为false，false时使用最速下降法计算路径
  use_grid_path: False
  # 是否使用二次逼近的趋势，默认为true，否则使用简单的线性方法
  use_quadratic: True

本地规划器配置文件：dwa_local_planner_params.yaml

DWAPlannerROS:
# Robot Configuration Parameters
  max_vel_x: 0.2    # x方向最大线速度绝对值，单位:m/s
  min_vel_x: -0.2    # x方向最小线速度绝对值，负数代表可后退，单位:m/s
 
  max_vel_y: 0.2    # y方向最大线速度绝对值，单位:m/s
  min_vel_y: -0.2    # y方向最小线速度绝对值，负数代表可后退，单位:m/s
 
  max_vel_trans: 0.3    # 机器人最大平移速度的绝对值，单位为:m/s
  min_vel_trans: 0.01    # 机器人最小平移速度的绝对值，单位为:m/s（不可为零）
 
  max_vel_theta: 0.1     # 机器人的最大旋转角速度的绝对值，单位为:rad/s 
  min_vel_theta: 0.01     # 器人的最小旋转角速度的绝对值，单位为:rad/s
 
  acc_lim_x: 1    # 机器人在x方向的极限加速度，单位为:m/s^2
  acc_lim_y: 1    # 机器人在y方向的极限加速度，单位为:m/s^2
  acc_lim_theta: 1    # 机器人的极限旋转加速度，单位为:rad/s^2
 
# Goal Tolerance Parametes
  xy_goal_tolerance: 0.05    # 到到目标点时，控制器在x和y方向上的容差（m）
  yaw_goal_tolerance: 3.14    # 到达目标点时，控制器在偏航/旋转时的弧度容差（rad）
  latch_xy_goal_tolerance: false    # 设置为true时表示：如果到达容错距离内，机器人就会原地旋转
 
# Forward Simulation Parameters
  sim_time: 2.0    # 前向模拟轨迹的时间，单位为s
  vx_samples: 20    # x方向速度空间的采样点数
  vy_samples: 20    # y方向速度空间的采样点数
  vth_samples: 40    # 旋转方向的速度空间采样点数
  controller_frequency: 2.0
 
# Trajectory Scoring Parameters
  path_distance_bias: 32.0    # 控制器与给定路径接近程度的权重
  goal_distance_bias: 20.0    # 控制器与局部目标点的接近程度的权重，也用于速度控制
  occdist_scale: 0.02    # 控制器躲避障碍物的程度
  forward_point_distance: 0.325    # 以机器人为中心，额外放置一个计分点的距离
  stop_time_buffer: 0.2     # 机器人在碰撞发生前必须拥有的最少时间量
  scaling_speed: 0.25    # 开始缩放机器人足迹时的速度的绝对值，单位为m/s
  max_scaling_factor: 0.2    # 最大缩放因子
 
# Oscillation Prevention Parameters
  oscillation_reset_dist: 0.05    # 机器人必须运动多少米远后才能复位震荡标记
# Debugging
  publish_traj_pc : true    # 将规划的轨迹在RVIZ上进行可视化
  publish_cost_grid_pc: true    # 将代价值进行可视化显示

这里偏航角容差yaw_goal_tolerance设成了pi，因为我的无人机进行路径规划导航时只要求位置到达目标点即可，航向没有要求，所以航向角容差比较大。

2.3 创建launch文件

完成所有配置文件创建后，现在开始创建launch文件启动move_base节点，并加载相关配置文件。在launch文件夹目录下，创建名为move_base.launch的文件，文件代码如下

<launch>
  <arg name="cmd_vel_topic" default="/cmd_vel" />
  <arg name="odom_topic" default="odom" />
  <arg name="move_forward_only" default="false"/>
 
  <node pkg="move_base" type="move_base" respawn="false" name="move_base" output="screen" clear_params="true" >
    <param name="base_global_planner" value="global_planner/GlobalPlanner" />
    <param name="base_local_planner" value="dwa_local_planner/DWAPlannerROS" />
    <rosparam file="$(find my_nav)/config/costmap_common_params.yaml" command="load" ns="global_costmap" />
    <rosparam file="$(find my_nav)/config/costmap_common_params.yaml" command="load" ns="local_costmap" />
    <rosparam file="$(find my_nav)/config/local_costmap_params.yaml" command="load" />
    <rosparam file="$(find my_nav)/config/global_costmap_params.yaml" command="load" />
    <rosparam file="$(find my_nav)/config/move_base_params.yaml" command="load" />
    <rosparam file="$(find my_nav)/config/dwa_local_planner_params.yaml" command="load" />
    <rosparam file="$(find my_nav)/config/global_planner_params.yaml" command="load" />
    <remap from="cmd_vel" to="$(arg cmd_vel_topic)"/>
    <remap from="odom" to="$(arg odom_topic)"/>
    <param name="DWAPlannerROS/min_vel_x" value="0.0" if="$(arg move_forward_only)" />
    <param name="DWAPlannerROS/max_vel_y" value="0.0" if="$(arg move_forward_only)" />
    <param name="DWAPlannerROS/min_vel_y" value="0.0" if="$(arg move_forward_only)" />
  </node>
 
</launch>

然后创建一个运行所有导航功能节点的顶层launch文件nav_demo_indoor3.launch，

<launch>
 
  <node pkg="tf" type="static_transform_publisher" name="base_to_laser_broadcaster" args="0 0 0 0 0 0 base_link iris_0/laser_2d 100" />
 
  <!-- 设置地图的配置文件 -->
  <arg name="map" default="indoor3.yaml" />
  <arg name="move_forward_only" default="false"/>
 
  <!-- 运行地图服务器，并且加载设置的地图 -->
  <node name="map_server" pkg="map_server" type="map_server" args="$(find my_nav)/map/$(arg map)" />
 
  <!-- 运行move_base节点 -->
  <include file="$(find my_nav)/launch/move_base.launch" >
    <arg name="move_forward_only" value="$(arg move_forward_only)"/>
    <arg name="cmd_vel_topic" default="/xtdrone/iris_0/cmd_vel_flu" />
    <arg name="odom_topic" default="/iris_0/mavros/local_position/odom" />
  </include>
 
  <!-- 运行虚拟定位，兼容AMCL输出 -->
  <!-- fake localization包含map到odom的tf坐标转换 -->
  <node pkg="fake_localization" type="fake_localization" name="fake_localization" output="screen" >
    <remap from="base_pose_ground_truth" to="iris_0/mavros/local_position/odom" />
    <param name="global_frame_id" value="map" />
    <param name="base_frame_id" value="base_link" />
  </node>
 
  <!-- 运行rviz -->
  <node pkg="rviz" type="rviz" name="rviz" args="-d $(find my_nav)/rviz/2d_motion_planning.rviz" />
 
</launch>

至此，我们基本上已经完成了实现基于已知地图的路径规划算法仿真的文件配置，下面准备开始进行仿真。

三、仿真实现

1、首先打开地图环境，运行

roslaunch my_nav indoor3.launch

2、启动导航功能节点，运行

roslaunch my_nav nav_demo_indoor3.launch

完成启动后可以检查一下自己的tf tree结构是否完整，运行

rosrun rqt_tf_tree rqt_tf_tree

完整的tf tree结构如下

 检查无误后，就可以控制无人机起飞了。

3、启动通信脚本（这里我把脚本复制到了my_nav这个功能包下）

cd ~/catkin_ws/src/my_nav/script
python multirotor_communication.py iris 0

4、启动键盘控制脚本，控制无人机起飞

cd ~/catkin_ws/src/my_nav/script
python multirotor_keyboard_control.py iris 1 vel

在键盘控制终端激活情况下，首先按b键，将无人机切换至offboard模式；然后按t键解锁无人机，解锁后持续按i键，至upward vel增大至0.30以上，就可以看到无人机已经开始起飞了。

无人机起飞至合适高度后，按s或k键，使无人机悬停。

完成起飞后，需要关闭键盘控制的终端，否则路径规划导航时的指令会与键盘

5、在rviz界面上点击“2D Nav Goal”，然后在地图中任意位置点击并拖动鼠标确定方向，然后就可以看到地图上会自动显示一条绿色的轨迹，这是全局规划产生的路径，无人机坐标系附近还会有一条红色的轨迹，这是局部规划产生的路径。

 至此，基于已知地图的无人机路径规划算法仿真就基本实现了。接下来还会进一步实现多个目标点的自动路径规划与导航实现。