A star 算法介绍与可视化代码实现
今天给大家带来的是寻路算法中常用的一个算法：A star。阅读今天的这篇文章，你将了解A star算法的基本原理，在文章的后半部分，你将学到使用Python的matplotlib进行可视化模拟A star 算法的寻路过程。
A star Demo
如果你只需要源代码，请直接往下翻。
A star 算法介绍
A star 算法是一种在图形平面上，有多个节点的路径，求出最低成本的算法。该算法综合了Best-First Search和Dijkstra算法的优点：在进行启发式搜索提高算法效率的同时，可以保证找到一条基于评估函数的最优路径。
在此算法中，g(n)表示从起点到任意顶点n的实际距离，h(n)表示任意顶点n到目标顶点的估算距离（根据所采用的评估函数的不同而变化），那么A*算法的估算函数为：
f(n)=g(n)+h(n)
由以上公式可以得到：
-
如果g(n)为0，即只计算任意顶点n到目标的评估函数h(n)，而不计算起点到顶点n的距离，则算法转化为使用贪心策略的
Best-First Search，速度最快，但可能得不出最优解； -
如果 h(n)不高于顶点n到目标顶点的实际距离，则一定可以求出最优解，而且h(n)越小，需要计算的节点越多，算法效率越低，常见的评估函数有——欧几里得距离、曼哈顿距离、切比雪夫距离；
-
如果h(n)为0，即只需求出起点到任意顶点 n的最短路径g(n)，而不计算任何评估函数h(n)，则转化为单源最短路径问题，即Dijkstra算法，此时需要计算最多的定点；
关于 A star算法更加详尽的解释，可以参考文章下面的参考链接，此处就不再进行过多的叙述（强烈推荐生动的寻路猫这篇博客）。
可视化代码实现
导入所需要的包
import matplotlib.pyplot as plt
import math
matplotlib是一个python的2D绘图库，可以很轻易的绘制优美的图表。在这个工程中，我们将使用它来实时的可视化A star寻路算法的寻路过程。
创建我们的节点类
class Node:
def __init__(self, x, y, cost, pind):
self.x = x
self.y = y
self.cost = cost
self.pind = pind
def __str__(self):
return str(self.x) + "," + str(self.y) + "," + str(self.cost) + "," + str(self.pind)
我们这个工程是使用A star算法在2D网格图中进行路径规划，因此首先我们需要构建一个class用以表示构成网格地图的每一个节点。
接着我们需要设定障碍物节点的集合，然后计算出网格地图。
设置障碍物
# Create the Obstacles node list
for i in range(60):
ox.append(i)
oy.append(0.0)
for i in range(60):
ox.append(60.0)
oy.append(i)
for i in range(61):
ox.append(i)
oy.append(60.0)
for i in range(61):
ox.append(0.0)
oy.append(i)
for i in range(40):
ox.append(20.0)
oy.append(i)
for i in range(40):
ox.append(40.0)
oy.append(60.0 - i)
ox，oy分别表示障碍物（Obstacles）的位置坐标，最终生成的障碍物如下图所示：
Obstacles
让计算机理解我们的地图
在前面我们设置了障碍物是哪些坐标，并且可视化了输出的地图，貌似挺帅的，下面我们得让计算机理解我们这个图是什么含义，即地图的大小，地图每一小块的是否可以行走。
我们定义了下面的函数用以完成上述功能：
def calc_obstacle_map(ox, oy, reso, vr):
print("[INFO] generating obstacle map")
minx = round(min(ox))
miny = round(min(oy))
maxx = round(max(ox))
maxy = round(max(oy))
print("[INFO] minx:", minx)
print("[INFO] miny:", miny)
print("[INFO] maxx:", maxx)
print("[INFO] maxy:", maxy)
xwidth = round(maxx - minx)
ywidth = round(maxy - miny)
print("[INFO] xwidth:", xwidth)
print("[INFO] ywidth:", ywidth)
# obstacle map generation
obmap = [[False for i in range(xwidth)] for i in range(ywidth)]
for ix in range(xwidth):
x = ix + minx
for iy in range(ywidth):
y = iy + miny
# print(x, y)
for iox, ioy in zip(ox, oy):
d = math.sqrt((iox - x)**2 + (ioy - y)**2)
if d <= vr / reso:
obmap[ix][iy] = True
break
# print ("[INFO] obmap: \n{0}".format(obmap))
print("[INFO] generated the obstacle map")
return obmap, minx, miny, maxx, maxy, xwidth, ywidth
def verify_node(node, obmap, minx, miny, maxx, maxy):
if node.x < minx:
return False
elif node.y < miny:
return False
elif node.x >= maxx:
return False
elif node.y >= maxy:
return False
if obmap[node.x][node.y]:
return False
return True
运行A star算法
计算节点索引
最终得到的obmap是由True和False组成的一串编码，用于描述地图的障碍物。由此计算机便理解了我们构建的地图，接下来我们就可以开始实现我们的A star寻路算法啦。
在正式运行A star算法之前，我们需要给得到没一个节点的索引，于是我们定义了下面的这个函数用于计算每一个点的索引。可以看出，几点索引的范围是[0, 36660]。
def calc_index(node, xwidth, xmin, ymin):
# （0, 60x60+60 = 3660）。
return (node.y - ymin) * xwidth + (node.x - xmin)
定义运动模式
在前面我们定义了一些静态的东西，接下来我们需要定义与运动相关的--运动模式，由于采用的是Euclid距离计算H值，因此这里需要定义8个方向的运动模式以及相应的代价。
def get_motion_model():
# dx, dy, cost
motion = [[1, 0, 1],
[0, 1, 1],
[-1, 0, 1],
[0, -1, 1],
[-1, -1, math.sqrt(2)],
[-1, 1, math.sqrt(2)],
[1, -1, math.sqrt(2)],
[1, 1, math.sqrt(2)]]
return motion
def calc_heuristic(n1, n2):
# 2D Euclid distance
w = 1.0 # weight of heuristic
d = w * math.sqrt((n1.x - n2.x)**2 + (n1.y - n2.y)**2)
return d
执行A star！
在作好了相应从准备之后，我们开始正式的执行A star算法，接下来i的代码都被封装在一个 a_star_planning函数中，下面让我们一步步完成它。
def a_star_planning(sx, sy, gx, gy, ox, oy, reso, rr):
"""
gx: goal x position [m]
gy: goal y position [m]
ox: x position list of Obstacles [m]
oy: y position list of Obstacles [m]
reso: grid resolution [m]
rr: robot radius[m]
"""
nstart = Node(round(sx / reso), round(sy / reso), 0.0, -1)
ngoal = Node(round(gx / reso), round(gy / reso), 0.0, -1)
ox = [iox / reso for iox in ox]
oy = [ioy / reso for ioy in oy]
obmap, minx, miny, maxx, maxy, xw, yw = calc_obstacle_map(ox, oy, reso, rr)
motion = get_motion_model()
print ("[INFO] motion: {0}".format(motion))
openset, closedset = dict(), dict()
openset[calc_index(nstart, xw, minx, miny)] = nstart
我们首先是将 起始节点（nstart）和目标节点（ngoal）实例化，并且通过calc_obstacle_map加载我们构建好了的网格地图，通过get_motion_model得到我们定义好了的运动模式。随后定义两个空dict openset, closedset，其中openset中的点是会被考虑的点而closedset中的点是不会被考虑的。最后并且将起始节点放入 openset中。
不言而喻，最终我们将进入一个死循环，直到找到最优路径：
while True:
# min (F)
c_id = min(
openset, key=lambda o: openset[o].cost + calc_heuristic(ngoal, openset[o]))
current = openset[c_id]
# show graph
if show_animation:
plt.plot(current.x * reso, current.y * reso, "xc")
if len(closedset.keys()) % 10 == 0:
plt.pause(0.001)
# goal
if current.x == ngoal.x and current.y == ngoal.y:
print("Find goal")
ngoal.pind = current.pind
ngoal.cost = current.cost
break
# Remove the item from the open set
del openset[c_id]
# Add it to the closed set
closedset[c_id] = current
# expand search grid based on motion model
for i in range(len(motion)):
node = Node(current.x + motion[i][0],
current.y + motion[i][1],
current.cost + motion[i][2], c_id)
n_id = calc_index(node, xw, minx, miny)
if n_id in closedset:
continue
# 位置是否合法
if not verify_node(node, obmap, minx, miny, maxx, maxy):
continue
if n_id not in openset:
openset[n_id] = node # Discover a new node
tcost = current.cost + calc_heuristic(current, node)
if tcost >= node.cost:
continue # this is not a better path
node.cost = tcost
openset[n_id] = node # This path is the best unitl now. record it!
rx, ry = calc_fianl_path(ngoal, closedset, reso)
return rx, ry
我们首先是通过以下代码段计算最小的F， 并且不断绘制图像。
# min (F)
c_id = min(
openset, key=lambda o: openset[o].cost + calc_heuristic(ngoal, openset[o]))
current = openset[c_id]
# show graph
if show_animation:
plt.plot(current.x * reso, current.y * reso, "xc")
if len(closedset.keys()) % 10 == 0:
plt.pause(0.001)
每一次循环，都会判断是否是目标节点：
# goal
if current.x == ngoal.x and current.y == ngoal.y:
print("Find goal")
ngoal.pind = current.pind
ngoal.cost = current.cost
break
在找到最小F值所对应的当前节点索引c_id之后，需要将他从openset中删除，并且加入到closedset中。
# Remove the item from the open set
del openset[c_id]
# Add it to the closed set
closedset[c_id] = current
随后我们会依据运动模式，进行搜索节点，以获取新的节点，得到新的节点的索引n_id，在位置合法并且不在closedset情况下，加入到openset中，随后进行cost的判断与更新。
# expand search grid based on motion model
for i in range(len(motion)):
node = Node(current.x + motion[i][0],
current.y + motion[i][1],
current.cost + motion[i][2], c_id)
n_id = calc_index(node, xw, minx, miny)
if n_id in closedset:
continue
# 位置是否合法
if not verify_node(node, obmap, minx, miny, maxx, maxy):
continue
if n_id not in openset:
openset[n_id] = node # Discover a new node
tcost = current.cost + calc_heuristic(current, node)
if tcost >= node.cost:
continue # this is not a better path
node.cost = tcost
openset[n_id] = node # This path is the best unitl now. record it!
如此循环，不断的依据运动模型扩展新的节点，不断的更新新的cost的值，最终得到closeset。随后我们定义一个函数来计算最终生成的路径。
def calc_fianl_path(ngoal, closedset, reso):
# generate final course
rx, ry = [ngoal.x * reso], [ngoal.y * reso]
pind = ngoal.pind
while pind != -1:
n = closedset[pind]
rx.append(n.x * reso)
ry.append(n.y * reso)
pind = n.pind
return rx, ry
由此我们便找到了最终生成的路径点的集合，接下来我们需要在main函数中将它们绘制出来
def main():
print(__file__ + " start!!")
# start and goal position
sx = 10.0 # [m]
sy = 10.0 # [m]
gx = 50.0 # [m]
gy = 50.0 # [m]
grid_size = 1.0 # [m]
robot_size = 1.0 # [m]
ox, oy = [], []
# Create the Obstacles node list
for i in range(60):
ox.append(i)
oy.append(0.0)
for i in range(60):
ox.append(60.0)
oy.append(i)
for i in range(61):
ox.append(i)
oy.append(60.0)
for i in range(61):
ox.append(0.0)
oy.append(i)
for i in range(40):
ox.append(20.0)
oy.append(i)
for i in range(40):
ox.append(40.0)
oy.append(60.0 - i)
if show_animation:
plt.plot(ox, oy, ".k")
plt.plot(sx, sy, "xr")
plt.plot(gx, gy, "xb")
plt.grid(True)
plt.axis("equal")
rx, ry = a_star_planning(sx, sy, gx, gy, ox, oy, grid_size, robot_size)
if show_animation:
plt.plot(rx, ry, "-r")
plt.show()
总结
最终实现的效果图如下图所示：
A star Demo
其中红色是最终生成的路径，黑色为障碍物，蓝色为寻路算法寻找的节点。由此，通过这篇文章，我们介绍了A star算法的基本原理，计算过程，并提供和分析了一个用于可视化的Python工程。源代码可以访问 源代码
如果你觉得这篇文章对你有用，请务必分享！
参考链接
不足之处，敬请斧正; 若你觉得文章还不错，请分享。或者扫码关注公众号继续支持我们。
image
