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

1. # -*- coding: utf-8 -*-
2. """
3. Created on Fri May 24 09:04:23 2024
6. """
8. import os
9. import sys
10. import math
11. import heapq
12. import matplotlib.pyplot as plt
13. import time
15. '''
16. 传统A*算法
17. '''
20. class Astar:
21.     '''
22.     AStar set the cost + heuristics as the priority
23.     AStar将成本+启发式设置为优先级
24.     '''
26.     def __init__(self, s_start, s_goal, heuristic_type, xI, xG):
27.         self.s_start = s_start
28.         self.s_goal = s_goal
29.         self.heuristic_type = heuristic_type
30.         self.u_set = [(-1, 0), (0, 1), (1, 0), (0, -1)]
32.         # self.obs = self.obs_map()  # 障碍物位置
33.         self.Open = []  # 优先级序列，open集合
34.         self.Closed = []  # 相邻点集合，访问visited序列
35.         self.parent = dict()  # 相邻父节点
36.         self.g = dict()  # 成本
37.         self.x_range = 51  # 设置背景大小
38.         self.y_range = 51
40.         self.xI, self.xG = xI, xG
41.         self.obs = self.obs_map()
43.     def animation(self, path_l, visited_l, name, path_color='g'):
45.         # 绘制地图基础元素
46.         obs_x = [x[0] for x in self.obs]
47.         obs_y = [x[1] for x in self.obs]
48.         plt.plot(self.xI[0], self.xI[1], "bs")  # 起点
49.         plt.plot(self.xG[0], self.xG[1], "gs")  # 终点
50.         plt.plot(obs_x, obs_y, "sk")  # 障碍物
51.         plt.title(name)
52.         plt.axis("equal")
54.         # 移除起点和终点于visited_l列表中，避免它们被标记为已访问点
55.         visited_l = [node for node in visited_l if node != self.xI and node != self.xG]
57.         # 绘制所有已访问节点
58.         for x in visited_l:
59.             plt.plot(x[0], x[1], color='gray', marker='o')
61.         # 绘制路径
62.         path_x = [point[0] for point in path_l]
63.         path_y = [point[1] for point in path_l]
64.         plt.plot(path_x, path_y, linewidth=3, color=path_color)
66.         # 显示最终图形
67.         plt.show(block=True)
69.     def obs_map(self):
70.         """
71.         Initialize obstacles' positions
72.         :return: map of obstacles
73.         初始化障碍物位置
74.         返回：障碍物
75.         """
76.         x = 51
77.         y = 31
78.         self.obs = set()
80.         # 绘制边界
81.         self.obs.update((i, 0) for i in range(x))
83.         self.obs.update((i, y - 1) for i in range(x))
85.         self.obs.update((0, i) for i in range(y))
87.         self.obs.update((x - 1, i) for i in range(y))
89.         # 给出障碍物坐标集1
90.         self.obs.update((i, 15) for i in range(10, 21))
91.         self.obs.update((20, i) for i in range(15))
93.         # 给出障碍物坐标集2
94.         self.obs.update((30, i) for i in range(15, 30))
96.         # 给出障碍物坐标集3
97.         self.obs.update((40, i) for i in range(16))
99.         return self.obs
101.     def searching(self):
102.         """
103.         A_star Searching.
104.         :return: path, visited order
106.         Astart搜索，返回路径、访问集，
107.         """
109.         self.parent[self.s_start] = self.s_start  # 初始化 起始父节点中只有起点。
110.         self.g[self.s_start] = 0  # 初始代价为0
111.         self.g[self.s_goal] = math.inf  # 目标节点代价为无穷大
113.         # 将元素(self.f_value(self.s_start), self.s_start)插入到Open堆中，
114.         # 并保持堆的性质（最小堆中父节点的值总是小于或等于其子节点的值））
115.         # 这行代码的意思是：计算起始节点s_start的评估值f_value(self.s_start)，
116.         # 然后将这对值(f_value, self.s_start)作为一个元组插入到self.Open这个最小堆中。
117.         # 这样做的目的是在诸如A*搜索算法等需要高效管理待探索节点的场景下，
118.         # 确保每次可以从堆顶（也就是当前评估值最小的节点）取出下一个待处理的节点。
119.         # 这对于寻找最短路径、最小成本解决方案等问题非常有用。
120.         heapq.heappush(self.Open, (self.f_value(self.s_start), self.s_start))
122.         while self.Open:
123.             # heappop会取出栈顶元素并将原始数据从堆栈中删除
124.             # 在这个例子中，heappop返回的元素假设是一个包含两个元素的元组，
125.             # 但代码中只关心第二个元素（实际的数据，比如一个状态、节点或其他任何类型的数据），
126.             # 所以用_占位符丢弃了第一个元素（通常是评估值或优先级），而把第二个元素赋值给了变量s
127.             _, s_current = heapq.heappop(self.Open)  # s_current存储的是当前位置的坐标
128.             # print('栈顶元素为{0}'.format(s_current))
130.             self.Closed.append(s_current)
132.             if s_current == self.s_goal:  # 迭代停止条件，判断出栈顶元素是否为目标点，如果为目标点，则退出
133.                 break
134.             # 如果不是，更新该点附近的代价值
135.             # get_neighbor为获取附近点的坐标
136.             for s_next in self.get_neighbor(s_current):
137.                 new_cost = self.g[s_current] + self.cost(s_current, s_next)
139.                 if s_next not in self.g:
140.                     self.g[s_next] = math.inf
142.                 if new_cost < self.g[s_next]:
143.                     self.g[s_next] = new_cost
144.                     self.parent[s_next] = s_current
145.                     # heappush入栈时需要存储的该点的代价值的计算方式为
146.                     heapq.heappush(self.Open, (self.f_value(s_next), s_next))
148.         # self.animation(self.extract_path(self.parent), self.Closed, "A*")
149.         return self.extract_path(self.parent), self.Closed
151.     def get_neighbor(self, s_current):
152.         """
154.         :param s_current:
155.         :return: 相邻点集合
156.         """
157.         return [(s_current[0] + u[0], s_current[1] + u[1]) for u in self.u_set]
159.     def cost(self, s_current, s_next):
160.         """
161.         :param s_current 表示当前点
162.         :param s_next 表示相邻点
163.         :return 若与障碍物无冲突，则范围欧式距离成本，否则为无穷大成本
164.         计算当前点与相邻点的距离成本
165.         """
166.         # 判断是否与障碍物冲突
167.         if self.is_collision(s_current, s_next):
168.             return math.inf
169.         # 这里返回欧式距离成本
170.         return math.hypot(s_next[0] - s_current[0], s_next[1] - s_current[1])
172.     def is_collision(self, s_current, s_next):
173.         """
174.             check if the line segment (s_start, s_end) is collision.
175.             :param s_current: start node
176.             :param s_next: end node
177.             :return: True: is collision / False: not collision
178.             检查起终点线段与障碍物是否冲突
179.         如果线段的起点或终点之一位于障碍物集合 self.obs 内，则直接判定为碰撞，返回 True。
180.         若线段不垂直也不水平（即斜线段），则分为两种情况检查：
181.             若线段为45度线（斜率为1:1或-1），则检查线段的端点形成的矩形框内是否有障碍物。
182.             否则检查线段端点形成的另一矩形框内是否有障碍物。
183.         若上述任一矩形框内有障碍，则判定为碰撞，返回 True
184.         若无碰撞情况，则返回 False
185.         """
186.         # obs是障碍物，如果遇到障碍物，则距离（成本）无穷大
187.         if s_current in self.obs or s_next in self.obs:
188.             return True
189.         ''''''
190.         # 如果该点s_start与相邻点s_end不相同
191.         if s_current[0] != s_next[0] and s_current[1] != s_next[1]:
193.             # 如果两点横纵坐标之差相等,即线段不垂直也不水平。135度线
194.             if s_next[0] - s_current[0] == s_current[1] - s_next[1]:
195.                 s1 = (min(s_current[0], s_next[0]), min(s_current[1], s_next[1]))
197.                 s2 = (max(s_current[0], s_next[0]), max(s_current[1], s_next[1]))
198.             # 如果两点横纵坐标之差不相等
199.             else:
200.                 s1 = (min(s_current[0], s_next[0]), max(s_current[1], s_next[1]))
201.                 s2 = (max(s_current[0], s_next[0]), min(s_current[1], s_next[1]))
202.             # obs是障碍物
203.             if s1 in self.obs or s2 in self.obs:
204.                 return True
205.         return False
207.     def f_value(self, s_currrent):
208.         """
209.         f = g + h. (g: Cost to come, h: heuristic value)
210.         :param s: current state
211.         :return: f
212.         """
214.         return self.g[s_currrent] + self.heuristic(s_currrent)
216.     def extract_path(self, parent):
218.         path = [self.s_goal]
219.         s = self.s_goal
221.         while True:
222.             s = parent[s]
223.             path.append(s)
225.             if s == self.s_start:
226.                 break
228.         return list(path)
230.     def heuristic(self, s_current):
232.         heuristic_type = self.heuristic_type  # heuristic type
233.         goal = self.s_goal  # goal node
234.         # 如果为manhattan，则采用曼哈顿距离，s存储的是中间点
235.         if heuristic_type == "manhattan":
236.             return abs(goal[0] - s_current[0]) + abs(goal[1] - s_current[1])
237.         # 否则就是欧几里得距离，符合勾股定理
238.         else:
239.             return math.hypot(goal[0] - s_current[0], goal[1] - s_current[1])
242. if __name__ == '__main__':
243.     time_start = time.time()
244.     s_start = (5, 5)
245.     s_goal = (45, 26)
247.     star_m = Astar(s_start, s_goal, "ee", s_start, s_goal)
249.     path, visited = star_m.searching()
250.     star_m.animation(path, visited, "A*")  # animation
251.     time_end = time.time()
252.     print("程序运行时间：", time_end - time_start)
253.        

复制代码

免责声明：如果侵犯了您的权益，请联系站长，我们会及时删除侵权内容，谢谢合作！更多信息从访问主页：qidao123.com:ToB企服之家，中国第一个企服评测及商务社交产业平台。