（6-4）自动驾驶中的全局路径精简计算：校内机器人配送系统

6.4 实战案例：校内机器人配送系统

校内机器人配送系统是一项智能化服务，旨在为校园内的学生、教职员工提供便捷的物品配送服务。通过该系统，用户可以通过手机应用或者指定终端设备下单，指定起始点和目的地，机器人将会根据用户需求自主规划最优路径，快速、准确地将物品送达目的地。这项系统的主要功能包括下单、路径规划、自主导航和快速配送，极大地提升了校园内物品传递的效率和便利性。

实例6-4：校内机器人配送系统（codes/6/school.py）

6.4.1 项目介绍

本校内机器人配送系统旨在利用校园内的机器人网络来帮助学生、教职员工和访客快速、便捷地到达目的地。在这个系统中，可以通过输入起点和终点编号来查询最短路径和距离，也可以查询各个地点的信息。

6.4.2 具体实现

本项目实现了一个校园导航系统，用户可以通过命令行界面查询校园内任意两点之间的最短路径和距离，并获取每个地点的名称和简介信息。通过使用简单的命令行界面，用户可以进行如下所示的操作。

1. 查找最短路径：用户可以输入起点和终点编号，系统将计算并显示两点之间的最短路径和距离。
2. 查询信息：用户可以输入地点编号，系统将显示该地点的名称和简介。
3. 退出程序：用户可以选择退出程序，结束系统的运行。

本系统的功能全面，包括地点信息的初始化、路径长度的计算、最短路径的查找等功能，提供了校园导航所需的基本功能。

实例文件school.py的具体实现流程如下所示。

（1）导入库文件，声明需要的变量和常量。

import numpy as np import matplotlib.pyplot as plt import matplotlib matplotlib.rcParams['font.family'] = 'sans-serif' matplotlib.rcParams['font.sans-serif'] = ['SimHei'] # 使用宋体或其他你下载的中文字体 Num = 16 Maxedge = np.iinfo(np.uint64).max shortest = np.full((Num, Num), Maxedge, dtype=np.uint64) path = np.full((Num, Num), -1, dtype=int)

上述代码的实现流程如下所示：

1. 导入所需的库：导入NumPy和Matplotlib库，用于处理数据和绘制图表。
2. 设置Matplotlib中文显示：通过设置Matplotlib的rcParams参数，将字体设置为中文（SimHei）。
3. 定义变量和数组：定义了学校的顶点数量Num、最大边界Maxedge，以及用于存储最短路径和路径信息的数组shortest和path。
4. 初始化数组：使用np.full函数将shortest和path数组初始化为指定大小，并且填充最大值和-1作为初始值。

（2）下面的代码定义了类Vertex，用于表示图中的顶点，每个顶点具有如下所示的属性。

1. name：顶点的名称。
2. introduce：顶点的介绍或描述。
3. x：顶点在平面图上的横坐标。
4. y：顶点在平面图上的纵坐标。

通过类Vertex可以创建表示不同顶点的对象，并为每个顶点指定名称、介绍以及在平面图上的坐标位置。

class Vertex: def __init__(self, name, introduce, x, y): self.name = name self.introduce = introduce self.x = x self.y = y

（3）下面的代码定义了一个包含了16个顶点的列表vertices，每个顶点包括地点名称、地点介绍以及在平面图上的坐标位置。同时，还定义了一个表示顶点之间边的矩阵edge，矩阵的值表示顶点之间的距离或权重。

vertices = [ Vertex("大门", "正门", 0, 0), Vertex("图书馆", "图书馆", 0, 5), Vertex("操场", "老校区操场", 2, 5), Vertex("6号宿舍", "老校区男生宿舍区", 3, 0), Vertex("小礼堂", "晚会活动的举办场地", 3, 3), Vertex("下沉广场", "老校区下沉环形广场", 5, 5), Vertex("学苑餐厅", "老校区餐厅", 7, 5), Vertex("专家公寓", "外教公寓", 10, 5), Vertex("教工3村", "教职工住址", 10, 0), Vertex("荷花池", "小景点", 8, 0), Vertex("9号教学楼", "文法学院教学楼", 5, 0), Vertex("12号教学楼", "音乐学院教学楼", 7, 0), Vertex("15号教学楼", "软件学院教学楼", 9, 0), Vertex("学生广场", "活动广场", 5, 3), Vertex("网球场", "新校区操场", 2, 7), Vertex("东南餐厅", "新校区餐厅", 0, 7), ] edge = np.full((Num, Num), Maxedge, dtype=int) edge[0][1] = edge[1][0] = 60 edge[0][2] = edge[2][0] = 90 edge[0][3] = edge[3][0] = 100 edge[1][2] = edge[2][1] = 20 edge[1][3] = edge[3][1] = 20 edge[1][4] = edge[4][1] = 40 edge[2][4] = edge[4][2] = 65 edge[2][8] = edge[8][2] = 250 edge[3][5] = edge[5][3] = 80 edge[4][5] = edge[5][4] = 55 edge[4][6] = edge[6][4] = 50 edge[4][7] = edge[7][4] = 100 edge[5][6] = edge[6][5] = 130 edge[6][7] = edge[7][6] = 150 edge[7][8] = edge[8][7] = 100 edge[8][9] = edge[9][8] = 40 edge[9][10] = edge[10][9] = 60 edge[9][11] = edge[11][9] = 30 edge[9][13] = edge[13][9] = 75 edge[10][12] = edge[12][10] = 50 edge[10][13] = edge[13][10] = 15 edge[11][12] = edge[12][11] = 45 edge[12][14] = edge[14][12] = 160 edge[13][14] = edge[14][13] = 100 edge[14][15] = edge[15][14] = 120

