【动手学运动规划】 4.5 A*算法

我宁愿永远做我自己，也不愿成为别人，即使那个人比你更快乐。

—《成为简·奥斯汀》

🏰代码及环境配置：请参考 环境配置和代码运行!

---

4.5.1 概述

Dijkstra算法是基于广度优先搜索策略来遍历空间内的所有节点，最终计算出全局最优的路径，其计算量非常大。而基于启发式的贪婪最佳优先搜索（greedy best first search，GBFS）速度快，但结果可能不是最优的。那么，如何将二者的优势结合呢，即在Dijkstra算法基础上，引入启发式策略。这就是A*算法。

🌟**Note：**最佳优先搜索算法是在广度优先搜索的基础上，用启发估价函数对将要被遍历到的点进行估价，然后选择代价小的进行遍历，直到找到目标节点或者遍历完所有点，算法结束。

4.5.2 算法详解

A*算法结合了GBFS算法和Dijkstra算法的优点，通过评估函数$f(n)$来指导搜索过程，$f(n)$定义为从起点到节点n 的实际代价$g(n)$与从节点n 到目标节点的估计代价$h(n)$之和，即$f(n)=g(n)+h(n)$。

• $g(n)$：从起点到节点n的实际代价（通常是距离或消耗）。
• $h(n)$：从节点n到目标节点的估计代价（启发式函数）。

🌟**Note：**为了确保$g(n)$和$h(n)$的相加有意义，这两个值必须使用相同的衡量单位。

4.5.1.1 启发式函数

启发式函数可以控制A*的行为：

• 一种极端情况，如果$h(n)$是0，则只有$g(n)$起作用，此时A*演变成Dijkstra算法，计算量增大，但可以确保找到最优路径。
• 如果$h(n)$经常都比从节点n移动到目标节点的实际代价小（或者相等），则A可以保证能找到一条最优路径。$h(n)$越小，A扩展的节点越多，计算量越大，运行得越慢。
• 如果$h(n)$精确地等于从节点n移动到目标节点的代价，则A*将会仅仅寻找最优路径而不扩展别的任何结点，这会运行得非常快。尽管这不可能在所有情况下发生，但仍可以在一些特殊情况下让它们精确地相等。
• 如果$h(n)$有时比从节点n移动到目标节点的实际代价高，则A*不能保证找到一条最优路径，但它运行得更快。
• 另一种极端情况，如果$h(n)$比$g(n)$大很多，则只有$h(n)$起作用，A*演变成GBFS算法。

下图可以清晰的看出不同的$h(n)$对算法的影响（图中绿色的为起始点，红色的为目标点）：

• $a=1,b=0$：如图Fig.1，此时A*算法变成了Dijkstra算法，虽然可以得到最优解，但是扩展了非常多的节点，计算量很大。
• $a=0,b=1$：如图Fig.2，此时A*算法变成了GBFS算法，计算量减少，但可能找不到最优解。
• $a=1,b=1$：如图Fig.3，即为A*算法，平衡了计算量和最优解之间的关系。

🌟Note: 图中的算法展示来源于：https://qiao.github.io/PathFinding.js/visual/

经过分析，可以看到在A*算法中，启发式函数$h(n)$扮演着及其重要的角色，以下是一些常见的启发式函数类型：

1. 曼哈顿距离（Manhattan Distance）：
   • 曼哈顿距离是A*算法中常用的启发式函数之一。它计算的是两点在标准坐标系上的绝对轴距总和，即只能沿着坐标轴（或网格的边界）移动的距离。对于网格地图，曼哈顿距离非常适合，因为它反映了实际移动中的限制（如只能上下左右移动）。
   • 公式：$h(n)=\left|x_n-x_{goal}\right|+\left|y_n-y_{goal}\right|$，其中$(x_n,y_n)$和$(x_{goal},y_{goal})$分别是当前点和目标点的坐标。
2. 欧几里得距离（Euclidean Distance）：
   • 欧几里得距离是两点之间的直线距离，在平面直角坐标系中，它可以通过勾股定理计算得到。虽然欧几里得距离在物理上更准确，但在网格地图中，由于只能沿网格线移动，它可能不总是反映实际的最短路径。然而，在某些情况下，为了简化计算或适应特定需求，也可以使用欧几里得距离作为启发式函数。
   • 公式：$h(n)=\sqrt{(x_n-x_{goal}{)}^2+(y_n-y_{goal}^2)}$
3. 切比雪夫距离（Chebyshev Distance）：
   • 切比雪夫距离是各坐标数值差的最大值，也称为棋盘距离或$L\infty$ 度量。在网格地图中，它表示从一个点到另一个点所需改变的最大坐标值（即沿任一坐标轴移动的最大步数）。虽然不如曼哈顿距离常用，但在某些特定场景下，切比雪夫距离也能作为有效的启发式函数。
   • 公式：$h(n)=max(\left|x_n-x_{goal}\right|,\left|y_n-y_{goal}\right|)$
