|
|
|
|
@ -0,0 +1,213 @@
|
|
|
|
|
import numpy as np
|
|
|
|
|
import matplotlib.pyplot as plt
|
|
|
|
|
|
|
|
|
|
#Astar算法
|
|
|
|
|
def A_star(org, des):
|
|
|
|
|
open_list = []
|
|
|
|
|
close_list = []
|
|
|
|
|
open_list.append(org)
|
|
|
|
|
while len(open_list) > 0:
|
|
|
|
|
|
|
|
|
|
temp_node = open_list[0]
|
|
|
|
|
for node in open_list:
|
|
|
|
|
if node.f < temp_node.f:
|
|
|
|
|
temp_node = node
|
|
|
|
|
current_node= temp_node
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
open_list.remove(current_node)
|
|
|
|
|
close_list.append(current_node)
|
|
|
|
|
#找到当前节点的所有邻近节点
|
|
|
|
|
node_list = []
|
|
|
|
|
if Is_valid(current_node.x, current_node.y - 1, open_list, close_list):
|
|
|
|
|
node_list.append(Node(current_node.x, current_node.y - 1))
|
|
|
|
|
if Is_valid(current_node.x, current_node.y + 1, open_list, close_list):
|
|
|
|
|
node_list.append(Node(current_node.x, current_node.y + 1))
|
|
|
|
|
if Is_valid(current_node.x - 1, current_node.y, open_list, close_list):
|
|
|
|
|
node_list.append(Node(current_node.x - 1, current_node.y))
|
|
|
|
|
if Is_valid(current_node.x + 1, current_node.y, open_list, close_list):
|
|
|
|
|
node_list.append(Node(current_node.x + 1, current_node.y))
|
|
|
|
|
if Is_valid(current_node.x - 1, current_node.y - 1, open_list, close_list):
|
|
|
|
|
node_list.append(Node(current_node.x - 1, current_node.y - 1))
|
|
|
|
|
if Is_valid(current_node.x + 1, current_node.y + 1, open_list, close_list):
|
|
|
|
|
node_list.append(Node(current_node.x + 1, current_node.y + 1))
|
|
|
|
|
if Is_valid(current_node.x - 1, current_node.y + 1, open_list, close_list):
|
|
|
|
|
node_list.append(Node(current_node.x - 1, current_node.y + 1))
|
|
|
|
|
if Is_valid(current_node.x + 1, current_node.y - 1, open_list, close_list):
|
|
|
|
|
node_list.append(Node(current_node.x + 1, current_node.y - 1))
|
|
|
|
|
neighbors = node_list
|
|
|
|
|
for node in neighbors:
|
|
|
|
|
if node not in open_list:
|
|
|
|
|
|
|
|
|
|
node.init_node(current_node, des)
|
|
|
|
|
open_list.append(node)
|
|
|
|
|
|
|
|
|
|
for node in open_list:
|
|
|
|
|
if (node.x == des.x) and (node.y == des.y):
|
|
|
|
|
return node
|
|
|
|
|
|
|
|
|
|
return None
|
|
|
|
|
|
|
|
|
|
#邻点是否可用
|
|
|
|
|
def Is_valid(x,y,open_list=[],close_list=[]):
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
if x < 0 or x >=len(MAZE) or y < 0 or y >= len(MAZE[0]):
|
|
|
|
|
return False
|
|
|
|
|
|
|
|
|
|
if MAZE[x][y] == 1:
|
|
|
|
|
return False
|
|
|
|
|
|
|
|
|
|
if Is_contain(open_list,x,y):
|
|
|
|
|
return False
|
|
|
|
|
|
|
|
|
|
if Is_contain(close_list,x,y):
|
|
|
|
|
return False
|
|
|
|
|
return True
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
def Is_contain(nodes, x, y):
|
|
|
|
|
for node in nodes:
|
|
|
|
|
if (node.x == x) and (node.y == y):
|
|
|
|
|
return True
|
|
|
|
|
return False
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
class Node:
|
|
|
|
|
def __init__(self, x, y):
|
|
|
|
|
self.x = x
|
|
|
|
|
self.y = y
|
|
|
|
|
self.f = 0
|
|
|
|
|
self.g = 0
|
|
|
|
|
self.h = 0
|
|
|
|
|
self.parent = None
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
#对角线移动是直行移动代价的1.4倍
|
|
|
|
|
#如果相邻节点已有父节点，则父节点是否更改以使g值最小为准
|
|
|
|
|
def init_node(self, parent, end):
|
|
|
|
|
if self.parent is not None:
|
|
|
|
|
if parent is not None:
|
|
|
|
|
if abs(self.x - parent.x) + abs(self.y - parent.y) == 1:
|
|
|
|
|
if self.g > parent.g + 10:
|
|
|
|
|
self.g = parent.g + 10
|
|
|
|
|
self.parent = parent
|
|
|
|
|
self.h = (abs(self.x - end.x) + abs(self.y - end.y))*10
|
|
|
|
|
self.f = self.g + self.h
|
|
|
|
|
else:
|
|
|
|
|
if self.g > parent.g + 20:
|
|
|
|
|
self.g = parent.g + 20
|
|
|
|
|
self.parent = parent
|
|
|
|
|
self.h = (abs(self.x - end.x) + abs(self.y - end.y))*10
|
|
|
|
|
self.f = self.g + self.h
|
|
|
|
|
else:
|
|
|
|
|
if parent is not None:
|
|
|
|
|
if abs(self.x - parent.x) + abs(self.y - parent.y) == 1:
|
|
|
|
|
self.g = parent.g + 10
|
|
|
|
|
self.parent = parent
|
|
|
|
|
self.h = (abs(self.x - end.x) + abs(self.y - end.y))*10
|
|
|
|
|
self.f = self.g + self.h
|
|
|
|
|
else:
|
|
|
|
|
self.g = parent.g + 20
|
|
|
|
|
self.parent = parent
|
|
|
|
|
self.h = (abs(self.x - end.x) + abs(self.y - end.y))*10
|
|
|
|
|
self.f = self.g + self.h
|
|
|
|
|
|
|
|
|
|
def draw_path(map_grid,path):
|
|
|
|
|
obstacle_x=[]
|
|
|
|
|
obstacle_y=[]
|
|
|
|
|
path_x=[]
|
|
|
|
|
path_y=[]
|
|
|
|
|
close_list_x=[]
|
|
|
|
|
close_list_y=[]
|
|
|
|
|
open_list_x=[]
|
|
|
|
|
open_list_y=[]
|
|
|
|
|
for i in range(len(map_grid)):
|
|
|
|
|
for j in range(len(map_grid[0])):
|
|
|
|
|
if map_grid[i][j]==1: #栅格地图上obstacle为障碍物标识
|
|
|
|
|
obstacle_x.append(j)
|
|
|
|
|
obstacle_y.append(len(map_grid)-1-i)
|
|
|
|
|
plt.figure(figsize=(10,10)) #为了防止x,y轴间隔不一样长，影响最后的表现效果，所以手动设定等长
|
|
|
|
|
plt.ylim(-0.5,len(map_grid)-0.5)
|
|
|
|
|
plt.xlim(-0.5,len(map_grid[0])-0.5)
|
|
|
|
|
my_y_ticks = np.arange(0, len(map_grid), 1)
|
|
|
|
|
my_x_ticks = np.arange(0, len(map_grid[0]), 1)
|
|
|
|
|
plt.xticks(my_x_ticks)#竖线的位置与间隔
|
|
|
|
|
plt.yticks(my_y_ticks)
|
|
|
|
|
plt.grid(True) #开启栅格
|
|
|
|
|
for point in path:
|
|
|
|
|
path_x.append(point.y)
|
|
|
|
|
path_y.append(len(map_grid)-1-point.x)
|
|
|
|
|
plt.plot(path_x,path_y,linewidth=3)
|
|
|
|
|
plt.scatter(open_list_x,open_list_y,s=500,c='cyan',marker='s')
|
|
|
|
|
plt.scatter(obstacle_x,obstacle_y,s=500,c='k',marker='s')
|
|
|
|
|
plt.scatter(close_list_x,close_list_y,s=500,c='g',marker='s')
|
|
|
|
|
plt.title("grid map simulation ")
|
|
|
|
|
plt.show()
|
|
|
|
|
"""
|
|
|
|
|
if map_grid[i][j]==closelist: #栅格地图上closelist为为闭列表中记录的位置标志
|
|
|
|
|
close_list_x.append(i)
|
|
|
|
|
close_list_y.append(j)
|
|
|
|
|
if map_grid[i][j]==openlist: #栅格地图上openlist为为闭列表中记录的位置标志
|
|
|
|
|
open_list_x.append(i)
|
|
|
|
|
open_list_y.append(j)
|
|
|
|
|
"""
|
|
|
|
|
#暂时不画closelist和openlist
|
|
|
|
|
|
|
|
|
|
#迷宫地图
|
|
|
|
|
MAZE = [
|
|
|
|
|
[0,0,0,0,0,0,0,0,0,0,0,1,1,0,1,0,1,0,1,1,0,1,0],
|
|
|
|
|
[0,0,0,0,0,0,1,1,0,1,0,1,0,0,0,0,0,1,1,0,0,0,1],
|
|
|
|
|
[0,0,0,1,0,0,0,1,0,0,0,1,1,0,1,0,1,0,0,1,0,0,1],
|
|
|
|
|
[0,0,1,1,0,0,0,0,0,0,0,0,0,1,1,0,1,0,1,1,0,1,0],
|
|
|
|
|
[0,0,0,0,0,0,0,1,1,0,0,0,0,1,1,0,1,0,0,0,0,0,0],
|
|
|
|
|
[0,0,0,1,1,1,0,1,0,0,0,1,0,1,0,0,0,1,1,0,1,1,0],
|
|
|
|
|
[0,1,0,0,1,1,0,0,0,0,0,1,1,0,1,0,0,0,0,0,1,0,0],
|
|
|
|
|
[0,0,0,0,0,0,0,0,0,0,0,0,0,0,1,0,0,1,0,1,0,1,0],
|
|
|
|
|
[1,0,0,0,0,0,1,1,0,1,0,0,0,0,0,0,1,1,0,0,1,0,1],
|
|
|
|
|
[0,0,0,1,0,0,0,1,0,0,0,1,0,0,0,1,0,0,0,0,1,1,0],
|
|
|
|
|
[1,0,1,1,0,0,0,0,0,0,0,0,1,1,0,1,0,1,1,0,1,0,1],
|
|
|
|
|
[0,0,0,0,0,0,0,1,1,0,0,0,0,0,1,0,0,0,1,0,1,0,1],
|
|
|
|
|
[0,0,0,1,1,1,0,1,0,0,0,0,1,1,1,1,0,1,0,0,0,1,1],
|
|
|
|
|
[0,1,0,0,1,1,0,0,0,0,0,1,0,0,0,0,0,0,0,0,0,1,1],
|
|
|
|
|
[0,0,0,0,0,1,0,0,0,0,0,0,0,1,1,0,1,0,1,0,0,1,1],
|
|
|
|
|
[0,0,0,0,0,0,0,0,0,0,0,1,1,0,1,0,1,0,1,1,0,1,0],
|
|
|
|
|
[0,0,0,0,0,0,1,1,0,1,0,1,0,0,0,0,0,1,1,0,0,0,1],
|
|
|
|
|
[0,0,0,1,0,0,0,1,0,0,0,1,1,0,1,0,1,0,0,1,0,0,1],
|
|
|
|
|
[0,0,1,1,0,0,0,0,0,0,0,0,0,1,1,0,1,0,1,1,0,1,0],
|
|
|
|
|
[0,0,0,0,0,0,0,1,1,0,0,0,0,1,1,0,1,0,0,0,0,0,0],
|
|
|
|
|
[0,0,0,1,1,1,0,1,0,0,0,1,0,1,0,0,0,1,1,0,1,1,0],
|
|
|
|
|
[0,1,0,0,1,1,0,0,0,0,0,1,1,0,1,0,0,0,0,0,1,0,0],
|
|
|
|
|
[0,0,0,0,0,0,0,0,0,0,0,0,0,0,1,0,0,1,0,1,0,1,0],
|
|
|
|
|
]
|
|
|
|
|
|
|
|
|
|
#设置起始点和终点
|
|
|
|
|
org = Node(22, 0)
|
|
|
|
|
des = Node(0, 22)
|
|
|
|
|
|
|
|
|
|
result_node = A_star(org, des)
|
|
|
|
|
|
|
|
|
|
path = []
|
|
|
|
|
|
|
|
|
|
minpath = result_node.g
|
|
|
|
|
count=0
|
|
|
|
|
|
|
|
|
|
while result_node is not None:
|
|
|
|
|
path.append(Node(result_node.x, result_node.y))
|
|
|
|
|
if result_node.parent is not None:
|
|
|
|
|
if abs(result_node.x - result_node.parent.x) + abs(result_node.y - result_node.parent.y) == 2:
|
|
|
|
|
count+=1
|
|
|
|
|
result_node = result_node.parent
|
|
|
|
|
|
|
|
|
|
minpath-=count*6
|
|
|
|
|
|
|
|
|
|
for i in range(0,len(MAZE)):
|
|
|
|
|
for j in range(0,len(MAZE[0])):
|
|
|
|
|
if Is_contain(path, i, j):
|
|
|
|
|
MAZE[i][j]='*'
|
|
|
|
|
print("*, ", end='')
|
|
|
|
|
else:
|
|
|
|
|
print(str(MAZE[i][j]) + ", ", end='')
|
|
|
|
|
print()
|
|
|
|
|
print("最短路为：",minpath)
|
|
|
|
|
draw_path(MAZE,path)
|
