Python实现A*寻路算法详解与代码解析
发布时间：2026-10-14 |
7人浏览过
本文详细解析了A*寻路算法的核心原理，包括OPEN集与CLOSED集的管理、G值与H值的计算逻辑及F值的优化策略。通过Python代码实例，展示了地图类、节点类的构建及主循环实现，涵盖4邻域与8邻域移动模式，并提供完整可运行代码供参考。
A* 算法简介
A* 算法运行过程中需维护两个关键数据结构：OPEN 集与 CLOSED 集。OPEN 集用于存储所有已发现但尚未评估的节点，初始状态下仅包含起始节点；CLOSED 集用于记录已评估完成的节点，初始为空。此外，每个节点需记录指向其父节点的指针，以便后续回溯路径。
算法为每个搜索到的节点计算一个综合评估值 F，该值由两部分组成：
- F = G + H
- 其中，G 表示从起始节点移动到当前节点的实际代价。若设定相邻节点间的移动代价为 1，则 G 值随距离起始点越远而递增。
- H：是从当前节点到目标节点的移动量估算值。
- 若允许向上下左右 4 个方向的邻域移动，应采用曼哈顿距离计算。
- 若允许向上下左右及四个对角线方向的 8 个邻域移动，则应采用对角线距离（欧几里得距离的近似或精确计算）。
算法包含一个主循环，重复执行以下步骤直至找到目标节点或确定无路可通： 1. 从 OPEN 集中选取 F 值最小的节点 n 作为当前检测节点。 2. 将节点 n 从 OPEN 集中移除，并加入 CLOSED 集。 3. 若节点 n 即为目标节点，则算法结束。 4. 否则，尝试将节点 n 的所有邻节点 n' 加入处理流程。
- 若邻节点已存在于 CLOSED 集中，说明该节点已被评估，无需再次处理。
- 邻节点在OPEN集中：
- 若新计算的 G 值小于邻节点当前保存的 G 值，则需更新该邻节点的 G 值、F 值及其父节点指向；
- 否则，保持该邻节点原有状态不变。
- 若邻节点尚未被处理，则将其加入 OPEN 集，设置其父节点为 n，并初始化其 G 值和 F 值。
需注意，若起始节点与目标节点之间不存在连通路径（即无法到达），算法在第 1 步中获取的节点 n 将为空，此时算法将直接退出。
关键代码介绍
保存基本信息的地图类
地图类负责随机生成供寻路算法使用的基础地图数据。
首先创建 Map 类，初始化参数设定地图的宽度与高度，并初始化二维数组 map 所有元素为 0，其中 0 表示该节点可通行。
class Map():
def __init__(self, width, height):
self.width = width
self.height = height
self.map = [[0 for x in range(self.width)] for y in range(self.height)]
在 Map 类中添加生成障碍节点的函数，将节点值设为 1，表示该节点不可通行。
def createBlock(self, block_num):
for i in range(block_num):
x, y = (randint(0, self.width-1), randint(0, self.height-1))
self.map[y][x] = 1
在 Map 类中添加显示地图的函数，通过打印所有节点值来可视化地图。值为 0 或 1 的含义如上所述；在后续展示寻路结果时，将使用值 2 来标记从起始节点到目标节点的路径。
def showMap(self):
print("+" * (3 * self.width + 2))
for row in self.map:
s = '+'
for entry in row:
s += ' ' + str(entry) + ' '
s += '+'
print(s)
print("+" * (3 * self.width + 2))
添加一个随机获取可通行节点的函数，用于生成起始点和目标点。
def generatePos(self, rangeX, rangeY):
x, y = (randint(rangeX[0], rangeX[1]), randint(rangeY[0], rangeY[1]))
while self.map[y][x] == 1:
x, y = (randint(rangeX[0], rangeX[1]), randint(rangeY[0], rangeY[1]))
return (x , y)
搜索到的节点类
每个被加入 OPEN 集的节点都会实例化为 SearchEntry 类，该类保存节点的位置坐标 (x, y)、计算得到的 G 值和 F 值，以及指向父节点的指针 pre_entry。
class SearchEntry():
def __init__(self, x, y, g_cost, f_cost=0, pre_entry=None):
self.x = x
self.y = y
# cost move form start entry to this entry
self.g_cost = g_cost
self.f_cost = f_cost
self.pre_entry = pre_entry
def getPos(self):
return (self.x, self.y)
算法主函数介绍
以下是上述算法主循环的 Python 代码实现。OPEN 集与 CLOSED 集采用字典结构，在常规情况下，查找、添加和删除节点的时间复杂度为 O(1)，遍历所有节点的时间复杂度为 O(n)，其中 n 为字典中对象的数量。
def AStarSearch(map, source, dest):
...
openlist = {}
closedlist = {}
location = SearchEntry(source[0], source[1], 0.0)
dest = SearchEntry(dest[0], dest[1], 0.0)
openlist[source] = location
while True:
location = getFastPosition(openlist)
if location is None:
# not found valid path
print("can't find valid path")
break;
if location.x == dest.x and location.y == dest.y:
break
closedlist[location.getPos()] = location
openlist.pop(location.getPos())
addAdjacentPositions(map, location, dest, openlist, closedlist)
#mark the found path at the map
while location is not None:
map.map[location.y][location.x] = 2
location = location.pre_entry
按照算法主循环的逻辑，逐一讲解所用函数。 以下函数用于从 OPEN 集中获取 F 值最小的节点；若 OPEN 集为空，则返回 None。
# find a least cost position in openlist, return None if openlist is empty
def getFastPosition(openlist):
fast = None
for entry in openlist.values():
if fast is None:
fast = entry
elif fast.f_cost > entry.f_cost:
fast = entry
return fast
addAdjacentPositions 函数对应算法主循环中“尝试添加节点 n 的所有邻节点 n'”的步骤。
# add available adjacent positions
def addAdjacentPositions(map, location, dest, openlist, closedlist):
poslist = getPositions(map, location)
for pos in poslist:
# if position is already in closedlist, do nothing
if isInList(closedlist, pos) is None:
findEntry = isInList(openlist, pos)
h_cost = calHeuristic(pos, dest)
g_cost = location.g_cost + getMoveCost(location, pos)
if findEntry is None :
# if position is not in openlist, add it to openlist
openlist[pos] = SearchEntry(pos[0], pos[1], g_cost, g_cost+h_cost, location)
elif findEntry.g_cost > g_cost:
# if position is in openlist and cost is larger than current one,
# then update cost and previous position
findEntry.g_cost = g_cost
findEntry.f_cost = g_cost + h_cost
findEntry.pre_entry = location
getPositions 函数用于获取所有可移动的邻接节点，此处提供两种移动模式：
- 允许上、下、左、右 4 个方向的邻域移动。
- 允许上、下、左、右及左上、右上、左下、右下共 8 个方向的邻域移动。
def getNewPosition(map, locatioin, offset):
x,y = (location.x + offset[0], location.y + offset[1])
if x < 0 or x >= map.width or y < 0 or y >= map.height or map.map[y][x] == 1:
return None
return (x, y)
def getPositions(map, location):
# use four ways or eight ways to move
offsets = [(-1,0), (0, -1), (1, 0), (0, 1)]
#offsets = [(-1,0), (0, -1), (1, 0), (0, 1), (-1,-1), (1, -1), (-1, 1), (1, 1)]
poslist = []
for offset in offsets:
pos = getNewPosition(map, location, offset)
if pos is not None:
poslist.append(pos)
return poslist
isInList 函数用于判断指定节点是否存在于 OPEN 集或 CLOSED 集中。
# check if the position is in list
def isInList(list, pos):
if pos in list:
return list[pos]
return None
calHeuristic 函数目前简单使用曼哈顿距离，后续可优化以更精确。 getMoveCost 函数根据是否为斜向移动来计算移动代价（斜向移动代价约为根号 2，即 1.4）。
# imporve the heuristic distance more precisely in future
def calHeuristic(pos, dest):
return abs(dest.x - pos[0]) + abs(dest.y - pos[1])
def getMoveCost(location, pos):
if location.x != pos[0] and location.y != pos[1]:
return 1.4
else:
return 1
代码的初始化
可调整地图的宽度、高度及障碍节点的数量。 可调整起始节点和目标点的取值范围。
WIDTH = 10
HEIGHT = 10
BLOCK_NUM = 15
map = Map(WIDTH, HEIGHT)
map.createBlock(BLOCK_NUM)
map.showMap()
source = map.generatePos((0,WIDTH//3),(0,HEIGHT//3))
dest = map.generatePos((WIDTH//2,WIDTH-1),(HEIGHT//2,HEIGHT-1))
print("source:", source)
print("dest:", dest)
AStarSearch(map, source, dest)
map.showMap()
执行效果图如下，第一张图表示随机生成的地图，值为 1 的节点代表不可通行。 第二张图中值为 2 的节点表示算法找到的路径。
完整代码
使用 python3.7 编译环境。
from random import randint
class SearchEntry():
def __init__(self, x, y, g_cost, f_cost=0, pre_entry=None):
self.x = x
self.y = y
# cost move form start entry to this entry
self.g_cost = g_cost
self.f_cost = f_cost
self.pre_entry = pre_entry
def getPos(self):
return (self.x, self.y)
class Map():
def __init__(self, width, height):
self.width = width
self.height = height
self.map = [[0 for x in range(self.width)] for y in range(self.height)]
def createBlock(self, block_num):
for i in range(block_num):
x, y = (randint(0, self.width-1), randint(0, self.height-1))
self.map[y][x] = 1
def generatePos(self, rangeX, rangeY):
x, y = (randint(rangeX[0], rangeX[1]), randint(rangeY[0], rangeY[1]))
while self.map[y][x] == 1:
x, y = (randint(rangeX[0], rangeX[1]), randint(rangeY[0], rangeY[1]))
return (x , y)
def showMap(self):
print("+" * (3 * self.width + 2))
for row in self.map:
s = '+'
for entry in row:
s += ' ' + str(entry) + ' '
s += '+'
print(s)
print("+" * (3 * self.width + 2))
def AStarSearch(map, source, dest):
def getNewPosition(map, locatioin, offset):
x,y = (location.x + offset[0], location.y + offset[1])
if x < 0 or x >= map.width or y < 0 or y >= map.height or map.map[y][x] == 1:
return None
return (x, y)
def getPositions(map, location):
# use four ways or eight ways to move
offsets = [(-1,0), (0, -1), (1, 0), (0, 1)]
#offsets = [(-1,0), (0, -1), (1, 0), (0, 1), (-1,-1), (1, -1), (-1, 1), (1, 1)]
poslist = []
for offset in offsets:
pos = getNewPosition(map, location, offset)
if pos is not None:
poslist.append(pos)
return poslist
# imporve the heuristic distance more precisely in future
def calHeuristic(pos, dest):
return abs(dest.x - pos[0]) + abs(dest.y - pos[1])
def getMoveCost(location, pos):
if location.x != pos[0] and location.y != pos[1]:
return 1.4
else:
return 1
# check if the position is in list
def isInList(list, pos):
if pos in list:
return list[pos]
return None
# add available adjacent positions
def addAdjacentPositions(map, location, dest, openlist, closedlist):
poslist = getPositions(map, location)
for pos in poslist:
# if position is already in closedlist, do nothing
if isInList(closedlist, pos) is None:
findEntry = isInList(openlist, pos)
h_cost = calHeuristic(pos, dest)
g_cost = location.g_cost + getMoveCost(location, pos)
if findEntry is None :
# if position is not in openlist, add it to openlist
openlist[pos] = SearchEntry(pos[0], pos[1], g_cost, g_cost+h_cost, location)
elif findEntry.g_cost > g_cost:
# if position is in openlist and cost is larger than current one,
# then update cost and previous position
findEntry.g_cost = g_cost
findEntry.f_cost = g_cost + h_cost
findEntry.pre_entry = location
# find a least cost position in openlist, return None if openlist is empty
def getFastPosition(openlist):
fast = None
for entry in openlist.values():
if fast is None:
fast = entry
elif fast.f_cost > entry.f_cost:
fast = entry
return fast
openlist = {}
closedlist = {}
location = SearchEntry(source[0], source[1], 0.0)
dest = SearchEntry(dest[0], dest[1], 0.0)
openlist[source] = location
while True:
location = getFastPosition(openlist)
if location is None:
# not found valid path
print("can't find valid path")
break;
if location.x == dest.x and location.y == dest.y:
break
closedlist[location.getPos()] = location
openlist.pop(location.getPos())
addAdjacentPositions(map, location, dest, openlist, closedlist)
#mark the found path at the map
while location is not None:
map.map[location.y][location.x] = 2
location = location.pre_entry
WIDTH = 10
HEIGHT = 10
BLOCK_NUM = 15
map = Map(WIDTH, HEIGHT)
map.createBlock(BLOCK_NUM)
map.showMap()
source = map.generatePos((0,WIDTH//3),(0,HEIGHT//3))
dest = map.generatePos((WIDTH//2,WIDTH-1),(HEIGHT//2,HEIGHT-1))
print("source:", source)
print("dest:", dest)
AStarSearch(map, source, dest)
map.showMap()
到此这篇关于 python 实现 A* 寻路算法的文章就介绍到这了,更多相关 python A* 寻路算法内容请搜索本站以前的文章或继续浏览下面的相关文章希望大家以后多多支持本站！
本站声明
本文内容仅供参考，不代表本站观点。转载或引用请注明出处。如发现内容问题，请通过联系我们页面反馈。
