决策规划 | 通俗易懂聊A*算法

作者 | Mason X 编辑 | 汽车人

原文链接：https://zhuanlan.zhihu.com/p/595716772

点击下方卡片，关注“自动驾驶之心”公众号

ADAS巨卷干货，即可获取

点击进入→自动驾驶之心【规划控制】技术交流群

后台回复【规划控制综述】获取自动驾驶、智能机器人规划控制最新综述论文！

本文是对《自动驾驶决策规划技术理论与实践》的学习总结，希望以一种正向的思考方式，由问题逐步推理出解决问题的算法。

1. A*算法是什么

A*算法是一种1968年提出的基于采样搜索的粗略路径规划算法。

早早被提出，至今还在被使用，说明A算法很经典。A算法被提出是想要解决路径规划问题，也就是找到一条从起点到终点的路径。路径规划是生活中常见的一类问题，比如让游戏中的人物自动从A点到B点，以及自动驾驶中让车辆自动从A点开到B点。比如下面这幅图，如何最快的从起点到终点并且绕开所有障碍物呢？

2. A*算法如何解决这些问题

上面那个图是不是感觉很难一眼就给出答案，那我们先将问题简化一下：看看下面这幅图，从绿色起点到红色终点，中间蓝色区域是障碍物，是不是答案马上就出来了

我们以这个简单问题为例子，看看问题是怎么被解决的：首先，相比于最开始的图，明显的变化之一就是将地图“网格化”了。“网格化”其实就是将连续的问题离散化，离散的数据更便于计算机处理，也便于我们理解。
虽然我们一眼就得出了答案，但是计算机不行。我们要做的，就是从得出答案的过程中整理出一套计算机可实现的逻辑。那我们是怎么一步步从起点走到终点的呢？
处在某个格子时，观察周围的格子，找到其中离终点最近，且未被障碍占据的格子，移动过去，直到抵达终点
好的，现在逻辑理出来，是不是可以开始写代码了。等等，还有些情况没考虑到，看下面这例子，

如果我们每次都向离终点最近的格子移动，那我们最终的路线应该是上图中的红线，但这显然是不合理的，为了应对这种情况，A*算法中，选取下一个格子的标准并不是离终点最近，而是离起点和终点的距离之和最近。
之前，我们只关心这个格子是不是离终点最近，但现在我们将一个格子的距离分为了两部分，离起点的距离+离终点的距离。我们关心的是这两距离之和最小。
那为什么加入了“历史距离”这个考量因素就能避免上述问题呢？我们可以这样想：极端一点，我们只考虑历史距离，不考虑和终点的距离，也就是选取下一个格子的标准是离起点最近，那整个搜索过程将会是一个以起点为圆心，逐渐向外扩散的圆。这样我们一定能找到那条最合理的路径，只是这样我们将需要搜索很多的点，效率会比较低。

ps：这里就引出了两个很重要的概念：效率和质量。这是解决问题时矛盾的两方面，需要人们进行权衡。速度快了，不能保证取得最优解；想100%取得最优解，就需要更多的时间

通常我们也会将距离称为代价f，和起点的距离称为历史代价g，和终点的距离称为未来预期代价h，f=g+h。距离最近也就是代价最小，就是（g+h）最小。

3. 如何用代码实现

好的，逻辑理的差不多了，下一步我们需要思考如何用代码实现它。格子通常抽象化后称为节点，后续统一使用”节点“这个名称。

• 准备

首先是确定我们需要什么样的结构来处理这些数据

我们需要从周围格子中挑出一个符合要求的格子，所以我们需要一个集合，存放那个待考察的格子，姑且叫”待考察集合“，通常称为Openlist；

我们肯定不会走回头路，所以需要另一个集合来记录我们走过哪些格子，姑且叫”已探索集合“，通常称为Closedlist；

另外我们要给格子附上一些属性，表示这个格子的状态：代价 f，历史代价 g，未来预期代价 h，父节点索引 parent_idx，是否在待考察集合中 is_in_Openlist，是否在已考察集合中 is_in_Closedlist

• 初始化

1. 初始化集合Openlist和Closedlist。初始化图中所有的节点，也就是将所有属性置空

2. 将被障碍物占据的节点加入Closedlist，并将节点的is_in_Closedlist置为1

3. 为初始节点完善相关属性：g = 0；计算h；parent_idx = null ；is_in_Openlist = 0；is_in_Closedlist = 0。（如何计算h后面介绍）

4. 将初始节点加入Openlist，并将初始节点的is_in_Openlist置为1

• 循环

1. 从Openlist中选取一个代价f最小的节点，如果多个节点同时最小，则取最后加入的。将这个节点记为Node_current

2. 找到Node_current周围的节点，也称为Node_current的子节点。对这些子节点进行逐一考察，记正在考察的子节点为Node_child

3. 判断Node_child的 is_in_Closedlist 是否为1，如果是，则表明它被障碍物占据，或者已经走过，将它抛弃， 不做考虑

4. 判断Node_child的 is_in_Openlist 是否为0，如果是，表明Node_child还未被探索过，此时执行操作A。如果Node_child的 is_in_Openlist 是否为1，表明它已经被其他节点在更早的迭代中发现过，此时执行操作B。（操作A和B后面介绍）

5. 将Node_current从Openlist中移除，移入Closedlist。对应属性改为：is_in_Openlist = 0；is_in_Closedlist = 1

• 终止条件

1. 判断上述子节点中是否包含终点，如果包含，表明路径已找到，终止循环

2. 判断Openlist是否为空，如果是，表明搜索完所有节点仍没有找到路径，终止循环

3. 判断是否达到设定的超时时间，如果是，表明在规定时间内未找到路径，终止循环

到目前为止，整个流程已经理完了，但是还留了3个坑，分别是如何计算h以及操作A和操作B是什么，现在一一来填一下

