对于ROS小车导航算法基本原理和使用方法,可以看笔记http://t.csdn.cn/NUWHt
1 启动小车导航节点:turn_on_wheeltec_robot navigation.launch
<launch>
<!-- 开启机器人底层相关节点 同时开启导航功能-->
<include file="$(find turn_on_wheeltec_robot)/launch/turn_on_wheeltec_robot.launch">
<arg name="navigation" default="true"/>
</include>
<!-- turn on lidar开启雷达 -->
<include file="$(find turn_on_wheeltec_robot)/launch/wheeltec_lidar.launch" />
<!-- 设置需要用于导航的地图 -->
<arg name="map_file" default="$(find turn_on_wheeltec_robot)/map/WHEELTEC.yaml"/>
<node name="map_server_for_test" pkg="map_server" type="map_server" args="$(arg map_file)">
</node>
<!-- 开启用于导航的自适应蒙特卡洛定位amcl-->
<include file="$(find turn_on_wheeltec_robot)/launch/include/amcl.launch" />
<!-- MarkerArray功能节点> -->
<node name='send_mark' pkg="turn_on_wheeltec_robot" type="send_mark.py">
</node>
</launch>
在launch文件中打开了以下launch文件:
1 turn_on_wheeltec_robot.launch
该文件启动了小车下位机电机控制节点,小车urdf文件及TF坐标,扩张卡尔曼滤波算法(robot_pose_ekf.launch 用于处理里程计信号),和建图导航算法
2 wheeltec_lidar.launch
该文件启动激光雷达相关功能
3 /map/WHEELTEC.yaml
该文件为地图的yaml文件,在小车建图中创建,这里我们默认打开文件名WHEELTEC,如果要切换导航地图可以在这里修改文件名
4 amcl.launch
该文件启动amcl定位算法,在launch/include 文件夹里的amcl.launch文件:
<launch>
<!-- <arg name="set_initpose" default="true"> -->
<!-- 使用map的topic而不是map文件-->
<arg name="use_map_topic" default="false"/>
<!-- 不可以使用PointCloud,只能使用LaserScan -->
<arg name="scan_topic" default="scan"/>
<arg name="map_topic" default="/map"/>
<node pkg="amcl" type="amcl" name="amcl" clear_params="true">
<!-- <rosparam command="load" file="$(find turn_on_wheeltec_robot)/map/WHEELTEC_end.yaml" />-->
<param name="use_map_topic" value="$(arg use_map_topic)"/>
<!-- 使用rtabmap分布的Map,连接TF树 -->
<remap from="/map" to="/$(arg map_topic)"/>
<!-- Publish scans from best pose at a max of 10 Hz -->
<param name="odom_model_type" value="omni-corrected"/>
<!-- <param name="odom_model_type" value="omnid"/> -->
<!-- <param name="odom_model_type" value="diff"/> -->
<!-- <param name="odom_model_type" value="diff-corrected"/> -->
<param name="odom_alpha5" value="0.1"/>
<param name="gui_publish_rate" value="10.0"/>
<param name="laser_max_beams" value="60"/>
<param name="laser_max_range" value="12.0"/>
<param name="min_particles" value="500"/>
<param name="max_particles" value="2000"/>
<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.2"/>
<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_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.25"/>
<param name="update_min_a" value="0.2"/>
<param name="odom_frame_id" value="odom_combined"/>
<param name="resample_interval" value="1"/>
<!-- Increase tolerance because the computer can get quite busy -->
<param name="transform_tolerance" value="1.0"/>
<param name="recovery_alpha_slow" value="0.0"/>
<param name="recovery_alpha_fast" value="0.0"/>
<remap from="scan" to="$(arg scan_topic)"/>
</node>
</launch>
可以看到这里有大量和算法相关的参数可以进行修改,之后了解算法原理后可以根据实际情况对参数进行调整
代价地图配置文件
代价地图有三个配置文件costmap_common_params.yaml,global_costmap_params.yaml,和local_costmap_params.yaml。其中costmap_common_params.yaml配置的是代价地图总体参数。后两个分别为全局和局域的代价地图参数。
在启动导航时一般先加载costmap_common_params文件,后加载local_costmap_params和global_costmap_params。对于相同的参数后加载的文件会覆盖先加载的文件。
costmap_common_params.yaml
#代价地图官方介绍:http://wiki.ros.org/costmap_2d
#静态层官方介绍:http://wiki.ros.org/costmap_2d/hydro/staticmap
#障碍层官方介绍:http://wiki.ros.org/costmap_2d/hydro/obstacles
#设置静态层参数
static_layer:
#是否开启静态层
enabled: true
#静态层的订阅的地图话题
map_topic: map
#地图话题中数据值为多少,会转换为静态层代价地图中的未知区域
unknown_cost_value: -1
#地图话题中数据值为多少,会转换为静态层代价地图中的完全占用区域
lethal_cost_threshold: 100
#是否仅把第一次订阅到的地图数据转换为静态层代价地图,无视后续订阅到的地图数据
first_map_only: false
#是否订阅话题 “map_topic”+“_updates”
subscribe_to_updates: false
#如果设置为false,地图话题中的未知区域在代价地图中会转换为自由区域
track_unknown_space: true
#如果设置为true,静态层代价地图只有未知、自由和完全占用三种情况
#如果设置为false,静态层代价地图可以有不同的占用程度
trinary_costmap: true
#设置障碍层参数
obstacle_layer:
#是否开启障碍层
enabled: true
#设置障碍层的观测源名称,可以一次设置多个观测源observation_sources: scan, scan2, camera
observation_sources: scan
#设置对应观测源参数
scan:
#观测源数据话题名称
topic: scan
#观测源的TF坐标名称,如果设置为空,会自动从话题数据寻找TF坐标名称
#以下三种数据格式支持自动寻找TF坐标名称
#sensor_msgs/LaserScan, sensor_msgs/PointCloud, and sensor_msgs/PointCloud2
sensor_frame: laser
#观测源话题的数据格式,可以为LaserScan、PointCloud、PointCloud2
data_type: LaserScan
#保留多久时间内的全部话题数据作为障碍层输入,设置为0代表只保留最近的一帧数据,单位:s
observation_persistence: 0.0
#读取观测源话题的频率,如果进行设置,频率应该设置的比传感器频率低一些。默认0,代表允许观测源一直不发布话题。单位:Hz
expected_update_rate: 0.0
#是否使用该观测源清除自由空间
clearing: true
#是否使用该观测源添加障碍物
marking: true
#高于多少的障碍物不加入观测范围,单位:m
max_obstacle_height: 2.0
#低于多少的障碍物不加入观测范围,单位:m
min_obstacle_height: 0.0
#多少范围内障碍物会被加入代价地图,单位:m
obstacle_range: 2.5
#多少范围内障碍物会被追踪,单位:m
raytrace_range: 3.0
#在观测源基础上再次进行设置的参数
#高于多少的障碍物不加入观测范围,单位:m
max_obstacle_height: 2.0
#多少范围内障碍物会被加入代价地图,单位:m
obstacle_range: 2.5
#多少范围内障碍物会被追踪,单位:m
raytrace_range: 3.0
#如果设置为true,障碍层代价地图会有未知、自由和完全占用三种情况
#如果设置为false,障碍层代价地图只有自由和完全占用两种情况
track_unknown_space: true
#障碍层如何与其它地图层处理的方法。
#0:障碍层覆盖其它地图层; 1:障碍物最大化方法,即各层的占用方格会覆盖其它层的自由方格,这是最常用的方法
#99:不改变其它地图层,应该是使障碍层层无效的方法
combination_method: 1
#如果障碍层类型是"costmap_2d::VoxelLayer",可以对以下参数进行设置
#代价地图的高度
#origin_z: 0.0
#障碍层的Z轴方格的高度
#z_resolution: 0.2
#障碍层Z轴上有几个方格
#z_voxels: 10
#被认为是“已知”的列中允许的未知单元格数
#unknown_threshold: 15
#被认为是“自由”的列中允许的标记单元格数
#mark_threshold: 0
#是否发布障碍层的投影地图层话题
#publish_voxel_map: false
#如果设置为true,机器人将把它所经过的空间标记为自由区域
#footprint_clearing_enabled: true
在这里有三个图层可以配置:静态层(static_layer),障碍层(obstacle_layer),和膨胀层(inflation_layer 这里没有配置)。这三个图层类似于PS里面的图层,会一起被用于导航
静态层:预先导入的SLAM地图
膨胀层:障碍物周围的膨胀区域
障碍层:实时检测到的障碍物
global_costmap_params.yaml
#代价地图官方介绍:http://wiki.ros.org/costmap_2d
#全局代价地图参数命名空间
global_costmap:
#代价地图的TF参考坐标系
global_frame: map
#机器人的TF坐标名称
robot_base_frame: base_footprint
#global_frame和robot_base_frame间的TF坐标停止发布多久后,控制机器人停止,单位:s
transform_tolerance: 1
#代价地图刷新频率,单位:Hz
update_frequency: 1.5
#代价地图的可视化话题发布频率,单位:Hz
publish_frequency: 1.0
#是否直接使用静态地图生成代价地图
#static_map: true #使用plugins手动配置代价地图时,该参数无效
#代价地图是否跟随机器人移动,static_map为true时该参数必须为false
rolling_window: false
#代价地图宽度,这里会被静态层扩宽,单位:m
width: 10.0
#代价地图高度,这里会被静态层扩宽,单位:m
height: 10.0
#代价地图分辨率(米/单元格)
resolution: 0.05
#为代价地图设置地图层,这里设置了三层,分别作为静态层、障碍层和膨胀层
plugins:
#定义地图层的名称,设置地图层的类型
- {name: static_layer, type: "costmap_2d::StaticLayer"}
#定义地图层的名称,设置地图层的类型。
#障碍层可以使用VoxelLayer代替ObstacleLayer
- {name: obstacle_layer, type: "costmap_2d::VoxelLayer"}
#定义地图层的名称,设置地图层的类型
- {name: inflation_layer, type: "costmap_2d::InflationLayer"}
#各地图层的参数,会以地图层名称作为命名空间
#各地图层的参数,会在【costmap_common_params.yaml】内进行设置
这里配置了全局代价地图的参数,包括了代价地图刷新率(update_frequency),地图话题发布频率(publish_frequency),以及该地图中包含哪几个图层,如这里全局代价地图包括了static_layer,obstracle_layer,inflation_layer这三个层。
局部代价地图配置文件和全局代价地图基本一样,这里不再赘述
路径规划器teb_local_planner.launch。teb路径规划器在turn_on_wheeltec_robot.launch文件中被启动
<launch>
<!-- Arguments参数 -->
<arg name="car_mode" default="" />
<!-- 导航路径规划公共参数 -->
<rosparam file="$(find turn_on_wheeltec_robot)/params_nav_common/move_base_params.yaml" command="load" ns="move_base"/>
<rosparam file="$(find turn_on_wheeltec_robot)/params_nav_common/base_global_planner_param.yaml" command="load" ns="move_base"/>
<!-- 导航TEB局部路径规划器参数 -->
<param name="move_base/base_local_planner" type="string" value="teb_local_planner/TebLocalPlannerROS"/>
<rosparam file="$(find turn_on_wheeltec_robot)/params_nav_common/teb_local_planner_params.yaml" command="load" ns="move_base"/>
<!-- 导航代价地图公共参数 -->
<rosparam file="$(find turn_on_wheeltec_robot)/params_costmap_common/costmap_common_params.yaml" command="load" ns="move_base/global_costmap"/>
<rosparam file="$(find turn_on_wheeltec_robot)/params_costmap_common/costmap_common_params.yaml" command="load" ns="move_base/local_costmap"/>
<rosparam file="$(find turn_on_wheeltec_robot)/params_costmap_common/local_costmap_params.yaml" command="load" ns="move_base"/>
<rosparam file="$(find turn_on_wheeltec_robot)/params_costmap_common/global_costmap_params.yaml" command="load" ns="move_base"/>
<!-- 启动导航节点 -->
<node pkg="move_base" type="move_base" respawn="false" name="move_base" output="screen">
<!-- 导航代价地图与路径规划器对应车型参数 -->
<rosparam file="$(find turn_on_wheeltec_robot)/params_costmap_car/param_$(arg car_mode)/teb_local_planner_params.yaml" command="load"/>
<rosparam file="$(find turn_on_wheeltec_robot)/params_costmap_car/param_$(arg car_mode)/costmap_car_params.yaml" command="load" ns="global_costmap" />
<rosparam file="$(find turn_on_wheeltec_robot)/params_costmap_car/param_$(arg car_mode)/costmap_car_params.yaml" command="load" ns="local_costmap" />
</node>
</launch>
在该launch文件中调用了全局与具有代价地图,并且引入了参数文件teb_local_planner_params.yaml。该文件配置了teb路径规划器的各类相关参数。
teb_local_planner_params.yaml (位置:turn_on_wheeltec_robot/params_nav_common/teb_local_planner_params.yaml)
#TebLocalPlanner官方介绍:http://wiki.ros.org/teb_local_planner
#局部路径规划器DWAPlannerROS命名空间
TebLocalPlannerROS:
odom_topic: odom #订阅的里程计话题
map_frame: map #代价地图的TF参考坐标系
#障碍物参数
min_obstacle_dist: 0.1 #和障碍物最小距离,直接影响机器人避障效果
include_costmap_obstacles: True #是否将动态障碍物预测为速度模型,
costmap_obstacles_behind_robot_dist: 1.5 #限制机器人后方规划时考虑的局部成本地图障碍物
obstacle_poses_affected: 15 #障碍物姿态受影响0~30
costmap_converter_plugin: ""
costmap_converter_spin_thread: True
costmap_converter_rate: 5
include_dynamic_obstacles: True
dynamic_obstacle_inflation_dist: 0.6
#目标点误差允许值
xy_goal_tolerance: 0.2 #机器人到达目标点时附近时的弧度偏差允许量,在该偏差内认为已经到达目标点,单位为:m
yaw_goal_tolerance: 0.1 #机器人到达目标点时附近时的弧度偏差允许量,在该偏差内认为已经到达目标点单位为:rad
free_goal_vel: False #允许机器人以最大速度驶向目的地
#轨道配置参数
teb_autosize: True #优化期间允许改变轨迹的时域长度
dt_ref: 0.45 #局部路径规划的解析度# minimum 0.01
dt_hysteresis: 0.1 #允许改变的时域解析度的浮动范围, 一般为 dt_ref 的 10% 左右 minimum0.002
global_plan_overwrite_orientation: False #覆盖全局路径中局部路径点的朝向
max_global_plan_lookahead_dist: 3.0 #考虑优化的全局计划子集的最大长度
feasibility_check_no_poses: 5 #检测位姿可到达的时间间隔 minimum 0
#轨迹优化参数
no_inner_iterations: 5
no_outer_iterations: 4
optimization_activate: True
optimization_verbose: False
penalty_epsilon: 0.1
obstacle_cost_exponent: 4
weight_max_vel_x: 2
weight_max_vel_theta: 1
weight_acc_lim_x: 1
weight_acc_lim_theta: 1
weight_kinematics_nh: 1000
weight_kinematics_forward_drive: 1
weight_kinematics_turning_radius: 1
weight_optimaltime: 1 #必须大于0
weight_shortest_path: 0
weight_obstacle: 100
weight_inflation: 0.2
weight_dynamic_obstacle: 10
weight_dynamic_obstacle_inflation: 0.2
weight_viapoint: 1
weight_adapt_factor: 2
#不同拓扑中的并行规划
enable_homotopy_class_planning: False
enable_multithreading: True
max_number_classes: 4
selection_cost_hysteresis: 1.0
selection_prefer_initial_plan: 0.95
selection_obst_cost_scale: 1.0
selection_alternative_time_cost: False
roadmap_graph_no_samples: 15
roadmap_graph_area_width: 5
roadmap_graph_area_length_scale: 1.0
h_signature_prescaler: 0.5
h_signature_threshold: 0.1
obstacle_heading_threshold: 0.45
switching_blocking_period: 0.0
viapoints_all_candidates: True
delete_detours_backwards: True
max_ratio_detours_duration_best_duration: 3.0
visualize_hc_graph: False
visualize_with_time_as_z_axis_scale: False
#恢复行为
shrink_horizon_backup: True
shrink_horizon_min_duration: 10
oscillation_recovery: False
oscillation_v_eps: 0.1
oscillation_omega_eps: 0.1
oscillation_recovery_min_duration: 10
oscillation_filter_duration: 10
导航系统完整架构: