A* Python 实现问题

Question

所以，我有一个作业，我需要在 Python 中实现 A* 算法，但我遇到了障碍。 我已经写了大部分内容，但我不明白为什么它不能正常工作（即没有产生最佳路径），

我想也许更有知识的人可能会指出它的明显问题。

from mpl_toolkits import mplot3d
from matplotlib import cm
from queue import PriorityQueue
import matplotlib.pyplot as plt
import numpy as np

#Defining working files and work area size
N = 100
# fileNameIn = 'surface_'+str(N)+'x'+str(N)+'_separator.txt'
# fileNameEnd = 'surface_'+str(N)+'x'+str(N)+'_separator.end_points.txt'
fileNameIn = 'surface_'+str(N)+'x'+str(N)+'.txt'
fileNameEnd = 'surface_'+str(N)+'x'+str(N)+'.end_points.txt'

#This is needed for 3D plotting, otherwise the surface is rendered above the obstacles and paths
surfaceAlpha = 0.5

#Initial values
x0 = 0
y0 = 0
xn = 0
yn = 0
lepes1 = 0
lepes2 = 0

#Clearing/creating result file
f = open('result.txt', 'w')
f.write("")
f.close()

#Allocating work area matrix
matrix = np.zeros((N, N))

#Reading in the work area
def ReadIn():
    fi = open(fileNameIn, 'r')
    lines = fi.readlines()
    fi.close()
    
    global matrix
    x = 0
    y = 0
    z = 0.0
    b = 0
    
    for line in lines:
        tmp = line.split()
        x = int(tmp[0])
        y = int(tmp[1])
        z = float(tmp[2])
        b = int(tmp[3])
        if (b != 0):
            z = -1 *z
        
        matrix[x][y] = z

#Reading in the end-points
def ReadEnd():
    fe = open(fileNameEnd, 'r')
    lines = fe.readlines()
    fe.close

    global x0, y0, xn, yn
    x0, y0 = map(int, lines[0].split())
    xn, yn = map(int, lines[1].split())

#Plotting in 2D
def Heatmap2D(Xe1, Ye1, Xe2, Ye2, matrix):

    Xo = []
    Yo = []

    for i in range(N):
        for j in range(N):
            if (matrix[i][j] < 0):
                Xo.append(i)
                Yo.append(j)
    
    plt.figure(1)
    plt.xlabel('X')
    plt.ylabel('Y')
    plt.imshow(abs(matrix.transpose()), cmap=cm.coolwarm)
    plt.colorbar()
    plt.plot(Xe1, Ye1, color='green')
    plt.plot(Xe2, Ye2, color='purple')
    plt.scatter(Xo, Yo, color='black', s=1)
    plt.scatter(x0, y0, color='yellow', s=10)
    plt.scatter(xn, yn, color='orange', s=10)

#Plotting in 3D
def Heatmap3D(X, Y, Xe1, Ye1, Ze1, Xe2, Ye2, Ze2, matrix):
    Z = abs(matrix)
    Zo = []

    for i in range(N):
        for j in range(N):
            if (matrix[i][j] < 0):
                Zo.append((i, j, abs(matrix[i][j])))

    fig3D = plt.figure(2)
    ax3D = fig3D.add_subplot(111, projection='3d')
    #ax3D.scatter(X, Y, Z, c=Z, cmap=cm.coolwarm, alpha=surfaceAlpha) #Worse performance
    ax3D.plot_surface(X, Y, Z, cmap=cm.coolwarm, alpha=surfaceAlpha)

    for it in Zo: ax3D.scatter(it[1], it[0], it[2], c='black', alpha=1, s=1)
    ax3D.scatter(y0, x0, abs(matrix[x0][y0]), c='yellow', s=10)
    ax3D.scatter(yn, xn, abs(matrix[xn][yn]), c='orange', s=10)

    ax3D.plot3D(Ye1, Xe1, Ze1, 'green', alpha=1)
    ax3D.plot3D(Ye2, Xe2, Ze2, 'purple', alpha=1)

#Calculating euclidean distance between two points
def Distance(x1, y1, x2, y2):
    z1 = abs(matrix[x1][y1])
    z2 = abs(matrix[x2][y2])
    
    return np.sqrt((x1 - x2)**2 + (y1 - y2)**2 + (z1 - z2)**2)

#Heuristic calculation
def H_score(x, y, H):
    z = abs(matrix[x][y])
    zn = abs(matrix[xn][yn])

    if (H == 0):
        return abs(x - xn) + abs(y - yn) + abs(z - zn) #Manhattan
    elif (H == 1):
        return max(abs(xn - x), abs(yn - y))#, abs(zn - z)) #Chebyshev
    else:
        return Distance(x, y, xn, yn) #Euclidean

#Exact calculation
def G_score(prevG, x, y, x1, y1, H):
    d = 0
    z = abs(matrix[x][y])
    z1 = abs(matrix[x1][y1])

    if (H == 0):
        d = abs(x - x1) + abs(y - y1) + abs(z - z1) #Manhattan
    elif (H == 1):
        d = max(abs(x1 - x), abs(y1 - y))#, abs(z1 - z)) #Chebyshev
    else:
        d = Distance(x, y, x1, y1)

    d = d + prevG

    return d / 1.0 #Division is only here for testing

#Check if 2 points are neighbours or not
def Neighbour(x1, y1, x2, y2):
    szomsz = False

    if ((abs(x1 - x2) <= 1) and (abs(y1 - y2) <= 1) and IsValid(x1, y1) and IsValid(x2, y2) and (not ((x1 == x2) and (y1 == y2)))):
        szomsz = True
    else:
        szomsz = False

    return szomsz

