11. move_base详解
11.1. move_base配置参数解读
参考链接:https://blog.csdn.net/banzhuan133/article/details/90239252
https://wenku.baidu.com/view/1ae6d9a56729647d27284b73f242336c1fb9304d.html
https://blog.csdn.net/qq_29313679/article/details/106237063
在仿真中,查看turtlebot3_navigation/launch/move_base.launch
,发现局部规划器用的是DWAPlannerROS
,同时加载了下面几个yaml文件:
costmap_common_params_$(arg model).yaml
local_costmap_params.yaml
global_costmap_params.yaml
move_base_params.yaml
dwa_local_planner_params_$(arg model).yaml
这里的(arg model)用的burger模型。
在rviz下,直接用鼠标选定2D navigation goal就可以指定目标了,但是,你可能会发现效果不太好,比如,疯狂原地打转,走S型,绕大圈等现象。这些都可以通过调参搞定。
重点看后两个move_base_params.yaml、dwa_local_planner_params_$(arg model).yaml文件,地图相关参数放到Costmap2DROS分析章节中。
move_base_params.yaml:
shutdown_costmaps: false
controller_frequency: 10.0
planner_patience: 5.0
controller_patience: 15.0
conservative_reset_dist: 3.0
planner_frequency: 5.0
oscillation_timeout: 10.0
oscillation_distance: 0.2
recovery_behavior_enabled: true
clearing_rotation_allowed: true
dwa_local_planner_params.yaml:
DWAPlannerROS:
max_vel_x: 0.22
min_vel_x: -0.22
max_vel_y: 0.0
min_vel_y: 0.0
max_vel_trans: 0.22
min_vel_trans: 0.11
max_vel_theta: 2.75
min_vel_theta: 1.37
acc_lim_x: 2.5
acc_lim_y: 0.0
acc_lim_theta: 3.2
xy_goal_tolerance: 0.05
yaw_goal_tolerance: 0.17
latch_xy_goal_tolerance: true
trans_stopped_vel: 0
sim_time: 1.5
vx_samples: 20
vy_samples: 0
vth_samples: 40
controller_frequency: 10.0
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
max_scaling_factor: 0.2
oscillation_reset_dist: 0.05
publish_traj_pc : true
publish_cost_grid_pc: true
11.2. 启动流程
move_base节点源文件位于/navigation-melodic-devel/move_base/move_base_node.cpp
,其调用了move_base.cpp
中的MoveBase的构造。主要的初始化过程也在这个构造中。main函数只有下面几句话:
tf2_ros::Buffer buffer(ros::Duration(10));
tf2_ros::TransformListener tf(buffer);
move_base::MoveBase move_base( buffer );
由节点关系图可以看到,这里监听的"/tf"来自定位模块,可以是amcl或其他定位模块,传输的是各个坐标系间的转换关系。
构造流程下:
- 构建MoveBaseActionServer,基于ROS的actionlib机制启动了一个action服务,主要作用是接收客户端发送的目标点,在回调中处理目标点并驱动AGV行走,回调函数是
executeCb
; - 各个参数的初始化,在上面的仿真中,很多默认参数已经被配置文件修改了,如上面的仿真中局部规划器被改为了DWA;
- 创建全局规划器线程;
- 创建全局地图和局部地图;
- 加载全局规划器、局部规划器的插件;
- 初始化状态机状态等等。
11.3. executeCb回调
该回调函数在收到目标点时会触发调用,收到新的目标点的话会通过ROS的actionlib机制进行处理,可以调用一些接口来判断,actionlib我们不关心,大概的流程如下:
- 通过一个信号量
planner_cond_
激发了全局规划器线程进行了规划, - 定义变量
ros::Rate r(controller_frequency_)
来控制下面while循环的频率,这个频率比较关键,最终体现在发送cmd_vel指令的频率上,我们称之为“控制频率”,程序中好多地方用到; - 中间过程不关心,调用了
executeCycle()
接口,来控制AGV行走,该接口会返回AGV是否到达目标点。
11.4. executeCycle接口
本函数的核心在状态CONTROLLING
处理逻辑中:
- 通过调用
LatchedStopRotateController
类的isGoalReached()
接口判断是否到达目标点,这个类参考后面的<目标点达到控制>章节; - 通过调用
dwa_planner_ros.cpp
中DWAPlannerROS
类的computeVelocityCommands()
接口来计算下发给AGV底盘的指令。这里AGV清除恢复CLEARING
及流程我们不关心。
11.5. DWAPlannerROS::computeVelocityCommands
DWA参考链接: http://gaoyichao.com/Xiaotu/?book=turtlebot&title=dwa_local_planner
本接口会进行一个是否到达目标点的判断(通过LatchedStopRotateController):
- 最开始部分会调用
planner_util_.getLocalPlan()
接口,将原/map坐标系下的全局路径转换到/odom坐标系下; - 调用
LatchedStopRotateController::isPositionReached
只判断XY是否到达,如果到达那么会调用latchedStopRotateController_.computeVelocityCommandsStopRotate()
来判断角度是否到达,角度没到达进行旋转控制,否则发送停止;如果没到达xy,那么会调用DWAPlannerROS::dwaComputeVelocityCommands()
来把最佳轨迹对应的速度作为下发底盘的速度cmd_vel
。xy到达后的旋转参考后面的<目标点到达控制>章节; - 未到达目标点,调用
DWAPlanner
类的findBestPath()
接口,它又调用了SimpleScoredSamplingPlanner
接口中的findBestTrajectory()
,后者会通过SimpleTrajectoryGenerator
类创建出轨迹,并调用代价算法来计算轨迹得分。
DWAPlanner
类中存储了7种代价计算方法,分别是:
OscillationCostFunction oscillation_costs_
ObstacleCostFunction obstacle_costs_
MapGridCostFunction goal_front_costs_
MapGridCostFunction alignment_costs_
MapGridCostFunction path_costs_
MapGridCostFunction goal_costs_
TwirlingCostFunction twirling_costs_
通过设定的比例系数,来对各个代价进行加权得出最后的代价数值。当计算出最优轨迹后,将轨迹对应的速度下发到底盘。
11.5.1. SimpleTrajectoryGenerator轨迹创建
该类中主要的接口有两个:initialise
和generateTrajectory
。
- initialise接口首先根据<x,y,th>三个加速度,根据当前速度和“控制频率”时间,计算一个时间片后的最大和最小速度;然后根据设定的采样数量对三个速度方向进行<min_vec,max_vec>范围采样,得到一堆速度(turtlebot3仿真中的参数得到了800个速度);
- generateTrajectory接口,一个速度计算一段轨迹,轨迹中点的个数计算公式:
线速度仿真距离 = 线速度 * 采样时间;
角速度速度仿真距离 = 角速度速度 * 采样时间;
线速度方向个数 = 线速度仿真距离 / 线速度仿真间隔;
角速度方向个数 = 角速度仿真距离 / 角速度仿真间隔;
总数量 = max(线速度方向个数,角速度方向个数);
11.6. MapGridCostFunction代价计算
有四种代价都是通过本类实现的,这里只关注了两个主要的:贴近全局轨迹和贴近全局轨迹与局部地图的交点。
这个类主要的接口有两个:prepare()
和scoreTrajectory()
。
- prepare():其中调用的逻辑是由
MapGrid
类实现的,需要提前调用,生成代价数据表来加速代价查询。该函数有两个作用:
一个是提前算好局部网格地图(注意是网格地图)中每个网格点距离全局路径在局部地图中的单位长度,如图,其中0是全局路径占用的网格,其他网格数据通过一个扩散的算法实现的(不是欧式距离),这里1 2 3代表离0的一个相对距离:
3 2 1 0 1 2 3
3 2 1 0 1 2 3
2 1 0 1 1 2 3
1 0 1 2 3 4 5
1 0 1 2 4 4 5
另一个是计算每个网格到全局路径和局部地图交点的单位长度,也是通过同样的扩散算法实现的,这里0是目标点:
- 2 1 0 1 2 -
- 3 2 1 2 3 -
- 4 3 2 3 4 -
- scoreTrajectory():这个函数很简单,就是直接查询代价表,来通过三种策略计算轨迹的代价值:
Last:默认是这个,只保留最后一个轨迹点距离全局轨迹的相对距离,来作为最终的代价;
Sum:所有点相对距离的累加;
Product:所有点相对距离的累乘。
11.7. OscillationCostFunction代价计算
本代价是为了减少车身的晃动。逻辑非常简单,总体来说就是AGV往前行驶一段距离的过程中,每次创建轨迹并进行打分时,都会考虑这些轨迹的速度和初始速度的正负关系,保证<x,y,th>都和初始速度方向一致。行驶一段距离后会重置初始的速度方向,重新开始统计。
关键的接口有:
- updateOscillationFlags() : 每次计算出最优轨迹时调用,如果超出了设定的距离,会重置检测标记;
- scoreTrajectory() : 对某个轨迹进行打分,就是看看轨迹的<x, y, th>速度方向是否和初始设置的一致。
11.8. 目标点到达控制(LatchedStopRotateController)
isPositionReached()
: 只判断XY是否到达,不判断角度是否到达。isGoalReached()
: 判断xy和朝向角都在误差范围、且会通过里程计信息判断<x,y,th>速度是满足到达误差。此函数会在CONTROLLING状态下每次控制频率到后都会调用,以此来切换控制状态。其中有个标志latch_xy_goal_tolerance_可以对xy判断进行锁存,意思是说如果如果该标记为true且曾经满足当前位置和目标位置的xy在误差范围内,那么后续再调用本接口时,不再判断xy是否满足误差范围,只判断角度和速度是否满足误差范围,这样到达目标点(xy到达)后调整角度过程中,如果xy偏离了也不关心。核心代码如下:
if ((latch_xy_goal_tolerance_ && xy_tolerance_latch_) ||
base_local_planner::getGoalPositionDistance(global_pose, goal_x, goal_y) <= xy_goal_tolerance) {
if (latch_xy_goal_tolerance_ && ! xy_tolerance_latch_) {
xy_tolerance_latch_ = true;
}
if (fabs(angle) <= limits.yaw_goal_tolerance) {
if (base_local_planner::stopped(base_odom, theta_stopped_vel, trans_stopped_vel)) {
return true;
}
}
return false;
computeVelocityCommandsStopRotate()
: 该函数的进入条件为xy已经到了,然后判断角度是否到了,如果到了那么直接发送0速度来强行停止(如果第一次到达xy并且角度也满足,这样做太粗暴了;如果角度不满足,这里就是旋转过程中控制停止的条件,是可以的)。如果没到那么会检查是否速度<x,y,th>已经满足停止条件:
- 满足:此时可以认为机器人已经停下了,则进行旋转操作rotateToGoal(),这里会根据最大最小限速及和目标角度的夹角计算角速度,
这里注意当离目标角度越近的时候,速度计算会直接采用这个距离,也就是旋转过程离目标点越近旋转速度越慢
,并下发下去,进行下次循环,继续在computeVelocityCommandsStopRotate
中判断是否角度到了发送0速度; - 不满足:则进行减速操作,通过加速度计算下个仿真周期减速后的<x,y,th>方向的速度,这里会调用轨迹检查接口,不用关心该调用。发送的速度计算:v-at,如果为负的,那么就是0。
这个目标点到达控制如果在到达目标点且角度也一样时,会强行停止,比如在行驶速度特别大的时候,可能会出现急停滑行的问题等等,实际应用的话还需要改进。
本文内容由网友自发贡献,版权归原作者所有,本站不承担相应法律责任。如您发现有涉嫌抄袭侵权的内容,请联系:hwhale#tublm.com(使用前将#替换为@)