【Roban教程】路径规划

路径规划是通过 humanoid_planner_2d 包来实现的。此 package 能订阅当前位置信息 /initial pose 、二维的平面图／map 以及给定的目标点／mov_base_simple/goal, 然后根据这些信息将到达目标点最短的路径画出来。实现结果如图1所示。

图1路径规划图

但是，在实际运行时发现，当机器人行走经过障碍物时，如果按照当前路径一只追求路径最短而太靠近障碍物，会导致机器人不能较好地躲避障碍物。所以我们希望机器人经过路径最短的前提下，尽量远离障碍物。因此，我们需要把可以经过的部分按照距离障碍物的远近计算权重，距离障碍物越近，权重就越大。以下是代码的核心部分：

首先我们先设定一个常量SHADOW_ RADIUS，字面意思，这代表地图障碍物所能影响到的范围。我们从地图的(0,0) 位置开始，以此对地图像素点进行分析。若此点已被占用，我们则给他赋值为OBSTACLE_ COST (障碍物)。如果此点未被占用，紧接着我们便从此点出发，以SHADOW_ RADIUS的距离范围向上下左右四个方向进行迭代，次数为设定的影子半径:若在影子范围内仍然是障碍物，则不做处理；反之，则根据距离起始点的距离设定权重一距离越近， 权重越大。结果显而易见，当通过两个障碍物的过道时，必然会从中间穿过以保证机器人所经过路径为权重最小的。如图2和图3为两个实际路径规划效果。

图2  优化之后的路径1

图3 优化之后的路径2

实现结果如图4所示

图4 rqt_graph