# 所有節點的g值並沒有初始化為無窮大 # 當兩個子節點的f值一樣時，程序選擇最先搜索到的一個作為父節點加入closed # 對相同數值的不同對待，導致不同版本的A*算法找到等長的不同路徑 # 最后closed表中的節點很多，如何找出最優的一條路徑 # 撞牆之后產生較多的節點會加入closed表，此時開始刪除closed表中不合理的節點，1.1版本的思路 # 1.2版本思路，建立每一個節點的方向指針，指向f值最小的上個節點 # 參考《無人駕駛概論》、《基於A*算法的移動機器人路徑規划》王淼馳，《人工智能及應用》魯斌 import numpy from pylab import * import copy # 定義一個含有障礙物的20×20的柵格地圖 # 10表示可通行點 # 0表示障礙物 # 7表示起點 # 5表示終點 map_grid = numpy.full((20, 20), int(10), dtype=numpy.int8) map_grid[3, 3:8] = 0 map_grid[3:10, 7] = 0 map_grid[10, 3:8] = 0 map_grid[17, 13:17] = 0 map_grid[10:17, 13] = 0 map_grid[10, 13:17] = 0 map_grid[5, 2] = 7 map_grid[15, 15] = 5 class AStar(object): """ 創建一個A*算法類 """ def __init__(self): """ 初始化 """ # self.g = 0 # g初始化為0 self.start = numpy.array([5, 2]) # 起點坐標 self.goal = numpy.array([15, 15]) # 終點坐標 self.open = numpy.array([[], [], [], [], [], []]) # 先創建一個空的open表, 記錄坐標，方向，g值，f值 self.closed = numpy.array([[], [], [], [], [], []]) # 先創建一個空的closed表 self.best_path_array = numpy.array([[], []]) # 回溯路徑表 def h_value_tem(self, son_p): """ 計算拓展節點和終點的h值 :param son_p:子搜索節點坐標 :return: """ h = (son_p[0] - self.goal[0]) ** 2 + (son_p[1] - self.goal[1]) ** 2 h = numpy.sqrt(h) # 計算h return h # def g_value_tem(self, son_p, father_p): # """ # 計算拓展節點和父節點的g值 # 其實也可以直接用1或者1.414代替 # :param son_p:子節點坐標 # :param father_p:父節點坐標，也就是self.current_point # :return:返回子節點到父節點的g值，但不是全局g值 # """ # g1 = father_p[0] - son_p[0] # g2 = father_p[1] - son_p[1] # g = g1 ** 2 + g2 ** 2 # g = numpy.sqrt(g) # return g def g_accumulation(self, son_point, father_point): """ 累計的g值 :return: """ g1 = father_point[0] - son_point[0] g2 = father_point[1] - son_point[1] g = g1 ** 2 + g2 ** 2 g = numpy.sqrt(g) + father_point[4] # 加上累計的g值 return g def f_value_tem(self, son_p, father_p): """ 求出的是臨時g值和h值加上累計g值得到全局f值 :param father_p: 父節點坐標 :param son_p: 子節點坐標 :return:f """ f = self.g_accumulation(son_p, father_p) + self.h_value_tem(son_p) return f def child_point(self, x): """ 拓展的子節點坐標 :param x: 父節點坐標 :return: 子節點存入open表，返回值是每一次拓展出的子節點數目，用於撞牆判斷 當搜索的節點撞牆后，如果不加處理，會陷入死循環 """ # 開始遍歷周圍8個節點 for j in range(-1, 2, 1): for q in range(-1, 2, 1): if j == 0 and q == 0: # 搜索到父節點去掉 continue m = [x[0] + j, x[1] + q] print(m) if m[0] < 0 or m[0] > 19 or m[1] < 0 or m[1] > 19: # 搜索點出了邊界去掉 continue if map_grid[int(m[0]), int(m[1])] == 0: # 搜索到障礙物去掉 continue record_g = self.g_accumulation(m, x) record_f = self.f_value_tem(m, x) # 計算每一個節點的f值 x_direction, y_direction = self.direction(x, m) # 每產生一個子節點，記錄一次方向 para = [m[0], m[1], x_direction, y_direction, record_g, record_f] # 將參數匯總一下 print(para) # 在open表中，則去掉搜索點，但是需要更新方向指針和self.g值 # 而且只需要計算並更新self.g即可，此時建立一個比較g值的函數 a, index = self.judge_location(m, self.open) if a == 1: # 說明open中已經存在這個點 if record_f <= self.open[5][index]: self.open[5][index] = record_f self.open[4][index] = record_g self.open[3][index] = y_direction self.open[2][index] = x_direction continue # 在closed表中,則去掉搜索點 b, index2 = self.judge_location(m, self.closed) if b == 1: if record_f <= self.closed[5][index2]: self.closed[5][index2] = record_f self.closed[4][index2] = record_g self.closed[3][index2] = y_direction self.closed[2][index2] = x_direction self.closed = numpy.delete(self.closed, index2, axis=1) self.open = numpy.c_[self.open, para] continue self.open = numpy.c_[self.open, para] # 參數添加到open中 print(self.open) def judge_location(self, m, list_co): """ 判斷拓展點是否在open表或者closed表中 :return:返回判斷是否存在，和如果存在，那么存在的位置索引 """ jud = 0 index = 0 for i in range(list_co.shape[1]): if m[0] == list_co[0, i] and m[1] == list_co[1, i]: jud = jud + 1 index = i break else: jud = jud # if a != 0: # continue return jud, index def direction(self, father_point, son_point): """ 建立每一個節點的方向，便於在closed表中選出最佳路徑 非常重要的一步，不然畫出的圖像參考1.1版本 x記錄子節點和父節點的x軸變化 y記錄子節點和父節點的y軸變化 如（0，1）表示子節點在父節點的方向上變化0和1 :return: """ x = son_point[0] - father_point[0] y = son_point[1] - father_point[1] return x, y def path_backtrace(self): """ 回溯closed表中的最短路徑 :return: """ best_path = [15, 15] # 回溯路徑的初始化 self.best_path_array = numpy.array([[15], [15]]) j = 0 while j <= self.closed.shape[1]: for i in range(self.closed.shape[1]): if best_path[0] == self.closed[0][i] and best_path[1] == self.closed[1][i]: x = self.closed[0][i]-self.closed[2][i] y = self.closed[1][i]-self.closed[3][i] best_path = [x, y] self.best_path_array = numpy.c_[self.best_path_array, best_path] break # 如果已經找到，退出本輪循環，減少耗時 else: continue j = j+1 # return best_path_array def main(self): """ main函數 :return: """ best = self.start # 起點放入當前點，作為父節點 h0 = self.h_value_tem(best) init_open = [best[0], best[1], 0, 0, 0, h0] # 將方向初始化為（0，0），g_init=0,f值初始化h0 self.open = numpy.column_stack((self.open, init_open)) # 起點放入open,open初始化 ite = 1 # 設置迭代次數小於200，防止程序出錯無限循環 while ite <= 1000: # open列表為空，退出 if self.open.shape[1] == 0: print('沒有搜索到路徑！') return self.open = self.open.T[numpy.lexsort(self.open)].T # open表中最后一行排序(聯合排序） # 選取open表中最小f值的節點作為best，放入closed表 best = self.open[:, 0] print('檢驗第%s次當前點坐標*******************' % ite) print(best) self.closed = numpy.c_[self.closed, best] if best[0] == 15 and best[1] == 15: # 如果best是目標點，退出 print('搜索成功！') return self.child_point(best) # 生成子節點並判斷數目 print(self.open) self.open = numpy.delete(self.open, 0, axis=1) # 刪除open中最優點 # print(self.open) ite = ite+1 class MAP(object): """ 畫出地圖 """ def draw_init_map(self): """ 畫出起點終點圖 :return: """ plt.imshow(map_grid, cmap=plt.cm.hot, interpolation='nearest', vmin=0, vmax=10) # plt.colorbar() xlim(-1, 20) # 設置x軸范圍 ylim(-1, 20) # 設置y軸范圍 my_x_ticks = numpy.arange(0, 20, 1) my_y_ticks = numpy.arange(0, 20, 1) plt.xticks(my_x_ticks) plt.yticks(my_y_ticks) plt.grid(True) # plt.show() def draw_path_open(self, a): """ 畫出open表中的坐標點圖 :return: """ map_open = copy.deepcopy(map_grid) for i in range(a.closed.shape[1]): x = a.closed[:, i] map_open[int(x[0]), int(x[1])] = 1 plt.imshow(map_open, cmap=plt.cm.hot, interpolation='nearest', vmin=0, vmax=10) # plt.colorbar() xlim(-1, 20) # 設置x軸范圍 ylim(-1, 20) # 設置y軸范圍 my_x_ticks = numpy.arange(0, 20, 1) my_y_ticks = numpy.arange(0, 20, 1) plt.xticks(my_x_ticks) plt.yticks(my_y_ticks) plt.grid(True) # plt.show() def draw_path_closed(self, a): """ 畫出closed表中的坐標點圖 :return: """ print('打印closed長度：') print(a.closed.shape[1]) map_closed = copy.deepcopy(map_grid) for i in range(a.closed.shape[1]): x = a.closed[:, i] map_closed[int(x[0]), int(x[1])] = 5 plt.imshow(map_closed, cmap=plt.cm.hot, interpolation='nearest', vmin=0, vmax=10) # plt.colorbar() xlim(-1, 20) # 設置x軸范圍 ylim(-1, 20) # 設置y軸范圍 my_x_ticks = numpy.arange(0, 20, 1) my_y_ticks = numpy.arange(0, 20, 1) plt.xticks(my_x_ticks) plt.yticks(my_y_ticks) plt.grid(True) # plt.show() def draw_direction_point(self, a): """ 從終點開始，根據記錄的方向信息，畫出搜索的路徑圖 :return: """ print('打印direction長度：') print(a.best_path_array.shape[1]) map_direction = copy.deepcopy(map_grid) for i in range(a.best_path_array.shape[1]): x = a.best_path_array[:, i] map_direction[int(x[0]), int(x[1])] = 6 plt.imshow(map_direction, cmap=plt.cm.hot, interpolation='nearest', vmin=0, vmax=10) # plt.colorbar() xlim(-1, 20) # 設置x軸范圍 ylim(-1, 20) # 設置y軸范圍 my_x_ticks = numpy.arange(0, 20, 1) my_y_ticks = numpy.arange(0, 20, 1) plt.xticks(my_x_ticks) plt.yticks(my_y_ticks) plt.grid(True) def draw_three_axes(self, a): """ 將三張圖畫在一個figure中 :return: """ plt.figure() ax1 = plt.subplot(221) ax2 = plt.subplot(222) ax3 = plt.subplot(223) ax4 = plt.subplot(224) plt.sca(ax1) self.draw_init_map() plt.sca(ax2) self.draw_path_open(a) plt.sca(ax3) self.draw_path_closed(a) plt.sca(ax4) self.draw_direction_point(a) plt.show() if __name__ == '__main__': a1 = AStar() a1.main() a1.path_backtrace() m1 = MAP() m1.draw_three_axes(a1)
