我宁愿永远做我自己，也不愿成为别人，即使那个人比你更快乐。
—《成为简·奥斯汀》
🏰代码及环境配置：请参考 环境配置和代码运行!
4.5.1 概述
Dijkstra算法是基于广度优先搜索策略来遍历空间内的所有节点，最终计算出全局最优的路径，其计算量非常大。而基于启发式的贪婪最佳优先搜索（greedy best first search，GBFS）速度快，但结果可能不是最优的。那么，如何将二者的优势结合呢，即在Dijkstra算法基础上，引入启发式策略。这就是A*算法。
🌟**Note：**最佳优先搜索算法是在广度优先搜索的基础上，用启发估价函数对将要被遍历到的点进行估价，然后选择代价小的进行遍历，直到找到目标节点或者遍历完所有点，算法结束。
4.5.2 算法详解
A*算法结合了GBFS算法和Dijkstra算法的优点，通过评估函数
f
(
n
)
f(n)
f(n)来指导搜索过程，
f
(
n
)
f(n)
f(n)定义为从起点到节点n 的实际代价
g
(
n
)
g(n)
g(n)与从节点n 到目标节点的估计代价
h
(
n
)
h(n)
h(n)之和，即
f
(
n
)
=
g
(
n
)
+
h
(
n
)
f(n)=g(n) + h(n)
f(n)=g(n)+h(n)。
-
g
(
n
)
g(n)
g(n)：从起点到节点
n的实际代价（通常是距离或消耗）。 -
h
(
n
)
h(n)
h(n)：从节点
n到目标节点的估计代价（启发式函数）。
🌟**Note：**为了确保 g ( n ) g(n) g(n)和 h ( n ) h(n) h(n)的相加有意义，这两个值必须使用相同的衡量单位。
4.5.1.1 启发式函数
启发式函数可以控制A*的行为：
- 一种极端情况，如果 h ( n ) h(n) h(n)是0，则只有 g ( n ) g(n) g(n)起作用，此时A*演变成Dijkstra算法，计算量增大，但可以确保找到最优路径。
- 如果
h
(
n
)
h(n)
h(n)经常都比从节点
n移动到目标节点的实际代价小（或者相等），则A可以保证能找到一条最优路径。 h ( n ) h(n) h(n)越小，A扩展的节点越多，计算量越大，运行得越慢。 - 如果
h
(
n
)
h(n)
h(n)精确地等于从节点
n移动到目标节点的代价，则A*将会仅仅寻找最优路径而不扩展别的任何结点，这会运行得非常快。尽管这不可能在所有情况下发生，但仍可以在一些特殊情况下让它们精确地相等。 - 如果
h
(
n
)
h(n)
h(n)有时比从节点
n移动到目标节点的实际代价高，则A*不能保证找到一条最优路径，但它运行得更快。 - 另一种极端情况，如果 h ( n ) h(n) h(n)比 g ( n ) g(n) g(n)大很多，则只有 h ( n ) h(n) h(n)起作用，A*演变成GBFS算法。
下图可以清晰的看出不同的 h ( n ) h(n) h(n)对算法的影响（图中绿色的为起始点，红色的为目标点）：
- a = 1 , b = 0 a=1, b=0 a=1,b=0：如图Fig.1，此时A*算法变成了Dijkstra算法，虽然可以得到最优解，但是扩展了非常多的节点，计算量很大。
- a = 0 , b = 1 a=0,b=1 a=0,b=1：如图Fig.2，此时A*算法变成了GBFS算法，计算量减少，但可能找不到最优解。
- a = 1 , b = 1 a=1,b=1 a=1,b=1：如图Fig.3，即为A*算法，平衡了计算量和最优解之间的关系。
🌟Note: 图中的算法展示来源于：https://qiao.github.io/PathFinding.js/visual/
经过分析，可以看到在A*算法中，启发式函数 h ( n ) h(n) h(n)扮演着及其重要的角色，以下是一些常见的启发式函数类型：
- 曼哈顿距离（Manhattan Distance）：
- 曼哈顿距离是A*算法中常用的启发式函数之一。它计算的是两点在标准坐标系上的绝对轴距总和，即只能沿着坐标轴（或网格的边界）移动的距离。对于网格地图，曼哈顿距离非常适合，因为它反映了实际移动中的限制（如只能上下左右移动）。
- 公式： h ( n ) = ∣ x n − x g o a l ∣ + ∣ y n − y g o a l ∣ h(n)=\left | x_{n} - x_{goal} \right | + \left | y_{n} - y_{goal} \right | h(n)=∣xn−xgoal∣+∣yn−ygoal∣，其中 ( x n , y n ) (x_{n},y_{n}) (xn,yn)和 ( x g o a l , y g o a l ) (x_{goal},y_{goal}) (xgoal,ygoal)分别是当前点和目标点的坐标。
- 欧几里得距离（Euclidean Distance）：
- 欧几里得距离是两点之间的直线距离，在平面直角坐标系中，它可以通过勾股定理计算得到。虽然欧几里得距离在物理上更准确，但在网格地图中，由于只能沿网格线移动，它可能不总是反映实际的最短路径。然而，在某些情况下，为了简化计算或适应特定需求，也可以使用欧几里得距离作为启发式函数。
- 公式： h ( n ) = ( x n − x g o a l ) 2 + ( y n − y g o a l 2 ) h(n)=\sqrt{(x_{n} - x_{goal})^{2} + (y_{n} - y_{goal}^{2} )} h(n)=(xn−xgoal)2+(yn−ygoal2)
- 切比雪夫距离（Chebyshev Distance）：
- 切比雪夫距离是各坐标数值差的最大值，也称为棋盘距离或 L ∞ L\infty L∞ 度量。在网格地图中，它表示从一个点到另一个点所需改变的最大坐标值（即沿任一坐标轴移动的最大步数）。虽然不如曼哈顿距离常用，但在某些特定场景下，切比雪夫距离也能作为有效的启发式函数。
- 公式： h ( n ) = m a x ( ∣ x n − x g o a l ∣ , ∣ y n − y g o a l ∣ ) h(n)=max(\left | x_{n} - x_{goal} \right |, \left | y_{n} - y_{goal} \right |) h(n)=max(∣xn−xgoal∣,∣yn−ygoal∣)
- 自定义启发式函数：
- 除了上述常见的启发式函数外，还可以根据具体问题设计自定义的启发式函数。自定义启发式函数可以更加精确地反映问题的实际情况，从而提高搜索效率和准确性。然而，设计有效的自定义启发式函数需要深入了解问题的本质和特性。
4.5.1.2 算法步骤
其算法步骤如下：
- 初始化：
- 创建一个数组
g[]，其中g[i]表示从源节点start到节点i的最小cost，初始时，源节点的cost为0。 - 创建一个数组
f[]，其中f[i]表示从源节点start到节点i的最小cost + 节点i到目标节点的启发式cost。 - 创建一个布尔数组
visited[]来跟踪每个节点是否已被访问过，初始时，所有顶点都未访问 - 创建一个优先队列
open_list，用于根据cost选择下一个要处理的节点。优先队列中的每个元素是一个包含顶点和cost的配对，初始时将源节点start和其f cost加入优先队列。
- 创建一个数组
- 循环处理优先队列中的节点：
- 从优先队列中取出f cost最小的节点
v，并将节点v标记为已访问，若节点v就是目标节点，则提前返回。 - 遍历节点
v中所有未被访问过的邻接节点，对于每一个邻接节点next：- 计算邻接节点
next的g cost和h cost。 - 若邻接节点
next不在优先队列中，则将邻接节点next和其对应的f cost加入优先队列，并在数组g[]和f[]中设置分别设置邻接节点next的g cost和f cost，最后将临接节点next的父节点设为v。 - 若邻接节点
next在优先队列中，则在数组g[]，f[]和优先队列open_list中更新节点next的相关信息，最后将临接节点next的父节点设为v。
- 计算邻接节点
- 继续处理优先队列中的顶点，直到队列为空或所有顶点都被访问。
- 从优先队列中取出f cost最小的节点
- 从目标节点
goal开始，通过父节点向回追溯，直到起始节点，最终得到一条路径。
其伪代码如下：
Algorithm A star(G, start, goal):
let **open_list** be a priority_queue
g[**start**] = 0
f[**start**] = g[**start**] + h[**start**]
**open_list.**push(**start,** f[**start**])
while **open_list** is not empty do
v = **open_list**.pop()
mark v as visted
if v is the **goal**:
return v
for **all unvisited neighbours** next of v in Graph G do
next_g_cost = g[v] + cost(v, next)
next_h_cost = h[next]
if next is not in **open_list**:
g[next] = next_g_cost
f[next] = next_g_cost + next_h_cost
**open_list**.push(next, f[next])
next.parent = v
else:
if g[next] > next_g_cost:
g[next] = next_g_cost
f[next] = next_g_cost + next_h_cost
**open_list**.**update_priority**(next, f[next])
next.parent = v
4.5.1.3 算法图解
以从下面的无向图中寻找出节点A 到节点E的最短路径为例，其中两个节点之间的权重表示他们之间的距离（或者cost），节点旁边的数字表示预定义的启发项，并初始话两个表：visited nodes info和unvisited nodes info。
所有节点的初始cost如下：
- 源节点到自身的g-cost为0，到目标节点的h-cost为6，因此源节点的f-cost为6。
- 源节点到其它节点的cost还没有确定，暂时先标记为无穷大。
- 从源节点
A开始，将其加入到已访问列表中，并从未访问列表中删除。同时在未访问列表中更新源节点A的邻近节点（节点B和节点C）的信息。- 节点
B：节点B最短路径为A —> B，g-cost为1.5，h-cost为5，f-cost为6.5，父节点为A - 节点
C：节点C最短路径为A —> C，g-cost为2，h-cost为3，f-cost为5，父节点为A
- 节点
- 查看未访问列表，节点
C的cost最小，因此将其加入到已访问列表中，并从未访问列表中删除。同时在未访问列表中更新节点C的邻近节点（节点E）的信息。- 节点
E：节点E最短路径为A —> C —> E，g-cost为5，h-cost为0，f-cost为5，父节点为C。
- 节点
- 查看未访问列表，节点
E的cost最小，因此将其加入到已访问列表中，并从未访问列表中删除，此时节点E为目标节点，因此搜索结束，经过回溯父节点，节点A到节点E最短路径为：A —> C —> E。
4.5.3 优缺点
- 优点
- 高效性：A*算法结合了最佳优先搜索（Best-First Search）和Dijkstra算法的优点，通过启发式函数（heuristic function）来评估节点的优先级，从而能够快速找到从起点到终点的最短路径。
- 最优性：在启发式函数满足一定条件（如 h ( n ) h(n) h(n)始终小于等于节点 n n n到目标节点的实际代价）的情况下，A*算法能够保证找到从起点到终点的最短路径。
- 缺点
- 启发式函数的选择：虽然A*算法允许选择不同的启发式函数，但启发式函数的选择会直接影响算法的性能和结果。如果启发式函数选择不当，可能会导致算法无法找到最短路径或搜索效率降低。
4.5.c A*代码解析
本节提供了A*算法的代码测试：
python3 tests/search_based_planning/a_star_test.py
4.5.c.1 构图的代码实现
基于图搜的运动规划中最重要的一步是构图，构建的图比较简单，主要包含map border和obstacles，读者也可根据需求修改构图方式。
def construct_env_info():
border_x = []
border_y = []
ox = []
oy = []
# map border.
for i in range(-10, 60):
border_x.append(i)
border_y.append(-10.0)
for i in range(-10, 60):
border_x.append(60.0)
border_y.append(i)
for i in range(-10, 61):
border_x.append(i)
border_y.append(60.0)
for i in range(-10, 61):
border_x.append(-10.0)
border_y.append(i)
# Obstacle 1.
for i in range(40, 55, 1):
for j in range(5, 15, 1):
ox.append(i)
oy.append(j)
# Obstacle 2.
for i in range(40):
for j in range(20, 25, 1):
ox.append(j)
oy.append(i)
# Obstacle 3.
for i in range(30):
for j in range(40, 45, 1):
ox.append(j)
oy.append(58.0 - i)
# Obstacle 4.
for i in range(0, 20, 1):
for j in range(35, 40, 1):
ox.append(i)
oy.append(j)
return border_x, border_y, ox, oy
4.5.c.2 A*的代码实现
在A*算法中，首先我们定义了节点Node ，这是图的基础元素。其中（x, y）表示节点的位置，cost表示从源节点到当前节点的cost，parent 表示当前节点的父节点。
class Node:
def __init__(self, x, y, cost, parent_index):
self.x = x
self.y = y
self.cost = cost
self.parent_index = parent_index
def __str__(self):
return (
str(self.x)
+ ","
+ str(self.y)
+ ","
+ str(self.cost)
+ ","
+ str(self.parent_index)
)
在A*中，启发式函数使用的是欧式距离，如下：
def calc_heuristic(n1, n2):
w = 1.0 # weight of heuristic
d = w * math.hypot(n1.x - n2.x, n1.y - n2.y)
return d
下面是A*的核心算法，基于环境信息，起始点位置，目标点位置搜到最优路径，其中：
sx:起始点的x坐标的值
sy:起始点的y坐标的值
gx:目标点的x坐标的值
gy:目标点的y坐标的值
最终返回一条路径：rx, ry
def planning(self, sx, sy, gx, gy):
start_node = self.Node(
self.calc_xy_index(sx, self.min_x),
self.calc_xy_index(sy, self.min_y),
0.0,
-1,
)
goal_node = self.Node(
self.calc_xy_index(gx, self.min_x),
self.calc_xy_index(gy, self.min_y),
0.0,
-1,
)
open_set, closed_set = dict(), dict()
open_set[self.calc_grid_index(start_node)] = start_node
while True:
if len(open_set) == 0:
print("Open set is empty..")
break
c_id = min(
open_set,
key=lambda o: open_set[o].cost
+ self.calc_heuristic(goal_node, open_set[o]),
)
current = open_set[c_id]
# show graph
if show_animation:
plt.plot(
self.calc_grid_position(current.x, self.min_x),
self.calc_grid_position(current.y, self.min_y),
"xc",
)
# for stopping simulation with the esc key.
plt.gcf().canvas.mpl_connect(
"key_release_event",
lambda event: [exit(0) if event.key == "escape" else None],
)
if len(closed_set.keys()) % 10 == 0:
plt.pause(0.001)
plt.savefig(gif_creator.get_image_path())
if current.x == goal_node.x and current.y == goal_node.y:
print("Find goal")
goal_node.parent_index = current.parent_index
goal_node.cost = current.cost
break
# Remove the item from the open set
del open_set[c_id]
# Add it to the closed set
closed_set[c_id] = current
# expand_grid search grid based on motion model
for i, _ in enumerate(self.motion):
node = self.Node(
current.x + self.motion[i][0],
current.y + self.motion[i][1],
current.cost + self.motion[i][2],
c_id,
)
n_id = self.calc_grid_index(node)
# If the node is not safe, do nothing
if not self.verify_node(node):
continue
if n_id in closed_set:
continue
if n_id not in open_set:
open_set[n_id] = node # discovered a new node
else:
if open_set[n_id].cost > node.cost:
# This path is the best until now. record it
open_set[n_id] = node
rx, ry = self.calc_final_path(goal_node, closed_set)
return rx, ry
4.5.c.3 A*的代码测试
在main 函数中设置起始点，目标点，grid的分辨率和机器人的半径，在创建grid map之后，并运行A算法，即可找到最优路径。如下图所示，红色路径即为最终A搜出来的最优路径。
def main():
# start and goal position
start_x = 10.0 # [m]
start_y = 10.0 # [m]
goal_x = 50.0 # [m]
goal_y = 0.0 # [m]
grid_size = 2.0 # [m]
robot_radius = 1.0 # [m]
# construct environment info.
border_x, border_y, ox, oy = construct_env_info()
if show_animation:
plt.plot(border_x, border_y, "s", color=(0.5, 0.5, 0.5), markersize=10)
plt.plot(ox, oy, "s", color="k")
plt.plot(start_x, start_y, "og", markersize=10)
plt.plot(goal_x, goal_y, "ob", markersize=10)
plt.grid(True)
plt.axis("equal")
ox.extend(border_x)
oy.extend(border_y)
a_star = AStarPlanner(ox, oy, grid_size, robot_radius)
rx, ry = a_star.planning(start_x, start_y, goal_x, goal_y)
if show_animation:
plt.plot(rx, ry, "-r")
plt.savefig(gif_creator.get_image_path())
plt.pause(0.01)
gif_creator.create_gif()
plt.show()
4.5.4 A*算法的变种
A*算法有很多的变种，此处不在详细介绍，感兴趣者可参考以下链接。
- Anytime A*
- Block A*
- D*
- Field D*
- Fringe
- Fringe Saving A (FSA*)*
- Generalized Adaptive A (GAA*)*
- Incremental heuristic search
- Iterative deepening A (IDA*)*
- Jump point search
- Lifelong Planning A (LPA*)*
- Simplified Memory bounded A (SMA*)*
- Theta*
4.5.5 参考
https://en.wikipedia.org/wiki/A*_search_algorithm#Pseudocode
推荐阅读
- 端到端理论与实战
- 动手学轨迹预测
- 动手学运动规划
- 动手学行为决策
- 强化学习入门笔记
