A*算法(python)

最近要用A star算法
推荐一个非常直观的学习A算法的网站
https://www.redblobgames.com/pathfinding/a-star/introduction.html
还有一个各种算法的对比
https://github.com/zhm-real/PathPlanning
下面是自己写的A算法

"""
@Date:  2021/11/10
@Author:Xmmm0569
@Brief: Path Planning:A Star Algorithm
"""
import numpy as np
import matplotlib.pyplot as plt
import copy


maps = np.full((10, 10), 8)  # maps[0][0] 起点  maps[9][9] 终点
for i in range(10):
    for j in range(10):
        if (i == 0 and j == 0) or (j == 9 and i == 9):
            continue
        if np.random.uniform(0, 10) < 3:
            maps[i][j] = 0
# print(maps)


class Astar:
    def __init__(self):
        self.start = [0, 0]
        self.end = [9, 9]
        self.open = []  # open表[x, y, detax, detay, g, f]
        self.close = []  # 同open表
        self.best_path = []

    def h_value(self, now):
        return abs(now[0] - self.end[0]) + abs(now[1] - self.end[1])

    @staticmethod
    def g_value(father, son):
        return np.sqrt((son[0] - father[0]) ** 2 + (son[1] - father[1]) ** 2) + father[4]

    def f_value(self, father, son):
        return self.g_value(father, son) + self.h_value(son)

    @staticmethod
    def check_in_list(now, alist):
        is_in = False
        index = 0
        for tem in range(len(alist)):
            if alist[tem][0] == now[0] and alist[tem][1] == now[1]:
                is_in = True
                index = tem
                break
        return is_in, index

    def check_son(self, now):
        #  障碍物为0 其他为8
        for detax, detay in [[0, -1], [-1, 0], [1, 0], [0, 1]]:
            son = [now[0] + detax, now[1] + detay]
            if son[0] > 9 or son[1] > 9 or son[0] < 0 or son[1] < 0:  # 出界
                continue
            if maps[son[0]][son[1]] == 0:  # 障碍物
                continue
            g_son = self.g_value(now, son)
            f_son = self.f_value(now, son)
            parament = [son[0], son[1], detax, detay, g_son, f_son]
            is_in, index = self.check_in_list(son, self.open)
            if is_in:
                if f_son < self.open[index][5]:
                    self.open[index] = parament
                continue
            is_in, index = self.check_in_list(son, self.close)
            if is_in:
                if f_son < self.close[index][5]:
                    self.close.remove(self.close[index])
                    self.open.append(parament)
                continue
            self.open.append(parament)

    def main(self):
        self.__init__()
        now = self.start
        h_now = self.h_value(now)
        self.open.append([now[0], now[1], 0, 0, 0, h_now])
        ite = 1
        while ite < 2000:
            if not self.open:
                # print("没有找到最优路径")
                return
            best = sorted(self.open, key=lambda x: x[-1])[0]
            self.close.append(best)
            if best[0] == self.end[0] and best[1] == self.end[1]:
                # print('已找到目标点')
                return
            self.check_son(best)
            self.open.remove(best)
            ite += 1

    def path_backtrace(self):
        path = self.end
        self.best_path.append(path)
        tem = 0
        while tem < len(self.close):
            for index in range(len(self.close)):
                if path[0] == self.close[index][0] and path[1] == self.close[index][1]:
                    x = path[0] - self.close[index][2]
                    y = path[1] - self.close[index][3]
                    path = [x, y]
                    self.best_path.append(path)
                    break
            if path == self.start:
                break
            tem += 1
        return self.best_path, len(self.best_path)

    def draw(self):
        new_map = copy.deepcopy(maps)
        new_map[0][0] = 6
        new_map[9][9] = 7
        for x, y in self.best_path:
            new_map[x, y] = 5
        plt.imshow(new_map, cmap='gray')
        ticks = np.arange(0, 10, 1)
        plt.xticks(ticks)
        plt.yticks(ticks)
        plt.grid(True)
        plt.show()


if __name__ == '__main__':
    a1 = Astar()
    a1.main()
    best_path, path_long = a1.path_backtrace()
    a1.draw()

其中h值的计算可以用很多方法，之后我的应用场景为 上下左右 移动。就用了曼哈顿距离，还可以用欧几里得距离、对角距离等，主要看自己的应用场景。
之后要做的是一个图里面不同起点终点找到两条不冲突的路径，弄好之好再来补充。