（4-5）Floyd-Warshall算法：高速公路路线查询系统

4.5 高速公路路线查询系统

本项目基于阿鲁巴岛的实际公路数据，实现了 Floyd-Warshall 算法来计算所有高速公路节点之间的最短路径。通过解析包含路线和节点地理位置信息的文本文件，程序构建了一个加权邻接矩阵，并利用哈佛赛因距离计算路径权重。最终，项目输出展示了阿鲁巴岛上各个路段的最短路径信息，为路网规划和分析提供了实用的工具和数据支持。

实例4-8：寻找高速公路节点之间的最短路径（codes/4/Floyd-Warshall）

4.5.1 背景介绍

随着自动驾驶技术的推广，路径规划是其中的关键之一。路径规划多个因素，如道路状况、交通规则和实时交通状态，以确保车辆能够安全、高效地到达目的地。阿鲁巴岛作为一个不断发展的地理区域，其公路网络是路径规划的关键。为了支持自动驾驶在阿鲁巴岛上的实际应用，本项目利用阿鲁巴岛的公路数据，实现了一个基于Floyd-Warshall算法的路径规划工具。

4.5.2 交通路网数据

在文件aruba.txt中保存了阿鲁巴岛的路网信息和节点信息，阿鲁巴岛是位于加勒比海南部的一个岛屿，是荷兰王国的一个自治国家，也是加勒比海地区的一个旅游热点。它位于南美洲的北部，距离委内瑞拉约27公里，是少数几个地理上属于南美洲但文化上和政治上与加勒比海国家更紧密联系的地区之一。

TMG 1.0 simple
28 37
Rt1A/Rt1B@SerCol 12.434152 -69.877118
Rt1A@Rt4_E&Rt1B@Rt4_E&Rt4@Rt1_S 12.480199 -69.979338
Rt4/Rt6 12.53521 -70.006943
Rt1A@Rt4_W&Rt1B@Rt4_W&Rt4@Rt1_N 12.545546 -70.053163
Rt4_N/Rt7_E 12.510335 -69.98114
Rt1B/Rt6 12.52327 -70.027542
Rt4_S/Rt7_W 12.509455 -69.980989
Rt1A/Rt1B@SpaLagWeg 12.458556 -69.959124
Rt1B_S/Rt2_S 12.524663 -70.034618
Rt6@Jam 12.491387 -69.954179
Rt1B/Rt7 12.518908 -70.027177
Rt2@CayaCan 12.575602 -70.034355
Rt6@Mat 12.539729 -69.976971
Rt2@CayaSolo 12.600407 -70.042068
Rt6/Rt7 12.500342 -69.95273
Rt2/Rt3 12.56336 -70.03207
Rt7@AriNP 12.49874 -69.946545
Rt7@AveWatVos 12.519269 -70.013337
Rt2/Rt4 12.545913 -70.030685
Rt1A/Rt1B@AveWatVos 12.506528 -70.008391
Rt1A_W/Rt1B_W 12.508785 -70.02736
Rt3@Cal 12.557747 -70.003767
Rt1A/Rt1B@BerStr 12.440442 -69.91902
Rt1A_E/Rt1B_E 12.521621 -70.041903
Rt3/Rt4 12.530665 -70.000543
Rt3/Rt1A/Rt1B 12.568957 -70.042745
Rt1A@Rt2_N&Rt1B@Rt2_N&Rt2@Rt1_N 12.600847 -70.05018
Rt3/Rt6 12.538808 -70.001477
14 9 Rt6
10 17 Rt7
17 6 Rt7
0 22 Rt1A,Rt1B
4 14 Rt7
22 7 Rt1A,Rt1B
14 16 Rt7
7 1 Rt1A,Rt1B
1 19 Rt1A,Rt1B
19 20 Rt1A,Rt1B
20 23 Rt1A
23 3 Rt1A,Rt1B
3 25 Rt1A,Rt1B
25 26 Rt1A,Rt1B
20 10 Rt1B
10 5 Rt1B
5 8 Rt1B
8 23 Rt1B
8 18 Rt2
18 15 Rt2
15 11 Rt2
11 13 Rt2
13 26 Rt2
25 15 Rt3
15 21 Rt3
21 27 Rt3
27 24 Rt3
3 18 Rt4
18 2 Rt4
2 24 Rt4
24 4 Rt4
4 6 Rt4,Rt7
6 1 Rt4
5 2 Rt6
2 27 Rt6
27 12 Rt6
12 14 Rt6

