基础Astar算法及节点优化python代码实现
地图文件示例
map7.txt
s#####bbb###########
#bb###bbb######bbb##
#bb###bbb######bbb##
######bbb######bbb##
######bbb###########
#bbb##bbb###########
#bbb##bbb###########
#bbb##bbb#bbbb######
#bbb##bbb#bbbb######
#bbb######bbbb######
##########bbbb######
####################
#####bbb###bbb#bbbb#
#####bbb###bbb#bbbb#
bbbb#######bbb#bbbb#
bbbb##bbb###########
######bbb#bb#####bbb
##########bb#####bbb
##########bb########
##########bb#######g
全部代码
import matplotlib.pyplot as plt
height = 0 # 行数
weight = 0 # 列数
start = (0, 0) # 起点
goal = (0, 0) # 终点
road = [] # 路坐标 数组
wall = [] # 墙坐标 数组
closeList = [] # close 数组
openList = [] # open 数组
dirt = {} # 父子节点 字典
valuedistance = {} # 与起点终点距离和 字典
finalList = [] # 最终路径 数组
map_name = 'map7.txt' # 地图文件
# 读取地图
def read_Map():
FILE_STORAGE_WAY = map_name
ifile = open(FILE_STORAGE_WAY, 'r') # 作为只读文件打开
global height, weight, start, goal # 声明全局变量
fileLine = ifile.readline() # 按行读
while len(fileLine) > 0:
weight = len(fileLine.split('\n')[0]) # 去除换行符
for i in range(0, weight):
if 's' == fileLine[i]: # 起点
start = (height, i)
road.append((height, i))
if 'g' == fileLine[i]: # 终点
goal = (height, i)
road.append((height, i))
if '#' == fileLine[i]: # 路
road.append((height, i))
if 'b' == fileLine[i]: # 墙
wall.append((height, i))
height += 1
fileLine = ifile.readline()
# 计算节点和终点的曼哈顿距离
def goal_distance(value):
# 计算在x、y轴的移动距离差
tmp = abs(abs(value[0] - goal[0]) - abs(value[1] - goal[1]))
if tmp == 0:
# 如果仅为斜向距离，斜方向移动距离单位为1.4
goal_distance = abs(value[0] - goal[0]) * 1.4
else:
# 如果为x或y轴距离+斜向距离
goal_distance = tmp + min(abs(value[0] - goal[0]), abs(value[1] - goal[1])) * 1.4
return goal_distance
# 通过递归父节点至起点， 计算节点与起点距离。（不能直接计算两点距离，中间有障碍物）
def start_distance(value):
father_value = dirt[value]
if abs(value[0] - father_value[0]) == 1 and abs(value[1] - father_value[1]): # 如果是斜距
start_distance = 1.4
else:
start_distance = 1
while father_value is not start: # 父节点不是起点
value = father_value
father_value = dirt[father_value]
if abs(value[0] - father_value[0]) == 1 and abs(value[1] - father_value[1]): # 如果是斜距
start_distance += 1.4
else:
start_distance += 1
return start_distance
# 实现 更新节点距离 功能
def change_father_value(value, new_value, dis):
# 领接节点不是当前节点 且领接节点在openList或closeList中
if new_value != dirt[value] and (new_value in openList or new_value in closeList):
value_distance = start_distance(value) + goal_distance(value)
new_value_distance = start_distance(new_value) + goal_distance(value)
if round(new_value_distance + dis, 1) < round(value_distance, 1): # round（x,1)保留1位小数
# print(new_value_distance, dis1, value_distance)
# print(new_value,value)
dirt[value] = new_value # 指向新父节点
# 实现 traversal函数的 加入openlist以及计算节点距离 功能
def add_openList_and_dirt(value, father_value):
if value in road and value not in closeList: # 该节点不是墙 且 未遍历
if value not in openList: # 如果不在 openList（领接节点）
openList.append(value) # 加入 openlist
dirt[value] = father_value # 指向 父节点
# 计算起点终点距离 小数相加保留1位小数
valuedistance[value] = round(start_distance(value) + goal_distance(value), 1)
# 优化父节点
if value in openList and dirt[value] != start: # 已在 openList 且父节点不是起点
# 传参：节点 领接节点 节点与领接节点距离 节点与原父节点距离
x = value[0]
y = value[1]
change_father_value(value, (value[0], value[1] + 1), 1) # 东
if (x, y + 1) in road and (x + 1, y) in road: # 如果东面、南面没有墙
change_father_value(value, (value[0] + 1, value[1] + 1), 1.4) # 东南
change_father_value(value, (value[0] + 1, value[1]), 1) # 南
if (x, y - 1) in road and (x + 1, y) in road: # 如果西面、南面没有墙
change_father_value(value, (value[0] + 1, value[1] - 1), 1.4) # 西南
change_father_value(value, (value[0], value[1] - 1), 1) # 西
if (x, y - 1) in road and (x - 1, y) in road: # 如果西面、北面没有墙
change_father_value(value, (value[0] - 1, value[1] - 1), 1.4) # 西北
change_father_value(value, (value[0] - 1, value[1]), 1) # 北
if (x, y + 1) in road and (x - 1, y) in road: # 如果东面、北面没有墙
change_father_value(value, (value[0] - 1, value[1] + 1), 1.4) # 东北
valuedistance[value] = round(start_distance(value) + goal_distance(value), 1) # 重新计算距离，该句不能删除
# 将节点加入closelist，将该节点可达的近邻节点放入openlist中 ，存储父子节点关系
def traversal(father_value):
closeList.append(father_value)
if father_value == goal: # 找到终点则退出
return
x = father_value[0]
y = father_value[1]
# 传参：领接节点坐标 距离
# 东
add_openList_and_dirt((x + 1, y), father_value)
# 东南
if (x + 1, y) in road and (x, y + 1) in road: # 如果东面、南面没有墙
add_openList_and_dirt((x + 1, y + 1), father_value, )
# 南
add_openList_and_dirt((x, y + 1), father_value)
# 西南
if (x - 1, y) in road and (x, y + 1) in road: # 如果西面、南面没有墙
add_openList_and_dirt((x - 1, y + 1), father_value)
# 西
add_openList_and_dirt((x - 1, y), father_value)
# 西北
if (x - 1, y) in road and (x, y - 1) in road: # 如果西面、北面没有墙
add_openList_and_dirt((x - 1, y - 1), father_value)
# 北
add_openList_and_dirt((x, y - 1), father_value)
# 东北
if (x + 1, y) in road and (x, y - 1) in road: # 如果东面、北面没有墙
add_openList_and_dirt((x + 1, y - 1), father_value)
# 控制台打印地图
def print_map():
print('*' * (weight + 2))
for i in range(0, height):
print('*', end='')
for j in range(0, weight):
if (i, j) == start:
print('s', end='')
elif (i, j) == goal:
print('g', end='')
elif (i, j) in closeList:
print('c', end='')
elif (i, j) in wall:
print('#', end='')
else:
print(' ', end='')
print('*')
print('*' * (weight + 2))
# 控制台打印地图和最终路径
def printfinalmap():
print('*' * (weight + 2))
for i in range(0, height):
print('*', end='')
for j in range(0, weight):
if (i, j) == start:
print('s', end='')
elif (i, j) == goal:
print('g', end='')
elif (i, j) in finalList:
print('c', end='')
elif (i, j) in wall:
print('#', end='')
else:
print(' ', end='')
print('*')
print('*' * (weight + 2))
# 从终点递归输出最终路径
def recursion(value):
finalList.append(value)
if value != start:
recursion(dirt[value])
# print(value,end='')
def draw_path(map_grid, obstacle, path, closelist, openlist, new_path=None):
'''
map_grid数组大小
obstacle到openlist都是整数，为其对应的标志。比如0代表obstacle，那么obstacle位置就填0
'''
obstacle_x = []
obstacle_y = []
path_x = []
path_y = []
close_list_x = []
close_list_y = []
open_list_x = []
open_list_y = []
for node in obstacle:
obstacle_y.append(node[0])
obstacle_x.append(node[1])
for node in path:
path_y.append(node[0])
path_x.append(node[1])
for node in closelist:
close_list_y.append(node[0])
close_list_x.append(node[1])
for node in openlist:
open_list_y.append(node[0])
open_list_x.append(node[1])
x = map_grid[1]
y = map_grid[0]
plt.figure(figsize=(20, 20)) # 为了防止x,y轴间隔不一样长，影响最后的表现效果，所以手动设定等长
plt.xlim(-1, map_grid[1])
plt.ylim(-1, map_grid[0])
my_x_ticks = [x_tick for x_tick in range(x)]
my_y_ticks = [y_tick for y_tick in range(y)]
plt.xticks(my_x_ticks)
plt.yticks(my_y_ticks)
plt.grid(True) # 开启栅格
plt.plot(path_x, path_y, linewidth=3)
# 地图越大正方形需要调小，影响展现效果
plt.scatter(open_list_x, open_list_y, s=2000, c='g', marker='s')
plt.scatter(obstacle_x, obstacle_y, s=2000, c='k', marker='s')
plt.scatter(close_list_x, close_list_y, s=2000, c='cyan', marker='s')
if new_path is not None:
new_path_x = []
new_path_y = []
for node in new_path:
new_path_x.append(node[1])
new_path_y.append(node[0])
plt.plot(new_path_x, new_path_y, linewidth=3)
plt.title(" ")
ax = plt.gca() # 获取到当前坐标轴信息
ax.set_aspect(1) # 等比例
ax.xaxis.set_ticks_position('top') # 将X坐标轴移到上面
ax.invert_yaxis() # 反转Y坐标轴
plt.show()
def Torngzhour(node0, node1, node2):
# Y坐标 或 X 坐标相等
# if (node0[0] == node1[0] and node0[0] == node2[0]) or (node0[1] == node1[1] and node0[1] == node2[1]):
# return True
# 方向 相等
if (node1[0] - node0[0] == node2[0] - node1[0]) and (node1[1] - node0[1] == node2[1] - node1[1]):
return True
else:
return False
def ZhaangAih(node1, node2):
print("T[i-1]", node1, "T[i+1]", node2)
max_y = max(node1[0], node2[0])
min_y = min(node1[0], node2[0])
max_x = max(node1[1], node2[1])
min_x = min(node1[1], node2[1])
hit = []
hit.append(node1)
hit.append(node2)
for x in range(min_x, max_x + 1):
y = (node1[0] - node2[0]) / (node1[1] - node2[1]) * (x - node2[1]) + node2[0]
print("y", y)
print((round((y - 0.5)), x), (round((y + 0.5)), x))
hit.append((round((y - 0.5)), x))
hit.append((round((y + 0.5)), x))
for y in range(min_y, max_y + 1):
x = (node1[1] - node2[1]) / (node1[0] - node2[0]) * (y - node2[0]) + node2[1]
print("x", x)
print((y, round((x - 0.5))), (y, round((x + 0.5))))
hit.append((y, round(x - 0.5)))
hit.append((y, round(x + 0.5)))
for i in range(len(hit)):
if hit[i] in wall:
print(hit[i])
return True
return False
if __name__ == '__main__':
read_Map() # 加载地图
traversal(start) # 遍历起点
while goal not in openList: # 以openList找到goal为终止
min_distance_value = min(valuedistance, key=lambda x: valuedistance[x]) # 找出最短距离点
valuedistance.pop(min_distance_value) # 移除节点距离（该节点与起点、终点距离和）
openList.remove(min_distance_value) # 从openList中移除该点
traversal(min_distance_value) # 遍历该节点
# print_map() # 在输出窗口打印当前地图
if not openList: # openlist为空，即所有能遍历的点都遍历过
print("无方案")
quit()
# print_map() # 打印当前地图
# print("节点：父节点", dirt)
# print()
# 递归输出最终路径
if goal in openList:
recursion(goal)
# 以上为传统A*算法
# 以下传统A*算法路径节点进行优化
print("map_grid = ", "(", height, ",", weight, ")")
print("obstacle =", wall)
print("openlist =", openList)
print("closelist =", closeList)
print("path =", finalList)
print('路程', start_distance(goal))
# printfinalmap()
map_grid = (height, weight)
# draw_path(map_grid, wall, finalList, closeList, openList)
# 步骤一 导入路径
path = finalList
path.reverse()
print("path = ", path)
# 步骤二 找出折点
T = []
size = len(path)
for i in range(size):
# 起点终点不计入
if i == 0 or i == size - 1:
continue
else:
if not Torngzhour(path[i - 1], path[i], path[i + 1]):
T.append(path[i])
print("T is ", T)
# 步骤三 和 步骤四 去除冗余折点
need_count = 0
size = len(T)
while need_count < size:
# 判断这一轮优化结果和上一轮相等，则退出
need_count = 0
size = len(T)
i = 0
while i < len(T):
if i == 0:
last_node = start
else:
last_node = T[i - 1]
if i == len(T) - 1:
next_node = goal
else:
next_node = T[i + 1]
if not ZhaangAih(last_node, next_node):
T.pop(i)
else:
i += 1
need_count += 1
print("T = ", T)
new_path = [start]
for i in range(len(T)):
new_path.append(T[i])
new_path.append(goal)
print("new_path ", new_path)
draw_path(map_grid, wall, finalList, closeList, openList, new_path)
``