【自动驾驶】全局路径规划算法——Dijkstra算法

文章目录

  • 参考资料
  • 1. 基本概念
    • 1.1 算法简介
    • 1.2 算法思想
    • 1.3 算法图解
    • 1.4 最短路径的最优子结构性质
  • 2. python代码实现

参考资料

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

1. 基本概念

1.1 算法简介

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

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

1.2 算法思想

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

1.3 算法图解

【自动驾驶】全局路径规划算法——Dijkstra算法_第1张图片

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

  1. 初始时,S只包含起点D;U包含除 D外的其他节点,且U中节点的距离为起点D到该节点的距离,如果该节点与起点D不相邻,距离为无穷大。

    【自动驾驶】全局路径规划算法——Dijkstra算法_第2张图片

  2. 从U中选出距离最短的节点C,并将节点C加入到S中;同时,从U中移除节点C。然后,更新U中各个节点到起点D的距离。

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

    【自动驾驶】全局路径规划算法——Dijkstra算法_第3张图片

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

  1. 选取节点E,将E加入到S中,同时更新U中节点的距离。以节点F为例,之前F到D的距离为9;但是将E加入到S之后,F到D的距离为6=(F,E)+(E,D)。
    【自动驾驶】全局路径规划算法——Dijkstra算法_第4张图片

  2. 将节点F加入到S中,同时更新U。
    【自动驾驶】全局路径规划算法——Dijkstra算法_第5张图片

  3. 将节点G加入到S中,同时更新U。
    【自动驾驶】全局路径规划算法——Dijkstra算法_第6张图片

  4. 将节点B加入到S中,同时更新U。
    【自动驾驶】全局路径规划算法——Dijkstra算法_第7张图片

  5. 将节点A加入到S中,同时更新U。
    【自动驾驶】全局路径规划算法——Dijkstra算法_第8张图片
    此时,起点D到各个节点的最短距离就计算出来了:A(22) B(13) C(3) D(0) E(4) F(6) G(12)。

  6. 最后D->A的最优路径为D->E->F->A
    【自动驾驶】全局路径规划算法——Dijkstra算法_第9张图片

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

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

证明

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

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仓库

你可能感兴趣的:(#,规划,数据结构,迪杰斯特拉,人工智能,自动驾驶,路径规划)