（4）下面的函数draw_map()实现了一个绘制学校路径图的功能。首先定义了顶点类Vertex，其中包括顶点的名称、简介以及在二维平面上的坐标。然后创建了一组顶点列表vertices，其中包含了学校的各个地点，并初始化了一个二维数组edge，表示各个地点之间的路径长度。

def draw_map(): plt.figure(figsize=(10, 7)) for vertex in vertices: plt.scatter(vertex.x, vertex.y, color='blue') plt.text(vertex.x, vertex.y, f'{vertex.name} ({vertices.index(vertex)+1})', fontsize=9) for i in range(Num): for j in range(Num): if edge[i][j] < Maxedge: plt.plot([vertices[i].x, vertices[j].x], [vertices[i].y, vertices[j].y], color='black') plt.title('学校路径图') plt.xlabel('X 坐标') plt.ylabel('Y 坐标') plt.grid(True) plt.show()

（5）下面的代码定义了函数informatation()，用于查询地点的信息。函数首先通过用户输入获取地点的编号，然后根据编号在vertices列表中查找对应的地点信息，并输出地点的名称和简介。如果输入的编号不在有效范围内，则提示用户重新输入。

def informatation(): idx = int(input("请输入查询地点的编号：")) - 1 if 0 <= idx < Num: print("\n名称：", vertices[idx].name) print("简介：", vertices[idx].introduce) else: print("输入有误！请重新输入！")

（6）下面的代码定义了函数menu()，用于提供导航系统的交互菜单。该函数通过一个无限循环，不断显示菜单选项，并根据用户输入的选项执行相应的操作。本项目包含了如下所示的菜单选项：

1. 输入 'c'：查询最短路径。
2. 输入 'x'：查询地点信息。
3. 输入 'm'：绘制学校路径图。
4. 输入 'e'：退出程序

函数menu()根据用户的选择，调用相应的函数来执行相应的操作。如果用户输入了无效的选项，则显示输入错误的提示信息。

def menu(): while True: print("\n\t\t\t欢迎使用导航系统\n") print("输入 'c' 查询最短路径") print("输入 'x' 查询信息") print("输入 'm' 绘制学校路径图") print("输入 'e' 退出程序") choice = input("请输入对应的英文小写字母：") if choice == 'c': shortestpath() elif choice == 'x': informatation() elif choice == 'm': draw_map() elif choice == 'e': print("\n\n\n\t\t\t谢谢使用！\n") break else: print("输入错误")

（7）下面的代码定义了函数floyd()，用于执行 Floyd-Warshall 算法，计算图中各个顶点之间的最短路径和最短距离。函数floyd()首先初始化了距离矩阵shortest和路径矩阵path，然后使用三重循环来计算最短路径。具体实现步骤如下所示。

1. 遍历图中所有顶点对(i, j)，初始化距离矩阵shortest[i][j]为顶点i到顶点j的直接距离（如果两个顶点之间有边相连）或者无穷大（如果两个顶点之间没有边相连）。同时，如果两个顶点之间有边相连，则将路径矩阵path[i][j]初始化为顶点i。
2. 使用三重循环遍历所有顶点对(i, j)和所有中间顶点k，尝试通过顶点k缩短顶点i到顶点j的距离。如果从顶点i经过顶点k到达顶点j的距离比当前记录的最短距离shortest[i][j]更短，则更新最短距离和路径矩阵。

通过以上步骤，函数floyd()最终计算出了所有顶点对之间的最短路径和最短距离。

