机器人之Move_Base

总框架

在这里插入图片描述

move_base 基本配置

  1. 当机器人认为自己被卡住时,move_base 节点可以选择执行恢复行为。默认情况下,move_base 节点将采取以下操作来尝试清理空间:
    首先,用户指定区域之外的障碍物将从机器人的地图中清除。接下来,如果可能,机器人将执行就地旋转以清理空间。如果这也失败了,机器人将更积极地清除它的地图,移除它可以就地旋转的矩形区域之外的所有障碍物。随后将进行另一次就地轮换。如果这一切都失败了,机器人将认为其目标不可行,并通知用户它已中止。这些恢复行为可以使用 recovery_behaviors 参数配置,并使用 recovery_behavior_enabled 参数禁用。
    在这里插入图片描述

  2. 导航launch文件

    <launch>
        <node pkg="move_base" type="move_base" respawn="false" name="move_base" output="screen" clear_params="true">
    		<rosparam file="$(find nav)/param/move_base_base.yaml" command="load" />
    		<rosparam file="$(find nav)/param/costmap_common_params.yaml" command="load" ns="global_costmap" />
    		<rosparam file="$(find nav)/param/costmap_common_params.yaml" command="load" ns="local_costmap" />
    		<rosparam file="$(find nav)/param/local_costmap_params.yaml" command="load" />
    		<rosparam file="$(find nav)/param/global_costmap_params.yaml" command="load" />
    		<rosparam file="$(find nav)/param/global_planner_params.yaml" command="load" />
    		<rosparam file="$(find nav)/param/local_planner_params.yaml" command="load" />
        </node>
    </launch>
    
  3. move_base基本配置文件 : move_base_base.yaml

    shutdown_costmaps 		:	false				# 默认false,当 move_base 处于非活动状态时是否关闭节点的 costmaps
    
    # planner config
    base_global_planner		:	"global_planner/GlobalPlanner"				# 新版全局路径规划器
    base_local_planner 		:	"teb_local_planner/TebLocalPlannerROS"		# TEB局部路径规划器
    
    controller_frequency 	:	4.0					# 默认20.0,速度发送命令的速率(Hz)
    controller_patience		:	3.0					# 默认15.0,在执行空间清理之前,控制器在没有收到有效控制的情况下等待时间(s)
    
    planner_patience 		:	3.0					# 默认5.0,在执行空间清理之前,planner等待寻找有效计划的时间(s)
    planner_frequency 		:	1.0					# 默认0.0,运行全局规划器循环的速率,0.0意味着仅接收到新目标时阻塞运行
    
    recovery_behavior_enabled	:	true			# 默认true,是否启用 move_base 恢复行为以尝试清理空间
    clearing_rotation_allowed 	:	true			# 默认true,仅当使用默认恢复行为有效,确定机器人在尝试清理空间时是否会尝试就地旋转
    
    oscillation_timeout 	:	5.0					# 默认0.0,在执行恢复行为之前允许振荡的时间(s)。值 0.0 对应于无限超时
    oscillation_distance 	:	0.1					# 默认0.5,机器人必须移动多远才能被视为不摆动
    
    max_planning_retries 	:	1					# 默认-1,在执行恢复行为之前允许planning重试多少次,值 -1.0 对应于无限重试
    conservative_reset_dist	:	3.0					# 默认值3.0,仅当使用默认恢复行为有效,当试图清除地图中的空间时,距离机器人的距离(以米为单位)将从成本地图中清除障碍物
    
    # recovery_behaviors
    # 恢复行为,name是官方状态转移图中各节点,type为具体对应动作
    recovery_behaviors:
      - name: 'conservative_reset'
        type: 'clear_costmap_recovery/ClearCostmapRecovery'
      #- name: 'aggressive_reset'
      #  type: 'clear_costmap_recovery/ClearCostmapRecovery'
      - name: 'clearing_rotation'
        type: 'rotate_recovery/RotateRecovery'
      #- name: 'move_slow_and_clear'
        #type: 'move_slow_and_clear/MoveSlowAndClear'
    
    # 0. 保守复位
    conservative_reset:
      reset_distance: 1.0
      #layer_names: [static_layer, obstacle_layer, inflation_layer]
      layer_names: [obstacle_layer]
    
    # 1. 激进复位
    aggressive_reset:
      reset_distance: 3.0
      #layer_names: [static_layer, obstacle_layer, inflation_layer]
      layer_names: [obstacle_layer]
    
    move_slow_and_clear:
      clearing_distance: 0.5
      limited_trans_speed: 0.1
      limited_rot_speed: 0.4
      limited_distance: 0.3
    

