1 # 版本1.3，2018—04—11
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 map_grid[3, 3:8] = 0
22 map_grid[3:10, 7] = 0
23 map_grid[10, 3:8] = 0
24 map_grid[17, 13:17] = 0
25 map_grid[10:17, 13] = 0
26 map_grid[10, 13:17] = 0
27 map_grid[5, 2] = 7
28 map_grid[15, 15] = 5
29
30
31 class AStar(object):
32 """
33 创建一个A*算法类
34 """
35
36 def __init__(self):
37 """
38 初始化
39 """
40 # self.g = 0 # g初始化为0
41 self.start = numpy.array([5, 2]) # 起点坐标
42 self.goal = numpy.array([15, 15]) # 终点坐标
43 self.open = numpy.array([[], [], [], [], [], []]) # 先创建一个空的open表, 记录坐标，方向，g值，f值
44 self.closed = numpy.array([[], [], [], [], [], []]) # 先创建一个空的closed表
45 self.best_path_array = numpy.array([[], []]) # 回溯路径表
46
47 def h_value_tem(self, son_p):
48 """
49 计算拓展节点和终点的h值
50 :param son_p:子搜索节点坐标
51 :return:
52 """
53 h = (son_p[0] - self.goal[0]) ** 2 + (son_p[1] - self.goal[1]) ** 2
54 h = numpy.sqrt(h) # 计算h
55 return h
56
57 # def g_value_tem(self, son_p, father_p):
58 # """
59 # 计算拓展节点和父节点的g值
60 # 其实也可以直接用1或者1.414代替
61 # :param son_p:子节点坐标
62 # :param father_p:父节点坐标，也就是self.current_point
63 # :return:返回子节点到父节点的g值，但不是全局g值
64 # """
65 # g1 = father_p[0] - son_p[0]
66 # g2 = father_p[1] - son_p[1]
67 # g = g1 ** 2 + g2 ** 2
68 # g = numpy.sqrt(g)
69 # return g
70
71 def g_accumulation(self, son_point, father_point):
72 """
73 累计的g值
74 :return:
75 """
76 g1 = father_point[0] - son_point[0]
77 g2 = father_point[1] - son_point[1]
78 g = g1 ** 2 + g2 ** 2
79 g = numpy.sqrt(g) + father_point[4] # 加上累计的g值
80 return g
81
82 def f_value_tem(self, son_p, father_p):
83 """
84 求出的是临时g值和h值加上累计g值得到全局f值
85 :param father_p: 父节点坐标
86 :param son_p: 子节点坐标
87 :return:f
88 """
89 f = self.g_accumulation(son_p, father_p) + self.h_value_tem(son_p)
90 return f
91
92 def child_point(self, x):
93 """
94 拓展的子节点坐标
95 :param x: 父节点坐标
96 :return: 子节点存入open表，返回值是每一次拓展出的子节点数目，用于撞墙判断
97 当搜索的节点撞墙后，如果不加处理，会陷入死循环
98 """
99 # 开始遍历周围8个节点
100 for j in range(-1, 2, 1):
101 for q in range(-1, 2, 1):
102
103 if j == 0 and q == 0: # 搜索到父节点去掉
104 continue
105 m = [x[0] + j, x[1] + q]
106 print(m)
107 if m[0] < 0 or m[0] > 19 or m[1] < 0 or m[1] > 19: # 搜索点出了边界去掉
108 continue
109
110 if map_grid[int(m[0]), int(m[1])] == 0: # 搜索到障碍物去掉
111 continue
112
113
114
115 record_g = self.g_accumulation(m, x)
116 record_f = self.f_value_tem(m, x) # 计算每一个节点的f值
117
118 x_direction, y_direction = self.direction(x, m) # 每产生一个子节点，记录一次方向
119
120 para = [m[0], m[1], x_direction, y_direction, record_g, record_f] # 将参数汇总一下
121 print(para)
122
123 # 在open表中，则去掉搜索点，但是需要更新方向指针和self.g值
124 # 而且只需要计算并更新self.g即可，此时建立一个比较g值的函数
125 a, index = self.judge_location(m, self.open)
126 if a == 1:
127 # 说明open中已经存在这个点
128
129 if record_f <= self.open[5][index]:
130 self.open[5][index] = record_f
131 self.open[4][index] = record_g
132 self.open[3][index] = y_direction
133 self.open[2][index] = x_direction
134
135 continue
136
137 # 在closed表中,则去掉搜索点
138 b, index2 = self.judge_location(m, self.closed)
139 if b == 1:
140
141 if record_f <= self.closed[5][index2]:
142 self.closed[5][index2] = record_f
143 self.closed[4][index2] = record_g
144 self.closed[3][index2] = y_direction
145 self.closed[2][index2] = x_direction
146 self.closed = numpy.delete(self.closed, index2, axis=1)
147 self.open = numpy.c_[self.open, para]
148 continue
149
150 self.open = numpy.c_[self.open, para] # 参数添加到open中
151 print(self.open)
152
153 def judge_location(self, m, list_co):
154 """
155 判断拓展点是否在open表或者closed表中
156 :return:返回判断是否存在，和如果存在，那么存在的位置索引
157 """
158 jud = 0
159 index = 0
160 for i in range(list_co.shape[1]):
161
162 if m[0] == list_co[0, i] and m[1] == list_co[1, i]:
163
164 jud = jud + 1
165
166 index = i
167 break
168 else:
169 jud = jud
170 # if a != 0:
171 # continue
172 return jud, index
173
174 def direction(self, father_point, son_point):
175 """
176 建立每一个节点的方向，便于在closed表中选出最佳路径
177 非常重要的一步，不然画出的图像参考1.1版本
178 x记录子节点和父节点的x轴变化
179 y记录子节点和父节点的y轴变化
180 如（0，1）表示子节点在父节点的方向上变化0和1
181 :return:
182 """
183 x = son_point[0] - father_point[0]
184 y = son_point[1] - father_point[1]
185 return x, y
186
187 def path_backtrace(self):
188 """
189 回溯closed表中的最短路径
190 :return:
191 """
192 best_path = [15, 15] # 回溯路径的初始化
193 self.best_path_array = numpy.array([[15], [15]])
194 j = 0
195 while j <= self.closed.shape[1]:
196 for i in range(self.closed.shape[1]):
197 if best_path[0] == self.closed[0][i] and best_path[1] == self.closed[1][i]:
198 x = self.closed[0][i]-self.closed[2][i]
199 y = self.closed[1][i]-self.closed[3][i]
200 best_path = [x, y]
201 self.best_path_array = numpy.c_[self.best_path_array, best_path]
202 break # 如果已经找到，退出本轮循环，减少耗时
203 else:
204 continue
205 j = j+1
206 # return best_path_array
207
208 def main(self):
209 """
210 main函数
211 :return:
212 """
213 best = self.start # 起点放入当前点，作为父节点
214 h0 = self.h_value_tem(best)
215 init_open = [best[0], best[1], 0, 0, 0, h0] # 将方向初始化为（0，0），g_init=0,f值初始化h0
216 self.open = numpy.column_stack((self.open, init_open)) # 起点放入open,open初始化
217
218 ite = 1 # 设置迭代次数小于200，防止程序出错无限循环
219 while ite <= 1000:
220
221 # open列表为空，退出
222 if self.open.shape[1] == 0:
223 print('没有搜索到路径！')
224 return
225
226 self.open = self.open.T[numpy.lexsort(self.open)].T # open表中最后一行排序(联合排序）
227
228 # 选取open表中最小f值的节点作为best，放入closed表
229
230 best = self.open[:, 0]
231 print('检验第%s次当前点坐标*******************' % ite)
232 print(best)
233 self.closed = numpy.c_[self.closed, best]
234
235 if best[0] == 15 and best[1] == 15: # 如果best是目标点，退出
236 print('搜索成功！')
237 return
238
239 self.child_point(best) # 生成子节点并判断数目
240 print(self.open)
241 self.open = numpy.delete(self.open, 0, axis=1) # 删除open中最优点
242
243 # print(self.open)
244
245 ite = ite+1
246
247
248 class MAP(object):
249 """
250 画出地图
251 """
252 def draw_init_map(self):
253 """
254 画出起点终点图
255 :return:
256 """
257 plt.imshow(map_grid, cmap=plt.cm.hot, interpolation='nearest', vmin=0, vmax=10)
258 # plt.colorbar()
259 xlim(-1, 20) # 设置x轴范围
260 ylim(-1, 20) # 设置y轴范围
261 my_x_ticks = numpy.arange(0, 20, 1)
262 my_y_ticks = numpy.arange(0, 20, 1)
263 plt.xticks(my_x_ticks)
264 plt.yticks(my_y_ticks)
265 plt.grid(True)
266 # plt.show()
267
268 def draw_path_open(self, a):
269 """
270 画出open表中的坐标点图
271 :return:
272 """
273 map_open = copy.deepcopy(map_grid)
274 for i in range(a.closed.shape[1]):
275 x = a.closed[:, i]
276
277 map_open[int(x[0]), int(x[1])] = 1
278
279 plt.imshow(map_open, cmap=plt.cm.hot, interpolation='nearest', vmin=0, vmax=10)
280 # plt.colorbar()
281 xlim(-1, 20) # 设置x轴范围
282 ylim(-1, 20) # 设置y轴范围
283 my_x_ticks = numpy.arange(0, 20, 1)
284 my_y_ticks = numpy.arange(0, 20, 1)
285 plt.xticks(my_x_ticks)
286 plt.yticks(my_y_ticks)
287 plt.grid(True)
288 # plt.show()
289
290 def draw_path_closed(self, a):
291 """
292 画出closed表中的坐标点图
293 :return:
294 """
295 print('打印closed长度：')
296 print(a.closed.shape[1])
297 map_closed = copy.deepcopy(map_grid)
298 for i in range(a.closed.shape[1]):
299 x = a.closed[:, i]
300
301 map_closed[int(x[0]), int(x[1])] = 5
302
303 plt.imshow(map_closed, 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_direction_point(self, a):
315 """
316 从终点开始，根据记录的方向信息，画出搜索的路径图
317 :return:
318 """
319 print('打印direction长度：')
320 print(a.best_path_array.shape[1])
321 map_direction = copy.deepcopy(map_grid)
322 for i in range(a.best_path_array.shape[1]):
323 x = a.best_path_array[:, i]
324
325 map_direction[int(x[0]), int(x[1])] = 6
326
327 plt.imshow(map_direction, cmap=plt.cm.hot, interpolation='nearest', vmin=0, vmax=10)
328 # plt.colorbar()
329 xlim(-1, 20) # 设置x轴范围
330 ylim(-1, 20) # 设置y轴范围
331 my_x_ticks = numpy.arange(0, 20, 1)
332 my_y_ticks = numpy.arange(0, 20, 1)
333 plt.xticks(my_x_ticks)
334 plt.yticks(my_y_ticks)
335 plt.grid(True)
336
337 def draw_three_axes(self, a):
338 """
339 将三张图画在一个figure中
340 :return:
341 """
342 plt.figure()
343 ax1 = plt.subplot(221)
344
345 ax2 = plt.subplot(222)
346 ax3 = plt.subplot(223)
347 ax4 = plt.subplot(224)
348 plt.sca(ax1)
349 self.draw_init_map()
350 plt.sca(ax2)
351 self.draw_path_open(a)
352 plt.sca(ax3)
353 self.draw_path_closed(a)
354 plt.sca(ax4)
355 self.draw_direction_point(a)
356
357 plt.show()
358
359
360 if __name__ == '__main__':
361
362 a1 = AStar()
363 a1.main()
364 a1.path_backtrace()
365 m1 = MAP()
366 m1.draw_three_axes(a1)
