1 # 版本1.2，2018—04—09
2 # 所有节点的g值并没有初始化为无穷大
3 # 当两个子节点的f值一样时，程序选择最先搜索到的一个作为父节点加入closed
4 # 对相同数值的不同对待，导致不同版本的A*算法找到等长的不同路径
5 # 最后closed表中的节点很多，如何找出最优的一条路径
6 # 撞墙之后产生较多的节点会加入closed表，此时开始删除closed表中不合理的节点，1.1版本的思路
7 # 1.2版本思路，建立每一个节点的方向指针，指向f值最小的上个节点
8 # 参考《无人驾驶概论》、《基于A*算法的移动机器人路径规划》王淼驰，《人工智能及应用》鲁斌
9
10
11 import numpy
12 from pylab import *
13 import copy
14
15 # 定义一个含有障碍物的20×20的栅格地图
16 # 10表示可通行点
17 # 0表示障碍物
18 # 7表示起点
19 # 5表示终点
20 map_grid = numpy.full((20, 20), int(10), dtype=numpy.int8)
21 # print(map_grid)
22 map_grid[3, 3:8] = 0
23 map_grid[3:10, 7] = 0
24 map_grid[10, 3:8] = 0
25 map_grid[17, 13:17] = 0
26 map_grid[10:17, 13] = 0
27 map_grid[10, 13:17] = 0
28 map_grid[5, 2] = 7
29 map_grid[15, 15] = 5
30 # 画出定义的栅格地图
31
32 # plt.imshow(map_grid, cmap=plt.cm.hot, interpolation='nearest', vmin=0, vmax=10)
33 # plt.colorbar()
34 # xlim(-1, 20) # 设置x轴范围
35 # ylim(-1, 20) # 设置y轴范围
36 # my_x_ticks = numpy.arange(0, 20, 1)
37 # my_y_ticks = numpy.arange(0, 20, 1)
38 # plt.xticks(my_x_ticks)
39 # plt.yticks(my_y_ticks)
40 # plt.grid(True)
41 # plt.show()
42
43
44 class AStar(object):
45 """
46 创建一个A*算法类
47 """
48
49 def __init__(self):
50 """
51 初始化
52 """
53 self.f = 0
54 self.g = 0
55 self.last_point = numpy.array([])
56 self.current_point = numpy.array([]) # 当前目标点不断取得更新
57 self.open = numpy.array([[], []]) # 先创建一个空的open表
58 self.closed = numpy.array([[], []]) # 先创建一个空的closed表
59 # self.taboo = numpy.array([[], []]) # 创建一个禁忌表，用于放置不再搜索的点
60 self.start = numpy.array([5, 2]) # 起点坐标
61 self.goal = numpy.array([15, 15]) # 终点坐标
62 self.record_direction = numpy.array([[], [], [], []]) # 记录坐标点和方向
63 self.best_path_array = numpy.array([[], []]) # 回溯路径表
64 self.point_f = numpy.array([[], [], []]) # 记录每个点的f值
65
66 def h_value_tem(self, cur_p):
67 """
68 计算拓展节点和终点的h值
69 :param cur_p:子搜索节点坐标
70 :return:
71 """
72 h = (cur_p[0] - 15) ** 2 + (cur_p[1] - 15) ** 2
73 h = numpy.sqrt(h) # 计算h
74 return h
75
76 def g_value(self):
77 """
78 累计的g值
79 :param x:
80 :param y:
81 :return:
82 """
83 self.g = self.g + self.g_value_tem(self.current_point, self.last_point)
84
85 def g_value_tem(self, chl_p, cu_p):
86 """
87 计算拓展节点和父节点的g值
88 其实也可以直接用1或者1.414代替
89 :param chl_p:子节点坐标
90 :param cu_p:父节点坐标，也就是self.current_point
91 :return:返回子节点到父节点的g值，但不是全局g值
92 """
93 g1 = cu_p[0] - chl_p[0]
94 g2 = cu_p[1] - chl_p[1]
95 g = g1 ** 2 + g2 ** 2
96 g = numpy.sqrt(g)
97 return g
98
99 def f_value_tem(self, chl_p, cu_p):
100 """
101 求出的是临时g值和h值的和，还需加上累计g值得到全局f值
102 :param chl_p: 父节点坐标
103 :param cu_p: 子节点坐标
104 :return:
105 """
106 f = self.g_value_tem(chl_p, cu_p) + self.h_value_tem(cu_p)
107 return f
108
109 def min_f(self):
110 """
111 找出open中f值最小的节点坐标，记录为current_point
112 :return:返回open表中最小值的位置索引和在map_grid中的坐标
113 对撞墙后的处理方式是，随机选择一个方向进行搜索
114 并且将open列表清零，不然一直是死循环
115 这种处理方式以后待改进！！！--1.1
116 1.2更新：
117 建立方向指针
118 """
119 tem_f = [] # 创建一个记录f值的临时列表
120 for i in range(self.open.shape[1]):
121 # 计算拓展节点的全局f值
122 f_value = self.f_value_tem(self.current_point, self.open[:, i]) + self.g
123 tem_f.append(f_value)
124 index = tem_f.index(min(tem_f)) # 返回最小值索引
125 location = self.open[:, index] # 返回最小值坐标
126 print('打印位置索引和地图坐标：')
127 print(index, location)
128 return index, location
129
130 def child_point(self, x):
131 """
132 拓展的子节点坐标
133 :param x: 父节点坐标
134 :return: 子节点存入open表，返回值是每一次拓展出的子节点数目，用于撞墙判断
135 当搜索的节点撞墙后，如果不加处理，会陷入死循环
136 """
137 # tem_open = numpy.array([[], []]) # 统计拓展节点的临时数组
138 # tem_open_shape = 0 # 统计临时数组的长度
139 # 开始遍历周围8个节点
140 for j in range(-1, 2, 1):
141 for q in range(-1, 2, 1):
142 if j == 0 and q == 0: # 搜索到父节点去掉
143 continue
144 m = numpy.array([x[0] + j, x[1] + q])
145
146 # print(map_grid[int(x[0] + j), int(x[1] + q)])
147 if map_grid[int(m[0]), int(m[1])] == 0: # 搜索到障碍物去掉
148 continue
149 if m[0] < 0 or m[0] > 19 or m[1] < 0 or m[1] > 19: # 搜索点出了边界去掉
150 continue
151
152 self.direction(x, m) # 每产生一个子节点，记录一次方向
153 # 在closed表中,则去掉搜索点
154 b = self.judge_location(m, self.closed)
155 if b == 1:
156 numpy.delete(self.record_direction, -1, axis=1)
157 continue
158
159 # 在open表中，则去掉搜索点，但是需要更新方向指针和self.g值
160 # 而且只需要计算并更新self.g即可，此时建立一个比较g值的函数
161 a = self.judge_location(m, self.open)
162 if a == 1:
163 # 此时删除上面记录的方向并重新记录
164 numpy.delete(self.record_direction, -1, axis=1)
165 # self.direction(self.last_point, m)
166
167 continue
168
169 # # 在taboo表中，则去掉搜索点
170 # c = self.judge_location(x, j, q, self.taboo)
171 # if c == 1:
172 # continue
173
174 # tem_open = numpy.c_[tem_open, m] #
175
176 # tem_open_shape = tem_open.shape[1] # 求出tem_open的长度
177
178 self.open = numpy.c_[self.open, m] # 搜索出的子节点加入open
179 # print('打印第一次循环后的open：')
180 # print(self.open)
181 # return tem_open_shape
182
183 def judge_location(self, m, list_co):
184 """
185 判断拓展点是否在open表或者closed表中
186 :return:
187 """
188 jud = 0
189 for i in range(list_co.shape[1]):
190
191 if m[0] == list_co[0, i] and m[1] == list_co[1, i]:
192
193 jud = jud + 1
194 else:
195 jud = jud
196 # if a != 0:
197 # continue
198 return jud
199
200 def direction(self, father_point, son_point):
201 """
202 建立每一个节点的方向，便于在closed表中选出最佳路径
203 非常重要的一步，不然画出的图像参考1.1版本
204 x记录子节点和父节点的x轴变化
205 y记录子节点和父节点的y轴变化
206 如（0，1）表示子节点在父节点的方向上变化0和1
207 :return:
208 """
209 x = son_point[0] - father_point[0]
210 y = son_point[1] - father_point[1]
211 xy = [son_point[0], son_point[1], x, y]
212 self.record_direction = numpy.c_[self.record_direction, xy]
213 # return x, y
214
215 # def g_compare(self, father, son, son_grandson):
216 # """
217 # g值比较函数
218 # :param father:
219 # :param son:
220 # :param son_grandson:
221 # :return:
222 # """
223 # pass
224
225 def path_backtrace(self):
226 """
227 回溯closed表中的最短路径
228 :return:
229 """
230 best_path = [15, 15] # 回溯路径的初始化
231 self.best_path_array = numpy.array([[15], [15]])
232 j = 0
233 while j <= self.record_direction.shape[1]:
234 for i in range(self.record_direction.shape[1]):
235 if best_path[0] == self.record_direction[0][i] and best_path[1] == self.record_direction[1][i]:
236 x = self.record_direction[0][i]-self.record_direction[2][i]
237 y = self.record_direction[1][i]-self.record_direction[3][i]
238 best_path = [x, y]
239 self.best_path_array = numpy.c_[self.best_path_array, best_path]
240 break # 如果已经找到，退出本轮循环，减少耗时
241 else:
242 continue
243 j = j+1
244 # return best_path_array
245
246 def main(self):
247 """
248 main函数
249 :return:
250 """
251 self.open = numpy.column_stack((self.open, self.start)) # 起点放入open
252 self.current_point = self.start # 起点放入当前点，作为父节点
253 # self.closed
254 ite = 1
255 while ite <= 200:
256
257 # open列表为空，退出
258 if self.open.shape[1] == 0:
259 print('没有搜索到路径！')
260 return
261
262 self.last_point = self.current_point # 上一个目标点不断取得更新
263
264 index, self.current_point = self.min_f() # 判断open表中f值
265 print('检验第%s次当前点坐标' % ite)
266 print(self.current_point)
267
268 # 选取open表中最小f值的节点作为best，放入closed表
269 self.closed = numpy.c_[self.closed, self.current_point]
270
271 if self.current_point[0] == 15 and self.current_point[1] == 15: # 如果best是目标点，退出
272 print('搜索成功！')
273 return
274
275 # tem_open_shape = self.child_point(self.current_point) # 生成子节点并判断数目
276 self.child_point(self.current_point) # 生成子节点并判断数目
277
278 self.open = delete(self.open, index, axis=1) # 删除open中最优点
279
280 # if tem_open_shape == 0:
281 # self.closed = delete(self.closed, -1, axis=1) # 删除closed中不合理的点
282 # self.taboo = numpy.c_[self.taboo, self.current_point] # 将撞墙点加入禁忌表
283 # self.current_point = last_point
284 #
285 # continue
286
287 self.g_value()
288
289 # print(self.open)
290
291 ite = ite+1
292
293
294 class MAP(object):
295 """
296 画出地图
297 """
298 def draw_init_map(self):
299 """
300 画出起点终点图
301 :return:
302 """
303 plt.imshow(map_grid, cmap=plt.cm.hot, interpolation='nearest', vmin=0, vmax=10)
304 # plt.colorbar()
305 xlim(-1, 20) # 设置x轴范围
306 ylim(-1, 20) # 设置y轴范围
307 my_x_ticks = numpy.arange(0, 20, 1)
308 my_y_ticks = numpy.arange(0, 20, 1)
309 plt.xticks(my_x_ticks)
310 plt.yticks(my_y_ticks)
311 plt.grid(True)
312 # plt.show()
313
314 def draw_path_open(self, a):
315 """
316 画出open表中的坐标点图
317 :return:
318 """
319 map_open = copy.deepcopy(map_grid)
320 for i in range(a.closed.shape[1]):
321 x = a.closed[:, i]
322
323 map_open[int(x[0]), int(x[1])] = 1
324
325 plt.imshow(map_open, cmap=plt.cm.hot, interpolation='nearest', vmin=0, vmax=10)
326 # plt.colorbar()
327 xlim(-1, 20) # 设置x轴范围
328 ylim(-1, 20) # 设置y轴范围
329 my_x_ticks = numpy.arange(0, 20, 1)
330 my_y_ticks = numpy.arange(0, 20, 1)
331 plt.xticks(my_x_ticks)
332 plt.yticks(my_y_ticks)
333 plt.grid(True)
334 # plt.show()
335
336 def draw_path_closed(self, a):
337 """
338 画出closed表中的坐标点图
339 :return:
340 """
341 print('打印closed长度：')
342 print(a.closed.shape[1])
343 map_closed = copy.deepcopy(map_grid)
344 for i in range(a.closed.shape[1]):
345 x = a.closed[:, i]
346
347 map_closed[int(x[0]), int(x[1])] = 5
348
349 plt.imshow(map_closed, cmap=plt.cm.hot, interpolation='nearest', vmin=0, vmax=10)
350 # plt.colorbar()
351 xlim(-1, 20) # 设置x轴范围
352 ylim(-1, 20) # 设置y轴范围
353 my_x_ticks = numpy.arange(0, 20, 1)
354 my_y_ticks = numpy.arange(0, 20, 1)
355 plt.xticks(my_x_ticks)
356 plt.yticks(my_y_ticks)
357 plt.grid(True)
358 # plt.show()
359
360 def draw_direction_point(self, a):
361 """
362 从终点开始，根据记录的方向信息，画出搜索的路径图
363 :return:
364 """
365 print('打印direction长度：')
366 print(a.record_direction.shape[1])
367 map_direction = copy.deepcopy(map_grid)
368 for i in range(a.best_path_array.shape[1]):
369 x = a.best_path_array[:, i]
370
371 map_direction[int(x[0]), int(x[1])] = 6
372
373 plt.imshow(map_direction, cmap=plt.cm.hot, interpolation='nearest', vmin=0, vmax=10)
374 # plt.colorbar()
375 xlim(-1, 20) # 设置x轴范围
376 ylim(-1, 20) # 设置y轴范围
377 my_x_ticks = numpy.arange(0, 20, 1)
378 my_y_ticks = numpy.arange(0, 20, 1)
379 plt.xticks(my_x_ticks)
380 plt.yticks(my_y_ticks)
381 plt.grid(True)
382
383 def draw_three_axes(self, a):
384 """
385 将三张图画在一个figure中
386 :return:
387 """
388 plt.figure()
389 ax1 = plt.subplot(221)
390
391 ax2 = plt.subplot(222)
392 ax3 = plt.subplot(223)
393 ax4 = plt.subplot(224)
394 plt.sca(ax1)
395 self.draw_init_map()
396 plt.sca(ax2)
397 self.draw_path_open(a)
398 plt.sca(ax3)
399 self.draw_path_closed(a)
400 plt.sca(ax4)
401 self.draw_direction_point(a)
402
403 plt.show()
404
405
406 if __name__ == '__main__':
407
408 a1 = AStar()
409 a1.main()
410 a1.path_backtrace()
411 m1 = MAP()
412 m1.draw_three_axes(a1)
