路径规划——贪婪最佳优先搜索
学习A算法前先学习一下贪婪最佳优先搜索算法，在学习过程中在网上找了一些博客、文章还有视频来看以深入理解此算法，但是实际情况却是非常令人恼怒。有些文章标题是贪婪最佳优先搜索，内容却是其他的算法，还有的是前面一部分是GBFS，后面却突然变成了其他的算法，更有甚者，先是讲了GBFS的原理，然后展示算法流程，算法流程却是Dijkstra算法或A算法，另外，还有算法效果图胡乱配的，很明显就不是GBFS算法的效果图，让我看得脑子懵懵的，刚开始我还以为是我代码中算法步骤哪里出错了，或者是启发式函数不对，本来GBFS并不复杂，一会儿就能够理解掌握，我却花费了两天(其实是两个半天了，毕竟每天都是睡到中午)来学此算法，这些技术博主/分享者能不能都负责一点啊！真的是要枯了~
好，言归正传，下面学习贪婪最佳优先搜索算法，其实主要是我学习过程中的记录，也是和大家分享一下，希望能够帮助需要学习或是回顾此算法的朋友，如果有误还请指出，谢谢！
贪婪最佳优先搜索(Greedy Best First Search)与Dijkstra类似，却又有不同之处，Dijkstra算法使用当前节点与起点间的实际距离对优先队列排序，而GBFS算法使用当前节点到目标点的估计距离对优先队列排序，将首先探索当前节点的邻节点中离目标点最近的节点，每次迈出的都是最贪婪的一步。
算法原理
贪婪最佳优先搜索是一种启发式搜索算法，核心是每次选择当前节点中启发式函数值最小的节点进行扩展搜索。对于一个图G，从图G中一起点开始遍历，使用启发式函数计算起点的若干可通行的邻节点到目标点的预估代价，并使用优先队列存储待遍历的各个节点，然后下次遍历时从优先队列中选择预估代价最小的节点，重复上述操作直到搜索到目标点或者是遍历完所有节点。
算法流程如下：
在运算过程中每次从open中选择启发式函数值即到目标点的预估距离最小的节点作为当前节点进行遍历；
如果当前节点是目标节点则搜索成功，算法结束；
否则将当前节点从open中移除，并添加到closed中以标记该节点已被遍历；
对于当前节点的若干邻节点，如果已经添加到了closed中了则跳过，如果不在closed，检查是否在open中出现，如果不在open中，则计算其启发式函数值，并将其添加到open中（在这里有人认为还要再加上如果已经出现在open中了则再次计算其启发式函数值，如果小于之前的值则更新，但是目标点是固定的，对于一个节点来讲其启发式函数值也是固定的，又何必要更新呢）。重复以上操作直到open为空或找到目标点。
下面用几张图来展示算法流程：
从起点开始在向上搜索的过程上面的节点和右边的节点的启发式函数值是一样的，这里以向上为优先，这个优先是邻节点存储的顺序次序。
算法实现
import heapq
class Node:
def __init__(self, name, h=0):
self.name = name # 节点名称
self.h = h # 启发式函数值
self.parent = None
def __lt__(self, other):
# 便于优先队列利用启发式函数值h进行排序
return self.h < other.h
def gbfs(start, goal, neighbors, heuristic):
open_list = []
closed_list = set()
start_node = Node(start, heuristic(start))
heapq.heappush(open_list, start_node)
while open_list:
current_node = heapq.heappop(open_list)
if current_node.name == goal:
path = []
while current_node:
path.append(current_node.name)
current_node = current_node.parent
return path[::-1]
closed_list.add(current_node.name)
for neighbor in neighbors[current_node.name]:
if neighbor in closed_list:
continue
neighbor_node = Node(neighbor, heuristic(neighbor))
neighbor_node.parent = current_node
if neighbor_node not in open_list:
heapq.heappush(open_list, neighbor_node)
return None
# 启发式函数
def heuristic(node):
# 这里是具体的启发式函数计算过程
return 0
下面举个栗子：
在栅格地图上找到从起点到终点的最佳路径，这里假设只允许朝上下左右四个方向搜索，启发式函数使用曼哈顿距离函数
import heapq
import math
import numpy as np
import matplotlib.pyplot as plt
import matplotlib.animation as animation
class GBFS:
def __init__(self,grid,start,goal,board_size):
self.grid = grid
self.start = start
self.goal = goal
self.board_size = board_size
class Node:
def __init__(self, position, h=0, parent=None):
self.position = position # position of node
self.h = h # heuristic value
self.parent = parent
def __lt__(self, other):
# In order to priority queue using heuristic function values for sorting
return self.h < other.h
def plan(self):
open = []
closed = set() # Used to mark nodes that are closed
self.searched = [] # Used to record nodes that are searched
start_node = self.Node(self.start, self.heuristic(self.start))
heapq.heappush(open, start_node)
while open:
# Select the node closest to the start node
current_node = heapq.heappop(open)
if current_node.position in closed:
continue
# Append the current node into searched nodes
# self.searched.append(current_node.position)
closed.add(current_node.position)
# Break when the current node is the goal
if current_node.position == self.goal:
self.path = []
while current_node:
self.path.append(current_node.position)
current_node = current_node.parent
self.path = self.path[::-1]
return len(self.path)-1
# Find the neighbors of the current node and determine in turn if they have already been closed
neighbors = self.get_neighbors(current_node.position)
for neighbor in neighbors:
h = self.heuristic(neighbor)
# If the current node has been searched, skip it
if neighbor in self.searched:
continue
neighbor_node = self.Node(neighbor,h,current_node)
heapq.heappush(open,neighbor_node)
self.searched.append(neighbor)
return None
def get_neighbors(self, node):
neighbors = []
next_directions = [(0,1),(1,0),(0,-1),(-1,0)]
# next_directions = [(-1,0),(1,0),(0,1),(0,-1),(1,1),(1,-1),(-1,1),(-1,-1)]
for next_d in next_directions:
neighbor = (node[0] + next_d[0], node[1] + next_d[1])
if self.board_size <= neighbor[0] < len(self.grid)-self.board_size and self.board_size <= neighbor[1] < len(self.grid[0])-self.board_size:
if self.grid[neighbor[0]][neighbor[1]] == 0:
neighbors.append(neighbor)
return neighbors
def heuristic(self, node):
# Manhattan Distance
return abs(node[0] - self.goal[0]) + abs(node[1] - self.goal[1])
# def heuristic(self, node):
# # Chebyshev Distance
# D = 1
# D2 = 1.414
# dx = abs(node[0] - self.goal[0])
# dy = abs(node[1] - self.goal[1])
# return D * (dx + dy) + (D2 - 2 * D) * min(dx, dy)
# def heuristic(self, node):
# # Euclidean Distance
# D = 1
# dx = abs(a[0] - self.goal[0])
# dy = abs(a[1] - self.goal[1])
# return D * math.sqrt(dx * dx + dy * dy)
def visualize_grid(self, cost):
fig, ax = plt.subplots()
grid = np.array(self.grid)
plt.imshow(grid, cmap='Greys', origin='upper')
plt.title("BFS\n"+"Cost: "+str(cost))
# Mark the start and goal point
plt.scatter(self.start[1], self.start[0], c='green', marker='o', s=60)
plt.scatter(self.goal[1], self.goal[0], c='blue', marker='o', s=60)
# # Mark locations which has been visited
# for node in self.searched:
# plt.gca().add_patch(plt.Rectangle((node[1]-0.5, node[0]-0.5), 1, 1, fill=True, color='gray', alpha=0.5))
# # 标记路径
# if self.path:
# path_x, path_y = zip(*self.path)
# plt.plot(path_y, path_x, c='red', linewidth=2)
visited_patches = []
path_line, = ax.plot([], [], c='red', linewidth=2)
# for order in range(len(self.searched)):
# node = self.searched[order]
# plt.Rectangle((node[1]-0.5, node[0]-0.5), 1, 1, fill=True, color='gray', alpha=0.5)
def update(frame):
if frame < len(self.searched):
node = self.searched[frame]
patch = plt.Rectangle((node[1]-0.5, node[0]-0.5), 1, 1, fill=True, color='gray', alpha=0.5)
visited_patches.append(patch)
ax.add_patch(patch)
elif self.path:
path_x, path_y = zip(*self.path)
path_line.set_data(path_y, path_x)
return visited_patches + [path_line]*10
ani = animation.FuncAnimation(fig, update, frames=len(self.searched) + 10, interval=100, repeat=False)
ani.save("map_animate1.gif",writer='pillow')
# ani.save("map_animate.mp4",writer='ffmpeg')
plt.savefig("map_animate1.png",bbox_inches='tight')
plt.show()
结果：
启发式函数用对角距离试一下：
import heapq
import math
import numpy as np
import matplotlib.pyplot as plt
import matplotlib.animation as animation
class GBFS:
def __init__(self,grid,start,goal,board_size):
self.grid = grid
self.start = start
self.goal = goal
self.board_size = board_size
class Node:
def __init__(self, position, h=0, parent=None):
self.position = position # position of node
self.h = h # heuristic value
self.parent = parent
def __lt__(self, other):
# In order to priority queue using heuristic function values for sorting
return self.h < other.h
def plan(self):
open = []
closed = set() # Used to mark nodes that are visited
self.searched = [] # Used to record nodes that are searched
start_node = self.Node(self.start, self.heuristic(self.start))
heapq.heappush(open, start_node)
while open:
# Select the node closest to the start node
current_node = heapq.heappop(open)
if current_node.position in closed:
continue
# Append the current node into searched nodes
# self.searched.append(current_node.position)
closed.add(current_node.position)
# Break when the current node is the goal
if current_node.position == self.goal:
self.path = []
while current_node:
self.path.append(current_node.position)
current_node = current_node.parent
self.path = self.path[::-1]
return len(self.path)-1
# Find the neighbors of the current node and determine in turn if they have already been closed
neighbors = self.get_neighbors(current_node.position)
for neighbor in neighbors:
h = self.heuristic(neighbor)
# If the current node has been searched, skip it
if neighbor in self.searched:
continue
neighbor_node = self.Node(neighbor,h,current_node)
heapq.heappush(open,neighbor_node)
self.searched.append(neighbor)
return None
def get_neighbors(self, node):
neighbors = []
next_directions = [(0,1),(1,0),(0,-1),(-1,0)]
# next_directions = [(-1,0),(1,0),(0,1),(0,-1),(1,1),(1,-1),(-1,1),(-1,-1)]
for next_d in next_directions:
neighbor = (node[0] + next_d[0], node[1] + next_d[1])
if self.board_size <= neighbor[0] < len(self.grid)-self.board_size and self.board_size <= neighbor[1] < len(self.grid[0])-self.board_size:
if self.grid[neighbor[0]][neighbor[1]] == 0:
neighbors.append(neighbor)
return neighbors
# def heuristic(self, node):
# # Manhattan Distance
# return abs(node[0] - self.goal[0]) + abs(node[1] - self.goal[1])
def heuristic(self, node):
# Chebyshev Distance
D = 1
D2 = 1.414
dx = abs(node[0] - self.goal[0])
dy = abs(node[1] - self.goal[1])
return D * (dx + dy) + (D2 - 2 * D) * min(dx, dy)
# def heuristic(self, node):
# # Euclidean Distance
# D = 1
# dx = abs(a[0] - self.goal[0])
# dy = abs(a[1] - self.goal[1])
# return D * math.sqrt(dx * dx + dy * dy)
def visualize_grid(self, cost):
fig, ax = plt.subplots()
grid = np.array(self.grid)
plt.imshow(grid, cmap='Greys', origin='upper')
plt.title("BFS\n"+"Cost: "+str(cost))
# Mark the start and goal point
plt.scatter(self.start[1], self.start[0], c='green', marker='o', s=60)
plt.scatter(self.goal[1], self.goal[0], c='blue', marker='o', s=60)
# # Mark locations which has been visited
# for node in self.searched:
# plt.gca().add_patch(plt.Rectangle((node[1]-0.5, node[0]-0.5), 1, 1, fill=True, color='gray', alpha=0.5))
# # 标记路径
# if self.path:
# path_x, path_y = zip(*self.path)
# plt.plot(path_y, path_x, c='red', linewidth=2)
visited_patches = []
path_line, = ax.plot([], [], c='red', linewidth=2)
# for order in range(len(self.searched)):
# node = self.searched[order]
# plt.Rectangle((node[1]-0.5, node[0]-0.5), 1, 1, fill=True, color='gray', alpha=0.5)
def update(frame):
if frame < len(self.searched):
node = self.searched[frame]
patch = plt.Rectangle((node[1]-0.5, node[0]-0.5), 1, 1, fill=True, color='gray', alpha=0.5)
visited_patches.append(patch)
ax.add_patch(patch)
elif self.path:
path_x, path_y = zip(*self.path)
path_line.set_data(path_y, path_x)
return visited_patches + [path_line]*10
ani = animation.FuncAnimation(fig, update, frames=len(self.searched) + 10, interval=100, repeat=False)
ani.save("map_animate.gif",writer='pillow')
# ani.save("map_animate.mp4",writer='ffmpeg')
plt.savefig("map_animate.png",bbox_inches='tight')
plt.show()
结果：
这个例子中使用曼哈顿距离和使用对角距离得到的路径虽然不同但是路径长度却是相等的，那么，总是相等的吗？答案是否定的，是否相等取决于实际情况。如下情况：
使用曼哈顿距离：
使用对角距离：
这种情况下使用曼哈顿距离路径较短。
再来看一种情况：
使用曼哈顿距离：
使用对角距离：
这里是使用对角距离启发式函数得到的路径较短。
在图搜索中，如果只允许朝上下左右四个方向移动，则使用曼哈顿距离；如果可以朝上下左右以及对角线方向共八个方向移动，则使用对角距离；如果可以朝任意方向移动，则使用欧几里得距离。但是启发式函数是用于预估当前位置到目标点间的距离，所以使用不同的启发式函数，结果也会有所不同。在上面的程序中尽管只允许朝上下左右四个方向移动，有时使用对角距离也会得到较曼哈顿距离好更的结果。
大家可以将搜索邻居节点时的方向由4个改为8个试试。
next_directions = [(0,1),(1,0),(0,-1),(-1,0)]
# next_directions = [(-1,0),(1,0),(0,1),(0,-1),(1,1),(1,-1),(-1,1),(-1,-1)]
关于图搜索中的启发式函数的应用，下次在A* 算法中会再具体学习。