代价地图配置

  1. 全局/局部代价地图通用配置:costmap_common_params.yaml
    # 多边形机器人配置端点,左上角开始
    footprint: [[-0.12, -0.12], [-0.12, 0.12],[0.12, 0.12], [0.12, -0.12]]
    # 圆形机器人配置半径 
    #robot_radius : 0.12
    
    # 0. 静态层
    static_layer:
    	enabled:           		true
    	map_topic:            	"/map"
    
    # 1. 障碍层
    obstacle_layer:
    	enabled: 				true		# 是否启用
    	max_obstacle_height: 	0.6			# 障碍物的最大高度(m)
    	min_obstacle_height:	0.0			# 障碍物的最小高度(m)
    	obstacle_range:			2.0			# 障碍物离机器人的最大距离
    	raytrace_range:			5.0			# 机器人移动过程中,代价地图动态更新障碍物的范围
    	inflation_radius:		0.10		# 障碍物膨胀半径,一般设为地盘直接大小
    	combination_method:		1			# 更改障碍层如何处理其外部层传入数据的行为
    	observation_sources:	laser_scan_sensor	# 障碍层传感器观测来源
    	track_unknown_space:	true		# 导航时是否允许通过地图中未知区域
    	
    	origin_z:				0.0			# 地图Z轴原点
    	z_resolution:			0.1			# z分辨率
    	z_voxels: 				10			# 垂直列的体素数,网格高度 = z_resolution * z_voxels
    	unknown_threshold: 		15			# 未知单元的阈值
    	mark_threshold: 		0			# 标记为空闲单元的阈值
    	publish_voxel_map: 		true		# 是否发布
    	footprint_clearing_enabled: true	# 机器人足迹是否清除
    	
    	# 申明一个雷达,供上文使用
    	laser_scan_sensor:
    	  data_type: 			LaserScan	# 雷达数据类型
    	  topic: 				/scan		# 传感器数据话题名
    	  marking: 				true		# 是否允许此传感器在障碍物层创建障碍物
    	  clearing: 			true		# 是否允许此传感器在障碍物层消除障碍物
    	  expected_update_rate: 0			# 期望传感器数据更新频率(s),0为两次读取有无限时间
    	  min_obstacle_height: 	0.00		# 传感器视为有效的最小障碍物高度(m)
    	  max_obstacle_height: 	0.30		# 传感器视为有效的最大障碍物高度(m),大于全局min_obstacle_height时无效,一般小于全局min_obstacle_height以过滤
    
    # 2. 膨胀层 
    inflation_layer:
    	enabled:              	true
    	cost_scaling_factor:  	10.0  # exponential rate at which the obstacle cost drops off (default: 10)
    	inflation_radius:     	0.20  # max. distance from an obstacle at which costs are incurred for planning paths.
    
  2. 全局代价地图特有配置:global_costmap_params.yaml
    global_costmap:
    	global_frame:			map			# 参考系
    	robot_base_frame: 		base_footprint	# 参考系
    	update_frequency: 		1.0			# 全局代价地图更新频率(Hz)
    	publish_frequency: 		1.0			# 全局代价地图发布频率(Hz)
    	#static_map: true					# 是否由mapserver提供的地图服务进行代价地图的初始化
    	rolling_window: 		false		# 机器人移动时,保持机器人在代价地图的中心
    	resolution: 			0.05		# 代价地图的分辨率
    	transform_tolerance: 	1.2			# 获取TF的超时时间(s)
    	#map_type: costmap					# 声明global_costmap的地图类型
    	# 制作代价地图时,所插入的costmap插件
    	plugins:
    	        - {name: static_layer,       type: "costmap_2d::StaticLayer"}
    	        - {name: inflation_layer,    type: "costmap_2d::InflationLayer"}
    # 是否允许全局规划器在未知区域通行
    GlobalPlanner:        
    	allow_unknown:  true
    
  3. 局部代价地图特有配置:local_costmap_params.yaml
    local_costmap:
    	global_frame: 			odom		# 参考系
    	#global_frame: /odom_combined
    	robot_base_frame: 	base_footprint
    	update_frequency: 		5.0			# 代价地图更新频率(Hz)
    	publish_frequency:		5.0			# 代价地图发布频率(Hz)
    	#static_map: false
    	rolling_window: 		true		# 机器人移动时,保持机器人在代价地图的中心
    	width: 					3.0			# 代价地图的宽度(m)
    	height: 				3.0			# 代价地图的高度(m)
    	resolution: 			0.05		# 代价地图的分辨率(m/格)
    	transform_tolerance: 	1.2			# 获取TF的超时时间(s)
    	#   map_type: costmap
    
    	# 制作代价地图时,所插入的costmap插件
    	plugins:
    	- {name: static_layer,        type: "costmap_2d::StaticLayer"}
    	- {name: obstacle_layer,      type: "costmap_2d::VoxelLayer"}
    	- {name: inflation_layer,     type: "costmap_2d::InflationLayer"}
    

