1 # 版本2，2018—04—09 2 # 所有节点的g值并没有初始化为无穷大 3 # 当两个子节点的f值一样时，程序选择最先搜索到的一个作为父节点加入closed 4 # 对相同数值的不同对待，导致不同版本的A*算法找到等长的不同路径 5 # 最后closed表中的节点很多，如何找出最优的一条路径 6 # 撞墙之后产生较多的节点会加入closed表，此时开始删除closed表中不合理的节点 7 8 9 import numpy 10 from pylab import * 11 import copy 12 13 # 定义一个含有障碍物的20×20的栅格地图 14 # 10表示可通行点 15 # 0表示障碍物 16 # 7表示起点 17 # 5表示终点 18 map_grid = numpy.full((20, 20), int(10), dtype=numpy.int8) 19 # print(map_grid) 20 map_grid[3, 3:8] = 0 21 map_grid[3:10, 7] = 0 22 map_grid[10, 3:8] = 0 23 map_grid[17, 13:17] = 0 24 map_grid[10:17, 13] = 0 25 map_grid[10, 13:17] = 0 26 map_grid[5, 2] = 7 27 map_grid[15, 15] = 5 28 # 画出定义的栅格地图 29 30 # plt.imshow(map_grid, cmap=plt.cm.hot, interpolation='nearest', vmin=0, vmax=10) 31 # plt.colorbar() 32 # xlim(-1, 20) # 设置x轴范围 33 # ylim(-1, 20) # 设置y轴范围 34 # my_x_ticks = numpy.arange(0, 20, 1) 35 # my_y_ticks = numpy.arange(0, 20, 1) 36 # plt.xticks(my_x_ticks) 37 # plt.yticks(my_y_ticks) 38 # plt.grid(True) 39 # plt.show() 40 41 42 class AStar(object): 43 """ 44 创建一个A*算法类 45 """ 46 47 def __init__(self): 48 """ 49 初始化 50 """ 51 self.f = 0 52 self.g = 0 53 self.last_point = numpy.array([]) # 上一个目标点不断取得更新 54 self.current_point = numpy.array([]) # 当前目标点不断取得更新 55 self.open = numpy.array([[], []]) # 先创建一个空的open表 56 self.closed = numpy.array([[], []]) # 先创建一个空的closed表 57 self.taboo = numpy.array([[], []]) # 创建一个禁忌表，用于放置不再搜索的点 58 self.start = numpy.array([5, 2]) # 起点坐标 59 self.goal = numpy.array([15, 15]) # 终点坐标 60 61 def h_value_tem(self, cur_p): 62 """ 63 计算拓展节点和终点的h值 64 :param cur_p:子搜索节点坐标 65 :return: 66 """ 67 h = (cur_p[0] - 15) ** 2 + (cur_p[1] - 15) ** 2 68 h = numpy.sqrt(h) # 计算h 69 return h 70 71 def g_value_tem(self, chl_p, cu_p): 72 """ 73 计算拓展节点和父节点的g值 74 其实也可以直接用1或者1.414代替 75 :param chl_p:子节点坐标 76 :param cu_p:父节点坐标，也就是self.current_point 77 :return:返回子节点到父节点的g值，但不是全局g值 78 """ 79 g1 = cu_p[0] - chl_p[0] 80 g2 = cu_p[1] - chl_p[1] 81 g = g1 ** 2 + g2 ** 2 82 g = numpy.sqrt(g) 83 return g 84 85 def f_value_tem(self, chl_p, cu_p): 86 """ 87 求出的是临时g值和h值的和，还需加上累计g值得到全局f值 88 :param chl_p: 父节点坐标 89 :param cu_p: 子节点坐标 90 :return: 91 """ 92 f = self.g_value_tem(chl_p, cu_p) + self.h_value_tem(cu_p) 93 return f 94 95 def min_f(self): 96 """ 97 找出open中f值最小的节点坐标，记录为current_point 98 :return:返回open表中最小值的位置索引和在map_grid中的坐标 99 对撞墙后的处理方式是，随机选择一个方向进行搜索 100 并且将open列表清零，不然一直是死循环 101 这种处理方式以后待改进！！！ 102 """ 103 tem_f = [] # 创建一个记录f值的临时列表 104 for i in range(self.open.shape[1]): 105 # 计算拓展节点的全局f值 106 f_value = self.f_value_tem(self.current_point, self.open[:, i]) + self.g 107 tem_f.append(f_value) 108 index = tem_f.index(min(tem_f)) # 返回最小值索引 109 location = self.open[:, index] # 返回最小值坐标 110 print('打印位置索引和地图坐标：') 111 print(index, location) 112 return index, location 113 114 def child_point(self, x): 115 """ 116 拓展的子节点坐标 117 :param x: 父节点坐标 118 :return: 子节点存入open表，返回值是每一次拓展出的子节点数目，用于撞墙判断 119 当搜索的节点撞墙后，如果不加处理，会陷入死循环 120 """ 121 tem_open = numpy.array([[], []]) # 统计拓展节点的临时数组 122 tem_open_shape = 0 # 统计临时数组的长度 123 # 开始遍历周围8个节点 124 for j in range(-1, 2, 1): 125 for q in range(-1, 2, 1): 126 if j == 0 and q == 0: # 搜索到父节点去掉 127 continue 128 129 # print(map_grid[int(x[0] + j), int(x[1] + q)]) 130 if map_grid[int(x[0] + j), int(x[1] + q)] == 0: # 搜索到障碍物去掉 131 continue 132 if x[0] + j < 0 or x[0] + j > 19 or x[1] + q < 0 or x[1] + q > 19: # 搜索点出了边界去掉 133 continue 134 # 在open表中，则去掉搜索点 135 a = self.judge_location(x, j, q, self.open) 136 if a == 1: 137 continue 138 # 在closed表中,则去掉搜索点 139 b = self.judge_location(x, j, q, self.closed) 140 if b == 1: 141 continue 142 143 # 在taboo表中，则去掉搜索点 144 c = self.judge_location(x, j, q, self.taboo) 145 if c == 1: 146 continue 147 148 m = numpy.array([x[0] + j, x[1] + q]) 149 150 tem_open = numpy.c_[tem_open, m] # 151 152 tem_open_shape = tem_open.shape[1] # 求出tem_open的长度 153 154 self.open = numpy.c_[self.open, m] # 搜索出的子节点加入open 155 # print('打印第一次循环后的open：') 156 # print(self.open) 157 return tem_open_shape 158 159 def judge_location(self, x, j, q, list_co): 160 """ 161 判断拓展点是否在open表或者closed表中 162 :return: 163 """ 164 jud = 0 165 for i in range(list_co.shape[1]): 166 167 if x[0] + j == list_co[0, i] and x[1] + q == list_co[1, i]: 168 169 jud = jud + 1 170 else: 171 jud = jud 172 # if a != 0: 173 # continue 174 return jud 175 176 def draw_path_closed(self): 177 """ 178 画出closed表中的坐标点图 179 :return: 180 """ 181 map_closed = copy.deepcopy(map_grid) 182 for i in range(self.closed.shape[1]): 183 x = self.closed[:, i] 184 185 map_closed[int(x[0]), int(x[1])] = 5 186 187 plt.imshow(map_closed, cmap=plt.cm.hot, interpolation='nearest', vmin=0, vmax=10) 188 # plt.colorbar() 189 xlim(-1, 20) # 设置x轴范围 190 ylim(-1, 20) # 设置y轴范围 191 my_x_ticks = numpy.arange(0, 20, 1) 192 my_y_ticks = numpy.arange(0, 20, 1) 193 plt.xticks(my_x_ticks) 194 plt.yticks(my_y_ticks) 195 plt.grid(True) 196 # plt.show() 197 198 def draw_init_map(self): 199 """ 200 画出起点终点图 201 :return: 202 """ 203 204 plt.imshow(map_grid, cmap=plt.cm.hot, interpolation='nearest', vmin=0, vmax=10) 205 # plt.colorbar() 206 xlim(-1, 20) # 设置x轴范围 207 ylim(-1, 20) # 设置y轴范围 208 my_x_ticks = numpy.arange(0, 20, 1) 209 my_y_ticks = numpy.arange(0, 20, 1) 210 plt.xticks(my_x_ticks) 211 plt.yticks(my_y_ticks) 212 plt.grid(True) 213 # plt.show() 214 215 def draw_path_open(self): 216 """ 217 画出open表中的坐标点图 218 :return: 219 """ 220 map_open = copy.deepcopy(map_grid) 221 for i in range(self.closed.shape[1]): 222 x = self.closed[:, i] 223 224 map_open[int(x[0]), int(x[1])] = 1 225 226 plt.imshow(map_open, cmap=plt.cm.hot, interpolation='nearest', vmin=0, vmax=10) 227 # plt.colorbar() 228 xlim(-1, 20) # 设置x轴范围 229 ylim(-1, 20) # 设置y轴范围 230 my_x_ticks = numpy.arange(0, 20, 1) 231 my_y_ticks = numpy.arange(0, 20, 1) 232 plt.xticks(my_x_ticks) 233 plt.yticks(my_y_ticks) 234 plt.grid(True) 235 # plt.show() 236 237 def draw_three_axes(self): 238 """ 239 将三张图画在一个figure中 240 :return: 241 """ 242 plt.figure() 243 ax1 = plt.subplot(221) 244 245 ax2 = plt.subplot(222) 246 ax3 = plt.subplot(223) 247 plt.sca(ax1) 248 self.draw_init_map() 249 plt.sca(ax2) 250 self.draw_path_open() 251 plt.sca(ax3) 252 self.draw_path_closed() 253 plt.show() 254 255 def main(self): 256 """ 257 main函数 258 :return: 259 """ 260 self.open = numpy.column_stack((self.open, self.start)) # 起点放入open 261 self.current_point = self.start # 起点放入当前点，作为父节点 262 # self.closed 263 ite = 1 264 while ite <= 2000: 265 266 # open列表为空，退出 267 if self.open.shape[1] == 0: 268 print('没有搜索到路径！') 269 return 270 271 last_point = self.current_point # 上一个目标点不断取得更新 272 273 while True: 274 275 index, self.current_point = self.min_f() # 判断open表中f值 276 print('检验第%s次当前点坐标' % ite) 277 print(self.current_point) 278 279 # 选取open表中最小f值的节点作为best，放入closed表 280 self.closed = numpy.c_[self.closed, self.current_point] 281 282 if self.current_point[0] == 15 and self.current_point[1] == 15: # 如果best是目标点，退出 283 print('搜索成功！') 284 return 285 286 tem_open_shape = self.child_point(self.current_point) # 生成子节点并判断数目 287 self.open = delete(self.open, index, axis=1) # 删除open中最优点 288 289 if tem_open_shape == 0: 290 self.closed = delete(self.closed, -1, axis=1) # 删除closed中不合理的点 291 self.taboo = numpy.c_[self.taboo, self.current_point] # 将撞墙点加入禁忌表 292 self.current_point = last_point 293 self.g = self.g + self.g_value_tem(self.current_point, last_point) 294 continue 295 else: 296 break 297 298 # print(self.open) 299 300 301 302 ite = ite+1 303 304 305 if __name__ == '__main__': 306 307 a1 = AStar() 308 a1.main() 309 a1.draw_three_axes()