• 计算预期代价h

首先需要明确的是，预期代价之所以叫预期代价就是因为在存在障碍物时，它不可能准确获取，只能用某种方式近似预估。而历史代价是可以准确计算，计算方式会在后面介绍。

那么预估两点之间的距离，会想到哪种方式？一个是欧式距离，一个是曼哈顿距离。欧式距离就是我们常说的两点之间，直线最短，直接 ( (x1 - x2)**2 + (y1 - y2)**2 ) **0.5 。而曼哈顿距离就是两点投影到坐标轴上的距离差之和，也就是 | x1 - x2 | + | y1 - y2 | 。由于欧式距离需要开方，计算耗时更长，所有这里采用曼哈顿距离作为两点间距离的估计。

欧式距离一定是低估了代价，而曼哈顿距离则可能高估代价

下图中的绿线就是欧式距离，而红线，蓝线，黄线都是曼哈顿距离

PS：之所以叫曼哈顿距离，是因为美国曼哈顿街道就像上面的白色区块一样横平竖直。曼哈顿距离又被称为城市区块距离

• 操作A

第一步是完善Node_child的各属性，其中将Node_child的父节点记为 parent_idx = Node_current。

第二步是计算Node_child的历史代价g。子节点的历史代价可以由父节点的历史代价加上父节点到子节点的距离得到，这里的距离可以采用欧式距离计算。即 Node_child · g = Node_current · g + || Node_current · Node_child ||

第三步是根据上述方法计算Node_child的预期代价h

第四步是将 g 和 h 相加得到总代价f

第五步是将Node_child放入Openlist，并将Node_child的is_in_Openlist置为1

• 操作B

注意，操作B适用的是那些已经被加入Openlist的节点，也就是说这些节点之前肯定已经执行过操作A了

举个例子解释操作B的核心逻辑：你想找大佬办事，托朋友A引荐，但是朋友A要收好处10块。这时候你发现朋友B也可以帮你引荐，并且只收5块，那你这时候是不是应该放弃朋友A这条路，选朋友B这条路

回到我们的问题上，操作B的核心逻辑就是判断将Node_child的父节点置为Node_current能否使Node_child的总代价f变小

假设Node_child之前的父节点为Node_oldparent，那么Node_child现在的代价f就是

Node_child · f = Node_oldparent · g + || Node_oldparent · Node_child || + Node_child · h

这一数值其实已经被记录在Node_child的f属性上了。那假设将Node_child的父节点换成Node_current的话，总代价f为：

Node_child · f = Node_current · g + || Node_current · Node_child || + Node_child · h

比较上述两个表达式的大小，如果后者更小，则将Node_child的父节点更新为 parent_idx = Node_current。同时更新Node_child的历史代价g和总代价f

伪代码

下面给出A*算法的伪代码

输入：网格连通图，起点Node_start，终点Node_end
输出：联通起点和终点的路径

# 初始化
初始化Openlist为空，初始化图中所有的节点，也就是将所有属性置空。
将被障碍物占据的节点的is_in_Closedlist置为1
为初始节点完善相关属性：g = 0；利用曼哈顿距离计算h；parent_idx = null ；is_in_Openlist = 0；is_in_Closedlist = 0
将初始节点加入Openlist，并将初始节点的is_in_Openlist置为1

# 循环
while Openlist ！= 空 ：
    从Openlist中选取一个代价f最小的节点，记为Node_current
    拓展Node_current，将其所有子节点放置在临时集合A中
    for each Node_child in 集合A ：
        if (Node_child 超出地图边界)) or (Node_child的is_in_Closedlist==1):
            continue
        if Node_child的is_in_Openlist==0 :
            计算Node_child的历史代价g，预期代价h，总代价f
            将Node_child的父节点记为 parent_idx = Node_current
            将Node_child放入Openlist，并将Node_child的is_in_Openlist置为1
        else ：
            计算将Node_child的父节点换成Node_current的话，总代价f_alternative
            if f_alternative < Node_child原有的f ：
                将Node_child的父节点更新为 parent_idx = Node_current
                更新Node_child的历史代价g和总代价f
    将Node_current从Openlist中移除。
    将Node_current对应属性改为：is_in_Openlist = 0；is_in_Closedlist = 1
    if 集合A中包含终点Node_end ：
        break

# 输出路径
构建空列表B，将Node_end放入列表B
if 列表B中最后一个节点的父节点不是空 ：
    B.append(将列表B中最后一个节点的父节点)
将列表B逆序打印就是路径

补充知识

刚才说了A算法中的代价函数是f=g+h，我们可以扩展成 f=g+ah。这里的a是一个系数，当a>1时，表示在整个代价f中，h的影响更大，也就是更倾向于选择离终点近的点，这样整体的搜索效率会提升，但是不能保证得出的是最优化的路径。

类似的，如果0<r<1时，表示在整个代价f中，h的影响小，g的影响更大，也就更倾向于选择离起点更近的点，极端情况下，r=0，那么遍历行为将变成以起点为圆心而扩展的圆，这样一定能找到最优的路径，但是搜索的点将会很多，效率会下降。

综上，在搜索过程中，通常效率和质量是无法兼顾的，需要找到一个平衡点

【自动驾驶之心】全栈技术交流群

自动驾驶之心是首个自动驾驶开发者社区，聚焦目标检测、语义分割、全景分割、实例分割、关键点检测、车道线、目标跟踪、3D目标检测、BEV感知、多传感器融合、SLAM、光流估计、深度估计、轨迹预测、高精地图、NeRF、规划控制、模型部署落地、自动驾驶仿真测试、硬件配置、AI求职交流等方向；

添加汽车人助理微信邀请入群

备注：学校/公司+方向+昵称