A*算法在栅格地图上的路径搜索（python实现）

  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()