0x00 为何需要路径规划？ 现在我们已经可以构建当前环境地图，可以实现机器人在当前地图中的定位。那接下来就可以在地图上给定目标点，开始规划路径控制机器人移动过去，这样就可以完成在已知地图中进行自动导航的任务了。在已知地图中进行路径规划可以举一个很形象的例子，大家在公园里游玩时，经常需要找特定的游玩地点。例如想要寻找熊猫馆，那么就需要在公园提供的游览地图中首先找自己的定位在公园地图的哪一点上，然后开始寻找熊猫馆，这样我们就可以在当前地点和目标地点之间自己找到一条全局路径了，一般我们寻找的路径都是找最近的路径。这就好比是利用了dijkstra算法。有了全局路径还不行，因为我们在行动过程中，还会有各种障碍物出现在我们规划的路径上，那么我们就需要再规划自己的局部路径来暂时性的避开障碍物，规划的局部路径要尽可能的服从于全局路径。这样我们的全局路径就好比长远目标，局部路径好比短期目标。把一个个短期目标都实现了，那么实现全局目标就是必然的事了。
0x01 配置move_base参数 前面介绍配置代价地图相关的参数时就应注意到，我们若配置路径规划参数那么首先就需要先配置一下move_base相关的参数，因为在move_base中有多种路径规划器算法可选，我们需要告诉move_base路径规划器使用哪种算法。一般来说，全局路径的规划插件包括：
- navfn:ROS中比较旧的代码实现了dijkstra和A*全局规划算法。
- global_planner:重新实现了Dijkstra和A*全局规划算法,可以看作navfn的改进版。
- parrot_planner:一种简单的算法实现全局路径规划算法。
局部路径的规划插件包括：
- base_local_planner:实现了Trajectory Rollout和DWA两种局部规划算法。
- dwa_local_planner:实现了DWA局部规划算法，可以看作是base_local_planner的改进版本。
当前上面这些插件只是官方实现的，我们也可以来实现自己的规划算法，以插件的形式包含进move_base，这样就可以来改进这些路径规划算法了。我们可以根据move_base的launch文件来看看也就明白配置哪个文件了：
我们在stdr_move_base下的config目录中创建move_base_params.yaml，配置文件内容如下：
#FileName: move_base_params.yaml
#Copyright: 2016-2018 ROS小课堂www.corvin.cn
#Author: corvin
#Description:
# move_base软件包的通用配置参数,现在依次解释每个参数意义:
# shutdown_costmaps:当move_base在不活动状态时,是否关掉costmap.
# controller_frequency:向底盘控制移动话题cmd_vel发送命令的频率.
# controller_patience:在空间清理操作执行前,控制器花多长时间等有效控制下发.
# planner_frequency:全局规划操作的执行频率.如果设置为0.0,则全局规划器仅
# 在接收到新的目标点或者局部规划器报告路径堵塞时才会重新执行规划操作.
# planner_patience:在空间清理操作执行前,留给规划器多长时间来找出一条有效规划.
# oscillation_timeout:执行修复机制前,允许振荡的时长.
# oscillation_distance:来回运动在多大距离以上不会被认为是振荡.
# base_local_planner:指定用于move_base的局部规划器插件名称.
# base_global_planner:指定用于move_base的全局规划器插件名称.
#History:
# 20180726: initial this comment.
#
shutdown_costmaps: false
controller_frequency: 5.0
controller_patience: 3.0
planner_frequency: 1.0
planner_patience: 5.0
oscillation_timeout: 8.0
oscillation_distance: 0.3
base_local_planner: "dwa_local_planner/DWAPlannerROS"
base_global_planner: "global_planner/GlobalPlanner"
下面来依次解释下各参数的意义：
- shutdown_costmaps:当move_base在不活动状态时,是否关掉costmap.
- controller_frequency:向底盘控制移动话题cmd_vel发送命令的频率.
- controller_patience:在空间清理操作执行前,控制器花多长时间等有效控制下发.
- planner_frequency:全局规划操作的执行频率.如果设置为0.0,则全局规划器仅在接收到新的目标点或者局部规划器报告路径堵塞时才会重新执行规划操作.
- planner_patience:在空间清理操作执行前,留给规划器多长时间来找出一条有效规划.
- oscillation_timeout:执行修复机制前,允许振荡的时长.
- oscillation_distance:来回运动在多大距离以上不会被认为是振荡.
- base_local_planner:指定用于move_base的局部规划器名称.
- base_global_planner:指定用于move_base的全局规划器插件名称.
0x02 配置全局路径参数 我们在进行全局路径规划时，需要由外部来告知目标点。同时还需要知道全局代价地图，因为在路径规划时需要避开代价高的危险区域，不然规划的路径就撞到墙上了。 我们这里仍然只需要配置好global_planner规划器的参数即可。我们在stdr_move_base的config目录中创建global_planner_params.yaml文件，文件内容如下：
GlobalPlanner:
allow_unknown: false
default_tolerance: 0.2
visualize_potential: false
use_dijkstra: true
use_quadratic: true
use_grid_path: false
old_navfn_behavior: false
lethal_cost: 253
neutral_cost: 50
cost_factor: 3.0
publish_potential: true
orientation_mode: 0
orientation_window_size: 1
下面来依次解释下各参数意义：
- allow_unknown:是否允许规划器规划穿过未知区域的路径,只设计该参数为true还不行,还要在costmap_commons_params.yaml中设置track_unknown_space参数也为true才行。
- default_tolerance：当设置的目的地被障碍物占据时,需要以该参数为半径寻找到最近的点作为新目的地点.
- visualize_potential：是否显示从PointCloud2计算得到的势区域.
- use_dijkstra：设置为true,将使用dijkstra算法,否则使用A*算法.
- use_quadratic：设置为true,将使用二次函数近似函数,否则使用更加简单的计算方式,这样节省硬件计算资源.
- use_grid_path：如果设置为true,则会规划一条沿着网格边界的路径,偏向于直线穿越网格,否则将使用梯度下降算法,路径更为光滑点.
- old_navfn_behavior：若在某些情况下,想让global_planner完全复制navfn的功能,那就设置为true,但是需要注意navfn是非常旧的ROS系统中使用的,现在已经都用global_planner代替navfn了,所以不建议设置为true.
- lethal_cost：致命代价值,默认是设置为253,可以动态来配置该参数.
- neutral_cost：中等代价值,默认设置是50,可以动态配置该参数.
- cost_factor：代价地图与每个代价值相乘的因子.
- publish_potential：是否发布costmap的势函数.
- orientation_mode：如何设置每个点的方向（None = 0,Forward = 1,Interpolate = 2,ForwardThenInterpolate = 3,Backward = 4,Leftward = 5,Rightward = 6）（可动态重新配置）
- orientation_window_size：根据orientation_mode指定的位置积分来得到使用窗口的方向.默认值1,可以动态重新配置.
0x03 配置局部路径规划参数 局部路径规划参数相当重要，因为它是直接控制机器人的移动底盘运动的插件，它负责来向移动底盘的/cmd_vel话题中发布控制命令。机器人移动的效果好不好，这个局部路径规划可是影响最大的。 在这里我们使用dwa_local_planner，它是一个能够驱动底盘移动的控制器,该控制器连接了路径规划器和机器人.使用地图,规划器产生从起点到目标点的运动轨迹,在移动时,规划器在机器人周围产生一个函数,用网格地图表示。控制器的工作就是利用这个函数来确定发送给机器人的速度dx, dy, dtheta
- DWA算法的基本思想
1.在机器人控制空间离散采样(dx, dy, dtheta) 2.对每一个采样的速度进行前向模拟,看看在当前状态下,使用该采样速度移动一小段时间后会发生什么. 3.评价前向模拟得到的每个轨迹,是否接近障碍物,是否接近目标,是否接近全局路径以及速度等等.舍弃非法路径 4.选择得分最高的路径,发送对应的速度给底座 DWA与Trajectory Rollout的区别主要是在机器人的控制空间采样差异.Trajectory Rollout采样点来源于整个前向模拟阶段所有可用速度集合,而DWA采样点仅仅来源于一个模拟步骤中的可用速度集合.这意味着相比之下DWA是一种更加有效算法,因为其使用了更小采样空间;然而对于低加速度的机器人来说可能Trajectory Rollout更好, 因为DWA不能对常加速度做前向模拟。 我们需要在stdr_move_base下的config目录中创建dwa_local_planner_params.yaml文件，该配置文件的内容如下：
DWAPlannerROS:
# Robot Configuration Parameters - stdr robot
acc_lim_x: 0.3 # maximum is theoretically 2.0
acc_lim_y: 0.0 # diff drive robot
acc_lim_th: 0.3
max_trans_vel: 0.3 #choose slightly less than the base's capability
min_trans_vel: 0.1 #this is the min trans velocity when there is negligible rotational velocity
max_vel_x: 0.3
min_vel_x: -0.1
max_vel_y: 0.0 #diff drive robot,don't need set vel_y
min_vel_y: 0.0
max_rot_vel: 0.5 #choose slightly less than the base's capability
min_rot_vel: 0.1 #this is the min angular velocity when there is negligible translational velocity
# Goal Tolerance Parameters
yaw_goal_tolerance: 0.1 # 0.1 rad = 5.7 degree
xy_goal_tolerance: 0.12
latch_xy_goal_tolerance: false
# Forward Simulation Parameters
sim_time: 2.0 # 1.7
sim_granularity: 0.025
vx_samples: 6 # default 3
vy_samples: 1 # diff drive robot, there is only one sample
vth_samples: 20 # 20
controller_frequency: 5.0
# Trajectory Scoring Parameters
path_distance_bias: 90.0 # 32.0
goal_distance_bias: 24.0 # 24.0
occdist_scale: 0.3 # 0.01
forward_point_distance: 0.325 # 0.325
stop_time_buffer: 0.2 # 0.2
scaling_speed: 0.20 # 0.25
max_scaling_factor: 0.2 # 0.2
publish_cost_grid: false
# Oscillation Prevention Parameters
oscillation_reset_dist: 0.05 # default 0.05
# Global Plan Parameters
prune_plan: false
下面来依次解释下各参数的意义：
- acc_lim_x:x方向的加速度绝对值
- acc_lim_y:y方向的加速度绝对值,该值只有全向移动的机器人才需配置.
- acc_lim_th:旋转加速度的绝对值.
- max_trans_vel:平移速度最大值绝对值
- min_trans_vel:平移速度最小值的绝对值
- max_vel_x:x方向最大速度的绝对值
- min_vel_x:x方向最小值绝对值,如果为负值表示可以后退.
- max_vel_y:y方向最大速度的绝对值.
- min_vel_y:y方向最小速度的绝对值.
- max_rot_vel:最大旋转速度的绝对值.
- min_rot_vel:最小旋转速度的绝对值.
- yaw_goal_tolerance:到达目标点时偏行角允许的误差，单位弧度.
- xy_goal_tolerance:到达目标点时,在xy平面内与目标点的距离误差.
- latch_xy_goal_tolerance:设置为true,如果到达容错距离内,机器人就会原地旋转,即使转动是会跑出容错距离外.
- sim_time:向前仿真轨迹的时间.
- sim_granularity:步长,轨迹上采样点之间的距离,轨迹上点的密集程度.
- vx_samples:x方向速度空间的采样点数.
- vy_samples:y方向速度空间采样点数.
- vth_samples:旋转方向的速度空间采样点数.
- controller_frequency:发送给底盘控制移动指令的频率.
- path_distance_bias:定义控制器与给定路径接近程度的权重.
- goal_distance_bias:定义控制器与局部目标点的接近程度的权重.
- occdist_scale:定义控制器躲避障碍物的程度.
- stop_time_buffer:为防止碰撞,机器人必须提前停止的时间长度.
- scaling_speed:启动机器人底盘的速度.
- max_scaling_factor:最大缩放参数.
- publish_cost_grid:是否发布规划器在规划路径时的代价网格.如果设置为true,那么就会在~/cost_cloud话题上发布sensor_msgs/PointCloud2类型消息.
- oscillation_reset_dist:机器人运动多远距离才会重置振荡标记.
- prune_plan:机器人前进是是否清除身后1m外的轨迹.
0x04 查看路径规划效果
在上图中蓝色的路径是全局路径规划器使用dijkstra算法来计算得到的路径。在机器人前面有一小段红色的路径就是局部路径规划器使用dwa计算得到的局部路径，而在局部代价地图中有一段黄色的路径。它与全局的蓝色路径重叠在一起，这个就是局部路径规划器发布出来的跟踪的全局路径，因为在规划局部路径时的就是需要参考全局路径的，规划到达目的地路径的大方向不能错了，不能自己规划一个局部路径却偏离了目标点。 下面来查看下全局路径规划器的话题列表：
下面查看局部路径规划器发布的话题列表：
0x05 自动导航效果演示 现在我们基本上已经把自动导航相关的参数都配置好了，现在就可以来看看导航效果了，如下图所示：
通过上面动图可以得知，我们是在Rviz中的2D Nav Goal来选择导航目标点的：
其实在该按钮的背后是向/move_base_simple/goal话题中发布了一条目标点命令而已，如下图所示：
如果我们不想使用Rviz中的2D Nav Goal按钮来设置目标点，我们也可以直接通过往/move_base_simple/goal话题中发布目标点坐标命令： rostopic pub /move_base_simple/goal geometry_msgs/PoseStamped ‘{ header: { frame_id: “map” },pose: { position: {x: 9.59, y: 1.36, z: 0.0}, orientation: {x: 0, y: 0, z: 0.68, w: 0.73}}}’ 通过上述命令，我们就可以发现效果跟我们通过点击2D Nav Goal按钮选择目标点是一样的。其实，这样就是为我们自动化测试自动导航提供了条件，后面我们可以写一个程序来不断的往/move_base_simple/goal话题中发布目标点，然后等待导航结果。 同样也还可以实现定点巡航的需求，我们可以通过2D Nav Goal按钮来选择地图上自己想要定点巡航的点，然后通过echo出来/move_base_simple/goal相应点的坐标，记录下这些坐标点后，我们就可以在循环中不断的发布这些目标点，这样就可以实现机器人在这几个坐标点上来回不断的移动了。
0x06 Bibliography & Reference [1]. 白巧克力亦唯心. global_planner整体分析. https://blog.csdn.net/heyijia0327/article/details/45030929 [2]. ROS Wiki上global_planner主页. http://wiki.ros.org/global_planner [3]. 中国大学MOOC. 机器人操作系统入门讲义. https://sychaichangkun.gitbooks.io/ros-tutorial-icourse163/content/ [4]. ROS Wiki上navfn主页. http://wiki.ros.org/navfn?distro=melodic [5]. PIBOT导航机器人. ROS机器人底盘(16)-move_base(4)-planner. https://www.jianshu.com/p/6d15ab640f34
0x07 Feedback 大家在按照教程操作过程中有任何问题，可以关注ROS小课堂的官方微信公众号，在公众号中给我发消息反馈问题即可，我基本上每天都会处理公众号中的留言！当然如果你要是顺便给ROS小课堂打个赏，我也会感激不尽的，打赏30块还会邀请进ROS小课堂的微信群与更多志同道合的小伙伴一起学习和交流！
