python 最优路线规划算法 路径规划python

文章目录

• 参考资料
• 1. 基本概念

• 1.1 算法简介
• 1.2 算法思想
• 1.3 算法图解
• 1.4 最短路径的最优子结构性质
• 1.5 算法说明

• 2. python代码实现
• 4. c++代码实现

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 算法图解

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

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

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

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

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



6. 将节点F加入到S中，同时更新U。



8. 将节点G加入到S中，同时更新U。



10. 将节点B加入到S中，同时更新U。



12. 将节点A加入到S中，同时更新U。



13. 此时，起点D到各个节点的最短距离就计算出来了：A(22) B(13) C(3) D(0) E(4) F(6) G(12)。
15. 最后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$，那么$P(i,j)=P(i,k)+P$。则与$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仓库。