当两点之间存在视线时，如何计算父节点和邻居节点之间的距离？
How could I calculate the distance between the parent and neighbors node when there is a line of sight between two points?
我已经在 Python 中完成了我的 Astar 算法，现在我需要将它转换为 Theta star 算法，我已经在下面构建了我的视线算法，但是当我来到我的 Theta star 算法时我当有视线通过计算距离时会遇到一些问题，我怎样才能让它跳过有视线的点。在 运行 我的代码之后，我看到没有任何效果，我看到它作为 Astar 算法工作。有什么帮助吗？
我觉得有问题的片段：
sight = lineOfsight(grid, y, x, y2, x2)
if sight == True:
g2 = g + delta[i][2] + math.sqrt((x2 - x)**2 + (y2 - y)**2)
h2 = math.sqrt((x2 - goal[0])**2 + (y2 - goal[1])**2)
f2 = g2 + h2
else:
g2 = g + delta[i][2]
h2 = math.sqrt((x2 - goal[0])**2 + (y2 - goal[1])**2)
f2 = g2 + h2
open.append([f2,g2,h2,x2,y2])
我的视线代码：
def lineOfsight(grid, y1, x1, y2, x2):
y_size = len(grid)
x_size = len(grid)
#Distance
dy=y2-y1
dx=x2-x1
if dy < 0:
dy = -dy
sy = -1
else:
sy = 1
if dx < 0:
dx = -dx
sx = -1
else:
sx = 1
f = 0
if dx >= dy:
while x1 != x2:
f = f + dy
if f >= dx and 0 < y1+(sy-1)/2 and y1+(sy-1)/2 < y_size and 0 < x1+(sx-1)/2 and x1+(sx-1)/2 < x_size:
if grid[x1+int((sx-1)/2)][y1+int((sy-1)/2)]:
return False
y1 = y1 + sy
f = f - dx
elif 0 < y1+(sy-1)/2 and y1+(sy-1)/2 < y_size and 0 < x1+(sx-1)/2 and x1+(sx-1)/2 < x_size:
if f != 0 and grid[x1+(sx-1)/2][y1+(sy-1)/2]:
return False
elif 1<y1 and y1<y_size and 0 < x1+(sx-1)/2 and x1+(sx-1)/2 < x_size:
if dy==0 and grid[x1+int((sx-1)/2)][y1] and grid[x1+int((sx-1)/2)][y1-1] :
return False
x1 = x1 + sx
else:
while y1 != y2:
f = f + dx
if f >= dy and 0 < y1+(sy-1)/2 and y1+(sy-1)/2 < y_size and 0< x1+(sx-1)/2 and x1+(sx-1)/2 < x_size:
if grid[x1+int((sx-1)/2)][y1+int((sy-1)/2)]:
return False
x1 = x1 + sx
f = f - dy
elif 0 < y1+(sy-1)/2 and y1+(sy-1)/2 < y_size and 0 < x1+(sx-1)/2 and x1+(sx-1)/2 < x_size:
if f !=0 and grid[x1+int((sx-1)/2)][y1+int((sy-1)/2)]:
return False
elif 0 < y1+(sy-1)/2 and y1+(sy-1)/2 < y_size and 1 < x1 and x1 < x_size:
if dx == 0 and grid[x1][y1+ int((sy-1)/2)] and grid[x1-1][y1+int((sy-1)/2)]:
return False
y1=y1+sy
return True
我的西塔星码：
import matplotlib.pyplot as plt
from lineofsightss import *
#grid format
# 0 = navigable space
# 1 = occupied space
import random
import math
grid = [[0, 1, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0],
[0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0],
[0, 1, 1, 0, 1, 0, 0, 0, 0, 0, 0, 0],
[0, 1, 0, 1, 0, 0, 0, 1, 0, 0, 0, 0],
[0, 0, 0, 1, 1, 0, 0, 1, 0, 0, 0, 0],
[0, 0, 0, 1, 1, 0, 0, 1, 0, 0, 0, 0],
[0, 0, 0, 1, 1, 0, 0, 1, 0, 0, 0, 0],
[0, 0, 0, 1, 1, 0, 0, 1, 0, 0, 0, 0],
[0, 0, 0, 1, 1, 0, 0, 0, 0, 0, 0, 0],
[0, 0, 0, 1, 1, 0, 0, 0, 0, 0, 0, 0]]
init = [0,0] #Start location is (5,5) which we put it in open list.
goal = [len(grid)-1,len(grid[0])-1]
heuristic = [[0 for row in range(len(grid[0]))] for col in range(len(grid))]
for i in range(len(grid)):
for j in range(len(grid[0])):
heuristic[i][j] = abs(i - goal[0]) + abs(j - goal[1])
plt.plot(0,10)
plt.plot(0,-len(grid)-10)
plt.grid(True)
plt.axis("equal")
plt.plot([-1, len(grid[0])],[[-x/2 for x in range(-1,len(grid)*2+1)], [-y/2 for y in range(-1,len(grid)*2+1)]], ".k")
plt.plot([[x/2 for x in range(-2,len(grid[0])*2+1)],[x/2 for x in range(-2,len(grid[-1])*2+1)]],[1, -len(grid)],".k")
plt.plot(init[1],-init[0],"og")
plt.plot(goal[1],-goal[0],"ob")
#Below the four potential actions to the single field
delta = [[1, 0, 1],
[0, 1, 1],
[-1, 0, 1],
[0, -1, 1],
[-1, -1, math.sqrt(2)],
[-1, 1, math.sqrt(2)],
[1, -1, math.sqrt(2)],
[1, 1, math.sqrt(2)]]
delta_name = ['V','>','<','^','//','\','\','//']
def search():
pltx,plty=[],[]
#open list elements are of the type [g,x,y]
closed = [[0 for row in range(len(grid[0]))] for col in range(len(grid))]
action = [[-1 for row in range(len(grid[0]))] for col in range(len(grid))]
#We initialize the starting location as checked
closed[init[0]][init[1]] = 1
expand=[[-1 for row in range(len(grid[0]))] for col in range(len(grid))]
# we assigned the cordinates and g value
x = init[0]
y = init[1]
g = 0
h = math.sqrt((x - goal[0])**2 + (y - goal[1])**2)
f = g + h
#our open list will contain our initial value
open = [[f, g, h, x, y]]
found = False #flag that is set when search complete
resign = False #Flag set if we can't find expand
count = 0
#print('initial open list:')
#for i in range(len(open)):
#print(' ', open[i])
#print('----')
while found is False and resign is False:
#Check if we still have elements in the open list
if len(open) == 0: #If our open list is empty, there is nothing to expand.
resign = True
print('Fail')
print('############# Search terminated without success')
print()
else:
#if there is still elements on our list
#remove node from list
open.sort() #sort elements in an increasing order from the smallest g value up
open.reverse() #reverse the list
next = open.pop() #remove the element with the smallest g value from the list
#print('list item')
#print('next')
#Then we assign the three values to x,y and g. Which is our expantion.
x = next[3]
y = next[4]
g = next[1]
#elvation[x][y] = np.random.randint(100, size=(5,6))
expand[x][y] = count
count+=1
#Check if we are done
if x == goal[0] and y == goal[1]:
found = True
print(next) #The three elements above this "if".
print('############## Search is success')
print()
else:
#expand winning element and add to new open list
for i in range(len(delta)): #going through all our actions the four actions
#We apply the actions to x and y with additional delta to construct x2 and y2
x2 = x + delta[i][0]
y2 = y + delta[i][1]
#if x2 and y2 falls into the grid
if x2 >= 0 and x2 < len(grid) and y2 >=0 and y2 <= len(grid[0])-1:
#if x2 and y2 not checked yet and there is not obstacles
if closed[x2][y2] == 0 and grid[x2][y2]==0:
sight = lineOfsight(grid, y, x, y2, x2)
if sight == True:
g2 = g + delta[i][2] + math.sqrt((x2 - x)**2 + (y2 - y)**2)
h2 = math.sqrt((x2 - goal[0])**2 + (y2 - goal[1])**2)
f2 = g2 + h2
else:
g2 = g + delta[i][2]
h2 = math.sqrt((x2 - goal[0])**2 + (y2 - goal[1])**2)
f2 = g2 + h2
open.append([f2,g2,h2,x2,y2])
#we add them to our open list
pltx.append(y2)
plty.append(-x2)
#print('append list item')
#print([g2,x2,y2])
#Then we check them to never expand again
closed[x2][y2] = 1
action[x2][y2] = i
for i in range(len(expand)):
print(expand[i])
print()
policy=[[' ' for row in range(len(grid[0]))] for col in range(len(grid))]
x=goal[0]
y=goal[1]
policy[x][y]='*'
visx = [y]
visy = [-x]
while x !=init[0] or y !=init[1]:
x2=x-delta[action[x][y]][0]
y2=y-delta[action[x][y]][1]
policy[x2][y2]= delta_name[action[x][y]]
x=x2
y=y2
visx.append(y)
visy.append(-x)
for i in range(len(policy)):
print(policy[i])
print()
plt.plot(visx,visy, "-r")
plt.show()
search()
下面是我的输出：
在Theta*中，当相邻节点看到parent时，您应该尝试将该相邻节点直接连接到当前节点的parent节点。这是导致any-angle、non-grid-aligned路径的过程。
节点与任意 parent（不一定是网格中的邻居）的这种关联实际上在解决方案中缺失（因此在完成搜索时不会正确重建路径).
这涉及对问题代码的一些更改：
路径重建应该以不同的方式实现，包含八个运动方向之一的"action"数组是不够的。一个可行的替代方案是它包含 parent 节点的 (x, y) 坐标，即 action[x][y] = (parent_x, parent_y).
在if sight == True里面的代码中你应该计算邻居的g-score作为使用从parent到邻居的直线的路径（虚线在上图中）。此时g-score的计算考虑到了当前节点，这不是必须的
下面是对已发布代码的修改，其中包含其中一些更改。其他问题可能仍然存在，但这是朝着正确方向迈出的一步。
def search():
pltx,plty=[],[]
closed = [[0 for row in range(len(grid[0]))] for col in range(len(grid))]
action = [[(-1, -1) for row in range(len(grid[0]))] for col in range(len(grid))]
closed[init[0]][init[1]] = 1
expand = [[-1 for row in range(len(grid[0]))] for col in range(len(grid))]
# we assigned the coordinates and g value
x = init[0]
y = init[1]
g = 0
h = math.sqrt((x - goal[0])**2 + (y - goal[1])**2)
f = g + h
open = [[f, g, h, x, y]]
found = False # flag that is set when search complete
resign = False # flag set if we can't find expand
count = 0
while found is False and resign is False:
# check if we still have elements in the open list
if len(open) == 0: # if our open list is empty, there is nothing to expand.
resign = True
print('Fail')
print('############# Search terminated without success')
print()
else:
# if there is still elements on our list
# remove node from list
open.sort() # sort elements in an increasing order from the smallest g value up
open.reverse() # reverse the list
next = open.pop() # remove the element with the smallest g value from the list
# then we assign the three values to x,y and g. Which is our expantion.
x = next[3]
y = next[4]
g = next[1]
# elvation[x][y] = np.random.randint(100, size=(5,6))
expand[x][y] = count
count += 1
# check if we are done
if x == goal[0] and y == goal[1]:
found = True
print(next) # the three elements above this "if".
print('############## Search is success')
print()
else:
# expand winning element and add to new open list
for i in range(len(delta)): # going through all our actions the four actions
# we apply the actions to x and y with additional delta to construct x2 and y2
x2 = x + delta[i][0]
y2 = y + delta[i][1]
# if x2 and y2 falls into the grid
if 0 <= x2 < len(grid) and 0 <= y2 <= len(grid[0]) - 1:
#if x2 and y2 not checked yet and there is not obstacles
if closed[x2][y2] == 0 and grid[x2][y2] == 0:
sight = lineOfsight(grid, y, x, y2, x2)
parent_x, parent_y = action[x][y]
if sight and parent_x >= 0:
g2 = g + math.sqrt((x2 - parent_x)**2 + (y2 - parent_y)**2)
h2 = math.sqrt((x2 - goal[0])**2 + (y2 - goal[1])**2)
f2 = g2 + h2
action[x2][y2] = (parent_x, parent_y)
else:
g2 = g + delta[i][2]
h2 = math.sqrt((x2 - goal[0])**2 + (y2 - goal[1])**2)
f2 = g2 + h2
action[x2][y2] = (x, y)
open.append([f2,g2,h2,x2,y2])
# we add them to our open list
pltx.append(y2)
plty.append(-x2)
closed[x2][y2] = 1
for i in range(len(expand)):
print(expand[i])
print()
policy=[[' ' for row in range(len(grid[0]))] for col in range(len(grid))]
x=goal[0]
y=goal[1]
visx = [y]
visy = [-x]
while x !=init[0] or y !=init[1]:
x2=action[x][y][0]
y2=action[x][y][1]
x=x2
y=y2
visx.append(y)
visy.append(-x)
print()
plt.plot(visx,visy, "-r")
plt.show()
这会产生以下路径：
我已经在 Python 中完成了我的 Astar 算法，现在我需要将它转换为 Theta star 算法，我已经在下面构建了我的视线算法，但是当我来到我的 Theta star 算法时我当有视线通过计算距离时会遇到一些问题，我怎样才能让它跳过有视线的点。在 运行 我的代码之后，我看到没有任何效果，我看到它作为 Astar 算法工作。有什么帮助吗？
我觉得有问题的片段：
sight = lineOfsight(grid, y, x, y2, x2)
if sight == True:
g2 = g + delta[i][2] + math.sqrt((x2 - x)**2 + (y2 - y)**2)
h2 = math.sqrt((x2 - goal[0])**2 + (y2 - goal[1])**2)
f2 = g2 + h2
else:
g2 = g + delta[i][2]
h2 = math.sqrt((x2 - goal[0])**2 + (y2 - goal[1])**2)
f2 = g2 + h2
open.append([f2,g2,h2,x2,y2])
我的视线代码：
def lineOfsight(grid, y1, x1, y2, x2):
y_size = len(grid)
x_size = len(grid)
#Distance
dy=y2-y1
dx=x2-x1
if dy < 0:
dy = -dy
sy = -1
else:
sy = 1
if dx < 0:
dx = -dx
sx = -1
else:
sx = 1
f = 0
if dx >= dy:
while x1 != x2:
f = f + dy
if f >= dx and 0 < y1+(sy-1)/2 and y1+(sy-1)/2 < y_size and 0 < x1+(sx-1)/2 and x1+(sx-1)/2 < x_size:
if grid[x1+int((sx-1)/2)][y1+int((sy-1)/2)]:
return False
y1 = y1 + sy
f = f - dx
elif 0 < y1+(sy-1)/2 and y1+(sy-1)/2 < y_size and 0 < x1+(sx-1)/2 and x1+(sx-1)/2 < x_size:
if f != 0 and grid[x1+(sx-1)/2][y1+(sy-1)/2]:
return False
elif 1<y1 and y1<y_size and 0 < x1+(sx-1)/2 and x1+(sx-1)/2 < x_size:
if dy==0 and grid[x1+int((sx-1)/2)][y1] and grid[x1+int((sx-1)/2)][y1-1] :
return False
x1 = x1 + sx
else:
while y1 != y2:
f = f + dx
if f >= dy and 0 < y1+(sy-1)/2 and y1+(sy-1)/2 < y_size and 0< x1+(sx-1)/2 and x1+(sx-1)/2 < x_size:
if grid[x1+int((sx-1)/2)][y1+int((sy-1)/2)]:
return False
x1 = x1 + sx
f = f - dy
elif 0 < y1+(sy-1)/2 and y1+(sy-1)/2 < y_size and 0 < x1+(sx-1)/2 and x1+(sx-1)/2 < x_size:
if f !=0 and grid[x1+int((sx-1)/2)][y1+int((sy-1)/2)]:
return False
elif 0 < y1+(sy-1)/2 and y1+(sy-1)/2 < y_size and 1 < x1 and x1 < x_size:
if dx == 0 and grid[x1][y1+ int((sy-1)/2)] and grid[x1-1][y1+int((sy-1)/2)]:
return False
y1=y1+sy
return True
我的西塔星码：
import matplotlib.pyplot as plt
from lineofsightss import *
#grid format
# 0 = navigable space
# 1 = occupied space
import random
import math
grid = [[0, 1, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0],
[0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0],
[0, 1, 1, 0, 1, 0, 0, 0, 0, 0, 0, 0],
[0, 1, 0, 1, 0, 0, 0, 1, 0, 0, 0, 0],
[0, 0, 0, 1, 1, 0, 0, 1, 0, 0, 0, 0],
[0, 0, 0, 1, 1, 0, 0, 1, 0, 0, 0, 0],
[0, 0, 0, 1, 1, 0, 0, 1, 0, 0, 0, 0],
[0, 0, 0, 1, 1, 0, 0, 1, 0, 0, 0, 0],
[0, 0, 0, 1, 1, 0, 0, 0, 0, 0, 0, 0],
[0, 0, 0, 1, 1, 0, 0, 0, 0, 0, 0, 0]]
init = [0,0] #Start location is (5,5) which we put it in open list.
goal = [len(grid)-1,len(grid[0])-1]
heuristic = [[0 for row in range(len(grid[0]))] for col in range(len(grid))]
for i in range(len(grid)):
for j in range(len(grid[0])):
heuristic[i][j] = abs(i - goal[0]) + abs(j - goal[1])
plt.plot(0,10)
plt.plot(0,-len(grid)-10)
plt.grid(True)
plt.axis("equal")
plt.plot([-1, len(grid[0])],[[-x/2 for x in range(-1,len(grid)*2+1)], [-y/2 for y in range(-1,len(grid)*2+1)]], ".k")
plt.plot([[x/2 for x in range(-2,len(grid[0])*2+1)],[x/2 for x in range(-2,len(grid[-1])*2+1)]],[1, -len(grid)],".k")
plt.plot(init[1],-init[0],"og")
plt.plot(goal[1],-goal[0],"ob")
#Below the four potential actions to the single field
delta = [[1, 0, 1],
[0, 1, 1],
[-1, 0, 1],
[0, -1, 1],
[-1, -1, math.sqrt(2)],
[-1, 1, math.sqrt(2)],
[1, -1, math.sqrt(2)],
[1, 1, math.sqrt(2)]]
delta_name = ['V','>','<','^','//','\','\','//']
def search():
pltx,plty=[],[]
#open list elements are of the type [g,x,y]
closed = [[0 for row in range(len(grid[0]))] for col in range(len(grid))]
action = [[-1 for row in range(len(grid[0]))] for col in range(len(grid))]
#We initialize the starting location as checked
closed[init[0]][init[1]] = 1
expand=[[-1 for row in range(len(grid[0]))] for col in range(len(grid))]
# we assigned the cordinates and g value
x = init[0]
y = init[1]
g = 0
h = math.sqrt((x - goal[0])**2 + (y - goal[1])**2)
f = g + h
#our open list will contain our initial value
open = [[f, g, h, x, y]]
found = False #flag that is set when search complete
resign = False #Flag set if we can't find expand
count = 0
#print('initial open list:')
#for i in range(len(open)):
#print(' ', open[i])
#print('----')
while found is False and resign is False:
#Check if we still have elements in the open list
if len(open) == 0: #If our open list is empty, there is nothing to expand.
resign = True
print('Fail')
print('############# Search terminated without success')
print()
else:
#if there is still elements on our list
#remove node from list
open.sort() #sort elements in an increasing order from the smallest g value up
open.reverse() #reverse the list
next = open.pop() #remove the element with the smallest g value from the list
#print('list item')
#print('next')
#Then we assign the three values to x,y and g. Which is our expantion.
x = next[3]
y = next[4]
g = next[1]
#elvation[x][y] = np.random.randint(100, size=(5,6))
expand[x][y] = count
count+=1
#Check if we are done
if x == goal[0] and y == goal[1]:
found = True
print(next) #The three elements above this "if".
print('############## Search is success')
print()
else:
#expand winning element and add to new open list
for i in range(len(delta)): #going through all our actions the four actions
#We apply the actions to x and y with additional delta to construct x2 and y2
x2 = x + delta[i][0]
y2 = y + delta[i][1]
#if x2 and y2 falls into the grid
if x2 >= 0 and x2 < len(grid) and y2 >=0 and y2 <= len(grid[0])-1:
#if x2 and y2 not checked yet and there is not obstacles
if closed[x2][y2] == 0 and grid[x2][y2]==0:
sight = lineOfsight(grid, y, x, y2, x2)
if sight == True:
g2 = g + delta[i][2] + math.sqrt((x2 - x)**2 + (y2 - y)**2)
h2 = math.sqrt((x2 - goal[0])**2 + (y2 - goal[1])**2)
f2 = g2 + h2
else:
g2 = g + delta[i][2]
h2 = math.sqrt((x2 - goal[0])**2 + (y2 - goal[1])**2)
f2 = g2 + h2
open.append([f2,g2,h2,x2,y2])
#we add them to our open list
pltx.append(y2)
plty.append(-x2)
#print('append list item')
#print([g2,x2,y2])
#Then we check them to never expand again
closed[x2][y2] = 1
action[x2][y2] = i
for i in range(len(expand)):
print(expand[i])
print()
policy=[[' ' for row in range(len(grid[0]))] for col in range(len(grid))]
x=goal[0]
y=goal[1]
policy[x][y]='*'
visx = [y]
visy = [-x]
while x !=init[0] or y !=init[1]:
x2=x-delta[action[x][y]][0]
y2=y-delta[action[x][y]][1]
policy[x2][y2]= delta_name[action[x][y]]
x=x2
y=y2
visx.append(y)
visy.append(-x)
for i in range(len(policy)):
print(policy[i])
print()
plt.plot(visx,visy, "-r")
plt.show()
search()
下面是我的输出：
在Theta*中，当相邻节点看到parent时，您应该尝试将该相邻节点直接连接到当前节点的parent节点。这是导致any-angle、non-grid-aligned路径的过程。
节点与任意 parent（不一定是网格中的邻居）的这种关联实际上在解决方案中缺失（因此在完成搜索时不会正确重建路径).
这涉及对问题代码的一些更改：
路径重建应该以不同的方式实现，包含八个运动方向之一的"action"数组是不够的。一个可行的替代方案是它包含 parent 节点的 (x, y) 坐标，即 action[x][y] = (parent_x, parent_y).
在
if sight == True里面的代码中你应该计算邻居的g-score作为使用从parent到邻居的直线的路径（虚线在上图中）。此时g-score的计算考虑到了当前节点，这不是必须的
下面是对已发布代码的修改，其中包含其中一些更改。其他问题可能仍然存在，但这是朝着正确方向迈出的一步。
def search():
pltx,plty=[],[]
closed = [[0 for row in range(len(grid[0]))] for col in range(len(grid))]
action = [[(-1, -1) for row in range(len(grid[0]))] for col in range(len(grid))]
closed[init[0]][init[1]] = 1
expand = [[-1 for row in range(len(grid[0]))] for col in range(len(grid))]
# we assigned the coordinates and g value
x = init[0]
y = init[1]
g = 0
h = math.sqrt((x - goal[0])**2 + (y - goal[1])**2)
f = g + h
open = [[f, g, h, x, y]]
found = False # flag that is set when search complete
resign = False # flag set if we can't find expand
count = 0
while found is False and resign is False:
# check if we still have elements in the open list
if len(open) == 0: # if our open list is empty, there is nothing to expand.
resign = True
print('Fail')
print('############# Search terminated without success')
print()
else:
# if there is still elements on our list
# remove node from list
open.sort() # sort elements in an increasing order from the smallest g value up
open.reverse() # reverse the list
next = open.pop() # remove the element with the smallest g value from the list
# then we assign the three values to x,y and g. Which is our expantion.
x = next[3]
y = next[4]
g = next[1]
# elvation[x][y] = np.random.randint(100, size=(5,6))
expand[x][y] = count
count += 1
# check if we are done
if x == goal[0] and y == goal[1]:
found = True
print(next) # the three elements above this "if".
print('############## Search is success')
print()
else:
# expand winning element and add to new open list
for i in range(len(delta)): # going through all our actions the four actions
# we apply the actions to x and y with additional delta to construct x2 and y2
x2 = x + delta[i][0]
y2 = y + delta[i][1]
# if x2 and y2 falls into the grid
if 0 <= x2 < len(grid) and 0 <= y2 <= len(grid[0]) - 1:
#if x2 and y2 not checked yet and there is not obstacles
if closed[x2][y2] == 0 and grid[x2][y2] == 0:
sight = lineOfsight(grid, y, x, y2, x2)
parent_x, parent_y = action[x][y]
if sight and parent_x >= 0:
g2 = g + math.sqrt((x2 - parent_x)**2 + (y2 - parent_y)**2)
h2 = math.sqrt((x2 - goal[0])**2 + (y2 - goal[1])**2)
f2 = g2 + h2
action[x2][y2] = (parent_x, parent_y)
else:
g2 = g + delta[i][2]
h2 = math.sqrt((x2 - goal[0])**2 + (y2 - goal[1])**2)
f2 = g2 + h2
action[x2][y2] = (x, y)
open.append([f2,g2,h2,x2,y2])
# we add them to our open list
pltx.append(y2)
plty.append(-x2)
closed[x2][y2] = 1
for i in range(len(expand)):
print(expand[i])
print()
policy=[[' ' for row in range(len(grid[0]))] for col in range(len(grid))]
x=goal[0]
y=goal[1]
visx = [y]
visy = [-x]
while x !=init[0] or y !=init[1]:
x2=action[x][y][0]
y2=action[x][y][1]
x=x2
y=y2
visx.append(y)
visy.append(-x)
print()
plt.plot(visx,visy, "-r")
plt.show()
这会产生以下路径：
