A*算法
- 前言
- 一、A*算法实现步骤
- 二、python代码
- 1.，地图及移动成本
- 2.设置列表等数据
- 3.设置子节点
- 4.初始化起点和终点坐标及地图大小
- 5.初始化父坐标
- 6.将起点加入到open列表中
- 7.从open列表最小F节点，存入close列表中
- 8. 获取子节点
- 已存在路径和当前路径，选择最优
- 10.获取最终路径
- 11. 主程序-任务执行
- 总结
前言
A算法是一种静态网中最短路径最有效的直接搜索方法。多用于游戏和物流中的路径规划问题。
A算法的详细解读可以看iwiniwin —/iwiniwin/，这个博主写的很不错
一、A*算法实现步骤
A*算法的大致过程：
1.将初始节点放入到open列表中。
2.判断open列表。如果为空，则搜索失败。如果open列表中存在目标节点，则搜索成功。
3.从open列表中取出F值最小的节点作为当前节点，并将其加入到close列表中。
4.计算当前节点的相邻的所有可到达节点，生成一组子节点。对于每一个子节点：
-如果该节点在close列表中，则丢弃它
-如果该节点在open列表中，则检查其通过当前节点计算得到的F值是否更小，如果更小则更新其F值，并将其父节点设置为当前节点。
-如果该节点不在open列表中，则将其加入到open列表，并计算F值，设置其父节点为当前节点。
转到2步骤
二、python代码
1.，地图及移动成本
地图是栅格地图，以二维数组的形式展开，每一个数代表一个节点（道路），数的取值代表节点（道路）信息，其中0表示道路通畅，-1表示移动对象（起点），1表示道路阻塞，2表示终点。
行动成本是前进一格需要花费的成本，在这里将水平或垂直移动一格的成本设置为10，对角移动一个的成本设置为14。
# 地图，栅格状地图，二维数组,每个节点有两个状态，可通过0与不可通过1
maps = np.array([
[0, 1, 0, 0, 0, 0],
[-1, 0, 0, 1, 0, 1],
[0, 0, 1, 1, 0, 1],
[1, 0, 1, 0, 1, 1],
[0, 0, 1, 0, 1, 0],
[1, 0, 1, 0, 0, 2]
], dtype=int)
#上北下南
##移动 ，水平或垂直的移动成本是10，对角的移动成本是14
move_long10 = 10
move_long14 = 14
2.设置列表等数据
def __init_start(self):
self.end = True
self.pos = { # 起点，终点，地图大小
"start_w": 0,
"start_h": 0,
"end_w": 0,
"end_h": 0,
'threshold_we': 0,
'threshold_sn': 0
}
# 储存路径数据，节点信息（节点坐标，节点方向，节点数目），路径总长度
# [[父节点坐标], [当前点坐标], f(路径计分), g(从起点到网格上给定正方形的移动成本，遵循到达那里的路径。), h(最短距离), string(节点信息) ]
self.parent_map = {
"parent": {'w': 0, 'h': 0},
"son": {'w': 0, 'h': 0},
"f": 0,
"g": 0,
"h": 0,
"str": 0}
# 坐标信息
self.map_coor = {
'coor_pos': {'w': 0, 'h': 0},
'f': 0,
'g': 0,
'h': 0,
'walkable': 0
}
self.open_lists = []
self.close_list = []
3.设置子节点
每次获取子节点的几个方向是动态的，会随障碍物或者边缘，删除或添加某个方向的节点
（对角暂时未用），相信你，看完代码，上手实践后，就会用了
def __childer(self):
#上北下南
# # 每个节点有8个方向，（边缘节点除外）下，左，上，右，，，，
# # East东, south南, west西, north北, southeast东南, northeast东北, southwest西南, northwest西北
# # 子节点
Childer = {
#'wn': {'x': 0, 'y': 0, 'f': 0, 'g': 0, 'h': 0},
'n': {'x': 0, 'y': 0, 'f': 0, 'g': 0, 'h': 0},
#'en': {'x': 0, 'y': 0, 'f': 0, 'g': 0, 'h': 0},
'w': {'x': 0, 'y': 0, 'f': 0, 'g': 0, 'h': 0},
'e': {'x': 0, 'y': 0, 'f': 0, 'g': 0, 'h': 0},
#'ws': {'x': 0, 'y': 0, 'f': 0, 'g': 0, 'h': 0},
's': {'x': 0, 'y': 0, 'f': 0, 'g': 0, 'h': 0},
#'es': {'x': 0, 'y': 0, 'f': 0, 'g': 0, 'h': 0}
}
return Childer
4.初始化起点和终点坐标及地图大小
def __init_pos(self, map_array, start_str, end_str):
start_pos = np.argwhere(map_array == start_str)
end_pos = np.argwhere(map_array == end_str)
threshold = np.shape(map_array)
self.pos = {
"start_h": start_pos[0][0],
"start_w": start_pos[0][1],
"end_h": end_pos[0][0],
"end_w": end_pos[0][1],
'threshold_we': threshold[0],
'threshold_sn': threshold[1]
}
5.初始化父坐标
#初始化父节点
def __init_parent_map(self):
g = 0
h = self.move_long10*(abs(self.pos['start_w']-self.pos['end_w'])+abs(self.pos['start_h']-self.pos['end_h']))
self.parent_map = {
"parent": {'w': self.pos['start_w'], 'h': self.pos['start_h']},
"son": {'w': self.pos['start_w'], 'h': self.pos['start_h']},
"f": g+h,
"g": g,
"h": h,
"str": -1}
6.将起点加入到open列表中
#起点加入open list
self.map_coor = {
'coor_pos': {'w': self.pos['start_w'], 'h': self.pos['start_h']},
'f': self.parent_map['f'],
'g': self.parent_map['g'],
'h': self.parent_map['h'],
'walkable': 0
}
self.open_lists.append(self.parent_map)
7.从open列表最小F节点，存入close列表中
def __find_open_fmin(self):
f = 65536
w_h = None
for i in range(len(self.open_lists)):
if self.open_lists[i]['str'] != 1:
if self.open_lists[i]['f'] < f:
f = self.open_lists[i]['f']
w_h = i
self.close_list.append(self.open_lists[w_h])
self.open_lists.pop(w_h)
8. 获取子节点
获取该节点的子节点
# # East东, south南, west西, north北, southeast东南, northeast东北, southwest西南, northwest西北
# def fgh_g(self, ):
def __getchild_one(self, child, parent_pos, pos, g):
for i in child:
if i == 'wn':
if child[i]['x'] == 0 and child[i]['y'] == 0:
child[i]['x'] = parent_pos['son']['w'] - 1 if parent_pos['son']['w'] - 1 >= 0 else -1
child[i]['y'] = parent_pos['son']['h'] - 1 if parent_pos['son']['h'] - 1 >= 0 else -1
child[i]['h'] = (abs(child[i]['x'] - pos['end_w']) + abs(
child[i]['y'] - pos['end_h']))*self.move_long10
child[i]['g'] = g + 14
child[i]['f'] = child[i]['h'] + child[i]['g']
elif i == 'n':
if child[i]['x'] == 0 and child[i]['y'] == 0:
child[i]['x'] = parent_pos['son']['w']
child[i]['y'] = parent_pos['son']['h'] - 1 if parent_pos['son']['h'] - 1 >= 0 else -1
child[i]['h'] = (abs(child[i]['x'] - pos['end_w']) + abs(
child[i]['y'] - pos['end_h'])) * self.move_long10
child[i]['g'] = g + 10
child[i]['f'] = child[i]['h'] + child[i]['g']
elif i == 'en':
if child[i]['x'] == 0 and child[i]['y'] == 0:
child[i]['x'] = parent_pos['son']['w'] + 1 if parent_pos['son']['w'] + 1 < pos["threshold_we"] else -1
child[i]['y'] = parent_pos['son']['h'] - 1 if parent_pos['son']['h'] - 1 >= 0 else -1
child[i]['h'] = (abs(child[i]['x'] - pos['end_w']) + abs(
child[i]['y'] - pos['end_h'])) * self.move_long10
child[i]['g'] = g + 14
child[i]['f'] = child[i]['h'] + child[i]['g']
elif i == 'w':
if child[i]['x'] == 0 and child[i]['y'] == 0:
child[i]['x'] = parent_pos['son']['w'] - 1 if parent_pos['son']['w'] - 1 >= 0 else -1
child[i]['y'] = parent_pos['son']['h']
child[i]['h'] = (abs(child[i]['x'] - pos['end_w']) + abs(
child[i]['y'] - pos['end_h'])) * self.move_long10
child[i]['g'] = g + 10
child[i]['f'] = child[i]['h'] + child['n']['g']
elif i == 'e':
if child[i]['x'] == 0 and child[i]['y'] == 0:
child[i]['x'] = parent_pos['son']['w'] + 1 if parent_pos['son']['w'] + 1 < pos["threshold_we"] else -1
child[i]['y'] = parent_pos['son']['h']
child[i]['h'] = (abs(child[i]['x'] - pos['end_w']) + abs(
child[i]['y'] - pos['end_h'])) * self.move_long10
child[i]['g'] = g + 10
child[i]['f'] = child[i]['h'] + child[i]['g']
elif i == 'ws':
if child[i]['x'] == 0 and child[i]['y'] == 0:
child[i]['x'] = parent_pos['son']['w'] - 1 if parent_pos['son']['w'] - 1 >= 0 else -1
child[i]['y'] = parent_pos['son']['h'] + 1 if parent_pos['son']['h'] + 1 < pos["threshold_sn"] else -1
child[i]['h'] = (abs(child[i]['x'] - pos['end_w']) + abs(
child[i]['y'] - pos['end_h'])) * self.move_long10
child[i]['g'] = g + 14
child[i]['f'] = child[i]['h'] + child[i]['g']
elif i == 's':
if child[i]['x'] == 0 and child[i]['y'] == 0:
child[i]['x'] = parent_pos['son']['w']
child[i]['y'] = parent_pos['son']['h'] + 1 if parent_pos['son']['h'] + 1 < pos["threshold_sn"] else -1
child[i]['h'] = (abs(child[i]['x'] - pos['end_w']) + abs(
child[i]['y'] - pos['end_h'])) * self.move_long10
child[i]['g'] = g + 10
child[i]['f'] = child[i]['h'] + child[i]['g']
elif i == 'es':
if child[i]['x'] == 0 and child[i]['y'] == 0:
child[i]['x'] = parent_pos['son']['w'] + 1 if parent_pos['son']['w'] + 1 < pos["threshold_we"] else -1
child[i]['y'] = parent_pos['son']['h'] + 1 if parent_pos['son']['h'] + 1 < pos["threshold_sn"] else -1
child[i]['h'] = (abs(child[i]['x'] - pos['end_w']) + abs(
child[i]['y'] - pos['end_h']))*self.move_long10
child[i]['g'] = g + 14
child[i]['f'] = child[i]['h'] + child[i]['g']
return child
判断子节点是否有效，无效则删除（节点是障碍物或者超出地图边缘）
def __getchild_two(self, child):
mydel = []
for i in child:
if child[i]['x'] < 0 or child[i]['y'] < 0:
mydel.append(i)
elif self.maps[child[i]['y']][child[i]['x']] == 1:
mydel.append(i)
else:
for n in self.close_list:
if n['son']['w'] == child[i]['x'] and n['son']['h'] == child[i]['y']:
mydel.append(i)
for i in mydel:
child.pop(i)
return child
已存在路径和当前路径，选择最优
#若当前处理节点的相邻格子已经在Open List中，则检查这条路径是否更优，
# 即计算经由当前处理节点到达那个方格是否具有更小的 G值。如果没有，不做任何操作。
# 相反，如果G值更小，则把那个方格的父节点设为当前处理节点 ( 我们选中的方格 )
# ，然后重新计算那个方格的 F 值和 G 值
def __child_open_pk(self, child):
gai_list = []
num_list = []
find_list = True
for i in child:
open_list = {
"parent": {'w': self.parent_map['parent']['w'], 'h': self.parent_map['parent']['h']},
"son": {'w': child[i]['x'], 'h': child[i]['y']},
"f": child[i]['f'],
"g": child[i]['g'],
"h": child[i]['h'],
"str": self.maps[child[i]['y']][child[i]['x']]}
if open_list['str'] == 2:
self.close_list.append(open_list)
self.end = False
break
for n in range(len(self.open_lists)):
if self.open_lists[n]['son']['w'] == child[i]['x'] and self.open_lists[n]['son']['h'] == child[i]['y']:
if child[i]['g'] < self.open_lists[n]['g']:
num_list.append(n)
gai_list.append(open_list)
find_list = False
if find_list == True:
gai_list.append(open_list)
find_list = True
if gai_list:
if num_list:
for n in num_list:
self.open_lists.pop(n)
for i in gai_list:
self.open_lists.append(i)
10.获取最终路径
def __answer_road(self):
answer_roads = []
answer_road = None
for i in self.close_list:
if i['str'] == 2:
answer_roads.append(i)
answer_road = i
if answer_road:
for n in self.close_list:
for i in self.close_list:
if i['son']['w'] == answer_road['parent']['w'] and\
i['son']['h'] == answer_road['parent']['h']:
answer_roads.append(i)
answer_road = i
if i['str'] == -1:
break
while True:
if answer_roads[-2]['str'] == -1:
answer_roads.pop(-2)
else:
break
for f in answer_roads:
if f['str'] == 0:
self.maps[f['son']['h']][f['son']['w']] = 4
answer_roads.reverse()
return answer_roads
11. 主程序-任务执行
#主函数--任务执行
def A_navigate(self):
#初始化坐标
self.__init_start()
self.__init_pos(self.maps, -1, 2)
print("开始: 起点，终点，地图大小 （高，宽）", self.pos)
#初始化父节点
self.__init_parent_map()
#起点加入open list
self.map_coor = {
'coor_pos': {'w': self.pos['start_w'], 'h': self.pos['start_h']},
'f': self.parent_map['f'],
'g': self.parent_map['g'],
'h': self.parent_map['h'],
'walkable': 0
}
self.open_lists.append(self.parent_map)
self.__find_open_fmin()
child = self.__getchild_one(self.__childer(),self.parent_map,self.pos,self.parent_map['g'])
child = self.__getchild_two(child)
for i in child:
_parent_map = {
"parent": {'w': self.parent_map['parent']['w'], 'h': self.parent_map['parent']['h']},
"son": {'w': child[i]['x'], 'h': child[i]['y']},
"f": child[i]['f'],
"g": child[i]['g'],
"h": child[i]['h'],
"str": self.maps[child[i]['y']][child[i]['x']]}
self.open_lists.append(_parent_map)
#循环检测，直到找到最优路径
while self.end:
#排序
self.open_lists.sort(key=lambda i:i['f'], reverse=False)
self.close_list.append(self.open_lists[0])
self.parent_map = {
"parent": {'w': self.open_lists[0]['son']['w'], 'h': self.open_lists[0]['son']['h']},
"son": {'w': self.open_lists[0]['son']['w'], 'h': self.open_lists[0]['son']['h']},
"f": self.open_lists[0]['f'],
"g": self.open_lists[0]['g'],
"h": self.open_lists[0]['h'],
"str": self.open_lists[0]['str']
}
self.open_lists.pop(0)
child = self.__getchild_one(self.__childer(), self.parent_map, self.pos, self.parent_map['g'])
child = self.__getchild_two(child)
self.__child_open_pk(child)
for i in self.close_list:
if i['str'] == 0:
self.maps[i['son']['h']][i['son']['w']] = 5
self.list_road = self.__answer_road()
return self.list_road
总结
可以在评论区进行讨论哦！
谢谢大家能点个赞！
希望和大家一起交流！