【路径规划】全局路径规划算法——Dijkstra算法（含python实现

这篇具有很好参考价值的文章主要介绍了【路径规划】全局路径规划算法——Dijkstra算法（含python实现 | c++实现）。希望对大家有所帮助。如果存在错误或未考虑完全的地方，请大家不吝赐教，您也可以点击"举报违法"按钮提交疑问。

参考资料

路径规划与轨迹跟踪系列算法学习
最短路径算法-迪杰斯特拉(Dijkstra)算法
迪杰斯特拉dijkstra算法的python实现
Python实现迪杰斯特拉算法

1. 基本概念

1.1 算法简介

迪杰斯特拉算法(Dijkstra)是由荷兰计算机科学家狄克斯特拉于1959 年提出的，因此又叫狄克斯特拉算法。是从一个节点遍历其余各节点的最短路径算法，解决的是有权图中最短路径问题。

它的主要特点是以起始点为中心向外层层扩展(广度优先遍历思想)，直到扩展到终点为止。

它是图搜索算法的一种。

1.2 算法思想

设G=(V,E)是一个带权图，V为节点集合。通过Dijkstra计算图G中的最短路径时，需要指定一个起点(假设为D，即从顶点D开始计算)。
此外，引进两个点集S和U。初始时S中只有一个起点，S的作用是记录已求出最短路径的节点(以及相应的最短路径长度)；而U则是记录还未确定最短路径的节点(以及该节点到起点D的距离)。
初始时，数组S中只有起点D，而数组U中是除起点D之外的节点集合，并且数组U中记录各节点到起点D的距离。如果节点与起点D不相邻，距离设为无穷大。
然后，从数组U中找出路径最短的节点K，并将其加入到数组S中；同时，从数组U中移除节点K。接着，更新数组U中的各节点到起点D的距离。
重复第4步操作，直到遍历完所有节点。

1.3 算法图解

全局路径规划算法,# 规划,数据结构,迪杰斯特拉,人工智能,自动驾驶,路径规划

以上图为例，设节点D为起点。

初始时，S只包含起点D；U包含除 D外的其他节点，且U中节点的距离为起点D到该节点的距离，如果该节点与起点D不相邻，距离为无穷大。
从U中选出距离最短的节点C，并将节点C加入到S中；同时，从U中移除节点C。然后，更新U中各个节点到起点D的距离。

之所以更新U中节点的距离，是由于确定了C是求出最短路径过程中的节点，从而可以利用C来更新其它节点的距离；因为起点D到节点v的距离(D,v)可能大于(D,C)+(C,v)的距离。

接下来重复步骤1，2即可。

选取节点E，将E加入到S中，同时更新U中节点的距离。以节点F为例，之前F到D的距离为9；但是将E加入到S之后，F到D的距离为6=(F,E)+(E,D)。
将节点F加入到S中，同时更新U。
将节点G加入到S中，同时更新U。
将节点B加入到S中，同时更新U。
将节点A加入到S中，同时更新U。

此时，起点D到各个节点的最短距离就计算出来了：A(22) B(13) C(3) D(0) E(4) F(6) G(12)。
最后D->A的最优路径为D->E->F->A

1.4 最短路径的最优子结构性质

如果 $P(i,j)=\{V_i…V_k…V_m…V_j\}$ 是从顶点 $i$ 到 $j$ 的最短路径， $k$ 和 $m$ 是这条路径上的一个中间顶点，那么 $P (k, m)$ 必定是从 $k$ 到 $m$ 的最短路径。

证明：反证法

假设 $P(i,j)=\{V_i…V_k…V_m…V_j\}$ 是从顶点 $i$ 到 $j$ 的最短路径，则有 $P (i, j) = P (i, k) + P (k, m) + P (m, j)$ 。而 $P (k, m)$ 不是从 $k$ 到 $m$ 的最短距离，那么必定存在另一条从 $k$ 到 $m$ 的最短路径 $P^{'} (k, m)$ ，那么 $P (i, j) = P (i, k) + P^{'} (k, m) + P (m, j) < P (i, j)$ 。则与 $P (i, j)$ 是从 $i$ 到 $j$ 的最短路径相矛盾。因此该性质得证。

1.5 算法说明

