A* 算法 Python 代码

说明：
不含代码讲解。该代码为matlab代码改写成的python代码并经过我的相关优化，有一定参考性，可直接作为库调用，有很强的完整性。在pycharm debug调试中你可以测试该算法逻辑，希望对你有帮助。

使用方法：
(1)
from 文件名 import A_star
a_star = A_star(matrix=[], step=1000, way=[“R”, “L”, “D”, “U”, “RU”, “RD”, “LU”, “LD”], wall=0)
road = a_star.run(start_point=[],end_point=[]) #可重复使用

或者直接
from 文件名 import A_star
road = A_star(matrix=[], step=1000, way=[“R”, “L”, “D”, “U”, “RU”, “RD”, “LU”, “LD”], wall=0).run(start_point=[],end_point=[]) 

输出为二位点坐标数组eg:[[10, 10], [9, 9], [8, 8], [7, 7], [6, 6], [5, 5], [4, 4], [3, 3], [2, 2], [1, 1], [0, 0]]

(2)参数说明：
start_point 为起始坐标.
end_point为终点坐标.
matrix为地图/迷宫地图，bool类型.
wall为matrix里True/False哪个为墙，False为墙写0，True为墙写1.
weight为权值（不用特别调整）.
corner_amend为拐角优化，1开启，0关闭（不用特别调整）.
step表示运行一定循环后退出程序，防止因为地图过大无限运行。代码默认无限循环即step=float(“inf”)，推荐加入限制条件，减少特定程序时间.
way表示可以走的方向，eg：[“R”, “L”, “D”, “U”, “RU”, “RD”, “LU”, “LD”].

import numpy as np
from math import sqrt


