来源丨古月居
点击进入—>3D视觉工坊学习交流群
地图数据常常可以用图(Graph)这类数据结构表示，那么在图结构中常用的搜索算法也可以应用到路径规划中。
本文将从图搜索算法的基本流程入手，层层递进地介绍几种图搜索算法。首先是两种针对无权图的基本图搜索算法：深度优先搜索(Depth First Search, DFS)、广度优先搜索(Breadth First Search, BFS)。
它们的区别在于openlist(后面介绍)所选用的数据结构类型不同，前者使用栈，后者使用队列；之后引入一种启发式搜索算法：贪婪最佳优先算法(Greedy Best First Search, GBFS)，用来提高搜索效率，但是不能确保找到最优路径。
最后介绍两种在路径规划中非常经典的算法：Dijkstra算法、A*算法，前者是广度优先算法(BFS)在带权图中的扩展，后者则是在前者中加入启发函数得到的算法，兼顾效率和完备性。
配置空间
在学习路径规划算法之前，首先了解一下配置空间(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
本文仅做学术分享，如有侵权，请联系删文。
点击进入—>3D视觉工坊学习交流群
干货下载与学习
后台回复：巴塞罗那自治大学课件，即可下载国外大学沉淀数年3D Vison精品课件
后台回复：计算机视觉书籍，即可下载3D视觉领域经典书籍pdf
后台回复：3D视觉课程，即可学习3D视觉领域精品课程
3D视觉工坊精品课程官网：3dcver.com
1.面向自动驾驶领域的3D点云目标检测全栈学习路线！(单模态+多模态/数据+代码)
2.彻底搞透视觉三维重建：原理剖析、代码讲解、及优化改进
3.国内首个面向工业级实战的点云处理课程
4.激光-视觉-IMU-GPS融合SLAM算法梳理和代码讲解
5.彻底搞懂视觉-惯性SLAM：基于VINS-Fusion正式开课啦
6.彻底搞懂基于LOAM框架的3D激光SLAM: 源码剖析到算法优化
7.彻底剖析室内、室外激光SLAM关键算法原理、代码和实战(cartographer+LOAM +LIO-SAM)
16.透彻理解视觉ORB-SLAM3：理论基础+代码解析+算法改进
重磅！粉丝学习交流群已成立
交流群主要有3D视觉、CV&深度学习、SLAM、三维重建、点云后处理、自动驾驶、多传感器融合、CV入门、三维测量、VR/AR、3D人脸识别、医疗影像、缺陷检测、行人重识别、目标跟踪、视觉产品落地、视觉竞赛、车牌识别、硬件选型、ORB-SLAM系列源码交流、深度估计、TOF、求职交流等方向。
扫描以下二维码，添加小助理微信（dddvisiona）,一定要备注：研究方向+学校/公司+昵称，例如：”3D视觉 + 上海交大 + 静静“。请按照格式备注，可快速被通过且邀请进群。原创投稿也请联系。
▲长按加微信群或投稿，微信号：dddvisiona
3D视觉从入门到精通知识星球：针对3D视觉领域的视频课程（三维重建系列、三维点云系列、结构光系列、手眼标定、相机标定、激光/视觉SLAM、自动驾驶等）、源码分享、知识点汇总、入门进阶学习路线、最新paper分享、疑问解答等进行深耕，更有各类大厂的算法工程人员进行技术指导。与此同时，星球将联合知名企业发布3D视觉相关算法开发岗位以及项目对接信息，打造成集技术与就业为一体的铁杆粉丝聚集区，6000+星球成员为创造更好的AI世界共同进步，知识星球入口：
学习3D视觉核心技术，扫描查看，3天内无条件退款
高质量教程资料、答疑解惑、助你高效解决问题
觉得有用，麻烦给个赞和在看~
扫一扫
1549
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