Dijkstra算法过程包括了三个循环，第一个循环的时间复杂度为 $O (n)$ ，第二、三个循环为循环嵌套，时间复杂度为 $O(n^2)$ 。

可以看出，Dijkstra最短路径算法的执行时间和占用空间与图(或网)中结点数目有关，当结点数目较大时，Dijkstra算法的时间复杂度急剧增加。当图规模较大时，直接应用该算法就会存在速度慢或空间不够的问题。所以在大的城市交通网络图中直接应用Dijkstra最短路径算法是很困难的。路径规划作为无人驾驶汽车导航系统的重要功能模块，其算法的优劣是非常重要的，评价该算法的主要性能指标是它的实时性和准确性。Dijkstra算法作为经典的路径规划算法，在实验地图数据量较小情况下会得到很好的规划结果，但在实验地图数据量较大情况下很难满足路径规划的实时性要求。

2. python代码实现

参考自资料。


def dijkstra(matrix, source):
    """迪杰斯特拉算法实现
    Args:
        matrix (_type_): 用邻接矩阵表示带权图
        source (_type_): 起点

    Returns:
        _type_: 最短路径的节点集合，最短路径的节点的最短距离，每个节点到起点的最短路径
    """
    INF = float('inf')
    n = len(matrix)
    m = len(matrix[0])
    assert n == m, "Error, please examine matrix dim"
    assert source < n, "Error, start point should be in the range!"
    S = [source]        # 已找到最短路径的节点集合
    U = [v for v in range(n) if v not in S]  # 记录还未确定最短路径的节点集合
    distance = [INF] * n          # source到已找到最短路径的节点的最短距离
    distance[source] = 0  # 起点到自己的距离
    path_optimal = [[]]*n           # source到其他节点的最短路径
    path_optimal[source] = [source]
    while len(S) < n:   # 当已找到最短路径的节点小于n时
        min_value = INF
        col = -1
        row = -1
        for s in S:     # 以已找到最短路径的节点所在行为搜索对象
            for u in U:   # 从U中搜索尚未记录的节点
                if matrix[s][u] + distance[s] < min_value:  # 找出最小值
                    # 在某行找到最小值要加上source到该行的最短路径
                    min_value = matrix[s][u] + distance[s]
                    row = s         # 记录所在行列
                    col = u
        if col == -1 or row == -1:  # 若没找出最小值且节点还未找完，说明图中存在不连通的节点
            break
        S.append(col)  # 在S中添加已找到的节点
        U.remove(col)  # 从U中移除已找到的节点
        distance[col] = min_value # source到该节点的最短距离即为min_value
        path_optimal[col] = path_optimal[row][:]    # 复制source到已找到节点的上一节点的路径
        path_optimal[col].append(col)       # 再其后添加已找到节点即为source到该节点的最短路径
    return S, distance, path_optimal


def main():
    INF = float('inf')
    # 使用邻接矩阵存储图
    # A B C D E F G
    matrix = [[0, 12, INF, INF, INF, 16, 14],
            [12, 0, 10, INF, INF, 7, INF],
            [INF, 10, 0, 3, 5, 6, INF],
            [INF, INF, 3, 0, 4, INF, INF],
            [INF, INF, 5, 4, 0, 2, 8],
            [16, 7, 6, INF, 2, 0, 9],
            [14, INF, INF, INF, 8, 9, 0]]
    S, distance, path_optimal = dijkstra(matrix, 3)
    print('S:')
    print(S)
    print('distance:')
    print(distance)
    print('path_optimal:')
    for p in path_optimal:
        print(p)

if __name__ == '__main__':
    main()

详细请见github仓库

4. c++代码实现

由于在自动驾驶中算法实现一般使用C++，所以我也使用C++实现了相关功能，代码结构与pythonRobotics的代码实现类似，这边就不再做相关代码解释了。完整代码详见另一个github仓库。文章来源地址https://www.toymoban.com/news/detail-717666.html

到了这里，关于【路径规划】全局路径规划算法——Dijkstra算法（含python实现 | c++实现）的文章就介绍完了。如果您还想了解更多内容，请在右上角搜索TOY模板网以前的文章或继续浏览下面的相关文章，希望大家以后多多支持TOY模板网！