规划器配置

  1. 全局规划器配置:global_planner_params.yaml

    # 规划器选择
    base_global_planner: 		"global_planner/GlobalPlanner"  
    base_local_planner: 		"teb_local_planner/TebLocalPlannerROS"
    
    # 全局规划器私有配置,http://wiki.ros.org/global_planner
    GlobalPlanner:
    	old_navfn_behavior: 	false      		# 使用老版,true时导致其他参数无效
    	use_quadratic: 			true      		# 使用势的二次近似。否则,使用更简单的计算,默认true
    	use_dijkstra: 			true       		# 使用Dijkstra算法,false时使用 A star 算法
    	use_grid_path: 			false        	# 创建遵循网格边界的路径(折线),否则使用梯度下降法,默认false
    	
    	allow_unknown:  		true     		# 允许planner通过未知空间进行规划,默认true;需要有 track_unknown_space: true 在障碍物/体素层(在 costmap_commons_param 中)才能工作
    
    	planner_window_x: 		0.0         	# default 0.0
    	planner_window_y: 		0.0            	# default 0.0
    	default_tolerance: 		0.0       		# 如果目标在障碍物中,则计划到半径内最近的点 default_tolerance,默认 0.0
    
  2. 局部规划器(TEB)配置:local_planner_params.yaml

    # ros wiki for teb : http://wiki.ros.org/teb_local_planner
    # 参数:https://charon-cheung.github.io/2021/06/07/%E8%B7%AF%E5%BE%84%E8%A7%84%E5%88%92/TEB%E7%AE%97%E6%B3%95/TEB%E6%89%80%E6%9C%89%E5%8F%82%E6%95%B0%E5%90%AB%E4%B9%89/#%E8%AF%9D%E9%A2%98
    TebLocalPlannerROS:
    	odom_topic: 			odom			# 里程计坐标系
    	map_frame: 				odom			# 所使用的地图参考系,???
    	# Trajectory params
    	teb_autosize: 			True			# 优化期间允许改变轨迹的时域长度
    	dt_ref: 				0.45
    	dt_hysteresis: 			0.1
    	min_samples:			2
    	max_samples:			500
    	force_reinit_new_goal_dist: 		1.0
    	global_plan_overwrite_orientation: 	True
    	global_plan_viapoint_sep: 			-0.1
    	max_global_plan_lookahead_dist: 	3.0
    	force_reinit_new_goal_dist:			1.0
    	feasibility_check_no_poses: 		5
    	publish_feedback:		false
    	shrink_horizon_backup:	true
    	allow_init_with_backwards_motion:	true
    	exact_arc_length:		false
    	shrink_horizon_min_duration:		10.0
    
    	# Obstacles
    	min_obstacle_dist: 0.13
    	include_costmap_obstacles: True
    	costmap_obstacles_behind_robot_dist: 1.0
    	obstacle_poses_affected: 7
    	costmap_converter_plugin: ""
    	costmap_converter_spin_thread: True
    	costmap_converter_rate: 5
    
    	# Optimization
    	no_inner_iterations: 5
    	no_outer_iterations: 4
    	optimization_activate: True
    	optimization_verbose: False
    	penalty_epsilon: 0.1
    	weight_max_vel_x: 1
    	weight_max_vel_theta: 1
    	weight_acc_lim_x: 1
    	weight_acc_lim_theta: 1
    	weight_kinematics_nh: 1000
    	weight_kinematics_forward_drive: 60
    	weight_kinematics_turning_radius: 1
    	weight_optimaltime: 1
    	weight_obstacle: 50
    	weight_dynamic_obstacle: 10 # not in use yet
    	selection_alternative_time_cost: False # not in use yet
    
    	# Homotopy Class Planner
    	enable_homotopy_class_planning: False
    	enable_multithreading: True
    	simple_exploration: False
    	max_number_classes: 4
    	roadmap_graph_no_samples: 15
    	roadmap_graph_area_width: 5
    	h_signature_prescaler: 0.5
    	h_signature_threshold: 0.1
    	obstacle_keypoint_offset: 0.1
    	obstacle_heading_threshold: 0.45
    	visualize_hc_graph: False
    
  • 1
    点赞
  • 40
    收藏
    觉得还不错? 一键收藏
  • 打赏
    打赏
  • 4
    评论
评论 4
添加红包

请填写红包祝福语或标题

红包个数最小为10个

红包金额最低5元

当前余额3.43前往充值 >
需支付:10.00
成就一亿技术人!
领取后你会自动成为博主和红包主的粉丝 规则
hope_wisdom
发出的红包

打赏作者

酸奶可乐

你的鼓励将是我创作的最大动力

¥1 ¥2 ¥4 ¥6 ¥10 ¥20
扫码支付:¥1
获取中
扫码支付

您的余额不足,请更换扫码支付或充值

打赏作者

实付
使用余额支付
点击重新获取
扫码支付
钱包余额 0

抵扣说明:

1.余额是钱包充值的虚拟货币,按照1:1的比例进行支付金额的抵扣。
2.余额无法直接购买下载,可以购买VIP、付费专栏及课程。

余额充值