Я пытаюсь вычислить длину сгенерированного пути из алгоритма A *. Следовательно, я предполагаю, что каждый пиксель представляет собой прямоугольную область размером pixel_width * pixel_length, тогда я могу измерить пройденное расстояние, суммируя расстояние между центрами пикселей, когда алгоритм решает взять следующий пиксель.
В моем коде также есть высота, поэтому это означает, что в строке 173 я вставил что-то вроде ниже:
path_length += math.sqrt((x2-x)**2+(y2-y)**2+(e2-elevation1[x][y])**2)
После нанесения получил вот этот номер 123630.43677339483. Это логично? Любые идеи?
Код:
import matplotlib.pyplot as plt
#grid format
# 0 = navigable space
# 1 = occupied space
import random
import math
import time
grid = [[random.randint(0,1) for i in range(100)]for j in range(100)]
# clear starting and end point of potential obstacles
grid[2][2] = 0
grid[95][95] = 0
init = [2,2] #Start location is (5,5) which we put it in open list.
goal = [len(grid)-5,len(grid[0])-5] #Our goal in (40,38) and here are the coordinates of the cell.
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','>','<','^','//','\\','\\','//']
cost = 1 #Each step costs you one
drone_height = 60
kg = 0.6
ke = 1
kh = 0.8
penalty1 = 10
penalty2 = 1.5
risk = [i for i in range(50,60)]
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))]
elevation1 = [[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:
elevation1[i][j] = random.randint(1,100)
if elevation1[i][j] >= drone_height:
plt.plot(j,-i,".k", markersize=10)
elif elevation1[i][j] in risk:
plt.plot(j,-i,".r", markersize=10)
else:
plt.plot(j,-i,"sm")
else:
elevation1[i][j] = 0
elevation2 = [[0 for row in range(len(grid[0]))] for col in range(len(grid))]
for i in range(len(elevation1)):
for j in range(len(elevation1[0])):
if elevation1[i][j] >= drone_height:
elevation2[i][j] = elevation1[i][j]*penalty1
elif elevation1[i][j] in risk:
elevation2[i][j] = elevation1[i][j]*penalty2
else:
elevation2[i][j] = elevation1[i][j]
# we assigned the cordinates and g value
x = init[0]
y = init[1]
g = 0
path_length = 0
#h = heuristic[x][y]
h = math.sqrt((x - goal[0])**2 + (y - goal[1])**2)
e = elevation2[x][y]
f = kg*g + kh*h + ke*e
#our open list will contain our initial value
open = [[f, g, h, x, y]]
'''
We are going to use two flags
1- found and it will be True when the goal position is found.
2- resign it will be True if we couldn't find the goal position and explore everything.
'''
found = False #flag that is set when search complete
resign = False #Flag set if we can't find expand
count = 0
t1 = time.time()
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]
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 elevation[x2][y2] < drone_height and elevation[x2][y2] not in risk:
if closed[x2][y2] == 0:
g2 = g + cost #we increment the cose
#h2 = heuristic[x2][y2]
h2 = math.sqrt((x2 - goal[0])**2 + (y2 - goal[1])**2)
e2 = elevation2[x2][y2]
f2 = kg*g2 + kh*h2 + ke*e2
path_length+=math.sqrt((x2-x)**2+(e2-elevation1[x][y])**2)
open.append([f2,g2,h2,x2,y2])
#we add them to our open list
pltx.append(y2)
plty.append(-x2)
#Then we check them to never expand again
closed[x2][y2] = 1
action[x2][y2] = i
t2 = time.time()
runtime = t2-t1
print('The elapsed time= ',runtime)
print()
print(path_length)
print(len(pltx))
print(len(plty))
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)
# visualization
for x in range(0,len(pltx),20):
plt.plot(pltx[:x],plty[:x],"xc")
plt.pause(0.001)
plt.plot(visx,visy, "-r")
plt.show()
search()