如何计算每个节点的障碍物高度，并影响A星算法Formula的寻路？
How to calculate the height of an obstacle in every node and take its effect on the path finding of the A star algorithm Formula?
我正在研究 A 星算法以生成最佳轨迹。对于我的问题，我试图找到两点之间无人机的最佳路径，但我需要考虑障碍物的高度。如您所知，算法会通过搜索计算每个节点中的 g (cost) 和 h(heuristic)，并通过此公式 F=G+H 选择最佳的一个。我还需要计算障碍物的高度并将其添加到我的公式中成为 F=G+H+E。 E将代表障碍物的高度。
如果无人机在特定高度飞行，遇到很高的障碍物会掉头，如果障碍物的高度接近无人机高度，说明风险很大，会考虑做空飞越它的障碍。
我生成了一张与我的网格大小相同的地图，并为障碍物提供了随机数（随机高度），然后将其应用到我的公式中。在我的扩展阶段，我设定了一个条件，如果障碍物的高度减去无人机的高度，那么无人机就可以飞越它，但我发现没有任何效果。我能得到任何帮助吗？
这是我的代码下方：
#grid format
# 0 = navigable space
# 1 = occupied space
import random
grid = [[0, 1, 0, 0, 0, 0],
[0, 1, 0, 0, 0, 0],
[0, 1, 0, 0, 0, 0],
[0, 1, 0, 0, 0, 0],
[0, 0, 0, 0, 1, 0]]
heuristic = [[9, 8, 7, 6, 5, 4],
[8, 7, 6, 5, 4, 3],
[7, 6, 5, 4, 3, 2],
[6, 5, 4, 3, 2, 1],
[5, 4, 3, 2, 1, 0]]
init = [0,0] #Start location is (0,0) which we put it in open list.
goal = [len(grid)-1,len(grid[0])-1] #Our goal in (4,5) and here are the coordinates of the cell.
#Below the four potential actions to the single field
delta = [[-1 , 0], #up
[ 0 ,-1], #left
[ 1 , 0], #down
[ 0 , 1]] #right
delta_name = ['^','<','V','>'] #The name of above actions
cost = 1 #Each step costs you one
drone_h = 60 #The altitude of drone
def search():
#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))]
elevation = [[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])):
if grid[i][j] == 1:
elevation[i][j] = random.randint(1,100)
print(elevation[i][j])
else:
elevation[i][j] = 0
# we assigned the cordinates and g value
x = init[0]
y = init[1]
g = 0
h = heuristic[x][y]
e = elevation[x][y]
f = g + h + e
#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:
resign = True
print('Fail')
print('############# Search terminated without success')
print()
else:
open.sort()
open.reverse()
next = open.pop()
#print('list item')
#print('next')
x = next[3]
y = next[4]
g = next[1]
expand[x][y] = count
count+=1
#Check if we are done
if x == goal[0] and y == goal[1]:
found = True
print(next)
print('############## Search is success')
print()
else:
#expand winning element and add to new open list
for i in range(len(delta)):
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 and elevation[x2][y2]< drone_h:
g2 = g + cost
h2 = heuristic[x2][y2]
e = elevation[x2][y2]
f2 = g2 + h2 + e
open.append([f2,g2,h2,x2,y2])
#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]='*'
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
for i in range(len(policy)):
print(policy[i])
search()
我得到的结果：
[11, 11, 0, 4, 5]
############## Search is success
[0, -1, -1, -1, -1, -1]
[1, -1, -1, -1, -1, -1]
[2, -1, -1, -1, -1, -1]
[3, -1, 8, 9, 10, 11]
[4, 5, 6, 7, -1, 12]
['V', ' ', ' ', ' ', ' ', ' ']
['V', ' ', ' ', ' ', ' ', ' ']
['V', ' ', ' ', ' ', ' ', ' ']
['V', ' ', ' ', '>', '>', 'V']
['>', '>', '>', '^', ' ', '*']
您需要将成本与障碍物的高度相关联。所以，你需要弄清楚，就距离而言，高障碍物所代表的风险有多大。
所以，基本上你的启发式保持你的实际距离是 G(到现在)+E(到现在)，你的启发式保持 G(到现在)+E(到现在)+H
既然你没有说明承担风险有多糟糕以及你有多想避免它，比方说，除非没有其他办法，否则你永远不想承担风险。
然后您可以按如下方式关联海拔高度的成本：
E(x) = 0 if obstacle is low
E(x) = maximal_possible_distance + (elevation - drone height)
那样的话，绕路总是更好，而且它有利于较小的海拔（如果你更喜欢较小的海拔，请添加一个因子或指数）
我正在研究 A 星算法以生成最佳轨迹。对于我的问题，我试图找到两点之间无人机的最佳路径，但我需要考虑障碍物的高度。如您所知，算法会通过搜索计算每个节点中的 g (cost) 和 h(heuristic)，并通过此公式 F=G+H 选择最佳的一个。我还需要计算障碍物的高度并将其添加到我的公式中成为 F=G+H+E。 E将代表障碍物的高度。
如果无人机在特定高度飞行，遇到很高的障碍物会掉头，如果障碍物的高度接近无人机高度，说明风险很大，会考虑做空飞越它的障碍。
我生成了一张与我的网格大小相同的地图，并为障碍物提供了随机数（随机高度），然后将其应用到我的公式中。在我的扩展阶段，我设定了一个条件，如果障碍物的高度减去无人机的高度，那么无人机就可以飞越它，但我发现没有任何效果。我能得到任何帮助吗？
这是我的代码下方：
#grid format
# 0 = navigable space
# 1 = occupied space
import random
grid = [[0, 1, 0, 0, 0, 0],
[0, 1, 0, 0, 0, 0],
[0, 1, 0, 0, 0, 0],
[0, 1, 0, 0, 0, 0],
[0, 0, 0, 0, 1, 0]]
heuristic = [[9, 8, 7, 6, 5, 4],
[8, 7, 6, 5, 4, 3],
[7, 6, 5, 4, 3, 2],
[6, 5, 4, 3, 2, 1],
[5, 4, 3, 2, 1, 0]]
init = [0,0] #Start location is (0,0) which we put it in open list.
goal = [len(grid)-1,len(grid[0])-1] #Our goal in (4,5) and here are the coordinates of the cell.
#Below the four potential actions to the single field
delta = [[-1 , 0], #up
[ 0 ,-1], #left
[ 1 , 0], #down
[ 0 , 1]] #right
delta_name = ['^','<','V','>'] #The name of above actions
cost = 1 #Each step costs you one
drone_h = 60 #The altitude of drone
def search():
#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))]
elevation = [[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])):
if grid[i][j] == 1:
elevation[i][j] = random.randint(1,100)
print(elevation[i][j])
else:
elevation[i][j] = 0
# we assigned the cordinates and g value
x = init[0]
y = init[1]
g = 0
h = heuristic[x][y]
e = elevation[x][y]
f = g + h + e
#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:
resign = True
print('Fail')
print('############# Search terminated without success')
print()
else:
open.sort()
open.reverse()
next = open.pop()
#print('list item')
#print('next')
x = next[3]
y = next[4]
g = next[1]
expand[x][y] = count
count+=1
#Check if we are done
if x == goal[0] and y == goal[1]:
found = True
print(next)
print('############## Search is success')
print()
else:
#expand winning element and add to new open list
for i in range(len(delta)):
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 and elevation[x2][y2]< drone_h:
g2 = g + cost
h2 = heuristic[x2][y2]
e = elevation[x2][y2]
f2 = g2 + h2 + e
open.append([f2,g2,h2,x2,y2])
#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]='*'
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
for i in range(len(policy)):
print(policy[i])
search()
我得到的结果：
[11, 11, 0, 4, 5]
############## Search is success
[0, -1, -1, -1, -1, -1]
[1, -1, -1, -1, -1, -1]
[2, -1, -1, -1, -1, -1]
[3, -1, 8, 9, 10, 11]
[4, 5, 6, 7, -1, 12]
['V', ' ', ' ', ' ', ' ', ' ']
['V', ' ', ' ', ' ', ' ', ' ']
['V', ' ', ' ', ' ', ' ', ' ']
['V', ' ', ' ', '>', '>', 'V']
['>', '>', '>', '^', ' ', '*']
您需要将成本与障碍物的高度相关联。所以，你需要弄清楚，就距离而言，高障碍物所代表的风险有多大。
所以，基本上你的启发式保持你的实际距离是 G(到现在)+E(到现在)，你的启发式保持 G(到现在)+E(到现在)+H
既然你没有说明承担风险有多糟糕以及你有多想避免它，比方说，除非没有其他办法，否则你永远不想承担风险。
然后您可以按如下方式关联海拔高度的成本：
E(x) = 0 if obstacle is low
E(x) = maximal_possible_distance + (elevation - drone height)
那样的话，绕路总是更好，而且它有利于较小的海拔（如果你更喜欢较小的海拔，请添加一个因子或指数）