作者：鄔楊明
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()
