ARA*（Anytime A* with Provable Bounds on Sub-Optimality）代码详解
在此之前我们看完了A* 与双向A* 算法，随后我们继续看A* 的另外一种改进版本：ARA* 。
首先，在此之前我们先了解两个概念：WA* 与AA* 。
WA*
WA* 全称Weighted A* 。顾名思义，就是加权A* ，回顾一下经典算法的代价函数：
f ( n ) = g ( n ) + h ( n ) f(n)=g(n)+h(n) f(n)=g(n)+h(n)
Weighted A* 则是在A* 的基础上设计了一个权重因子w。
f ( n ) = g ( n ) + w ∗ h ( n ) f(n)=g(n)+w*h(n) f(n)=g(n)+w∗h(n)
给定w不同的参数，算法得到不同的搜索方式，极端的，当其为0时，算法退化为Dijkstra；当其为1时，算法退化为A*。
例如在算来的A* 算法基础上，将代价函数分别给予1.5以及2的权重因子，得到不同结果如下：
1.5：
2.0：
使用合适的权重因子可以在一定程度上提高搜索效率。
AA*
在寻路算法中，实时性非常重要，因此对路径规划有严格的时间要求时，比如限制10ms内必须出一次寻路结果。解决这种限时寻路的问题的算法就叫Anytime。
据说解决这个问题的第一个A* based算法是 AA* (Multiple sequence alignment using A*)，其基本原理如下：
这里最重要的地方在于最后两行，它似乎对点进行了一定的处理，但是按照我个人的理解：
如果这个点属于close集合，即这个点走过，则会将其从close中消除，即再走一次。那不就是跟A* 一毛一样嘛。不知道有没有理解的小伙伴解惑一下。
ARA*
ARA* 全称《Anytime A* with Provable Bounds on Sub-Optimality》。它是A* 算法的变种，用于解决最短路径问题。ARA* 算法在搜索过程中逐渐减小启发函数的权重，从而在有限时间内找到一个次优解在后续迭代中逐渐优化解。其基本原理如下：
1.设置两个集合：开放列表(open list)和关闭列表(closed list)。开放列表存放待探索的节点，关闭列表存放已探索的节点。
2.将起始节点放入开放列表，并初始化该节点的启发函数值（估计到目标节点的距离）和路径代价值。设定一个初始的启发函数权重。
3.从开放列表中选择启发函数值+路径代价值最小的节点作为当前节点，并将其移入关闭列表。
4.对当前节点的相邻节点进行探索，算它们的估计值、路径代价值以及综合估值（启发函数与路径代价值的和）。
5.若相邻节点尚未在开放列表或关闭列表中，将其默认代价值设位无穷大(用于下一步比较)。
6.若第4步计算得到的代价值比原来的值要小，更新代价值，同时如果这个点未遍历过，将其加入open list进行遍历，如果这个点已经遍历过一次，则加入incons list。
7.重复步骤3至6，直到到达目标节点或者开放列表为空（表示无可达路径）。
8.若搜索时间未耗尽，按照一定规则减小启发函数权重，并清空开放列表和关闭列表，返回步骤2进行一次迭代。
论文中的伪代码如下：
ARA*算法之所以快速的原因就是在于它的第6步。对于已经遍历过的点，不会再去遍历第二遍，即使可能再遍历一遍可以找到更优解。这样，算法就可以快速的先找出一条可行解。
同时，ARA* 算法的自适应性在于它的主函数中对于它对于权重的更新，根据第5行以及第13行我们可以看到每次执行搜索后都会判断是否找到更优解，如果有的话就会更新权重，直到再次搜索后没有比当前解更优的结果时，算法会停止并跳出。
代码示例：
import os
import sys
import math
import heapq
import matplotlib.pyplot as plt
class AraStar:
def __init__(self, s_start, s_goal, e, heuristic_type, xI, xG):
self.s_start, self.s_goal = s_start, s_goal
self.heuristic_type = heuristic_type
self.u_set = [(-1, 0), (-1, 1), (0, 1), (1, 1),
(1, 0), (1, -1), (0, -1), (-1, -1)] # feasible input set
self.e = e # weight
self.g = dict() # Cost to come
self.OPEN = dict() # priority queue / OPEN set
self.CLOSED = set() # CLOSED set
self.INCONS = {} # INCONSISTENT set
self.PARENT = dict() # relations
self.path = [] # planning path
self.visited = []
self.x_range = 51 # size of background
self.y_range = 31
self.motions = [(-1, 0), (0, 1),
(1, 0), (0, -1)]
self.obs = self.obs_map()
self.xI, self.xG = xI, xG
def update_obs(self, obs):
self.obs = obs
def obs_map(self):
"""
Initialize obstacles' positions
:return: map of obstacles
"""
x = 51
y = 31
obs = set()
for i in range(x):
obs.add((i, 0))
for i in range(x):
obs.add((i, y - 1))
for i in range(y):
obs.add((0, i))
for i in range(y):
obs.add((x - 1, i))
for i in range(10, 21):
obs.add((i, 15))
for i in range(15):
obs.add((20, i))
for i in range(15, 30):
obs.add((30, i))
for i in range(16):
obs.add((40, i))
return obs
def update_obs(self, obs):
self.obs = obs
def animation(self, path, visited, name):
self.plot_grid(name)
self.plot_visited(visited)
self.plot_path(path)
plt.show()
def animation_ara_star(self, path, visited, name):
self.plot_grid(name)
cl_v, cl_p = self.color_list()
for k in range(len(path)):
self.plot_visited(visited[k], cl_v[k])
self.plot_path(path[k], cl_p[k], True)
plt.pause(0.5)
plt.show()
def plot_grid(self, name):
obs_x = [x[0] for x in self.obs]
obs_y = [x[1] for x in self.obs]
plt.plot(self.xI[0], self.xI[1], "bs")
plt.plot(self.xG[0], self.xG[1], "gs")
plt.plot(obs_x, obs_y, "sk")
plt.title(name)
plt.axis("equal")
def plot_visited(self, visited, cl='gray'):
if self.xI in visited:
visited.remove(self.xI)
if self.xG in visited:
visited.remove(self.xG)
count = 0
for x in visited:
count += 1
plt.plot(x[0], x[1], color=cl, marker='o')
plt.gcf().canvas.mpl_connect('key_release_event',
lambda event: [exit(0) if event.key == 'escape' else None])
if count < len(visited) / 3:
length = 20
elif count < len(visited) * 2 / 3:
length = 30
else:
length = 40
#
# length = 15
if count % length == 0:
plt.pause(0.001)
plt.pause(0.01)
def plot_path(self, path, cl='r', flag=False):
path_x = [path[i][0] for i in range(len(path))]
path_y = [path[i][1] for i in range(len(path))]
if not flag:
plt.plot(path_x, path_y, linewidth='3', color='r')
else:
plt.plot(path_x, path_y, linewidth='3', color=cl)
plt.plot(self.xI[0], self.xI[1], "bs")
plt.plot(self.xG[0], self.xG[1], "gs")
plt.pause(0.01)
@staticmethod
def color_list():
cl_v = ['silver',
'wheat',
'lightskyblue',
'royalblue',
'slategray']
cl_p = ['gray',
'orange',
'deepskyblue',
'red',
'm']
return cl_v, cl_p
def init(self):
"""
initialize each set.
"""
self.g[self.s_start] = 0.0
self.g[self.s_goal] = math.inf
self.OPEN[self.s_start] = self.f_value(self.s_start)
self.PARENT[self.s_start] = self.s_start
def searching(self):
self.init()
self.ImprovePath()
self.path.append(self.extract_path())
count = 1
while self.update_e() > 1: # continue condition
print(count)
count += 1
self.e -= 0.4 # increase weight
self.OPEN.update(self.INCONS)
self.OPEN = {s: self.f_value(s) for s in self.OPEN} # update f_value of OPEN set
self.INCONS = dict()
self.CLOSED = set()
self.ImprovePath() # improve path
self.path.append(self.extract_path())
return self.path, self.visited
def ImprovePath(self):
"""
:return: a e'-suboptimal path
"""
"""
:return: a e'-suboptimal path
"""
visited_each = []
count = 1
while True:
print(count)
count += 1
s, f_small = self.calc_smallest_f()
if self.f_value(self.s_goal) <= f_small:
break
self.OPEN.pop(s)
self.CLOSED.add(s)
for s_n in self.get_neighbor(s):
if s_n in self.obs:
continue
new_cost = self.g[s] + self.cost(s, s_n)
if s_n not in self.g or new_cost < self.g[s_n]:
self.g[s_n] = new_cost
self.PARENT[s_n] = s
visited_each.append(s_n)
if s_n not in self.CLOSED:
self.OPEN[s_n] = self.f_value(s_n)
else:
self.INCONS[s_n] = 0.0
self.visited.append(visited_each)
def calc_smallest_f(self):
"""
:return: node with smallest f_value in OPEN set.
"""
s_small = min(self.OPEN, key=self.OPEN.get)
return s_small, self.OPEN[s_small]
def get_neighbor(self, s):
"""
find neighbors of state s that not in obstacles.
:param s: state
:return: neighbors
"""
return {(s[0] + u[0], s[1] + u[1]) for u in self.u_set}
def update_e(self):
v = float("inf")
if self.OPEN:
v = min(self.g[s] + self.h(s) for s in self.OPEN)
if self.INCONS:
v = min(v, min(self.g[s] + self.h(s) for s in self.INCONS))
return min(self.e, self.g[self.s_goal] / v)
def f_value(self, x):
"""
f = g + e * h
f = cost-to-come + weight * cost-to-go
:param x: current state
:return: f_value
"""
return self.g[x] + self.e * self.h(x)
def extract_path(self):
"""
Extract the path based on the PARENT set.
:return: The planning path
"""
path = [self.s_goal]
s = self.s_goal
while True:
s = self.PARENT[s]
path.append(s)
if s == self.s_start:
break
return list(path)
def h(self, s):
"""
Calculate heuristic.
:param s: current node (state)
:return: heuristic function value
"""
heuristic_type = self.heuristic_type # heuristic type
goal = self.s_goal # goal node
if heuristic_type == "manhattan":
return abs(goal[0] - s[0]) + abs(goal[1] - s[1])
else:
return math.hypot(goal[0] - s[0], goal[1] - s[1])
def cost(self, s_start, s_goal):
"""
Calculate Cost for this motion
:param s_start: starting node
:param s_goal: end node
:return: Cost for this motion
:note: Cost function could be more complicate!
"""
if self.is_collision(s_start, s_goal):
return math.inf
return math.hypot(s_goal[0] - s_start[0], s_goal[1] - s_start[1])
def is_collision(self, s_start, s_end):
"""
check if the line segment (s_start, s_end) is collision.
:param s_start: start node
:param s_end: end node
:return: True: is collision / False: not collision
"""
if s_start in self.obs or s_end in self.obs:
return True
if s_start[0] != s_end[0] and s_start[1] != s_end[1]:
if s_end[0] - s_start[0] == s_start[1] - s_end[1]:
s1 = (min(s_start[0], s_end[0]), min(s_start[1], s_end[1]))
s2 = (max(s_start[0], s_end[0]), max(s_start[1], s_end[1]))
else:
s1 = (min(s_start[0], s_end[0]), max(s_start[1], s_end[1]))
s2 = (max(s_start[0], s_end[0]), min(s_start[1], s_end[1]))
if s1 in self.obs or s2 in self.obs:
return True
return False
def main():
s_start = (5, 5)
s_goal = (45, 25)
arastar = AraStar(s_start, s_goal, 2.5, "euclidean",s_start, s_goal)
path, visited = arastar.searching()
arastar.animation_ara_star(path, visited, "Anytime Repairing A* (ARA*)")
if __name__ == '__main__':
main()
效果如下：
如上图算法开始时会先快速找到一条路径，这条路径一共遍历了224个点。而同样的加权A*算法遍历一遍需要遍历494次。
然后算法对权重进行收敛，进行新一轮计算，最终得到结果如下：
算法一共执行了962次循环，实际上而言ARA* 如果在条件满足的情况下，迭代的次数会超过A* 算法本身。而它的优点则是可以在任意时间段内快速返回一条轨迹，如果时间允许的话，会优化这个轨迹，相比于A* ,它对于时间的鲁棒性更强。
参考：
《路径规划算法2.1 A算法变种 ARA、LPA*、 D*、D* Lite》
《A*算法及其变种》