路径规划中常用的Dijkstra算法

无论是无人机还是机器人，我们在做路径规划时，总要解决最短路径问题，在这里介绍一种常用的解决最短路劲问题的算法。

Dijkstra算法

思路：
把顶点集合V分成两组：
S：已解决的顶点的集合（初始时只含有源点$V_0$）
U：尚未确定的顶点集合
将U中顶点按递增的次序加入到S中，保证：
从源点$V_0$到S中其他各顶点的长度都不大于从$V_0$到U中任何顶点的最短路径长度
每个顶点对应一个距离值
S中顶点：从$V_0$到此顶点的长度
U中顶点：从$V_0$到此顶点的只包括S中顶点作中间顶点的最短路径长度

详细步骤

第一步

首先选定A点，则A点至A点的距离为0，并且初始化A点至其他各点的距离为正无穷（由于例子太简单，所以我初始化时把A至B和A至D的距离值给定出来了，但是一般的都初始化为无穷）

第二步

第二步选定与A点连接的各点中距离的点（在这里即为D点），将其补充进集合S中，并在集合U中给A至各点的距离赋值。

第三步

第三步再将距离最短的点补充进集合S中

第四步

继续相同的操作

第五步

最后当U集合为空时，得到的S集合便可知道A点至各个点的最短路径。