九、规划控制——路径规划（1）

最新推荐文章于 2024-04-19 09:31:31 发布

无……风

最新推荐文章于 2024-04-19 09:31:31 发布

阅读量789

点赞数 30

分类专栏： # Planning and Control 文章标签：自动驾驶人工智能机器学习算法

本文链接：https://blog.csdn.net/guozhengxian123/article/details/133940079

版权

Planning and Control 专栏收录该内容

20 篇文章 46 订阅

订阅专栏

本文详细介绍了路径规划中的Dijkstra算法，包括其基本原理、在有向带权图中的应用，以及算法实例演示，强调了理解和算法细节对自动驾驶路径规划的重要性。

摘要由CSDN通过智能技术生成

概要：本文对路径规划的方法中的Dijkstra方法进行了解。

前言

本文将对决策规划的任务及其存在的挑战进行了解，并对dijkstra算法进行了解。

一、Mission Planning

Mission planning 又叫Route planning，它的整个模块是生成参考线的基础，通常也叫它全局路径规划。
任务规划模块主要运用地图的抽象处理成为一个可供搜索的图，通过搜索的算法如A*，Dijkstra生成一个全局的路径。
在这里插入图片描述

二、Mission Planning Structure

在这里插入图片描述
该模块的输入就包括了：起始点、目标点、基于地图的lane graph。
成本函数：用户自定义如最快到达、最短距离。
输出的是道路或者路段的序列路线。

在这里插入图片描述
路线设计：

边、点的定义，连接路的定义；
损失函数的定义：车道改变、左右转、窄路的一个惩罚；
中间点/路径途径点的添加。

三、Mission Planning Dijkstra

Dijkstra是荷兰科学家Dijkstra在1956年发行的算法，用来寻找节点之间最短的路径，基于广度优先的搜索方法，用于解决有向赋权图或者无向的单源最短路径问题。

3.1 有向带权图

在这里插入图片描述
如图可以看出，每一个节点到另一个节点是带着方向的。
G = （V，E，W）
V（vertices）：代表该图的每个顶点；
E（edge）：代表该图的每一条边；
W（weight）：代表每一条边的权重，权重是非负数。
这样就构成了有向带权的路径图。

Dijkstra就是讨论起始点S到达其它顶点路径的最短长度，并且输出最路径本身，也就是单源路径最短问题（single-source shortest paths problem）。

3.2 Basics of Dijkstra’s Algorithm

找到起始节点到其它节点的最短距离；
发现源节点到另外节点的最短距离，就会标记已访问并且添加到路径中；
不断重复以上过程，就可以寻访到每一个节点。

算法原理：

创建数组d[]，来存储每个节点到其它节点的最短距离；
初始状态：在源节点上，的d[s]=0；其他节点设置无穷大，大于实际路径长度；
创建数组u[]，用来存放已经被标记或者被寻遍的顶点，初始状态，所有的顶点都是没有被标记的；
在算法迭代过程中，每个顶点都会被选为默认标记的最小长度顶点，第一个顶点v会选择源点S；
接下来会寻找下一个顶点，通过比较上一个节点到默认节点的距离d[v]和上一个节点到其它节点d[to]的大小length来确定最短节点路径；
通过不断迭代，就会找到从源点S到其它顶点的最短距离，有些没有办法从源点到达的其他节点会被设置为无穷大，从而停止算法迭代。

3.3 Dijkstra Algorithm Example

在这里插入图片描述

假设A到A距离为0，而A到 B、C、D、E设置为无穷大，所以，A最短距离就是它本身，A为起始源点；

在这里插入图片描述

基于当前顶点A寻找最近临近顶点，就会找到B、D；
基于寻找到的顶点，计算出当前顶点到B和D的距离；
计算出的当前节点距离和当前节点已知距离比较，更新最短距离，这样就会更新B、D的距离；
更新以前节点作为父节点，A就更新为父节点，同时把A放在已寻访过的子集；
再次从1开始重复，比较所有未寻访的距离（此时unvisited没有A），可以得到D的距离是最小的，所以，把D设置为下一个寻找点；
基于当前顶点D找寻最近临近顶点，这样就找到了B和E（还是基于起始点A进行距离计算如D->B其实是A->D->B距离为1+2=3）；
计算出的当前节点距离和当前节点已知距离比较，更新最短距离，这样就会更新B、E的距离
更新以前节点作为父节点，D就更新为父节点，同时把D放在已寻访过的子集（这个只针对6中找到的节点的父节点）；
再次从1开始重复，比较所有未寻访的距离（此时unvisited没有A，D），可以得到E的距离是最小的，所以，把E设置为下一个寻找点；
基于当前顶点E找寻最近临近顶点，这样就找到了B和C，（还是基于起始点A进行距离计算如E->B其实是A->D->E->B距离为1+1+2=4）；
计算出的当前节点距离和当前节点已知距离比较，更新最短距离，由于B现在的距离是3而算出的是4则保持不动，把C跟新；
更新以前节点作为父节点，E就更新为父节点，由于只更新了C，所以E只针对于C作为父节点，同时把E放在已寻访过的子集；
再次从1开始重复，比较所有未寻访的距离（此时unvisited没有 A，D，E），可以得到B的距离是最小的，所以，把B设置为下一个寻找点；
此时没有被寻访过的最近临近点只剩下C，所以计算C的距离,由于E刚才没有更新B的距离，所以B->C其实是A->D->B->C = 1+2+5=8；
计算出的当前节点距离和当前节点已知距离比较，更新最短距离，由于C现在的距离是7而算出的是8则保持不动；
更新以前节点作为父节点，由于没有更新，所以B不作为父节点，同时把B放在已寻访过的子集；
同样再来一次，由于只有C则C为最小距离；
没有临近点；
直接跳到最后，将C放到已寻访过子集。

在这里插入图片描述

在这里插入图片描述
如图所示，就可以知道任何一个点到A的最短路径，如从B到A，在表可知 B->D->A。

3.3 Dijkstra Algorithm implementation

Dijkstra 的本质：
1、选择未被寻访的顶点最小值作为节点d[v]；
2、寻找该节点临近点，并计算该节点到临近点的距离（该距离其实就是节点的边缘（edge）距离的d[to]）；
3、该距离与上一节点到该节点的距离进行比较，如果小于原来距离则会更新距离和上一节点；如果大于，则保持；
4、基于3的基础上不管是否更新距离或者节点，都应该将该节点放在visited中，标记为已经访问过。

算法运行的时间包括：

对于未标记最小值的n次顶点的搜索；
m次关联尝试。

该算法的复杂度对于密度大的地图的搜索效果是比较好的。当m趋近于n²时，算法复杂度是比较精准的；
对于稀疏的地图来说：由于m远小于n²，复杂度：nlogn+m计算方法。

总结

本文主要是对于路径规划有一个初步的了解，可以基于地图信息生成一个全局搜索路径，并了解了路径规划的结构包括起始点、目标点以及地图信息，本篇着重介绍了Dijkstra算法的起源、有向带权图以及算法的基本原理、并结合例字说明该算法的工作方法和流程，研究其算法时，需要把里面的细节吃透，如最后了解了其算法的复杂度计算，这篇文章希望可以对想要学习自动驾驶路径规划方向的同学们有一定的帮助。
喜欢的朋友们动动小手点个关注，我会定期分享我的一些知识总结和心得体会，感谢大家!