#Check if point is valid (i.e is inside of work area/matrix and is not an obstacle)
def IsValid(x, y):
    valid = False

    if ((x >= 0) and (x < N) and (y >= 0) and (y < N) and (matrix[x][y] >= 0.0)):
        valid = True
    else:
        valid = False

    return valid

#Check if point is end point
def IsGoal(x, y):
    if (x == xn and y == yn):
        return True
    else:
        return False

#Check if element is in OpenList
def OLCheck(elem):
    global OpenList

    check = False
    for it in OpenList.queue:
        if (it[0] >= elem[0] and it[1] == elem[1] and it[2] == elem[2]):# and it[3] == elem[3]):
            check = True
            break

    return check

#Check if element is in CloseList
def CLCheck(elem):
    global CloseList

    check = False
    for it in CloseList:
        if (it[0] == elem[1]):# and it[1] == elem[2] and it[2] == elem[3]):
            check = True
            break

    return check

#Getting neighbours
def Expand(elemP, H):
    global OpenList, CloseList
    X = [-1, 0, 1]
    Y = [-1, 0, 1]
    xp = elemP[1][0]
    yp = elemP[1][1]

    factor = 1.09 * 100 #For testing purposes

    for ix in X:
        x = xp + ix
        for iy in Y:
            y = yp + iy
            if (Neighbour(xp, yp, x, y)):
                G = G_score(elemP[3] / factor, xp, yp, x, y, H)

                f = G + H_score(x, y, H)

                elem = (f, (x, y), (xp, yp), G)

                if ((not CLCheck(elem)) and (not OLCheck(elem))):
                    OpenList.put(elem)

#A* algorithm
def A_star(H):
    global OpenList, CloseList, dist, Result

    if (OpenList.empty()):
        print('ERROR: Empty Open List!')
        return -1

    maxLepesek = 10000
    lepesek = 0

    while (not OpenList.empty()):
        elem = OpenList.get()
        x = elem[1][0] #Current
        y = elem[1][1]
        xp = elem[2][0] #Previous of current
        yp = elem[2][1]
        G = elem[3] #G_score of current

        CloseList.append(((x, y), (xp, yp), G))

        if (IsGoal(x, y)): break        #End of algorithm

        if (lepesek >= maxLepesek):
            print('ERROR:', maxLepesek, 'lepes elerve'); return -2     #Failsafe

        if (len(CloseList) <= 1): dist = 0.0
        else: dist = dist + G #Distance(x, y, xp, yp)

        Expand(elem, H)
        lepesek = lepesek + 1

    print('A* lepesek: ', lepesek, '/', maxLepesek)
    return 0

#Getting Result array from CloseList
def getResult():
    global distS
    Res = []

    #for it in CloseList: Res.append(it[0])
    #return Res

    Res.append((xn, yn))

    while ((x0, y0) not in Res):
        tempR = Res[len(Res) - 1]
        for it in reversed(CloseList):
            if (it[0] == tempR):
                distS = distS + it[2]
                Res.append(it[1])
                break
    
    return Res[::-1]

#Writing out
def Kiir(Result):
    print('Length:', dist)
    print('Length (post Result calculation):', distS)
    print('Steps:', lepes-1)
    print('\n')

    f = open('result.txt', 'a')

    f.write(str(Result))
    f.write("\n")

    f.close()




# Main part =======================================================
ReadIn()
ReadEnd()

# a =============================================
OpenList = PriorityQueue()
OpenList.put((0, (x0, y0), (x0, y0), 0))
CloseList = []
Result = []
dist = 0.0
distS = 0.0
retval = -1

Xe1 = []
Ye1 = []
Ze1 = []
retval = A_star(1)
if (retval == 0):
    Result = getResult()

lepes = len(Result) 

for it in Result:
    Xe1.append(it[0])
    Ye1.append(it[1])
    Ze1.append(matrix[it[0]][it[1]])

for i in range(len(Result)-1):
    x1 = Result[i][0]
    y1 = Result[i][1]
    z1 = matrix[x1][y1]
    x2 = Result[i+1][0]
    y2 = Result[i+1][1]
    z2 = matrix[x2][y2]

print("Chebyshev: (Green)")
Kiir(Result)

# b =============================================
OpenList = PriorityQueue()
OpenList.put((0, (x0, y0), (x0, y0), 0))
CloseList = []
Result = []
dist = 0.0
distS = 0.0
retval = -1

Xe2 = []
Ye2 = []
Ze2 = []
retval = A_star(2)
if (retval == 0):
    Result = getResult()

lepes = len(Result)

for it in Result:
    Xe2.append(it[0])
    Ye2.append(it[1])
    Ze2.append(matrix[it[0]][it[1]])

for i in range(len(Result)-1):
    x1 = Result[i][0]
    y1 = Result[i][1]
    z1 = matrix[x1][y1]
    x2 = Result[i+1][0]
    y2 = Result[i+1][1]
    z2 = matrix[x2][y2]

print("Euclidean: (Purple)")
Kiir(Result)

# plot ==========================================

X, Y = np.meshgrid(range(N), range(N))
Heatmap2D(Xe1, Ye1, Xe2, Ye2, matrix)
#Heatmap3D(X, Y, Xe1, Ye1, Ze1, Xe2, Ye2, Ze2, matrix)
plt.show()

我最好的猜测是我没有正确计算 G_score （我没有真正考虑到第一个节点的距离，因为当我这样做时，代码往往会永远运行：c）

以下是输入文件： surface_100x100.txt https://pastebin.com/N9dHG5K9

surface_100x100.end_points.txt

0 0
99 30