保研面试准备——A*算法在循迹机器人中的应用和实现

本文参考兰大大佬scdn：无人驾驶汽车系统入门（十六）——最短路径搜索之A*算法

适合两文配合食用（只是对代码的理解放在这里）

首先先介绍BFS广度优先算法是如何找到最优路径的：

下面是BFS的python伪代码：意思就是我首先从起点开始得到邻节点然后不断的查看没有遍历过的邻节点直至遍历结束。
　　def BFS(root)
　　　　Q=[]   //
　　　　Q.append(root[0])
　　　　while len(Q)>0:
　　　　　　node=Q.pop(0)//pop操作将list头的元素弹出并且返回该弹出元素
　　　　　　print (node)
　　　　　　#将所有子节点入队列
　　　　　　for i in node_child:
　　　　　　　　Q.append(node_child[i])

//——————————————————————修改算法为机器人路径寻找的算法————————————//

frontier = Queue()//创建一个队列（PYTHON中的List）
frontier.put(start)//将第一个起点放入队列
visited = {}//此字典用来记录已经遍历过的方格
visited[start] = True //起点是遍历过的节点

while not frontier.empty()://直到所有的节点都遍历完
   current = frontier.get()//这里应该是pop 遍历结束之后将其弹出。
   for next in graph.neighbors(current)://对每一个邻节点操作
      if next not in visited://判断邻节点是否遍历过
         frontier.put(next)//没有遍历过就将其放入下面要遍历的节点（frontier）中
         visited[next] = True //标记已经遍历

//——————————————————————修改算法为可以回溯路径的算法————————————//

frontier = Queue()
frontier.put(star)
came_from = {}
came_from[start] = None //存放的是其上一个节点（这里我们假设上个节点就是距离起始点最近的节点）

while not frontier.empty()://直到所有的节点都遍历完
   current = frontier.pop()//得到第一个frontier并且弹出
   for next in graph.neighbors(current)://对此frontier的邻居节点判断
      if next not in came_from://如果邻居节点不在之前遍历过的节点
         frontier.put(next)//那么就将其放入frontier中之后去遍历
         came_from[next] = current//将当前位置设置为下一个next节点的父节点（上一个节点）
//遍历结束之后就可以得到最优路径了
current = goal//从目的地触发
path = []
while current != start
   path.append(current)//不断的添加父节点
   current = came_from[current]
path.append(start)
path.reverse()

但是我们的BFS就是简单的穷举，遍历，如何更智能一点呢？

我们可以用上源节点和目的节点的位置信息来引导最佳路径的寻找，启发式的寻找

//如何加入这个目的点和原点信息来启发路径寻找呢？
//曼哈顿距离是度量两点的一种距离，我们用曼哈顿距离的小大来表示距离目标点的进远。
//所以所有的节点都有一个目标级
frontier = PriorityQueue()//因为我们有优先级了，所以要用优先级队列，曼哈顿距离越小，优先级越高
frontier.put(start, 0)
came_from = {}
came_from[start] = None

while not frontier.empty():
   current = frontier.get()

   if current == goal://一旦搜索到目标（不管中间的路径记录是不是最佳）我就马上停止遍历
      break

   for 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.基于距离的优先级搜索使得到最后到达目的节点中间遍历的节点，没有包含最优路径（类似于SLAM中你没有得到全局地图，你的路径规划也只能是贪心次优的）

3.在BFS中的确上一个点是距离原点最近的，但是之后启发式的算法，不一定就可以使得上个点是最好的回到原点的方式。

所以按照原文中说的，我们加上迪杰斯特拉算法：

迪杰斯特拉算法伪代码：
frontier = PriorityQueue()
frontier.put(start, 0)
came_from = {}//记录了某节点离源节点最近的路径的父节点
cost_so_far = {}//记录了某节点之前最近的父节点的cost（如果没有遍历过则为None）
came_from[start] = None
cost_so_far[start] = 0

while not frontier.empty():
   current = frontier.get()

   if current == goal:
      break

   for next in graph.neighbors(current):
      new_cost = cost_so_far[current] + 1
      if next not in cost_so_far or new_cost < cost_so_far[next]://如果“该节点的邻节点”没有遍历过，或者说遍历过并且之前的遍历发现的路径更远（cost更大）
         cost_so_far[next] = new_cost//则将此次路径更新到下个节点
         priority = new_cost//cost决定优先级
         frontier.put(next, priority)
         came_from[next] = current

//————————————————————————————————A*——————————————————————————————//
//一方面，我们需要算法有方向地进行扩散（启发式），另一方面我们需要得到尽可能最短的路径，因此A*就诞生了， 它结合了Dijkstra和启发式算法的优点
frontier = PriorityQueue()
frontier.put(start, 0)
came_from = {}
cost_so_far = {}
came_from[start] = None
cost_so_far[start] = 0

while not frontier.empty():
   current = frontier.get()

   if current == goal:
      break

   for next in graph.neighbors(current):
      new_cost = cost_so_far[current] + 1
      if next not in cost_so_far or new_cost < cost_so_far[next]:
         cost_so_far[next] = new_cost
         priority = new_cost + heuristic(goal, next)//两种代价共同决定优先级
         frontier.put(next, priority)
         came_from[next] = current