本文以这篇博主的文章为基础【精选】A*算法（超级详细讲解，附有举例的详细手写步骤）-CSDN博客

这篇文章的博主做了一个UI界面，但我感觉，这样对新手关注算法和代码本身反而不利，会被界面的代码所干扰。所以笔者在他的基础上舍去了界面的内容，只关注算法本身。

A星算法的作用就是：已知起点和终点坐标，将地图离散化为网格，可以使用A star算法寻路。

A star算法简单来说三个步骤：一是准备两个列表，开列表和闭列表，开列表将节点移进移出，闭列表只进不出。二是在每一步探索的时候，计算三个“花费”，起点走到当前点的实际花费 G，从当前点到目标点的预估花费 H， 总花费F = G + H。三是计算父节点，父节点的作用是推算花费以及到达终点后倒推路径。

具体来说，我们称现在所处的网格为 checking point k，检查checking point k周围所有的下一步可到达点，并『临时』将这些可到达点的父节点记为checking point k。可到达点是没有障碍物且不在闭列表中的网格（near point 1, near point 2, ......, near point i, ......, near point n），对于near point i，计算起点到near point i的实际花费：

起点到near point i的实际花费 = 起点到checking point k的实际花费 + checking point k到near point i的实际花费。

计算near point i到终点的预估花费：

near point i到终点的预估花费 = near point i到终点的曼哈顿距离。

near point i的总花费 = 实际花费 + 预估花费。这里只是“花费“的一种定义形式，也可以用其他的定义形式。

每个点都是我们建立的点类的类实例，在点类中储存这三个花费，以及储存父节点。

如果near point i不在开列表中，将其加进去。注意我们前面『临时』标记的父节点，这里正式标记checking point k为父节点。

如果near point i已经在开列表中，则说明，near point i在我们到达checking point k之前，处于另一个checking point j时，作为checking point j的可到达点被计算过了。这里我们比较一下near point i旧的总花费和新的总花费，如果新的总花费小，则更新花费，并把near point i的父节点更新为checking point k；如果旧的总花费小，则保持旧的花费，以及保持旧的父节点。

当checking point k的所有可到达点都被检查完后，将其移进闭列表。

之后，从开列表中选一个总花费最小的点作为新的checking point，重复上述的检查可达点操作，直到找到终点，起始时，将起点加入开列表，如果在途中开列表空了，则不存在可达路径。

完整代码如下：

import numpy as np
import json
import matplotlib.pyplot as plt


class Map:
    def __init__(self):
        # 设置起点，终点所在的行列数，左上角为0，0
        start_row = 17
        start_col = 2
        end_row = 9
        end_col = 16
        with open('map.txt', 'r') as f:
            my_map = json.loads(f.read())

        my_map = np.array(my_map)
        self.map = np.where(my_map == 0, 1, np.where(my_map == 1, 0, my_map))
        self.map[start_row, start_col] = -100   # 起点
        self.map[end_row, end_col] = 100        # 终点
        # self.map = np.array([[1, 1, 1, 1, 0, 100],
        #                      [1, 0, 0, 1, 0, 1],
        #                      [1, -100, 0, 1, 0, 1],
        #                      [1, 1, 0, 1, 0, 1],
        #                      [1, 1, 1, 1, 1, 1]])

    def get_start_point(self):
        indices = np.where(self.map == -100)
        return indices[0][0], indices[1][0]

    def get_end_point(self):
        indices = np.where(self.map == 100)
        return indices[0][0], indices[1][0]

    def check_grid(self, point):
        return self.map[point.x, point.y]


class Point:
    def __init__(self, x_, y_):
        self.x = x_
        self.y = y_
        self.father = None
        self.G = 0  # 起点到当前节点所花费的消耗
        self.H = 0  # 到终点的预估消耗
        self.F = 0

    def get_x_y(self):
        return self.x, self.y

    def set_GHF(self, G, H, F):
        self.H = H
        self.G = G
        self.F = F