class A_star:
    def __init__(self, matrix, weights=1, corner_amend=1, step=float("inf"), way=["R", "L", "D", "U", "RU", "RD", "LU", "LD"], wall=0):
        self.matrix = matrix
        self.weights = weights
        self.corner_amend = corner_amend
        self.matrix_length = len(self.matrix[0])
        self.matrix_width = len(self.matrix)
        self.step = step
        # 当你知道你的 way 配置保证问题的时候下面三行可进行更改，虽然没有加快多少速度
        self.way = list(map(lambda x: x.upper(), way))
        if len(set(self.way + ["R", "L", "D", "U", "RU", "RD", "LU", "LD"])) != 8:
            exit("Error way")
        self.wall = wall
        self.field = np.array(np.copy(self.matrix), dtype=float)
        for i in range(self.matrix_width):
            for j in range(self.matrix_length):
                if self.field[i][j] == self.wall:
                    self.field[i][j] = float("inf")

    def run(self, start_point, end_point):
        self.fieldpointers = np.array(np.copy(self.matrix), dtype=str)
        self.start_point = start_point
        self.end_point = end_point
        if int(self.matrix[self.start_point[0]][self.start_point[1]]) == self.wall or int(self.matrix[self.end_point[0]][self.end_point[1]] == self.wall):
            exit("start or end is wall")
        self.fieldpointers[self.start_point[0]][self.start_point[1]] = "S"
        self.fieldpointers[self.end_point[0]][self.end_point[1]] = "G"
        return self.a_star()

    def a_star(self):
        setopen = [self.start_point]
        setopencosts = [0]
        setopenheuristics = [float("inf")]
        setclosed = []
        setclosedcosts = []
        movementdirections = self.way
        while self.end_point not in setopen and self.step:
            self.step -= 1
            total_costs = list(np.array(setopencosts) + self.weights * np.array(setopenheuristics))
            temp = np.min(total_costs)
            ii = total_costs.index(temp)
            if setopen[ii] != self.start_point and self.corner_amend == 1:
                new_ii = self.Path_optimization(temp, ii, setopen, setopencosts, setopenheuristics)
                ii = new_ii
            [costs, heuristics, posinds] = self.findFValue(setopen[ii], setopencosts[ii])
            setclosed = setclosed + [setopen[ii]]
            setclosedcosts = setclosedcosts + [setopencosts[ii]]
            setopen.pop(ii)
            setopencosts.pop(ii)
            setopenheuristics.pop(ii)
            for jj in range(len(posinds)):
                if float("Inf") != costs[jj]:
                    if not posinds[jj] in setclosed + setopen:
                        self.fieldpointers[posinds[jj][0]][posinds[jj][1]] = movementdirections[jj]
                        setopen = setopen + [posinds[jj]]
                        setopencosts = setopencosts + [costs[jj]]
                        setopenheuristics = setopenheuristics + [heuristics[jj]]
                    elif posinds[jj] in setopen:
                        position = setopen.index(posinds[jj])
                        if setopencosts[position] > costs[jj]:
                            setopencosts[position] = costs[jj]
                            setopenheuristics[position] = heuristics[jj]
                            self.fieldpointers[setopen[position][0]][setopen[position][1]] = movementdirections[jj]
                    else:
                        position = setclosed.index(posinds[jj])
                        if setclosedcosts[position] > costs[jj]:
                            setclosedcosts[position] = costs[jj]
                            self.fieldpointers[setclosed[position][0]][setclosed[position][1]] = movementdirections[jj]
            if not setopen:
                exit("Can't")
        if self.end_point in setopen:
            rod = self.findWayBack(self.end_point)
            return rod
        else:
            exit("Can't")

    def Path_optimization(self, temp, ii, setOpen, setOpenCosts, setOpenHeuristics):
        [row, col] = setOpen[ii]
        _temp = self.fieldpointers[row][col]
        if "L" in _temp:
            col -= 1
        elif "R" in _temp:
            col += 1
        if "U" in _temp:
            row -= 1
        elif "D" in _temp:
            row += 1
        if [row, col] == self.start_point:
            new_ii = ii
        else:
            _temp = self.fieldpointers[row][col]
            [row2, col2] = [row, col]
            if "L" in _temp:
                col2 += 1
            elif "R" in _temp:
                col2 -= 1
            if "U" in _temp:
                row2 += 1
            elif "D" in _temp:
                row2 -= 1

            if 0 <= row2 <= self.matrix_width and 0 <= col2 <= self.matrix_length:
                new_ii = ii
            else:
                if self.fieldpointers[setOpen[ii][0]][setOpen[ii][1]] == self.fieldpointers[row][col]:
                    new_ii = ii
                elif [row2, col2] in setOpen:
                    untext_ii = setOpen.index([row2, col2])
                    now_cost = setOpenCosts[untext_ii] + self.weights * setOpenHeuristics[untext_ii]
                    if temp == now_cost:
                        new_ii = untext_ii
                    else:
                        new_ii = ii
                else:
                    new_ii = ii
        return new_ii

    def findFValue(self, currentpos, costsofar):
        cost = []
        heuristic = []
        posinds = []
        for way in self.way:
            if "D" in way:
                x = currentpos[0] - 1
            elif "U" in way:
                x = currentpos[0] + 1
            else:
                x = currentpos[0]
            if "R" in way:
                y = currentpos[1] - 1
            elif "L" in way:
                y = currentpos[1] + 1
            else:
                y = currentpos[1]
            if 0 <= y <= self.matrix_length - 1 and 0 <= x <= self.matrix_width - 1:
                posinds.append([x, y])
                heuristic.append(sqrt((self.end_point[1] - y) ** 2 + (self.end_point[0] - x) ** 2))
                cost.append(costsofar + self.field[x][y])
            else:
                posinds.append([0, 0])
                heuristic.append(float("inf"))
                cost.append(float("inf"))
        return [cost, heuristic, posinds]

    def findWayBack(self, goal):
        road = [goal]
        [x, y] = goal
        while self.fieldpointers[x][y] != "S":
            temp = self.fieldpointers[x][y]
            if "L" in temp:
                y -= 1
            if "R" in temp:
                y += 1
            if "U" in temp:
                x -= 1
            if "D" in temp:
                x += 1
            road.append([x, y])
        return road


if __name__ == "__main__":
    print(A_star(matrix=[[True for i in range(1000)] for j in range(1000)]).run(start_point=[0, 0], end_point=[999, 999]))

个人博客原地址（内有各个更新版本）：A* 算法 Python 代码 – Pancake's Personal Websitehttps://www.pancake2021.work/?p=838