AStar寻路算法的Python实现
人工智能课老师让整的,简单的python,加上matplotlib生成了一个散点图,矩阵点生成有10以内的误差,红色点是障碍物,百分之十的几率变成障碍物,绿色的点是要走的道路,初始化时可以设置地图大小,默认为10*20.
``'''
A-star寻路算法
'''
import random
import math
import queue
import matplotlib.pyplot as plt
# 点
class Point:
def __init__(self, row_index, col_index, access, index):
self.index = index
self.row_index = row_index
self.col_index = col_index
self.access = access
self.h = 0
self.color = "b"
self.head = None
self.temp_distance = 0
# 地图
class Map:
def __init__(self, max_col, max_row):
self.max_col = max_col
self.max_row = max_row
self.point = []
# 生成地图
def generate_map(self):
index_count = 0
# 生成坐标
for i in range(0, self.max_col * 10, 10):
for j in range(0, self.max_row * 10, 10):
col_ran = random.randint(1, 10)
row_ran = random.randint(1, 10)
point = Point(i + col_ran, j + row_ran, 0, index_count)
isAccessNum = random.randint(1, 10)
if isAccessNum == 5:
point.access = 1
point.color = 'r'
self.point.append(point)
index_count = index_count + 1
point_len = len(self.point)
# 求每个点的k值(到终点的预估距离,欧几里得距离)
final_x = self.point[point_len - 1].col_index
final_y = self.point[point_len - 1].row_index
for p in self.point:
p.h = math.sqrt(math.pow(final_x - p.col_index, 2) + math.pow(final_y - p.row_index, 2))
# AStar
class AStar:
def __init__(self, max_col, max_row):
self.max_col = max_col
self.max_row = max_row
self.map = Map(max_col, max_row)
self.map.generate_map()
self.g = 0
self.open_set = queue.PriorityQueue()
self.close_set = []
self.max_size = max_col * max_row - 1
# 输出地图
def print_map(self):
count = 0
for i in range(self.max_col * self.max_row):
point = self.map.point[i]
print("row: {}, col: {}, k:{}, {}".format(point.row_index, point.col_index, point.k, point.access), end=' ')
if count == self.max_row:
count = 0
print("\n")
count += 1
# 输出散点地图
def draw_map(self):
x = [i.col_index for i in self.map.point]
y = [i.row_index for i in self.map.point]
color = [i.color for i in self.map.point]
plt.scatter(x, y, c=color)
plt.show()
def a_star(self):
points = self.map.point
# 将首节点加入优先队列
self.open_set.put((points[0].h, points[0]))
head = None
while self.open_set.empty() != True:
# 取出优先队列最小数
X = self.open_set.get()[1]
self.g = self.g + X.temp_distance
X.head = head
head = X
X.color = 'g'
# 如果到达终点
if X == points[len(points) -1]:
print("已找到路线============")
break
# 获取邻居点
y_list = []
if X.index % self.max_row != (self.max_row - 1) and X.index + 1 >= 0 and X.index + 1 <= self.max_size:
y_list.append(points[X.index + 1])
if X.index % self.max_row != 0 and X.index - 1 >= 0 and X.index - 1 <= self.max_size:
y_list.append(points[X.index - 1])
if X.index - self.max_row >= 0 and X.index - self.max_row <= self.max_size:
y_list.append(points[X.index - self.max_row])
if X.index + self.max_row >= 0 and X.index + self.max_row <= self.max_size:
y_list.append(points[X.index + self.max_row])
if X.index % self.max_row != (self.max_row - 1) and X.index + self.max_row + 1 >= 0 and X.index + self.max_row + 1 <= self.max_size:
y_list.append(points[X.index + self.max_row + 1])
if X.index % self.max_row != 0 and X.index - 1 >= 0 and X.index + self.max_row - 1 >= 0 and X.index + self.max_row - 1 <= self.max_size:
y_list.append(points[X.index + self.max_row - 1])
if X.index % self.max_row != (self.max_row - 1) and X.index - self.max_row + 1 >= 0 and X.index - self.max_row + 1 <= self.max_size:
y_list.append(points[X.index - self.max_row + 1])
if X.index % self.max_row != 0 and X.index - self.max_row - 1 >= 0 and X.index - self.max_row - 1 <= self.max_size:
y_list.append(points[X.index - self.max_row - 1])
print('----------------')
# 遍历所有可达节点
# 如果Y在openset中,则判断
new_open_queue = queue.PriorityQueue()
for Y in y_list:
if Y.access == 1:
continue
# 计算到该点的距离和到终点的价值
distance = math.sqrt(math.pow(X.col_index - Y.col_index, 2) + math.pow(X.row_index - Y.row_index, 2))
Y.temp_distance = distance
value = self.g + distance + Y.h
print("index:{}, self.g: {}, distance: {}, Y.h: {}, value: {}".format(Y.index, self.g, distance, Y.h, value))
not_in = True
for opens in self.open_set.queue:
if Y == opens[1]:
if value < opens[0]:
new_open_queue.put((value, Y))
not_in = False
for closes in self.close_set:
if Y == closes[1]:
if value < closes[0]:
self.close_set.remove(closes)
new_open_queue.put((value, Y))
not_in = False
if not_in:
new_open_queue.put((value, Y))
self.open_set = new_open_queue
self.close_set.append((value, X))
# 输出路径
print('path: ')
while head != None:
print("col_index: {}, row_index: {}, index: {}, h: {}".format(head.col_index, head.row_index, head.index, head.h))
head.color = 'g'
head = head.head
if __name__ == '__main__':
star = AStar(10, 20)
star.a_star()
star.draw_map()