我将按照从简单到复杂的顺序，分三个阶段讲解寻路算法实现：广度优先搜索(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) |
| 是否最优 | 是（无权） | 是 | 是（启发式可采纳） |
| 适用场景 | 简单网格寻路 | 带权重路径规划 | 复杂场景高效搜索 |