# -*- coding: utf-8 -*-
import numpy as np
import math
import heapq
import matplotlib.pyplot as plt
class AStarPlanner:
def __init__(self, dem_file, resolution, safe_distance):
self.dem_file = dem_file
self.resolution = resolution
self.safe_distance = safe_distance
self.dem = None
self.dem_size = None
self.min_elevation = None
self.max_elevation = None
self.obstacle_cost = 99999
def load_dem(self):
dem = np.loadtxt(self.dem_file, delimiter=',')
self.dem_size = dem.shape
self.min_elevation = np.min(dem)
self.max_elevation = np.max(dem)
dem = (dem - self.min_elevation) / (self.max_elevation - self.min_elevation)
self.dem = dem
return dem
def get_elevation(self, lat, lon):
row = int((lat - self.resolution / 2) / self.resolution)
col = int((lon - self.resolution / 2) / self.resolution)
if row < 0 or row >= self.dem_size[0] or col < 0 or col >= self.dem_size[1]:
return None
return self.dem[row, col]
def get_cost(self, lat, lon, alt):
elevation = self.get_elevation(lat, lon)
if elevation is None:
return self.obstacle_cost
if alt < elevation + self.safe_distance:
return self.obstacle_cost
return 1 / (alt - elevation)
def heuristic(self, lat1, lon1, alt1, lat2, lon2, alt2):
dx = abs(lon1 - lon2) * 111000 * math.cos(lat1 / 180 * math.pi)
dy = abs(lat1 - lat2) * 111000
dz = abs(alt1 - alt2)
return math.sqrt(dx ** 2 + dy ** 2 + dz ** 2)
def plan(self, start_lat, start_lon, start_alt, end_lat, end_lon, end_alt):
self.load_dem()
start_row = int((start_lat - self.resolution / 2) / self.resolution)
start_col = int((start_lon - self.resolution / 2) / self.resolution)
end_row = int((end_lat - self.resolution / 2) / self.resolution)
end_col = int((end_lon - self.resolution / 2) / self.resolution)
if start_row < 0 or start_row >= self.dem_size[0] or start_col < 0 or start_col >= self.dem_size[1]:
raise ValueError("Start point is out of DEM range")
if end_row < 0 or end_row >= self.dem_size[0] or end_col < 0 or end_col >= self.dem_size[1]:
raise ValueError("End point is out of DEM range")
start_cost = self.get_cost(start_lat, start_lon, start_alt)
end_cost = self.get_cost(end_lat, end_lon, end_alt)
heap = []
heapq.heappush(heap, (start_cost, start_row, start_col, start_alt, None))
visited = set()
parent = {}
while heap:
cost, row, col, alt, p = heapq.heappop(heap)
if (row, col, alt) in visited:
continue
visited.add((row, col, alt))
parent[(row, col, alt)] = p
if row == end_row and col == end_col:
path = [(end_lat, end_lon, end_alt)]
while (row, col, alt) != (start_row, start_col, start_alt):
p_row, p_col, p_alt = parent[(row, col, alt)]
lat = (row + 0.5) * self.resolution
lon = (col + 0.5) * self.resolution
alt = alt - self.resolution / 2
path.append((lat, lon, alt))
row, col, alt = p_row, p_col, p_alt
path.append((start_lat, start_lon, start_alt))
path.reverse()
return path
for i in range(-1, 2):
for j in range(-1, 2):
for k in range(-1, 2):
if i == 0 and j == 0 and k == 0:
continue
new_row, new_col, new_alt = row + i, col + j, alt + k * self.resolution
if new_row < 0 or new_row >= self.dem_size[0] or new_col < 0 or new_col >= self.dem_size[1]:
continue
if (new_row, new_col, new_alt) in visited:
continue
cost = self.get_cost(new_row * self.resolution, new_col * self.resolution, new_alt)
if cost == self.obstacle_cost:
continue
h = self.heuristic(new_row * self.resolution, new_col * self.resolution, new_alt,
end_lat, end_lon, end_alt)
heapq.heappush(heap, (cost + h, new_row, new_col, new_alt, (row, col, alt)))
raise ValueError("Cannot find a valid path")
if __name__ == '__main__':
planner = AStarPlanner('dem.csv', 0.0001, 20)
path = planner.plan(39.9210, 116.4471, 100, 39.9039, 116.4273, 100)
path_lat = [p[0] for p in path]
path_lon = [p[1] for p in path]
plt.plot(path_lon, path_lat)
plt.show()
500
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
