diff --git a/doc/空地协同无人作战系统-软件设计规格说明书.doc b/doc/空地协同无人作战系统-软件设计规格说明书.doc new file mode 100644 index 0000000..81bea8f Binary files /dev/null and b/doc/空地协同无人作战系统-软件设计规格说明书.doc differ diff --git a/doc/空地协同无人作战系统-软件需求规格说明书.docx b/doc/空地协同无人作战系统-软件需求规格说明书.docx new file mode 100644 index 0000000..8083faa Binary files /dev/null and b/doc/空地协同无人作战系统-软件需求规格说明书.docx differ diff --git a/src/Astar/Astar-level6.py b/src/Astar/Astar-level6.py new file mode 100644 index 0000000..b4dca24 --- /dev/null +++ b/src/Astar/Astar-level6.py @@ -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) \ No newline at end of file