文件aruba.txt包含了阿鲁巴岛的路网信息和节点连接关系，主要分为如下所示的三个主要部分：

（1）路网信息：文件开始部分列出了每条路线的详细信息，包括路线名称、经度等。例如：

Rt1A/Rt1B@SerCol 12.434152 -69.877118

表示路线名称为 "Rt1A/Rt1B@SerCol"，起点纬度为 12.434152，经度为 -69.877118。

（2）节点连接关系：表示节点之间的连接关系，例如：

14 9 Rt6

表示从节点索引为 14 的节点到节点索引为 9 的节点有一条经过路线 "Rt6" 的有向连接。

（3）节点索引：文件命名列出节点的索引信息，每行表示一个节点索引和对应的路线信息。例如：

20 23 Rt1A

表示节点索引为 20，路线信息为 "Rt1A"。

上述三部分共同构成了文件aruba.txt中描述阿鲁巴岛的路网结构。它被用于实现和运行路网分析、路径规划或者导航系统等应用。

4.5.3 寻找最短路径

文件Floyd Warshall_Eric Einhaus.py的功能是使用Floyd-Warshall算法计算最短路径，并提供了用于解析高速公路图数据的函数。在本实例中，首先读取阿鲁巴岛的高速公路图数据文件，构建加权邻接矩阵表示图，并利用Floyd-Warshall算法计算所有节点之间的最短路径。

import math
import copy

class WeightedAdjacencyMatrix:

    __slots__ = ['_W']

    def __init__(self, size):
        """初始化具有大小节点的加权邻接矩阵，图中有大小节点和0条边。
        关键字参数：
        size -- 图中节点的数量。
        """
        self._W = list() #the list of lists
        for i in range(size):
            row_i = list()
            for j in range(size):
                if i == j: #diagonal 
                    row_i.append(0)
                else: #anything else
                    row_i.append(math.inf)
            self._W.append(row_i)
        
    def add_edge(self, u, v, weight):
        """添加从u到v的有向边，带有指定的权重。
        关键字参数：
        u -- 源顶点的ID（从0开始的索引）
        v -- 目标顶点的ID（从0开始的索引）
        weight -- 边的权重
        """
        self._W[u][v] = weight

    def floyd_warshall(self):
        """Floyd Warshall算法用于计算所有节点对之间的最短路径。
        返回一个矩阵D，其中包含所有节点对之间的最短路径的权重。
        此方法不会改变图本身的权重矩阵。
        额外的学分版本：返回一个元组（D，P），其中D是包含所有节点对之间的最短路径权重的矩阵，P是前任矩阵。
        """
        D = copy.deepcopy(self._W)
        n = len(D)

        for k in range(n):
            for i in range(n):
                for j in range(n):
                    if D[i][j] > D[i][k] + D[k][j]:
                        D[i][j] = D[i][k] + D[k][j]
        return D

def haversine_distance(lat1, lng1, lat2, lng2):
    """计算两点之间的经纬度哈弗辛距离。
    关键字参数：
    lat1 -- 第一个点的纬度
    lng1 -- 第一个点的经度
    lat2 -- 第二个点的纬度
    lng2 -- 第二个点的经度
    返回哈弗辛距离（单位：米）。
    """
    R = 6371000 #地球半径，单位米
    phi_1 = math.radians(lat1)
    phi_2 = math.radians(lat2)
    delta_phi = math.radians(lat2 - lat1)
    delta_lambda = math.radians(lng2 - lng1)
    a = math.sin(delta_phi / 2) * math.sin(delta_phi / 2) + math.cos(phi_1) * math.cos(phi_2) * math.sin(delta_lambda / 2) * math.sin(delta_lambda / 2)
    c = 2 * math.atan2(math.sqrt(a), math.sqrt(1 - a))
    d = R * c
    return d

