如何基于AIMA代码框架在栅格地图上实现Floyd Warshall最短路径算法

核心适配逻辑 #

栅格地图本质是稠密无权/带权图，每个可通行栅格对应图的节点，相邻可通行栅格之间的移动对应图的边，边权为移动代价。你可以复用现有AIMA框架的GridProblem能力，仅需补充节点枚举、邻接矩阵构建的逻辑即可。

第一步：GridProblem类扩展 #

首先给GridProblem新增获取所有可通行节点的方法，用于构建邻接矩阵：

# 给GridProblem类新增如下方法
def get_all_passable_states(self):
    # 先确定栅格的坐标边界
    all_points = list(self.obstacles) + [self.initial, self.goal]
    xs = [p[0] for p in all_points]
    ys = [p[1] for p in all_points]
    min_x, max_x = int(min(xs)/self.grid_size) - 1, int(max(xs)/self.grid_size) + 1
    min_y, max_y = int(min(ys)/self.grid_size) - 1, int(max(ys)/self.grid_size) + 1
    passable_states = []
    # 遍历所有栅格，筛选可通行的节点
    for gx in range(min_x, max_x + 1):
        for gy in range(min_y, max_y + 1):
            if (gx, gy) not in self.gridmap:
                # 栅格坐标转实际坐标
                state = (round(gx * self.grid_size, 3), round(gy * self.grid_size, 3))
                passable_states.append(state)
    return passable_states

第二步：Floyd Warshall算法实现 #

参照BFS的封装思路，实现输入为Problem实例、输出为Node实例的算法函数，保证和现有框架兼容：

def floyd_warshall_search(problem):
    # 1. 预处理可通行节点和映射关系
    passable_states = problem.get_all_passable_states()
    state_to_idx = {s: i for i, s in enumerate(passable_states)}
    idx_to_state = {i: s for i, s in enumerate(passable_states)}
    N = len(passable_states)
    INF = float('inf')
    
    # 2. 初始化距离矩阵和前驱矩阵
    dist = [[INF] * N for _ in range(N)]
    prev = [[None] * N for _ in range(N)]
    for i in range(N):
        dist[i][i] = 0
        s = idx_to_state[i]
        # 复用GridProblem的actions方法获取相邻可到达节点
        for s1 in problem.actions(s):
            if s1 not in state_to_idx:
                continue
            j = state_to_idx[s1]
            dist[i][j] = problem.action_cost(s, None, s1)
            prev[i][j] = i
    
    # 3. 执行Floyd Warshall核心迭代
    for k in range(N):
        for i in range(N):
            for j in range(N):
                if dist[i][k] + dist[k][j] < dist[i][j]:
                    dist[i][j] = dist[i][k] + dist[k][j]
                    prev[i][j] = prev[k][j]
    
    # 4. 查找起点终点对应的索引
    initial_idx = state_to_idx.get(problem.initial)
    goal_idx = state_to_idx.get(problem.goal)
    if initial_idx is None or goal_idx is None or dist[initial_idx][goal_idx] == INF:
        return Node.failure
    
    # 5. 回溯前驱矩阵构造路径，生成符合AIMA规范的Node节点
    path = []
    current_idx = goal_idx
    while current_idx is not None:
        path.append(idx_to_state[current_idx])
        current_idx = prev[initial_idx][current_idx]
    path = path[::-1] # 反转得到从起点到终点的序列
    
    # 构造Node链，兼容现有path_states等工具方法
    root = Node(path[0])
    current_node = root
    for i in range(1, len(path)):
        s = path[i-1]
        s1 = path[i]
        cost = current_node.path_cost + problem.action_cost(s, None, s1)
        current_node = Node(s1, parent=current_node, path_cost=cost)
    return current_node

注意事项 #

• Floyd Warshall算法时间复杂度为O(N³)，仅适合节点数少于200的小尺寸栅格地图使用，大尺寸地图更推荐使用Dijkstra或A*算法，性能会高几个数量级。
• 你当前GridProblem中的to_grid方法可以简化为lambda state: (int(state[0]/self.grid_size), int(state[1]/self.grid_size))，原有写法的乘10再除10操作是冗余的，还可能引入浮点计算误差。
• 如果你只需要求解单起点到单终点的最短路径，Floyd Warshall并不是最优选择，它的优势是一次计算可以得到所有节点对的最短路径，适合需要多次查询路径的场景。

内容的提问来源于stack exchange，提问作者Pascal Zurich