启发式探索是利用问题拥有的启发信息来引导搜索，达到减少探索范围、降低问题复杂度的目的。A*寻路算法是启发式探索的一个典型实践，在寻路搜索的过程中，给每个节点绑定了一个估计值(即启发式)，在对节点的遍历过程中采取估计值优先原则，估计值更优的节点会被优先遍历。
1、A*算法基本原理
A*算法是一种有序搜索算法，其特点在于对估价函数的定义上。公式表示为： f(n)=g(n)+h(n)，其中， f(n)是从初始状态经由状态n到目标状态的代价估计，g(n) 是在状态空间中从初始状态到状态n的实际代价，h(n)是从状态n到目标状态的最佳路径的估计代价。对于路径搜索问题，状态就是图中的节点，代价就是距离。
2、A*算法搜索步骤
1. 算法步骤
(1) 设置地图大小，起点S，终点E，障碍集合Blocklist。
(2) 添加起点S到Openlist(待搜索集合)。
(3) 将S取出，添加到Closelist(已搜索集合)。
(4) 查找S所有相邻节点，添加到Openlist，并设置S为它们的父节点；以绿色初始节点右侧的灰色节点为例：f(n)=g(n)+h(n)。g(n)=1,绿色初始节点到该节点的移动步数；h(n)=3,灰色节点移动到红色终点的步数(曼哈顿距离)，也可以使用欧氏距离。f(n)=g(n)+h(n)=1+3=4,其他相邻节点计算相同。如图1所示，曼哈顿距离向4个方向移动，距离公式为：
欧氏距离向8个方向移动，距离公式为：
■ 图 1步骤(4)示意图
(5) 选择Openlist中f值最小点，有两个分别为绿色右侧节点和绿色下方节点，将右侧节点添加至Closelist，并设置绿色节点为其父节点，选择下方节点也可以，本例节点顺序右下左上，根据启发式规则相同，结果和搜索效率与选取顺序无关,如图2所示。
■ 图2 步骤(5)示意图
(6) 此时Openlist中f的最小值是4，黄色相邻节点中只有上下节点可达，根据计算得到上下节点f值，此处f值为最小值，采用其他路径计算值均不小于4,如图3所示。
■ 图3 黄色相邻上下节点
因此，选取黄色节点下方f值小的节点添加至Closelist，此时Openlist中f的最小值为4，继续选取节点添加至Openlist,如图4所示。
■ 图4 步骤(6)示意图
接下来算法进入相同方式迭代过程。
A*算法最终执行结果如图5所示。
■ 图5 结果示意图
2. 路径搜索
开始从红色节点逆推，红色节点的父节点为⑦号节点，⑦号节点的父节点为⑥号节点，⑥号节点的父节点为⑤号节点，⑤号节点的父节点为②号节点，⑤号节点是搜索到②号节点时添加到Openlist中的，并且一直未被更新，②号节点的父节点为①号节点，最终的搜索路径为：起点-①-②-⑤-⑥-⑦-终点。
这里的搜索路径并不是最佳路径的唯一解，其中路径：起点-③-④-⑤-⑥-⑦和路径：起点-③-②-⑤-⑥-⑦都可以通过相应的算法求出，作为搜索的最佳路径，因为这些路径理论上是等同的，这里只以一种最佳路径作为演示。
对于上例演示的情况中存在无论如何重新计算Openlist中节点的f值都不会更小，也就是无法进行更新操作，因此再举一个例子，演示更新搜索。
现在考虑可以8个方向搜索，但是斜向搜索需要步数为4。
选择Openlist中f值最小的节点，选择了右侧f值为4的节点，此时计算右侧f值为4的节点的相邻节点的f值，如图6所示。其中左侧绿色起始点已经添加进Closelist中，右侧三个为黑色节点，因此不考虑这4个节点，其他节点均已存在于Openlist中，现对其f值进行更新。
(1) 先看左上角的相邻节点，通过黄色节点到达该节点，g(n) = 5，h(n)不变，f(n)反而更大了，因此不更新。左下角节点同理。
(2) 上方节点，通过黄色节点计算g(n) = 2, h(n)不变，f(n) = 6 < 8。所以，更新这个节点的f值，并将其父节点修改为黄色节点。下方居中节点同理,如图7所示。
3、使用Python实现上述流程
(1) 绘制地图全貌：起点、终点、障碍和可通行节点。
(2) 获得相邻节点。
(3) 采用曼哈顿或欧氏距离计算h(n)。
(4) 更新f值。
(5) 绘制搜索最优路径
4、最优路径规划
尝试采用A*算法对图8所示的地图进行最优路径规划。
■ 图8 路径规划图
附录B A*算法实现最优路径规划
(1) 已搜索的③号节点邻居节点添加至Openlist，Openlist中f最小值为6，根据右下左上顺序原则优先选取下方节点，如图B.1所示。
附B.1 Step7示意图
(2) 此时④号节点邻居节点添加至Openlist，Openlist中f最小值为6，根据右下左上顺序原则，优先选取右侧节点，如图B.2所示。
附B.2 Step8示意图
(3) 此时Openlist中f最小值为6，选择⑤号节点的右侧节点添加至Closelist，如图B.3所示。
附B.3 Step9示意图
(4) 选取⑥号节点右侧节点添加到Closelist，此时Openlist中已包含了红色终止节点，算法结束，如图B.4所示。
附B.4 Step10示意图
算法示例程序：
import math
from random import randint
import pygame
from enum import Enum
# 定义全局变量：地图中节点的像素大小
CELL_WIDTH = 30 #单元格宽度
CELL_HEIGHT = 30 #单元格长度
BORDER_WIDTH = 1 #边框宽度
BLOCK_NUM = 3 #地图中的障碍物数量
class Color(Enum):
''' 颜色 '''
RED = (255, 0, 0)
GREEN = (0, 255, 0)
BLUE = (0, 0, 255)
WHITE = (255, 255, 255)
BLACK = (0, 0, 0)
GREY=(128,128,128)
@staticmethod
def random_color():
'''设置随机颜色'''
r = randint(0, 255)
g = randint(0, 255)
b = randint(0, 255)
return (r, g, b)
class Map(object):
def __init__(self, mapsize):
self.mapsize = mapsize
def generate_cell(self, cell_width, cell_height):
'''
定义一个生成器，用来生成地图中的所有节点坐标
:param cell_width: 节点宽度
:param cell_height: 节点长度
:return: 返回地图中的节点
'''
x_cell = -cell_width
for num_x in range(self.mapsize[0] // cell_width):
y_cell = -cell_height
x_cell += cell_width
for num_y in range(self.mapsize[1] // cell_height):
y_cell += cell_height
yield (x_cell, y_cell)
class Node(object):
def __init__(self, pos):
self.pos = pos
self.father = None
self.gvalue = 0
self.fvalue = 0
def compute_fx(self, enode, father):
if father == None:
print('未设置当前节点的父节点！')
gx_father = father.gvalue
#采用欧氏距离计算父节点到当前节点的距离
#gx_f2n = math.sqrt((father.pos[0] - self.pos[0])**2 + (father.pos[1] - self.pos[1])**2)
gx_f2n = abs(father.pos[0] - self.pos[0]) + abs(father.pos[1] - self.pos[1])
gvalue = gx_f2n + gx_father
#hx_n2enode = math.sqrt((self.pos[0] - enode.pos[0])**2 + (self.pos[1] - enode.pos[1])**2)
hx_n2enode = abs(self.pos[0] - enode.pos[0])+ abs(self.pos[1] - enode.pos[1])
fvalue = gvalue + hx_n2enode
return gvalue, fvalue
def set_fx(self, enode, father):
self.gvalue, self.fvalue = self.compute_fx(enode, father)
self.father = father
def update_fx(self, enode, father):
gvalue, fvalue = self.compute_fx(enode, father)
if fvalue < self.fvalue:
self.gvalue, self.fvalue = gvalue, fvalue
self.father = father
class AStar(object):
def __init__(self, mapsize, pos_sn, pos_en):
self.mapsize = mapsize #表示地图的投影大小，并非屏幕上的地图像素大小
self.openlist, self.closelist, self.blocklist = [], [], []
self.snode = Node(pos_sn) #用于存储路径规划的起始节点
self.enode = Node(pos_en) #用于存储路径规划的目标节点
self.cnode = self.snode #用于存储当前搜索到的节点
def run(self):
self.openlist.append(self.snode)
while(len(self.openlist) > 0):
#查找openlist中fx最小的节点
fxlist = list(map(lambda x: x.fvalue, self.openlist))
index_min = fxlist.index(min(fxlist))
self.cnode = self.openlist[index_min]
del self.openlist[index_min]
self.closelist.append(self.cnode)
# 扩展当前fx最小的节点，并进入下一次循环搜索
self.extend(self.cnode)
# 如果openlist列表为空，或者当前搜索节点为目标节点，则跳出循环
if len(self.openlist) == 0 or self.cnode.pos == self.enode.pos:
break
if self.cnode.pos == self.enode.pos:
self.enode.father = self.cnode.father
return 1
else:
return -1
def get_minroute(self):
minroute = []
current_node = self.enode
while(True):
minroute.append(current_node.pos)
current_node = current_node.father
if current_node.pos == self.snode.pos:
break
minroute.append(self.snode.pos)
minroute.reverse()
return minroute
def extend(self, cnode):
nodes_neighbor = self.get_neighbor(cnode)
for node in nodes_neighbor:
#判断节点node是否在closelist和blocklist中，因为closelist和blocklist中元素均为Node类，所以要用map函数转换为坐标集合
if node.pos in list(map(lambda x:x.pos, self.closelist)) or node.pos in self.blocklist:
continue
else:
if node.pos in list(map(lambda x:x.pos, self.openlist)):
node.update_fx(self.enode, cnode)
else:
node.set_fx(self.enode, cnode)
self.openlist.append(node)
def setBlock(self, blocklist):
'''
获取地图中的障碍物节点，并存入self.blocklist列表中
注意：self.blocklist列表中存储的是障碍物坐标，不是Node类
:param blocklist:
:return:
'''
self.blocklist.extend(blocklist)
def get_neighbor(self, cnode):
#offsets = [(-1,1),(0,1),(1,1),(-1,0),(1,0),(-1,-1),(0,-1),(1,-1)]
offsets = [(-1, 0), (1, 0), (0, 1), (0, -1)]
nodes_neighbor = []
x, y = cnode.pos[0], cnode.pos[1]
for os in offsets:
x_new, y_new = x + os[0], y + os[1]
pos_new = (x_new, y_new)
#判断是否在地图范围内,超出范围跳过
if x_new < 0 or x_new > self.mapsize[0] - 1 or y_new < 0 or y_new > self.mapsize[1]:
continue
nodes_neighbor.append(Node(pos_new))
return nodes_neighbor
def main():
mapsize = tuple(map(int, input('请输入地图大小，以逗号隔开：').split(',')))
pos_snode = tuple(map(int, input('请输入起点坐标，以逗号隔开：').split(',')))
pos_enode = tuple(map(int, input('请输入终点坐标，以逗号隔开：').split(',')))
myAstar = AStar(mapsize, pos_snode, pos_enode)
blocklist = gen_blocks(mapsize[0], mapsize[1])
myAstar.setBlock(blocklist)
routelist = [] #记录搜索到的最优路径
if myAstar.run() == 1:
routelist = myAstar.get_minroute()
print(routelist)
showresult(mapsize, pos_snode, pos_enode, blocklist, routelist)
else:
print('路径规划失败！')
def gen_blocks(width, height):
'''
随机生成障碍物
:param width: 地图宽度
:param height: 地图高度
:return:返回障碍物坐标集合
'''
i, blocklist = 0, []
while(i < BLOCK_NUM):
for j in range(3):
block = (3, j+1)
if block not in blocklist:
blocklist.append(block)
i+=1
return blocklist
def showresult(mapsize, pos_sn, pos_en, blocklist, routelist):
# 初始化导入的Pygame模块
pygame.init()
# 此处要将地图投影大小转换为像素大小，此处设地图中每个单元格的大小为CELL_WIDTH*CELL_HEIGHT像素
mymap = Map((mapsize[0]*CELL_WIDTH, mapsize[1]*CELL_HEIGHT))
pix_sn = (pos_sn[0]*CELL_WIDTH, pos_sn[1]*CELL_HEIGHT)
pix_en = (pos_en[0]*CELL_WIDTH, pos_en[1]*CELL_HEIGHT)
#对blocklist和routelist中的坐标同样要转换为像素值
bl_pix = list(map(transform, blocklist))
rl_pix = list(map(transform, routelist))
# 初始化显示的窗口并设置尺寸
screen = pygame.display.set_mode(mymap.mapsize)
# 设置窗口标题
pygame.display.set_caption('A*算法路径搜索演示：')
#用白色填充屏幕
screen.fill(Color.WHITE.value)
#绘制屏幕中的所有单元格
for (x, y) in mymap.generate_cell(CELL_WIDTH, CELL_HEIGHT):
if (x,y) in bl_pix:
#绘制黑色的障碍物单元格，并留出2个像素的边框
pygame.draw.rect(screen, Color.BLACK.value, ((x+BORDER_WIDTH,y+BORDER_WIDTH), (CELL_WIDTH-2*BORDER_WIDTH, CELL_HEIGHT-2*BORDER_WIDTH)))
else:
# 绘制绿色的可通行单元格，并留出2个像素的边框
pygame.draw.rect(screen, Color.GREEN.value, ((x+BORDER_WIDTH,y+BORDER_WIDTH), (CELL_WIDTH-2*BORDER_WIDTH, CELL_HEIGHT-2*BORDER_WIDTH)))
#绘制起点和终点
pygame.draw.circle(screen, Color.BLUE.value, (pix_sn[0]+CELL_WIDTH//2, pix_sn[1]+CELL_HEIGHT//2), CELL_WIDTH//2 - 1)
pygame.draw.circle(screen, Color.RED.value, (pix_en[0]+CELL_WIDTH//2, pix_en[1]+CELL_HEIGHT//2), CELL_WIDTH//2 - 1)
#绘制搜索得到的最优路径
for (x, y) in mymap.generate_cell(CELL_WIDTH, CELL_HEIGHT):
if (x,y) in rl_pix and (x,y)!=(pix_sn[0],pix_sn[1]) and (x,y)!=(pix_en[0],pix_en[1]):
pygame.draw.rect(screen, Color.GREY.value, ((x+BORDER_WIDTH,y+BORDER_WIDTH), (CELL_WIDTH-2*BORDER_WIDTH, CELL_HEIGHT-2*BORDER_WIDTH)))
#pygame.draw.aalines(screen, Color.RED.value, False, rl_pix)
keepGoing = True
while keepGoing:
pygame.time.delay(100)
for event in pygame.event.get():
if event.type == pygame.QUIT:
keepGoing = False
pygame.display.flip()
def transform(pos):
xnew, ynew = pos[0]*CELL_WIDTH, pos[1]*CELL_HEIGHT
return (xnew, ynew)
if __name__ == '__main__':
main()
