无人车路径规划算法---（4）基于搜索的路径规划算法 II（贪心/Astar）

上篇博客中介绍了一些基本的图搜索算法，其中也重点介绍了基于势场来实现的Dijkstra算法。本篇博客将介绍关于Heuristic Function的图搜索算法

开源了一个结合Dijkstra,Greedy,以及Astar算法的代码，更新在了博主的github主页上，链接见文末

1. Greedy Best First Search Algorithm（贪心算法）

如上篇博客所介绍，BFS与Dijstra算法在搜索目标时是向所有方向进行节点拓展的。如果任务中是需要向多个目标点进行搜索，那么此类方法显然是合适的。但是如果仅仅只是需要向一个目标点进行搜索，那么这样的搜索会增加很多不必要的耗时。
那么此时我们可以尝试，将Dijkstra中的计算距离起点的cost修改为距离终点的cost。将节点的扩展导向终点。这种方法就可以称为贪心算法。
在贪心算法中，首先我们需要定义一个heuristic function来表述当前点距离终点的远近，通常我们会采用曼哈顿距离或者欧拉距离来实现此需求

1.1 算法伪代码

frontier = PriorityQueue()
frontier.put(start, 0)
came_from = dict()
came_from[start] = Nonewhile not frontier.empty():current = frontier.get()if current == goal:breakfor next in graph.neighbors(current):if next not in came_from:priority = heuristic(goal, next)frontier.put(next, priority)came_from[next] = current

1.2 效果对比

上图对比了Dijkstra与Greedy算法之间的效果区别，可以看出，

当地图中没有障碍物遮挡时，Greedy算法的搜索范围以及效率是高于Dijkstra的，因为它让搜索更加具有目的性

当地图中存在障碍物特别是凹形障碍物遮挡时，由于Greedy算法仅考虑与终点的距离，常常会造成"短视"的现象即距离障碍物特别近时才会改变方向重新搜索路径去往终点

2. Astar Algorithm（A* 算法）

上部分Greedy算法展示了引入终点因素后对于搜索算法的影响，其优点与缺点通过效果对比图可以清楚的看出。为了解决单纯考虑终点因素而导致的"短视"问题，我们引入了一个更加均衡的算法，Astar算法。

2.1 算法由来及定义

Astar算法既考虑了距离起点的actual cost同时也考虑了距离终点的heuristic cost，与Dijkstra以及Greedy算法相比较：

与Dijkstra相比，Astar由于考虑与终点的距离因素，因此搜索上会减少很多无效搜索

与Greedy算法相比，Astar通常可以搜索出最短路径（当Heuristic function距离采用曼哈顿距离时）

Astar中每个栅格的cost可以用以下公式表示：
f(n)=g(n)+h(n)f(n) = g(n) + h(n)f(n)=g(n)+h(n)
其中，
g(n)g(n)g(n)表示当前栅格与起点的actual cost
h(n)h(n)h(n)表示当前栅格距离终点的heristic cost
当g(n)g(n)g(n)恒等于0时，算法则变成了Greedy算法；当h(n)h(n)h(n)恒等于0时，算法则是Dijkstra算法。

2.2算法伪代码

frontier = PriorityQueue()
frontier.put(start, 0)
came_from = dict()
cost_so_far = dict()
came_from[start] = None
cost_so_far[start] = 0while not frontier.empty():current = frontier.get()if current == goal:breakfor next in graph.neighbors(current):new_cost = cost_so_far[current] + graph.cost(current, next)if next not in cost_so_far or new_cost < cost_so_far[next]:cost_so_far[next] = new_costpriority = new_cost + heuristic(goal, next)frontier.put(next, priority)came_from[next] = current

2.3 效果对比

上述部分分别介绍了Dijkstra，Greedy以及Astar算法的原理以及优缺点，下图展示了同样起终点以及地图的情况下各个算法的特点：

Dijkstra算法搜索时间较长，搜索范围较大，但是可以搜索出最短路径
Greedy算法搜索时间较短，搜索范围较小，但是路径不是最短路径
Astar算法搜索时间较短，同时搜索范围较小，路径是最短路径（部分情况下）

2.4 Astar算法的最短路径问题

上一篇博客所介绍的Dijkstra算法以及BFS算法，我们强调了其搜索效率可能较低，但是一定能够搜索出最短路径。而对于Astar算法而言，搜索出的路径是否是最短路径以及在实际工程应用中搜索最短路径的必要性都是需要去深入探讨与思考的。

Astar搜索到最短路径的条件。
首先，在介绍最短路径的条件之前，要介绍一个admissible heuristic的概念。其定义是说

对于任意节点n,cost_n代表该节点到达任意终点的代价值（其中到达终点的路径都为最短路径），如果heuristic cost h(n) <= cost_n, 那么此时的h(n)即为admissible heuristic (optimal)

简单的理解，就是当Astar算法的启发函数h(n)h(n)h(n)为admissible function时，则搜索出的路径一定为最短路径

通常heuristic function我们会选用欧拉距离或者曼哈顿距离作为距离终点的计算方式。结合上述admissible heuristic 定义，我们可以知道，当选用欧拉距离时，Astar搜索的路径一定是最短路径；当选用曼哈顿距离时，搜索的路径不一定是最短路径，因为当地图中出现障碍物时，曼哈顿距离算出的值很可能是穿过障碍物的，如下图所示

绿色圆形为起点，紫色五角星为终点，蓝色为障碍物，红色线表示的是欧拉距离，黄色的线表示的是曼哈顿距离，绿色线表示最终搜索出的路径。可以看出曼哈顿距离在存在障碍物时，可能搜出的并不是最短距离。

2.5 示例代码

代码开源在了github主页，仅写了一个简单的demo,其中包括了grid map模块，分别使用Dijkstra,Greedy以及Astar算法进行搜索的模块。链接如下：
https://github.com/FleverX/GridSearch

下一篇博客将介绍Astar实际工程中的优化应用以及基于Astar算法又衍生出的图搜索算法

参考：
https://www.redblobgames.com/pathfinding/a-star/introduction.html