路径规划 | 图搜索算法：DFS、BFS、GBFS、Dijkstra、A*
路径规划 | 图搜索算法：DFS、BFS、GBFS、Dijkstra、A*
路径规划是机器人、自动驾驶等领域的重要技术，而图搜索算法是实现路径规划的关键工具。本文将从图搜索算法的基本流程入手，层层递进地介绍几种常用的图搜索算法，包括深度优先搜索(DFS)、广度优先搜索(BFS)、贪婪最佳优先搜索(GBFS)、Dijkstra算法和A*算法。这些算法各有特点，适用于不同的场景，通过本文的学习，读者将能够掌握这些算法的原理和应用。
配置空间
在学习路径规划算法之前，首先了解一下配置空间(Configuration Space)这个概念。在实际环境，也就是机器人的工作空间(Workspace)中，机器人是有形状和大小的，这不利于进行运动规划。要将工作空间转换到配置空间中，即将机器人转化为一个质点，同时将障碍物按照机器人的体积进行膨胀，如下图：
这样，在进行路径规划时，就可以将机器人当做一个点来处理了。
基本流程
下面切入正题，图搜索算法的基本流程如下：
创建一个容器，一般称为openlist，用来存储将要访问的节点，将起点加入容器，开始循环：
- 弹出：从容器中取出一个节点
- 扩展：获取该节点周围的节点，将这些节点放入容器
深度优先搜索(DFS)
深度优先，顾名思义即深度越大的节点会被优先扩展。在DFS中，使用栈(Stack) 数据结构来实现上述特性。栈是一种后进先出(LIFO) 的容器，如下图
以在下面的无权图中找到从节点a到节点i的路径为例，说明一下DFS算法的工作流程
按照上节的图搜索算法的基本流程进行搜索，过程如下：
从i回溯得到路径：a->b->c->g->i，如下：
DFS能够快速地找到一条路径，是一种以时间换空间的方法。将其应用到二维地图的路径规划中，如下图，很显然找到的路径并不是移动机器人运动规划所需要的最优路径
广度优先搜索(BFS)
与DFS的“不撞南墙不回头”的个性不同，BFS在搜索时呈波状推进形式，一路稳扎稳打，它是一种以时间换空间的方法，能够保证搜索到的路径是最优的。为了实现波状推进搜索特性，BFS采用队列(Queue) 作为openlist的数据结构。队列是一种先进先出(FIFO) 的容器，如下图
其流程与上节中DFS类似，继续以上节的图举例，过程如下，首先创建一个队列作为容器，将节点a加入队列
接着将节点a弹出队列，将节点a周围没有访问过的节点加入队列
按照上面的流程不断地弹出、扩展节点，直到找到节点i为止，完整流程如下图：
从终点回溯，i的父节点为f，f的父节点为e，e的父节点为a，这样就可以得到a到i的最短路径为：a->e->f->i，如下
显而易见，相较于DFS，BFS中使用了大量的入队、出队操作，耗时增加，但是能保证找到最优路径。
启发式搜索算法
BFS和DFS的区别主要在于节点的弹出策略，根据弹出策略的区别，分别使用了队列和栈两种数据结构，而栈和队列作为两种相当基本的容器，只将节点进入容器的顺序作为弹出节点的依据，并未考虑目标位置等因素，这就使搜索过程变得漫无目的，导致效率低下。启发式搜索算法(Heuristic Algorithm)就是用来解决搜索效率问题的，下面将以贪婪最佳优先算法(Greedy Best First Search, GBFS)为例来介绍启发式搜索算法。
GBFS也是图搜索算法的一种，它的算法流程和BFS、DFS并没有本质的不同，区别仍然在于openlist采用的数据结构，GBFS使用的是优先队列(Priority Queue)，普通队列是一种先进先出的数据结构，而在优先队列中元素被赋予了优先级，最高优先级元素优先删除，也就是first in, largest out。(记住这种数据结构，后面的Dijkstra和A*算法都会用到这个结构)。
在图搜索算法中，使用代价函数f ( n )来作为优先级判断的标准，f ( n )越小，优先级越高，反之优先级越低。
GBFS作为一种启发式搜索算法，使用启发评估函数h ( n )
其中h ( n ) 是当前节点到终点的代价，它可以指引搜索算法往终点靠近，主要用欧氏距离(Euclidean Distance) 或者曼哈顿距离(Manhattan Distance) 来表示，它们的区别如下图：
假设有两个点( x 1 , y 1 ) 和( x 2 , y 2 )则它们的欧氏距离和曼哈顿距离分别为：
将GBFS应用在二维地图路径规划中，如下图，可以看到它的指向性或者说目的非常明显，从起点直扑终点。
但是在实际的地图中，常常会有很多障碍物，它就很容易陷入局部最优的陷阱。
下图的地图中有一个专门设置的局部最优陷阱，很显然GBFS虽然搜索速度够快，但是找不到最优路径。
将其应用到复杂二维地图路径规划中，效果如下：
Dijkstra算法
上面的算法中，只有广度优先搜索(BFS)具有完备性，能够保证搜索到最优路径。
但是可以看到BFS算法搜索到的路径只有向上/下/左/右移动这四个动作，它们是没有权值或者说权值都相同的，只能用于无权图的路径规划，无法实现能够对角移动的路径规划。
因此下面介绍一种能用于带权图的图搜索算法——Dijkstra算法(狄克斯特拉算法)。
Dijkstra算法是从一个顶点到其余各顶点的最短路径算法，其流程仍然与上述算法基本一致，它也是用优先队列作为openlist的数据结构，它和GBFS的区别在于代价函数f ( n )的定义，Dijkstra算的f ( n ) 定义为：
其中g ( n )表示从起点到当前点的移动代价。
以下图为例，计算左上角起点到右下角终点的最短路径，箭头上的数值表示两个节点间的距离
首先扩展第一个节点，计算其余节点与第一个节点的距离，用橙色标出已经扩展的节点，未扩展的节点仍用绿色标出，其中圆中的数值表示该节点的代价函数，字母则表示该节点没有直接到达此时已扩展节点的路径。
从未扩展的节点(绿色节点)中选择代价函数最小的节点进行拓展，并更新其余节点的代价函数，如下图
重复进行上面的步骤，直到所有节点都已扩展。
最后标出起点到终点的最短路径
将Dijkstra算法应用到二维地图路径规划中，如下图，可以看到Dijkstra算法能够得到最优路径，但是它的速度和BFS是一样的，采取的都是稳扎稳打、波状前进的方式，导致速度较慢。
A*算法
对比GBFS和Dijkstra算法，两者都采用优先队列作为openlist，而代价函数的不同导致两者具有不同的优点：GBFS用节点到目标点的距离作为代价函数，将搜索方向引向目标点，搜索效率高；而Dijkstra算法采用起点到当前扩展节点的移动代价作为代价函数，能够确保路径最优。
那么可不可以将两者的代价函数进行融合，从而在保证路径最优的同时提高搜索效率？答案是肯定的，融合后的算法就是A*算法。
A*算法也是一种启发式算法，它的代价函数表示为：
其中g ( n ) 为起点到当前扩展节点的移动代价函数，h ( n ) 是启发函数，用节点到目标点的距离函数来表示。
根据这个式子，可以得到A*算法的几个特点：
如果令h ( n ) = 0 A* 算法就退化为Dijkstra算法；如果令g ( n ) = 0A* 算法就退化为GBFS算法。
能否找到最优路径的关键是启发函数h ( n ) 的选取，如果h ( n ) 在大部分情况下比从当前节点到目标点的移动代价小，则能找到最优路径。
由于A* 算法的启发函数是位置上的距离，因此在不带位置信息的图数据中不适用。
将A*算法应用到二维地图路径规划中，如下图：
参考：
图的实现(c++)
https://blog.csdn.net/y1054765649/article/details/88763699
机器人路径规划之Dijkstra算法
https://blog.csdn.net/qq_42688495/article/details/106269382
版权声明：本文为CSDN博主「白鸟无言」的原创文章，遵循CC 4.0 BY-SA版权协议，转载请附上原文出处链接及本声明。
原文链接：
https://blog.csdn.net/qq_42688495/article/details/113108292
本文仅做学术分享，如有侵权，请联系删文。