堆優(yōu)化的A*算法-Python實(shí)現(xiàn)
A*算法解決二維網(wǎng)格地圖中的尋路問題
- 輸入：圖片（白色區(qū)域代表可行，深色區(qū)域代表不行可行）
- 輸出：路徑（在圖中繪制）
""" 方格地圖中的A*算法 (openList進(jìn)行了堆優(yōu)化)
A* 算法： F = G+H
F: 總移動(dòng)代價(jià)
G: 起點(diǎn)到當(dāng)前點(diǎn)的移動(dòng)代價(jià) 直:1, 斜:1.4
H: 當(dāng)前點(diǎn)到終點(diǎn)的預(yù)估代價(jià) 曼哈頓距離
++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
1.把起點(diǎn)加入 openList中
2.While True:
a.遍歷openList横媚，查找F值最小的節(jié)點(diǎn)泥技，作為current
b.current是終點(diǎn):
========結(jié)束========
c.從openList中彈出慕购，放入closeList中
d.對(duì)八個(gè)方位的格點(diǎn):
if 越界 or 是障礙物 or 在closeList中:
continue
if 不在openList中：
設(shè)置父節(jié)點(diǎn),F,G,H
加入openList中
else:
if 這條路徑更好:
設(shè)置父節(jié)點(diǎn),F,G
更新openList中的對(duì)應(yīng)節(jié)點(diǎn)
3.生成路徑path
+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
堆優(yōu)化:
openList：作為最小堆，按F值排序存儲(chǔ)坐標(biāo) （不更新只增加）
openDict：坐標(biāo):點(diǎn)詳細(xì)信息 （既更新又增加）
get_minfNode() 從openList中彈出坐標(biāo)，去openDict中取點(diǎn) （但由于不更新只增加烟号，坐標(biāo)可能冗余）
in_openList() 判斷坐標(biāo)是否在openDict中即可
"""
import math
from PIL import Image,ImageDraw
import numpy as np
import heapq # 堆
STAT_OBSTACLE='#'
STAT_NORMAL='.'
def manhattan(x1,y1, x2,y2):
"""兩個(gè)Point的曼哈頓距離"""
h = abs(x1-x2)+abs(y1-y2)
return h
class Node():
"""
開放列表和關(guān)閉列表的元素類型坯认，parent用來在成功的時(shí)候回溯路徑
"""
def __init__(self, x, y,parent=None, g=0, h=0):
self.parent = parent
self.x = x
self.y = y
self.g = g
self.h = h
self.update()
def update(self):
self.f = self.g+self.h
class A_Star:
""" x是行索引翻擒，y是列索引
"""
def __init__(self, test_map, start=None, end=None):
"""地圖，起點(diǎn)牛哺，終點(diǎn)"""
self.map = test_map
self.cols = len(test_map[0])
self.rows = len(test_map)
self.s_x, self.s_y = start if start else [0,0]
self.e_x, self.e_y = end if end else [self.rows-1,self.cols-1]
self.closeList = set()
self.path = []
self.openList = [] # 堆陋气，只添加，和彈出最小值點(diǎn)引润，
self.openDict = dict() # openList中的 坐標(biāo):詳細(xì)信息 -->不冗余的
def find_path(self):
"""A*算法尋路主程序"""
p = Node(self.s_x, self.s_y,
h=manhattan(self.s_x,self.s_y, self.e_x,self.e_y)) # 構(gòu)建開始節(jié)點(diǎn)
heapq.heappush(self.openList, (p.f,(p.x,p.y)))
self.openDict[(p.x,p.y)] = p # 加進(jìn)dict目錄
while True:
current = self.get_minfNode()
if current.x==self.e_x and current.y==self.e_y:
print('find path')
self.make_path(current)
break
self.closeList.add((current.x,current.y)) ## 加入closeList
del self.openDict[(current.x,current.y)]
self.extend_surrounds(current) # 會(huì)更新close list
def make_path(self,p):
"""從結(jié)束點(diǎn)回溯到開始點(diǎn)巩趁，開始點(diǎn)的parent==None"""
while p:
self.path.append((p.x, p.y))
p = p.parent
def extend_surrounds(self, node):
""" 將當(dāng)前點(diǎn)周圍可走的點(diǎn)加到openList中，
其中 不在openList中的點(diǎn) 設(shè)置parent淳附、F,G,H 加進(jìn)去议慰，
在openList中的點(diǎn) 更新parent、F,G,H
"""
motion_direction = [[1, 0], [0, 1], [-1, 0], [0, -1],
[1, 1], [1, -1], [-1, 1], [-1, -1]]
for dx, dy in motion_direction:
x,y = node.x+dx, node.y+dy
new_node = Node(x,y)
# 位置無效奴曙，或者是障礙物, 或者已經(jīng)在closeList中
if not self.is_valid_xy(x,y) or not self.not_obstacle(x,y) or self.in_closeList(new_node):
continue
if abs(dx)+abs(dy)==2: ## 斜向
h_x,h_y = node.x+dx,node.y # 水平向
v_x,v_y = node.x,node.y+dy # 垂直向
if not self.is_valid_xy(h_x,h_y) or not self.not_obstacle(h_x,h_y) or self.in_closeList(Node(h_x,h_y)):
continue
if not self.is_valid_xy(v_x,v_y) or not self.not_obstacle(v_x,v_y) or self.in_closeList(Node(v_x,v_y)):
continue
#============ ** 關(guān)鍵 ** ========================
#============ 不在openList中别凹，加進(jìn)去； ========================
#============ 在openList中洽糟，更新 ========================
#============對(duì)于openList和openDict來說炉菲，操作都一樣 ===========
new_g = node.g + self.cal_deltaG(node.x,node.y, x,y)
sign=False # 是否執(zhí)行操作的標(biāo)志
if not self.in_openList(new_node): # 不在openList中
# 加進(jìn)來，設(shè)置 父節(jié)點(diǎn), F, G, H
new_node.h = self.cal_H(new_node)
sign=True
elif self.openDict[(new_node.x,new_node.y)].g > new_g: # 已在openList中坤溃，但現(xiàn)在的路徑更好
sign=True
if sign:
new_node.parent = node
new_node.g = new_g
new_node.f = self.cal_F(new_node)
self.openDict[(new_node.x,new_node.y)]=new_node # 更新dict目錄
heapq.heappush(self.openList, (new_node.f,(new_node.x,new_node.y)))
def get_minfNode(self):
"""從openList中取F=G+H值最小的 (堆-O(1))"""
while True:
f, best_xy=heapq.heappop(self.openList)
if best_xy in self.openDict:
return self.openDict[best_xy]
def in_closeList(self, node):
"""判斷是否在closeList中 (集合-O(1)) """
return True if (node.x,node.y) in self.closeList else False
def in_openList(self, node):
"""判斷是否在openList中 (字典-O(1))"""
if not (node.x,node.y) in self.openDict:
return False
else:
return True
def is_valid_xy(self, x,y):
if x < 0 or x >= self.rows or y < 0 or y >= self.cols:
return False
return True
def not_obstacle(self,x,y):
return self.map[x][y] != STAT_OBSTACLE
def cal_deltaG(self,x1,y1,x2,y2):
""" 計(jì)算兩點(diǎn)之間行走的代價(jià)
（為簡化計(jì)算）上下左右直走拍霜，代價(jià)為1.0，斜走薪介，代價(jià)為1.4 G值
"""
if x1 == x2 or y1 == y2:
return 1.0
return 1.4
def cal_H(self, node):
""" 曼哈頓距離 估計(jì)距離目標(biāo)點(diǎn)的距離"""
return abs(node.x-self.e_x)+abs(node.y-self.e_y) # 剩余路徑的估計(jì)長度
def cal_F(self, node):
""" 計(jì)算F值 F = G+H
A*算法的精髓：已經(jīng)消耗的代價(jià)G祠饺，和預(yù)估將要消耗的代價(jià)H
"""
return node.g + node.h
def plot(test_map,path):
"""繪制地圖和路徑
test_map:二維數(shù)組
path:路徑坐標(biāo)數(shù)組
"""
out = []
for x in range(len(test_map)):
temp = []
for y in range(len(test_map[0])):
if test_map[x][y]==STAT_OBSTACLE:
temp.append(0)
elif test_map[x][y]==STAT_NORMAL:
temp.append(255)
elif test_map[x][y]=='*':
temp.append(127)
else:
temp.append(255)
out.append(temp)
for x,y in path:
out[x][y] = 127
out = np.array(out)
img = Image.fromarray(out)
img.show()
def path_length(path):
"""計(jì)算路徑長度"""
l = 0
for i in range(len(path)-1):
x1,y1 = path[i]
x2,y2 = path[i+1]
if x1 == x2 or y1 == y2:
l+=1.0
else:
l+=1.4
return l
def img_to_map(img_file):
"""地圖圖片變二維數(shù)組"""
test_map = []
img = Image.open(img_file)
img = img.resize((100,100)) ### resize圖片尺寸
img_gray = img.convert('L') # 地圖灰度化
img_arr = np.array(img_gray)
img_binary = np.where(img_arr<127,0,255)
for x in range(img_binary.shape[0]):
temp_row = []
for y in range(img_binary.shape[1]):
status = STAT_OBSTACLE if img_binary[x,y]==0 else STAT_NORMAL
temp_row.append(status)
test_map.append(temp_row)
return test_map
# ===== test case ===============
test_map=img_to_map('map_2.bmp')
a = A_Star(test_map)
a.find_path()
plot(test_map,a.path)
print('path length:',path_length(a.path))
測(cè)試用例及結(jié)果
map1.png
map2.png
map5.png
存在的問題
不確定是否是最優(yōu)路徑
原文描述：
“ If we overestimate this distance, however, it is not guaranteed to give us the shortest path. In such cases, we have what is called an "inadmissible heuristic.".
Technically, in this example, the Manhattan method is inadmissible because it slightly overestimates the remaining distance.”
即如果我們高估了H，則不能保證最短路徑昭灵。而曼哈頓距離略微高估了吠裆。
另外伐谈，筆者不確定程序是不是正確，以及是不是真正的A*算法试疙，請(qǐng)大神們指正诵棵。
