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)