1.2

  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)