I'm working on A star algorithm to generate an optimal trajectory. For my problem, I'm trying to find an optimal path to drone between two points, but I need to take into consideration the height of obstacles. As you know, through searching the algorithm will calculate the g (cost) and h(heuristic) in every node and choose the best one through this formula F=G+H. I need also to calculate the height of obstacles and add it to my formula to become F=G+H+E. E will represent the height of obstacles.
If the drone is flying with a specific height and faces a very high obstacle will turn around, if the high of the obstacle is close to the drone height, it means the risk will be very high and will consider the short obstacle to fly over it.
I have generated a map with the same size as my grid and I gave random numbers (random heights) to the obstacles, then I applied it to my formula. Throughout my expansion stage, I have made a condition if the height of obstacles less the height of drone height then the drone will be able to fly over it, but I see there is no effect. Could I get any assistance?.
This is below my code:
#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()
The results that I got:
[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']
['>', '>', '>', '^', ' ', '*']
You need to associate a cost with the height of obstacles. So, you need to figure out how costly, in terms of distance, the risk a high obstacle represents, is.
So, basically your heuristic stays your actual distance is G(up to now)+E(up to now), and your heuristic stays G(up to now)+E(up to now)+H
Since you haven't stated how bad taking the risk is and how much you want to avoid it, let's say, as an example, you never want to take the risk unless there is no other way.
Then you could associate a cost for the elevation as follows:
E(x) = 0 if obstacle is low
E(x) = maximal_possible_distance + (elevation - drone height)
That way, it is always better to take a detour, and it favors smaller elevations (add a factor or exponent if you want to favor smaller elevations even more)