在学习 A* 之前,建议先学习下 Dijkstra 算法
A* 原理
详见参考资料
算法原理没有什么难度,静下心来,你肯定能看懂,时间关系,我就简写了
A* 进阶
A* 算法大概包含两个基础算法:
基础1-启发式搜索
在 已知 起点 s 到 所有当前点(openlist)的距离 g 时,如何选择哪个当前点作为行走目标,用到了启发式搜索(或者叫 贪心策略),即
F = g + h
这个选择 并不能保证路径 全局最优,因为 h 仅仅是估计
基础2-动态规划
动态规划保证了 g 是最优的;
假设 s_1,s_2,s_3...s_n-1,s_n 为最短路径,那么 s_1,s_2,s_3...s_n-1 也一定是最短路径,因为如果存在 s_x 使得 s_1,s_2,s_x...s_n-1 更短,那么 s_1,s_2,s_3...s_n-1,s_n 就不是最短路径;
也就是说 A* 保证了每走一步,从起点到当前点的路径是最短的;
照这个思路,从 起点 走到 终点 的路径 是 全局最优 的;
启发函数
启发函数会影响A*算法的行为。
- 在极端情况下,当启发函数h(n)始终为0,则将由g(n)决定节点的优先级,此时算法就退化成了Dijkstra算法。
- 如果h(n)始终小于等于节点n到终点的代价,则A*算法保证一定能够找到最短路径。但是当h(n)的值越小,算法将遍历越多的节点,也就导致算法越慢。
- 如果h(n)完全等于节点n到终点的代价,则A*算法将找到最佳路径,并且速度很快。可惜的是,并非所有场景下都能做到这一点。因为在没有达到终点之前,我们很难确切算出距离终点还有多远。
- 如果h(n)的值比节点n到终点的代价要大,则A*算法不能保证找到最短路径,不过此时会很快。
- 在另外一个极端情况下,如果h(n)相较于g(n)大很多,则此时只有h(n)产生效果,这也就变成了最佳优先搜索。
由上面这些信息我们可以知道,通过调节启发函数我们可以控制算法的速度和精确度。因为在一些情况,我们可能未必需要最短路径,而是希望能够尽快找到一个路径即可。这也是A*算法比较灵活的地方。
A* 是一种带有启发性质的深度优先搜索,同时具有 死路回退 的性质,总体来讲,它是 广度优先+深度优先
算法优化
其实有很多小操作,有空再补充吧;
这里主要记录几点:
1. 最小二叉堆
2. Lazy Theta 【详见参考资料】
一种动态加权方式
【见论文 _基于改进Astar算法与动态窗口法融合的机器人随机避障方法研究】
示例代码
tm = [
'############################################################',
'#..........................................................#',
'#.............................#............................#',
'#.............................#............................#',
'#.............................#............................#',
'#.......S.....................#............................#',
'#.............................#............................#',
'#.............................#............................#',
'#.............................#............................#',
'#.............................#............................#',
'#.............................#............................#',
'#.............................#............................#',
'#.............................#............................#',
'#######.#######################################............#',
'#....#........#............................................#',
'#....#........#............................................#',
'#....##########............................................#',
'#..........................................................#',
'#..........................................................#',
'#..........................................................#',
'#..........................................................#',
'#..........................................................#',
'#...............................##############.............#',
'#...............................#........E...#.............#',
'#...............................#............#.............#',
'#...............................#............#.............#',
'#...............................#............#.............#',
'#...............................###########..#.............#',
'#..........................................................#',
'#..........................................................#',
'############################################################']
class Node(object):
def __init__(self, x, y, parent):
self.x = x
self.y = y
self.parent = parent # xy坐标,parent父节点
class Astar(object):
def __init__(self, x1, y1, x2, y2):
self.x1, self.y1 = x1, y1 # 起点坐标
self.x2, self.y2 = x2, y2 # 终点坐标
self.openlist = [] # 下一步可以走的路
self.closelist = [] # 已经走过的路
def isinlist(self, nodelist, node):
for i in nodelist:
if i.x == node.x and i.y == node.y:
return i
return
def dist(self, x1, y1, x2, y2):
if x1 == x2 or y1 == y2:
return 1
else:
return 1.4
def t(self):
s = Node(self.x1, self.y1, None) # 起点
s.dist = 0
while True:
# 当前节点的邻接点
sx = (-1, 0, 1, -1, 1, -1, 0, 1)
sy = (-1, -1, -1, 0, 0, 1, 1, 1)
for x, y in zip(sx, sy):
new_x, new_y = s.x + x, s.y + y
# 判断邻接点是否可走:不是障碍物; 不在关闭列表内。
# 如果可走,加入开放列表
if tm[new_y][new_x] == '#': continue # 障碍物不做任何处理
new_node = Node(new_x, new_y, s)
new_node.dist = s.dist + self.dist(s.x, s.y, new_node.x, new_node.y)
if self.isinlist(self.closelist, new_node): continue # 在关闭列表内不做任何处理
# 可行就加入开放列表
isin = self.isinlist(self.openlist, new_node)
if isin: # 已经在开放列表,检测更新g h 父节点
if new_node.dist < isin.dist:
isin.dist = new_node.dist
isin.parent = new_node.parent
else:
# 不在开放列表直接添加
self.openlist.append(new_node)
# 结束循环
if not self.openlist: return
if self.isinlist(self.openlist, Node(self.x2, self.y2, s)):
return s
# 找出F最小的节点
### 注意 F 最小的节点 和 当前节点 s 并无关系
minf = None
min_node = None
for i in self.openlist:
value = i.dist + (abs(i.x - self.x2) + abs(i.y - self.y2))
if minf == None:
minf = value
min_node = i
elif value < minf:
minf = value
min_node = i
else:
pass
print(min_node.parent.x, min_node.parent.y, s.x, s.y)
# 将F最小的节点从开放列表移除,并加入关闭列表
self.openlist.remove(min_node)
self.closelist.append(min_node)
# 起点更新为该节点
s = min_node
def search(self):
s = []
for i in self.openlist:
s.append((i.x, i.y))
for i in self.closelist:
s.append((i.x, i.y))
return s
def path(s):
mypath = []
while s.parent:
mypath.insert(0, (s.x, s.y))
s = s.parent
return mypath
def find_point(s):
for indy, row in enumerate(tm):
for indx, column in enumerate(row):
if column == s:
return indx, indy
def print_path(path, search):
lenx = len(tm[0])
leny = len(tm)
newtm = []
for i in range(leny):
row = ''
for j in range(lenx):
s = tm[i][j]
if (j, i) in search:
s = ' '
if (j, i) in path:
s = 'X'
row += s
newtm.append(row)
return '\n'.join(newtm)
if __name__ == '__main__':
x1, y1 = find_point('S')
x2, y2 = find_point('E')
print(x1, y1, x2, y2)
astar = Astar(x1, y1, x2, y2)
s = astar.t()
print(s.x, s.y)
search = astar.search()
mypath = path(s)
print(mypath)
print(print_path(mypath, search))
输出
############################################################
# X .........#
# X#X .........#
# X # X .........#
# X # X .........#
# XXXXXXXXXXXXXXXXXX # X ........#
# # X ........#
# # X ........#
# # X .......#
# # X .......#
# # X .......#
# # X ......#
# # XXXXXX ......#
####### #######################################X ......#
#....# #.................... X .....#
#....# #.................... X .....#
#....##########.................... X .....#
#.................................. X .....#
#................................. X ....#
#................................. X ....#
#................................. X ....#
#................................. X ....#
#...............................##############X ....#
#...............................#....... ..#X ....#
#...............................#....... X .#X ....#
#...............................#....... X #X ......#
#...............................#........ X #X .......#
#...............................########### X#X ........#
#.......................................... X ..........#
#........................................... ............#
############################################################
参考资料:
https://zhuanlan.zhihu.com/p/54510444 路径规划之 A* 算法
http://www.cppblog.com/christanxw/archive/2006/04/07/5126.html A* 寻路算法
A*算法改进——Any-Angle Path Planning的Theta*算法与Lazy Theta*算法
https://www.zhihu.com/question/39866705/answer/1672207891 为什么A*算法一定能找到最优解?
https://www.zhihu.com/question/436975755/answer/1650953540 在启发式允许的范围内,为什么A*算法是最优的?
https://www.zhihu.com/question/35311316 A*寻路是一种广度优先搜索?