def parse_highway_graph_data(filename):
    """解析高速公路图文件并返回一个WeightedAdjacencyMatrix对象。
    关键字参数：
    filename -- 文件名。
    返回一个WeightedAdjacencyMatrix对象。
    """
    latList = list() #存储顶点V的纬度列表
    lngList = list() #存储顶点V的经度列表
    edgeList = list() #存储由顶点对(E from-to)组成的元组列表
    V, E = 0, 0
    with open(filename) as f :
        f.readline() #跳过第一行
        VE = f.readline().split(' ')
        V, E = VE[0], VE[1]
        for line in f :
            for i in range(0, int(V)) :
                IDLatLong = line.split(' ')
                lat = float(IDLatLong[1])
                latList.append(lat) #顶点ID为i，lat[i]为纬度
                lng = float(IDLatLong[2])
                lngList.append(lng) #顶点ID为i，lng[i]为经度
                line = f.readline()
            for j in range(0, int(E)) :
                fromTo = line.split(' ')
                edgeList.append((fromTo[0], fromTo[1]))
                line = f.readline()

    G = WeightedAdjacencyMatrix(int(V))
    for i in range(0, len(edgeList)) :
        v1 = int(edgeList[i][0])
        v2 = int(edgeList[i][1])
        weight = haversine_distance(latList[v1], lngList[v1], latList[v2], lngList[v2])
        G.add_edge(v1, v2, weight)
        G.add_edge(v2, v1, weight)
    return G

def test_with_your_own_graphs():
    m = WeightedAdjacencyMatrix(4) #使用inf权重初始化WeightedAdjancencyMatrix（除了对角线，对角线权重为0）
    m.add_edge(1, 3, 5)
    m.add_edge(0, 1, 2)
    m.add_edge(0, 2, 4)
    m.add_edge(1, 2, -1)
    m.add_edge(3, 2, 6)
    m.add_edge(3, 0, 4)
    D = m.floyd_warshall()
    print("Weighted Adjacency Matrix")
    for i in range(len(D)) :
        for j in range(len(D)) :
            print(D[i][j], '\t', end=' ')
        print()

def test_with_highway_graph(L):
    G = parse_highway_graph_data('aruba.txt') #文件名放在这一行
    D = G.floyd_warshall()
    for i in range(len(D)) :
        for j in range(len(D)) :
            print(i, j, D[i][j], D[j][i], sep='\t')

if __name__ == "__main__":
    '''
        阿鲁巴的图有28个顶点
    '''
    L = []
    for x in range(28) :
        for y in range(28) :
            L.append((x, y))
    test_with_highway_graph(L)

对上述代码的具体说明如下所示：

函数parse_highway_graph_data(filename)：负责解析名为aruba.txt的高速公路图文件，该文件包含阿鲁巴岛公路网络的详细信息，包括顶点（路口或位置）的经纬度和边（道路段）的连接关系。解析后，将构建一个WeightedAdjacencyMatrix对象，表示图中各节点之间的加权邻接关系。
加权邻接口实现：类WeightedAdjacencyMatrix提供了加权邻接矩阵的实现。在初始化时，所有节点的初始权重设为无穷大（表示不超过），对角线元素为零（表示节点到自身的距离为零）。通过add_edge(u, v, weight)方法可以向矩阵中添加指定边的权重。
路径计算与最短路径查找：floyd_warshall()方法实现了Floyd-Warshall算法，用于计算图中所有节点之间的最短路径。算法的输入是加权邻接矩阵，输出是一个包含所有节点对最短路径的权重矩阵。通过动态规划的方式，该算法能够高效地处理中等规模的图，找到任意两点之间的最短路径。
计算哈弗辛距离：函数haversine_distance(lat1, lng1, lat2, lng2)用于计算地球表面上两点之间的哈弗辛距离。该距离计算方法考虑了地球面的曲率，适用于经纬度坐标系下的距离计算。在本项目中，该函数用于根据顶点的经纬度计算道路段的实际距离，并作为边的体重加权邻接矩阵中。
测试和应用：函数test_with_your_own_graphs()和test_with_highway_graph(L)用于测试路径计算功能。前者测试了在小规模图上的路径计算和最短路径查找，后者则将实际的阿鲁巴岛公路数据输入到系统中进行路径计算，并输出每对节点之间的最短路径重量。

执行文件Floyd Warshall_Eric Einhaus.py后会进行以下操作：

解析文件aruba.txt：会读取aruba.txt文件，并解析其中的节点（顶点）信息和边（路径）信息。该计划将用于构建加权邻接矩阵。
构建加权邻接口：使用WeightedAdjacencyMatrix类中的add_edge方法，将每路径的经纬度计算，并将结果作为路径的体重加入到矩阵中。
执行Floyd-Warshall算法：调用floyd_warshall方法对构建的邻接矩阵执行Floyd-Warshall算法，计算所有节点之间的最短路径。
显示结果：打印输出将显示每行节点的最短路径重量，展示每行一对节点的最短路径重量。例如输出：