Astar路径规划算法复现-python实现

# -*- coding: utf-8 -*-
"""
Created on Fri May 24 09:04:23 2024
"""
import os
import sys
import math
import heapq
import matplotlib.pyplot as plt
import time
'''
传统A*算法
'''
class Astar:
'''
AStar set the cost + heuristics as the priority
AStar将成本+启发式设置为优先级
'''
def __init__(self, s_start, s_goal, heuristic_type, xI, xG):
self.s_start = s_start
self.s_goal = s_goal
self.heuristic_type = heuristic_type
self.u_set = [(-1, 0), (0, 1), (1, 0), (0, -1)]
# self.obs = self.obs_map()  # 障碍物位置
self.Open = []  # 优先级序列，open集合
self.Closed = []  # 相邻点集合，访问visited序列
self.parent = dict()  # 相邻父节点
self.g = dict()  # 成本
self.x_range = 51  # 设置背景大小
self.y_range = 51
self.xI, self.xG = xI, xG
self.obs = self.obs_map()
def animation(self, path_l, visited_l, name, path_color='g'):
# 绘制地图基础元素
obs_x = [x[0] for x in self.obs]
obs_y = [x[1] for x in self.obs]
plt.plot(self.xI[0], self.xI[1], "bs")  # 起点
plt.plot(self.xG[0], self.xG[1], "gs")  # 终点
plt.plot(obs_x, obs_y, "sk")  # 障碍物
plt.title(name)
plt.axis("equal")
# 移除起点和终点于visited_l列表中，避免它们被标记为已访问点
visited_l = [node for node in visited_l if node != self.xI and node != self.xG]
# 绘制所有已访问节点
for x in visited_l:
plt.plot(x[0], x[1], color='gray', marker='o')
# 绘制路径
path_x = [point[0] for point in path_l]
path_y = [point[1] for point in path_l]
plt.plot(path_x, path_y, linewidth=3, color=path_color)
# 显示最终图形
plt.show(block=True)
def obs_map(self):
"""
Initialize obstacles' positions
:return: map of obstacles
初始化障碍物位置
返回：障碍物
"""
x = 51
y = 31
self.obs = set()
# 绘制边界
self.obs.update((i, 0) for i in range(x))
self.obs.update((i, y - 1) for i in range(x))
self.obs.update((0, i) for i in range(y))
self.obs.update((x - 1, i) for i in range(y))
# 给出障碍物坐标集1
self.obs.update((i, 15) for i in range(10, 21))
self.obs.update((20, i) for i in range(15))
# 给出障碍物坐标集2
self.obs.update((30, i) for i in range(15, 30))
# 给出障碍物坐标集3
self.obs.update((40, i) for i in range(16))
return self.obs
def searching(self):
"""
A_star Searching.
:return: path, visited order
Astart搜索，返回路径、访问集，
"""
self.parent[self.s_start] = self.s_start  # 初始化 起始父节点中只有起点。
self.g[self.s_start] = 0  # 初始代价为0
self.g[self.s_goal] = math.inf  # 目标节点代价为无穷大
# 将元素(self.f_value(self.s_start), self.s_start)插入到Open堆中，
# 并保持堆的性质（最小堆中父节点的值总是小于或等于其子节点的值））
# 这行代码的意思是：计算起始节点s_start的评估值f_value(self.s_start)，
# 然后将这对值(f_value, self.s_start)作为一个元组插入到self.Open这个最小堆中。
# 这样做的目的是在诸如A*搜索算法等需要高效管理待探索节点的场景下，
# 确保每次可以从堆顶（也就是当前评估值最小的节点）取出下一个待处理的节点。
# 这对于寻找最短路径、最小成本解决方案等问题非常有用。
heapq.heappush(self.Open, (self.f_value(self.s_start), self.s_start))
while self.Open:
# heappop会取出栈顶元素并将原始数据从堆栈中删除
# 在这个例子中，heappop返回的元素假设是一个包含两个元素的元组，
# 但代码中只关心第二个元素（实际的数据，比如一个状态、节点或其他任何类型的数据），
# 所以用_占位符丢弃了第一个元素（通常是评估值或优先级），而把第二个元素赋值给了变量s
_, s_current = heapq.heappop(self.Open)  # s_current存储的是当前位置的坐标
# print('栈顶元素为{0}'.format(s_current))
self.Closed.append(s_current)
if s_current == self.s_goal:  # 迭代停止条件，判断出栈顶元素是否为目标点，如果为目标点，则退出
break
# 如果不是，更新该点附近的代价值
# get_neighbor为获取附近点的坐标
for s_next in self.get_neighbor(s_current):
new_cost = self.g[s_current] + self.cost(s_current, s_next)
if s_next not in self.g:
self.g[s_next] = math.inf
if new_cost < self.g[s_next]:
self.g[s_next] = new_cost
self.parent[s_next] = s_current
# heappush入栈时需要存储的该点的代价值的计算方式为
heapq.heappush(self.Open, (self.f_value(s_next), s_next))
# self.animation(self.extract_path(self.parent), self.Closed, "A*")
return self.extract_path(self.parent), self.Closed
def get_neighbor(self, s_current):
"""
:param s_current:
:return: 相邻点集合
"""
return [(s_current[0] + u[0], s_current[1] + u[1]) for u in self.u_set]
def cost(self, s_current, s_next):
"""
:param s_current 表示当前点
:param s_next 表示相邻点
:return 若与障碍物无冲突，则范围欧式距离成本，否则为无穷大成本
计算当前点与相邻点的距离成本
"""
# 判断是否与障碍物冲突
if self.is_collision(s_current, s_next):
return math.inf
# 这里返回欧式距离成本
return math.hypot(s_next[0] - s_current[0], s_next[1] - s_current[1])
def is_collision(self, s_current, s_next):
"""
check if the line segment (s_start, s_end) is collision.
:param s_current: start node
:param s_next: end node
:return: True: is collision / False: not collision
检查起终点线段与障碍物是否冲突
如果线段的起点或终点之一位于障碍物集合 self.obs 内，则直接判定为碰撞，返回 True。
若线段不垂直也不水平（即斜线段），则分为两种情况检查：
若线段为45度线（斜率为1:1或-1），则检查线段的端点形成的矩形框内是否有障碍物。
否则检查线段端点形成的另一矩形框内是否有障碍物。
若上述任一矩形框内有障碍，则判定为碰撞，返回 True
若无碰撞情况，则返回 False
"""
# obs是障碍物，如果遇到障碍物，则距离（成本）无穷大
if s_current in self.obs or s_next in self.obs:
return True
''''''
# 如果该点s_start与相邻点s_end不相同
if s_current[0] != s_next[0] and s_current[1] != s_next[1]:
# 如果两点横纵坐标之差相等,即线段不垂直也不水平。135度线
if s_next[0] - s_current[0] == s_current[1] - s_next[1]:
s1 = (min(s_current[0], s_next[0]), min(s_current[1], s_next[1]))
s2 = (max(s_current[0], s_next[0]), max(s_current[1], s_next[1]))
# 如果两点横纵坐标之差不相等
else:
s1 = (min(s_current[0], s_next[0]), max(s_current[1], s_next[1]))
s2 = (max(s_current[0], s_next[0]), min(s_current[1], s_next[1]))
# obs是障碍物
if s1 in self.obs or s2 in self.obs:
return True
return False
def f_value(self, s_currrent):
"""
f = g + h. (g: Cost to come, h: heuristic value)
:param s: current state
:return: f
"""
return self.g[s_currrent] + self.heuristic(s_currrent)
def extract_path(self, parent):
path = [self.s_goal]
s = self.s_goal
while True:
s = parent[s]
path.append(s)
if s == self.s_start:
break
return list(path)
def heuristic(self, s_current):
heuristic_type = self.heuristic_type  # heuristic type
goal = self.s_goal  # goal node
# 如果为manhattan，则采用曼哈顿距离，s存储的是中间点
if heuristic_type == "manhattan":
return abs(goal[0] - s_current[0]) + abs(goal[1] - s_current[1])
# 否则就是欧几里得距离，符合勾股定理
else:
return math.hypot(goal[0] - s_current[0], goal[1] - s_current[1])
if __name__ == '__main__':
time_start = time.time()
s_start = (5, 5)
s_goal = (45, 26)
star_m = Astar(s_start, s_goal, "ee", s_start, s_goal)
path, visited = star_m.searching()
star_m.animation(path, visited, "A*")  # animation
time_end = time.time()
print("程序运行时间：", time_end - time_start)