4. 自定义启发式函数：
   • 除了上述常见的启发式函数外，还可以根据具体问题设计自定义的启发式函数。自定义启发式函数可以更加精确地反映问题的实际情况，从而提高搜索效率和准确性。然而，设计有效的自定义启发式函数需要深入了解问题的本质和特性。

4.5.1.2 算法步骤

其算法步骤如下：

1. 初始化：
   1. 创建一个数组g[]，其中g[i]表示从源节点start到节点i的最小cost，初始时，源节点的cost为0。
   2. 创建一个数组f[] ，其中f[i] 表示从源节点start到节点i的最小cost + 节点i 到目标节点的启发式cost。
   3. 创建一个布尔数组 visited[] 来跟踪每个节点是否已被访问过，初始时，所有顶点都未访问
   4. 创建一个优先队列open_list，用于根据cost选择下一个要处理的节点。优先队列中的每个元素是一个包含顶点和cost的配对，初始时将源节点start和其f cost加入优先队列。
2. 循环处理优先队列中的节点：
   1. 从优先队列中取出f cost最小的节点v ，并将节点v 标记为已访问，若节点v就是目标节点，则提前返回。
   2. 遍历节点v 中所有未被访问过的邻接节点，对于每一个邻接节点next ：
      1. 计算邻接节点next 的g cost和h cost。
      2. 若邻接节点next 不在优先队列中，则将邻接节点next 和其对应的f cost加入优先队列，并在数组g[] 和f[]中设置分别设置邻接节点next 的g cost和f cost，最后将临接节点next 的父节点设为v 。
      3. 若邻接节点next 在优先队列中，则在数组g[]，f[]和优先队列open_list中更新节点next 的相关信息，最后将临接节点next 的父节点设为v 。
   3. 继续处理优先队列中的顶点，直到队列为空或所有顶点都被访问。
3. 从目标节点goal开始，通过父节点向回追溯，直到起始节点，最终得到一条路径。

其伪代码如下：

Algorithm A star(G, start, goal):let **open_list** be a priority_queueg[**start**] = 0f[**start**] = g[**start**] + h[**start**]**open_list.**push(**start,** f[**start**])while **open_list** is not empty dov = **open_list**.pop()mark v as vistedif v is the **goal**:return vfor **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_costf[next] = next_g_cost + next_h_cost**open_list**.push(next, f[next])next.parent = velse:if g[next] > next_g_cost:g[next] = next_g_costf[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还没有确定，暂时先标记为无穷大。

1. 从源节点A 开始，将其加入到已访问列表中，并从未访问列表中删除。同时在未访问列表中更新源节点A 的邻近节点（节点B和节点C）的信息。
   1. 节点B ：节点B最短路径为A —> B ，g-cost为1.5，h-cost为5，f-cost为6.5，父节点为A
   2. 节点C ：节点C最短路径为A —> C ，g-cost为2，h-cost为3，f-cost为5，父节点为A

2. 查看未访问列表，节点C 的cost最小，因此将其加入到已访问列表中，并从未访问列表中删除。同时在未访问列表中更新节点C 的邻近节点（节点E）的信息。
   1. 节点E：节点E最短路径为A —> C —> E ，g-cost为5，h-cost为0，f-cost为5，父节点为C。

3. 查看未访问列表，节点E的cost最小，因此将其加入到已访问列表中，并从未访问列表中删除，此时节点E 为目标节点，因此搜索结束，经过回溯父节点，节点A到节点E最短路径为：A —> C —> E 。

4.5.3 优缺点

• 优点
  • 高效性：A*算法结合了最佳优先搜索（Best-First Search）和Dijkstra算法的优点，通过启发式函数（heuristic function）来评估节点的优先级，从而能够快速找到从起点到终点的最短路径。
  • 最优性：在启发式函数满足一定条件（如$h(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 = costself.parent_index = parent_indexdef __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 heuristicd = 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_nodewhile True:if len(open_set) == 0:print("Open set is empty..")breakc_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 graphif 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_indexgoal_node.cost = current.costbreak# Remove the item from the open setdel open_set[c_id]# Add it to the closed setclosed_set[c_id] = current# expand_grid search grid based on motion modelfor 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 nothingif not self.verify_node(node):continueif n_id in closed_set:continueif n_id not in open_set:open_set[n_id] = node  # discovered a new nodeelse:if open_set[n_id].cost > node.cost:# This path is the best until now. record itopen_set[n_id] = noderx, 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 positionstart_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

推荐阅读

• 端到端理论与实战
• 动手学轨迹预测
• 动手学运动规划
• 动手学行为决策
• 强化学习入门笔记