BFS广度优先算法（Breadth-First Search）
# 使用队列实现广度优先寻路算法
import queue
def bfs(env_data, loc):
visited = []
path = {}
final_path = []
q = queue.Queue()
q.put(loc)
while not q.empty():
# 机器人可移动的位置列表
move_list = [move_robot(loc, act) for act in valid_actions(env_data, loc)]
for move in move_list:
# 如果位置未被访问，则加入队列
if move not in visited:
q.put(move)
# 将移动位置和其之前的位置记录下来
path[move] = loc
# 取出队列最先的元素
loc = q.get()
# 位置被访问后，记录在visited列表中
if loc not in visited:
visited.append(loc)
if loc == loc_map['destination']:
print("到达终点", loc)
break
# 得到算法找到的路径
cur_loc = loc_map['destination']
final_path.append(cur_loc)
while loc_map['start'] not in final_path:
cur_loc = path[cur_loc]
final_path.append(cur_loc)
final_path = final_path[::-1]
print(final_path)
bfs(env_data, robot_current_loc)
A*算法的出现时因为
- 深度/广度优先算法找到的路径可能不是最优解，但是运行时间短；
- 而
Dijkstra算法能够保证找到最短路径，但是运行时间慢；
zzz那怎样才能在保证效果的情况话，让运行时间也短呢？
所以A*算法其实是结合了广度优先算法和Dijkstra算法，关键就是下面这个等式：
f(n) = g(n) + h(n)
g(n) = 从起点 A 移动到指定方格的移动代价。
h(n) = 从指定的方格移动到终点 B 的估算成本。
- 一种极端情况，如果
h(n)是0，则只有g(n)起作用，此时A*演变成Dijkstra算法，这保证能找到最短路径。
- 另一种极端情况，如果
h(n)比g(n)大很多，则只有h(n)起作用，A*演变成BFS算法。
所以出于运行时间和运行效果的考虑，你可以权衡g(n)和h(n)，修改任意一个，从而得到一条好路径而不是一条完美的路径。
A*算法的流程其实是比较简单的，现在我们把所有步骤放在一起：
- 把起点加入 open list 。
- 重复如下过程：
- 遍历 open list ，查找 F 值最小的节点，把它作为当前要处理的节点。
- 把这个节点移到 close list 。
- 操作当前方格的 8 个相邻方格的每一个方格：
- 如果它是不可抵达的或者它在 close list 中，忽略它。否则，做如下操作。
- 如果它不在 open list 中，把它加入 open list ，并且把当前方格设置为它的父亲，记录该方格的 F ， G 和 H 值。
- 如果它已经在 open list 中，检查这条路径 ( 即经由当前方格到达它那里 ) 是否更好，用 G 值作参考。更小的 G 值表示这是更好的路径。如果是这样，把它的父亲设置为当前方格，并重新计算它的 G 和 F 值。如果你的 open list 是按 F 值排序的话，改变后你可能需要重新排序。
- 停止，如果满足条件：
- 把终点加入到了 open list 中，此时路径已经找到了，或者
- 查找终点失败，并且 open list 是空的，此时没有路径。
- 保存路径。从终点开始，每个方格沿着父节点移动直至起点，这就是你的路径。
