作者：邬杨明
1 import numpy 2 from pylab import * 3 4 # 定义一个含有障碍物的20×20的栅格地图 5 # 10表示可通行点 6 # 0表示障碍物 7 # 7表示起点 8 # 5表示终点 9 map_grid = numpy.full((20, 20), int(10), dtype=numpy.int8) 10 # print(map_grid) 11 map_grid[3, 3:8] = 0 12 map_grid[3:10, 7] = 0 13 map_grid[10, 3:8] = 0 14 map_grid[17, 13:17] = 0 15 map_grid[10:17, 13] = 0 16 map_grid[10, 13:17] = 0 17 map_grid[5, 2] = 7 18 map_grid[15, 15] = 5 19 # 画出定义的栅格地图 20 21 # plt.imshow(map_grid, cmap=plt.cm.hot, interpolation='nearest', vmin=0, vmax=10) 22 # plt.colorbar() 23 # xlim(-1, 20) # 设置x轴范围 24 # ylim(-1, 20) # 设置y轴范围 25 # my_x_ticks = numpy.arange(0, 20, 1) 26 # my_y_ticks = numpy.arange(0, 20, 1) 27 # plt.xticks(my_x_ticks) 28 # plt.yticks(my_y_ticks) 29 # plt.grid(True) 30 # plt.show() 31 32 33 class AStar(object): 34 """ 35 创建一个A*算法类 36 """ 37 38 def __init__(self): 39 """ 40 初始化 41 """ 42 self.f = 0 43 self.g = 0 44 self.last_point = numpy.array([]) # 上一个目标点不断取得更新 45 self.current_point = numpy.array([]) # 当前目标点不断取得更新 46 self.open = numpy.array([[], []]) # 先创建一个空的open表 47 self.closed = numpy.array([[], []]) # 先创建一个空的closed表 48 self.start = numpy.array([5, 2]) # 起点坐标 49 self.goal = numpy.array([15, 15]) # 终点坐标 50 51 def h_value_tem(self, cur_p): 52 """ 53 计算拓展节点和终点的h值 54 :param cur_p:子搜索节点坐标 55 :return: 56 """ 57 h = (cur_p[0] - 15) ** 2 + (cur_p[1] - 15) ** 2 58 h = numpy.sqrt(h) # 计算h 59 return h 60 61 def g_value_tem(self, chl_p, cu_p): 62 """ 63 计算拓展节点和父节点的g值 64 其实也可以直接用1或者1.414代替 65 :param chl_p:子节点坐标 66 :param cu_p:父节点坐标，也就是self.current_point 67 :return:返回子节点到父节点的g值，但不是全局g值 68 """ 69 g1 = cu_p[0] - chl_p[0] 70 g2 = cu_p[1] - chl_p[1] 71 g = g1 ** 2 + g2 ** 2 72 g = numpy.sqrt(g) 73 return g 74 75 def f_value_tem(self, chl_p, cu_p): 76 """ 77 求出的是临时g值和h值的和，还需加上累计g值得到全局f值 78 :param chl_p: 父节点坐标 79 :param cu_p: 子节点坐标 80 :return: 81 """ 82 f = self.g_value_tem(chl_p, cu_p) + self.h_value_tem(cu_p) 83 return f 84 85 def min_f(self): 86 """ 87 找出open中f值最小的节点坐标，记录为current_point 88 :return:返回open表中最小值的位置索引和在map_grid中的坐标 89 对撞墙后的处理方式是，随机选择一个方向进行搜索 90 并且将open列表清零，不然一直是死循环 91 这种处理方式以后待改进！！！ 92 """ 93 tem_f = [] # 创建一个记录f值的临时列表 94 for i in range(self.open.shape[1]): 95 # 计算拓展节点的全局f值 96 f_value = self.f_value_tem(self.current_point, self.open[:, i]) + self.g 97 tem_f.append(f_value) 98 index = tem_f.index(min(tem_f)) # 返回最小值索引 99 location = self.open[:, index] # 返回最小值坐标 100 print('打印位置索引和地图坐标：') 101 print(index, location) 102 return index, location 103 104 def child_point(self, x): 105 """ 106 拓展的子节点坐标 107 :param x: 父节点坐标 108 :return: 无返回值，子节点存入open表 109 当搜索的节点撞墙后，如果不加处理，会陷入死循环 110 """ 111 # self.open = numpy.array([[], []]) # 先创建一个空的open表 112 # 开始遍历周围8个节点 113 for j in range(-1, 2, 1): 114 for q in range(-1, 2, 1): 115 if j == 0 and q == 0: # 搜索到父节点去掉 116 continue 117 118 # print(map_grid[int(x[0] + j), int(x[1] + q)]) 119 if map_grid[int(x[0] + j), int(x[1] + q)] == 0: # 搜索到障碍物去掉 120 continue 121 if x[0] + j < 0 or x[0] + j > 19 or x[1] + q < 0 or x[1] + q > 19: # 搜索点出了边界去掉 122 continue 123 # 在open表中，则去掉搜索点 124 a = self.judge_location(x, j, q, self.open) 125 if a == 1: 126 continue 127 # 在closed表中,则去掉搜索点 128 b = self.judge_location(x, j, q, self.closed) 129 if b == 1: 130 continue 131 132 m = numpy.array([x[0] + j, x[1] + q]) 133 self.open = numpy.c_[self.open, m] # 搜索出的子节点加入open 134 # print('打印第一次循环后的open：') 135 # print(self.open) 136 137 def judge_location(self, x, j, q, list_co): 138 """ 139 判断拓展点是否在open表或者closed表中 140 :return: 141 """ 142 jud = 0 143 for i in range(list_co.shape[1]): 144 145 if x[0] + j == list_co[0, i] and x[1] + q == list_co[1, i]: 146 147 jud = jud + 1 148 else: 149 jud = jud 150 # if a != 0: 151 # continue 152 return jud 153 154 def draw_path(self): 155 for i in range(self.closed.shape[1]): 156 x = self.closed[:, i] 157 158 map_grid[x[0], x[1]] = 5 159 160 plt.imshow(map_grid, cmap=plt.cm.hot, interpolation='nearest', vmin=0, vmax=10) 161 plt.colorbar() 162 xlim(-1, 20) # 设置x轴范围 163 ylim(-1, 20) # 设置y轴范围 164 my_x_ticks = numpy.arange(0, 20, 1) 165 my_y_ticks = numpy.arange(0, 20, 1) 166 plt.xticks(my_x_ticks) 167 plt.yticks(my_y_ticks) 168 plt.grid(True) 169 plt.show() 170 171 172 173 174 175 def main(self): 176 """ 177 main函数 178 :return: 179 """ 180 self.open = numpy.column_stack((self.open, self.start)) # 起点放入open 181 self.current_point = self.start # 起点放入当前点，作为父节点 182 # self.closed 183 ite = 1 184 while ite <= 2000: 185 # open列表为空，退出 186 if self.open.shape[1] == 0: 187 print('没有搜索到路径！') 188 return 189 190 last_point = self.current_point # 上一个目标点不断取得更新 191 192 index, self.current_point = self.min_f() # 判断open表中f值 193 print('检验第%s次当前点坐标' % ite) 194 print(self.current_point) 195 196 # 选取open表中最小f值的节点作为best，放入closed表 197 self.closed = numpy.c_[self.closed, self.current_point] 198 199 if self.current_point[0] == 15 and self.current_point[1] == 15: # 如果best是目标点，退出 200 print('搜索成功！') 201 return 202 203 self.child_point(self.current_point) # 生成子节点 204 self.open = delete(self.open, index, axis=1) # 删除open中最优点 205 # print(self.open) 206 207 self.g = self.g + self.g_value_tem(self.current_point, last_point) 208 209 ite = ite+1 210 211 212 a1 = AStar() 213 a1.main() 214 a1.draw_path()
