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)