作者：邬杨明
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()