def floyd(): for i in range(Num): for j in range(Num): shortest[i][j] = edge[i][j] if shortest[i][j] < Maxedge: path[i][j] = i else: path[i][j] = -1 for k in range(Num): for i in range(Num): for j in range(Num): if shortest[i][j] > shortest[i][k] + shortest[k][j]: shortest[i][j] = shortest[i][k] + shortest[k][j] path[i][j] = path[k][j]

（8）下面的代码定义了函数way(i, j)，用于打印从顶点i到顶点j的最短路径和最短距离。函数way(i, j)首先接受两个参数i和j，表示起始顶点和目标顶点。然后根据计算出的最短路径矩阵path和最短距离矩阵shortest，打印出从顶点i到顶点j的最短路径和最短距离。函数way(i, j)的具体实现步骤如下所示。

1. 首先，将起始顶点i和目标顶点j保存到变量a和b中，用于后续打印路径。
2. 如果从顶点i到顶点j存在最短路径（即shortest[i][j]不等于最大值），则打印出从顶点i到顶点j的最短路径和最短距离。打印过程中，使用路径矩阵path回溯出最短路径上的顶点，并依次打印路径上的顶点名称。最后打印最短距离。
3. 如果从顶点i到顶点j不存在最短路径（即shortest[i][j]等于最大值），则打印提示信息表示无法到达。

通过以上步骤，函数way(i, j)打印输出了从顶点i到顶点j的最短路径和最短距离信息。

def way(i, j): a, b = i, j if shortest[i][j] != Maxedge: print(f"\n从 {vertices[i].name} 到 {vertices[j].name} 的最短路径为：") print(vertices[i].name, end="") while path[i][j] != -1 and path[i][j] != i: print(" -到-", vertices[path[i][j]].name, end="") i = path[i][j] print(" -到-", vertices[b].name) print("\n最短距离为：", shortest[a][b], "米。") else: print(f"从 {vertices[i].name} 到 {vertices[j].name} 不能到达")

（9）下面的代码定义了函数shortestpath()，用于查询两个顶点之间的最短路径和最短距离，并在程序入口处进行了调用。函数shortestpath()首先通过用户输入获取要查询的两个顶点的编号，并将其转换为整数类型的起始顶点和目标顶点。然后检查输入的顶点编号是否在合法范围内（0到Num-1之间），如果在合法范围内，则调用floyd()函数计算最短路径矩阵和最短距离矩阵，然后调用way()函数打印从起始顶点到目标顶点的最短路径和最短距离。如果输入的顶点编号不在合法范围内，则打印提示信息要求重新输入。

def shortestpath(): start, end = map(int, input("请输入要查询的两点的编号：").split()) start -= 1 end -= 1 if 0 <= start < Num and 0 <= end < Num: floyd() way(start, end) else: print("输入有误！请重新输入！") if __name__ == "__main__": draw_map() menu()

在上述代码中，首先调用函数draw_map()绘制学校路径图，然后调用menu()函数进入用户交互菜单，等待用户输入操作指令。根据用户输入的指令，执行相应的功能：查询最短路径、查询顶点信息、绘制学校路径图或退出程序。

执行后首先打印出了校园平面图，展示了各个地点之间的位置信息，如图6-8所示。接着，提示用户输入对应的操作选项，用户可以选择查询最短路径、查询地点信息或退出程序。例如在下面的执行演示中，用户选择了查询最短路径操作，并输入了要查询的起点和终点地点编号。程序计算出从起点到终点的最短路径，并输出了路径上的地点以及最短距离。最后，再次展示了用户操作选项，等待用户下一步操作。

欢迎使用导航系统 输入 'c' 查询最短路径 输入 'x' 查询信息 输入 'm' 绘制学校路径图 输入 'e' 退出程序 请输入对应的英文小写字母：c 请输入要查询的两点的编号：1 2 大门 -到- 图书馆 最短距离为： 58 米。 欢迎使用导航系统 输入 'c' 查询最短路径 输入 'x' 查询信息 输入 'm' 绘制学校路径图 输入 'e' 退出程序 请输入对应的英文小写字母：c 请输入要查询的两点的编号：1 5 从 大门 到 小礼堂 的最短路径为： 大门 -到- 图书馆 -到- 小礼堂 最短距离为： 98 米。 欢迎使用导航系统 输入 'c' 查询最短路径 输入 'x' 查询信息 输入 'm' 绘制学校路径图 输入 'e' 退出程序 请输入对应的英文小写字母：c 请输入要查询的两点的编号：2 5 从 图书馆 到 小礼堂 的最短路径为： 图书馆 -到- 小礼堂 最短距离为： 40 米。 欢迎使用导航系统 输入 'c' 查询最短路径 输入 'x' 查询信息 输入 'm' 绘制学校路径图 输入 'e' 退出程序

图6-8 学校地点可视化图