class Astar:
    def __init__(self):
        self.openlist = []
        self.closelist = []
        self.map = Map()
        self.start_x, self.start_y = self.map.get_start_point()
        self.start_position = None
        self.end_x, self.end_y = self.map.get_end_point()
        self.find_path = False
        self.path = []

    def cal_GHF(self, checkpoint):
        if checkpoint.father is not None:
            G = checkpoint.father.G + 1   # 起点到父节点的花费加上父节点到本节点的花费
        else:
            G = 0
        H = abs(checkpoint.x - self.end_x) + abs(checkpoint.y - self.end_y)
        F = G + H
        return G, H, F

    def add_near_point(self, check_point):
        x, y = check_point.get_x_y()
        tmp_list = [Point(x-1, y-1), Point(x-1, y), Point(x-1, y+1),
                    Point(x, y-1), Point(x, y+1),
                    Point(x+1, y-1), Point(x+1, y), Point(x+1, y+1)]
        near_list = []
        for pi in tmp_list:
            if self.map.map.shape[0] > pi.x >= 0 and self.map.map.shape[1] > pi.y >= 0:     # 在地图范围内
                if self.map.check_grid(pi) == 100:
                    return [pi]
                elif self.map.check_grid(pi) == 1 and self.not_in_closelist(pi):
                    near_list.append(pi)

        return near_list

    def choose_min_F_point(self):
        minF = 1e10
        choosed_point = None
        for pi in self.openlist:
            if pi.F < minF:
                minF = pi.F
                choosed_point = pi
        return choosed_point

    def not_in_openlist(self, pi):
        not_in = True
        for pii in self.openlist:
            if pii.x == pi.x and pii.y == pi.y:
                not_in = False
        return not_in

    def not_in_closelist(self, pi):
        not_in = True
        for pii in self.closelist:
            if pii.x == pi.x and pii.y == pi.y:
                not_in = False
        return not_in

    def run(self):
        self.start_position = Point(self.start_x, self.start_y)
        G, H, F = self.cal_GHF(self.start_position)
        self.start_position.set_GHF(G, H, F)
        self.openlist.append(self.start_position)

        while True:
            checking_point = self.choose_min_F_point()
            if checking_point is None or self.find_path:
                print("End!")
                break
            self.openlist.remove(checking_point)
            self.closelist.append(checking_point)
            near_list = self.add_near_point(checking_point)
            for pi in near_list:
                if self.map.check_grid(pi) == 100:
                    self.find_path = True
                    # print("find path:\n{}".format(checking_point.get_x_y()))
                    self.path.append([checking_point.get_x_y()[0], checking_point.get_x_y()[1]])
                    reverse_point_father = checking_point.father
                    while reverse_point_father.father is not None:
                        # print(reverse_point_father.get_x_y())
                        self.path.append([reverse_point_father.get_x_y()[0], reverse_point_father.get_x_y()[1]])
                        reverse_point_father = reverse_point_father.father
                    break

                if self.not_in_openlist(pi):
                    pi.father = checking_point
                    G, H, F = self.cal_GHF(pi)
                    pi.set_GHF(G, H, F)
                    self.openlist.append(pi)
                else:
                    G_old = pi.G + 1
                    G_new = checking_point.G + 1
                    if G_new < G_old:
                        pi.father = checking_point
                        G, H, F = self.cal_GHF(pi)
                        pi.set_GHF(G, H, F)

        # 打印路性
        print("path: ")
        print(self.path)
        self.path = self.path[::-1]
        with open('my_path.txt', 'w') as file:
            for item in self.path:
                file.write(str(item) + '\n')

    def check(self):
        for pi in self.path:
            self.map.map[pi[0], pi[1]] = 2

        fig = plt.figure("A Star Algorithm")

        cmap = plt.cm.colors.ListedColormap(['yellow', 'black', 'white', 'blue', 'green'])

        # 创建一个离散的归一化器，根据不同数值映射到不同颜色
        bounds = [-101, -99, 0, 1, 2, 99, 101]
        norm = plt.cm.colors.BoundaryNorm(bounds, cmap.N)

        # 显示二维数组
        plt.imshow(self.map.map, cmap=cmap, norm=norm)

        # 添加颜色条，以便查看数值与颜色的对应关系
        cb = plt.colorbar()

        # 显示图
        plt.show()


if __name__ == "__main__":
    astar = Astar()
    astar.run()
    astar.check()

使用我的代码前，可以先运行文章开头提到的那个博主的代码，生成一个地图保存，我的代码加载他的地图（也可以使用我注释掉的那个地图），我的地图中，1表示可行网格，0表示障碍物网格。如果我们足够幸运的话，我们两篇文章的结果应该是一致的。