路径规划——贪婪最佳优先搜索

路径规划——贪婪最佳优先搜索

学习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