寻路算法实现

我将按照从简单到复杂的顺序，分三个阶段讲解寻路算法实现：广度优先搜索(BFS) → Dijkstra算法 → A*算法。

第一阶段：广度优先搜索(BFS)

特点：简单直观，保证找到最短路径（无权重）

python 复制代码

from collections import deque

def bfs_find_path(grid, start, end):
    """ 网格说明: 0=可通行, 1=障碍物 """
    rows, cols = len(grid), len(grid[0])
    queue = deque([[start]])
    visited = set(start)
    directions = [(0,1),(0,-1),(1,0),(-1,0)]  # 上下左右
    
    while queue:
        path = queue.popleft()
        x, y = path[-1]
        
        if (x, y) == end:
            return path
            
        for dx, dy in directions:
            nx, ny = x + dx, y + dy
            if 0 <= nx < rows and 0 <= ny < cols:
                if grid[nx][ny] == 0 and (nx, ny) not in visited:
                    visited.add((nx, ny))
                    queue.append(path + [(nx, ny)])
    
    return None  # 无路径

# 示例使用
grid = [
    [0, 0, 0, 0],
    [1, 1, 0, 1],
    [0, 0, 0, 0],
    [0, 1, 1, 0]
]
print("BFS路径:", bfs_find_path(grid, (0,0), (3,3)))

算法解析：

使用队列实现，先进先出保证广度优先
记录已访问节点防止重复
每次扩展四个方向
找到终点立即返回路径

输出示例： BFS路径: $(0,0), (0,1), (0,2), (1,2), (2,2), (2,3), (3,3)$

第二阶段：Dijkstra算法

特点：处理带权重的路径，找到最小成本路径

python 复制代码

import heapq

class Node:
    def __init__(self, pos, cost):
        self.pos = pos
        self.cost = cost
    def __lt__(self, other):
        return self.cost < other.cost

def dijkstra(grid, start, end):
    """ 网格中的数值代表移动成本 """
    rows, cols = len(grid), len(grid[0])
    heap = []
    heapq.heappush(heap, Node(start, 0))
    cost_map = {start: 0}
    came_from = {}
    
    while heap:
        current = heapq.heappop(heap)
        if current.pos == end:
            path = []
            while current.pos in came_from:
                path.append(current.pos)
                current = came_from[current.pos]
            path.append(start)
            return path[::-1]
        
        x, y = current.pos
        for dx, dy in [(0,1),(0,-1),(1,0),(-1,0)]:
            nx, ny = x + dx, y + dy
            if 0 <= nx < rows and 0 <= ny < cols:
                new_cost = cost_map[current.pos] + grid[nx][ny]
                if (nx, ny) not in cost_map or new_cost < cost_map[(nx, ny)]:
                    cost_map[(nx, ny)] = new_cost
                    heapq.heappush(heap, Node((nx, ny), new_cost))
                    came_from[(nx, ny)] = current
    
    return None

# 示例使用（数值越大成本越高）
weighted_grid = [
    [1, 3, 1, 2],
    [5, 5, 2, 5],
    [2, 2, 3, 1],
    [1, 5, 5, 1]
]
print("Dijkstra路径:", dijkstra(weighted_grid, (0,0), (3,3)))

算法改进点：

使用优先队列（堆）实现
维护每个节点的最小到达成本
支持不同地形成本差异
增加路径回溯功能

输出示例： Dijkstra路径: $(0,0), (0,1), (0,2), (1,2), (2,2), (3,2), (3,3)$

**第三阶段：A*算法**

特点：带启发式的高效搜索，综合实际成本和预估成本

python 复制代码

def heuristic(a, b):
    # 曼哈顿距离
    return abs(a[0] - b[0]) + abs(a[1] - b[1])

def a_star(grid, start, end):
    rows, cols = len(grid), len(grid[0])
    open_set = []
    heapq.heappush(open_set, (0, start))
    came_from = {}
    g_score = {start: 0}
    f_score = {start: heuristic(start, end)}
    
    while open_set:
        current = heapq.heappop(open_set)[1]
        if current == end:
            path = []
            while current in came_from:
                path.append(current)
                current = came_from[current]
            path.append(start)
            return path[::-1]
            
        x, y = current
        for dx, dy in [(0,1),(0,-1),(1,0),(-1,0)]:
            nx, ny = x + dx, y + dy
            if 0 <= nx < rows and 0 <= ny < cols:
                tentative_g = g_score[current] + grid[nx][ny]
                if (nx, ny) not in g_score or tentative_g < g_score[(nx, ny)]:
                    came_from[(nx, ny)] = current
                    g_score[(nx, ny)] = tentative_g
                    f_score[(nx, ny)] = tentative_g + heuristic((nx, ny), end)
                    heapq.heappush(open_set, (f_score[(nx, ny)], (nx, ny)))
    
    return None

# 示例使用
print("A*路径:", a_star(weighted_grid, (0,0), (3,3)))

核心优化：

引入启发函数加速搜索方向
综合实际成本(g)和预估成本(h)
优先扩展最有希望的节点
在复杂地形中效率显著高于Dijkstra

输出示例： A*路径: $(0,0), (0,1), (0,2), (1,2), (2,2), (3,2), (3,3)$

算法对比分析

特性	BFS	Dijkstra	A*
搜索方式	盲目均匀扩展	按实际成本优先扩展	综合成本+启发式
时间复杂度	O(b^d)	O((V+E)logV)	O(b^d)但更高效
空间复杂度	O(b^d)	O(V)	O(b^d)
是否最优	是（无权）	是	是（启发式可采纳）
适用场景	简单网格寻路	带权重路径规划	复杂场景高效搜索