📚 课程目录第4章经典路径规划算法（上）

4. 经典路径规划算法（上）

各位同学，今天我们来聊聊路径规划里最经典的两个算法：Dijkstra和A*。说实话，这两个算法我在本科时就学过，但真正理解它们的精髓，还是在做了几个机器人项目之后。你想想看，一个扫地机器人要绕过你家客厅的茶几，一个机械臂要抓取流水线上的零件——背后都离不开这些算法。

4.1 从图论说起：为什么路径规划需要图？

在讲算法之前，我们先统一一下语言。路径规划问题，说白了就是：给定起点和终点，找一条代价最小的路径。这里的「代价」可以是距离、时间、能耗，甚至是你自定义的任意指标。

我们通常用图（Graph）来表示环境。图由节点（Node）和边（Edge）组成。节点代表位置，边代表连通关系，边上可以带权重（Weight）表示代价。

核心概念速览

节点：机器人可以停留的位置
边：节点之间的可行路径
权重：通过这条边的代价（距离/时间/能量）
路径：从起点到终点的一系列节点

我在做仓储机器人项目时，一开始直接把仓库地图转成栅格图，每个格子就是一个节点。结果发现路径规划慢得离谱——因为节点太多了。后来我改用拓扑图，只保留货架间的关键通道，速度提升了10倍不止。嗯，这里要记住：图的构建方式直接影响算法效率。

4.2 Dijkstra算法：最朴素的「地毯式搜索」

Dijkstra算法是荷兰计算机科学家Edsger Dijkstra在1956年提出的。它的思路非常直观：从起点开始，一层层向外扩展，直到找到终点。就像你在黑暗中用手电筒照路，一步步照亮周围的环境。

4.2.1 算法核心思想

Dijkstra维护两个关键数据结构：

dist[]：记录从起点到每个节点的当前最短距离
visited[]：标记节点是否已确定最短路径

算法流程其实就三步：

初始化：起点dist=0，其他节点dist=∞
每次从未访问节点中选dist最小的，标记为已访问
更新该节点所有邻居的dist值

重复步骤2和3，直到所有节点都被访问，或者找到终点。

我的小经验：Dijkstra算法保证找到最短路径，但代价是「慢」。因为它要探索所有可能的方向，像个无头苍蝇一样到处乱撞。在大型地图上，这效率简直让人抓狂。

4.2.2 代码实现（Python）

import heapq

def dijkstra(graph, start, end):
    # graph: 邻接表 {node: [(neighbor, weight), ...]}
    # 初始化
    dist = {node: float('inf') for node in graph}
    dist[start] = 0
    prev = {node: None for node in graph}  # 记录前驱节点
    
    # 优先队列：(当前距离, 节点)
    pq = [(0, start)]
    visited = set()
    
    while pq:
        current_dist, current = heapq.heappop(pq)
        
        if current in visited:
            continue
        visited.add(current)
        
        # 找到终点，提前退出
        if current == end:
            break
            
        # 更新邻居
        for neighbor, weight in graph[current]:
            if neighbor in visited:
                continue
            new_dist = current_dist + weight
            if new_dist < dist[neighbor]:
                dist[neighbor] = new_dist
                prev[neighbor] = current
                heapq.heappush(pq, (new_dist, neighbor))
    
    # 回溯路径
    path = []
    node = end
    while node is not None:
        path.append(node)
        node = prev[node]
    path.reverse()
    
    return path, dist[end]

避坑指南：我曾经在项目中直接用列表来选最小dist节点，结果地图一大（几千个节点），每次选最小都要遍历整个列表，复杂度O(n²)。后来改用优先队列（heapq），复杂度降到O((V+E)logV)，速度提升肉眼可见。记住：Dijkstra一定要用优先队列实现。

4.3 A*算法：给搜索装上「指南针」

Dijkstra的问题在于它太「盲目」了。它不知道终点在哪边，只能向所有方向均匀扩展。A*算法则聪明得多——它用启发式函数（Heuristic Function）给搜索指明了方向。

4.3.1 启发式函数：A*的灵魂

A*的核心公式很简单：

f(n) = g(n) + h(n)

g(n)：从起点到节点n的实际代价（和Dijkstra一样）
h(n)：从节点n到终点的估计代价（启发式函数）
f(n)：综合评估值，A*优先扩展f值最小的节点

你想想看，h(n)就像个指南针，告诉算法「往这个方向走更有可能到达终点」。这样算法就不会在错误的方向上浪费太多时间。

常用的启发式函数

地图类型	启发式函数	说明
网格地图（四方向）	曼哈顿距离	\|x1-x2\| + \|y1-y2\|
网格地图（八方向）	切比雪夫距离	max(\|x1-x2\|, \|y1-y2\|)
任意地图	欧几里得距离	√((x1-x2)² + (y1-y2)²)

4.3.2 A*的「可采纳性」

这里有个关键点：h(n)必须小于等于实际代价，A*才能保证找到最短路径。这叫「可采纳性」（Admissibility）。

为什么会这样？因为如果h(n)高估了距离，算法可能会错过真正的最短路径。举个例子，你从北京去上海，h(n)估计是1000公里，实际只有1200公里，那没问题。但如果h(n)估计是2000公里，算法可能就绕远路了。

我的经验：在机器人项目中，我通常用欧几里得距离作为h(n)。它虽然简单，但满足可采纳性，而且计算快。有一次我尝试用更复杂的启发式函数，结果计算开销反而拖慢了整体速度。有时候，简单就是最好的。

4.3.3 代码实现（Python）

import heapq
import math

def heuristic(a, b):
    # 欧几里得距离
    return math.sqrt((a[0]-b[0])**2 + (a[1]-b[1])**2)

def a_star(graph, start, end, positions):
    # positions: 节点坐标字典 {node: (x, y)}
    open_set = [(0, start)]
    g_score = {node: float('inf') for node in graph}
    g_score[start] = 0
    f_score = {node: float('inf') for node in graph}
    f_score[start] = heuristic(positions[start], positions[end])
    prev = {node: None for node in graph}
    
    while open_set:
        current_f, current = heapq.heappop(open_set)
        
        if current == end:
            # 回溯路径
            path = []
            node = end
            while node is not None:
                path.append(node)
                node = prev[node]
            path.reverse()
            return path, g_score[end]
        
        for neighbor, weight in graph[current]:
            tentative_g = g_score[current] + weight
            if tentative_g < g_score[neighbor]:
                prev[neighbor] = current
                g_score[neighbor] = tentative_g
                f_score[neighbor] = tentative_g + heuristic(positions[neighbor], positions[end])
                heapq.heappush(open_set, (f_score[neighbor], neighbor))
    
    return None, float('inf')  # 无路径

4.4 Dijkstra vs A*：什么时候用哪个？

很多同学会问：「既然A*更快，那为什么还要学Dijkstra？」

这个问题问得好。我个人的经验是：

Dijkstra：适合所有边权重都为正的场景，而且你必须找到最短路径。比如在自动驾驶中，安全第一，多花点时间计算也值得。
A*：适合有明确目标方向的场景，而且你对实时性有要求。比如游戏中的NPC寻路，玩家可不想等几秒钟才看到角色动起来。

对比总结

特性	Dijkstra	A*
保证最短路径	✅ 是	✅ 是（h(n)可采纳时）
搜索方向	均匀向外	朝向目标
时间复杂度	O((V+E)logV)	O((V+E)logV)，但实际更快
适用场景	小地图、需要精确解	大地图、实时性要求高

4.5 实战中的坑与技巧

最后分享几个我在项目中踩过的坑：

地图太大怎么办？ 别直接用原始分辨率。先做地图压缩，或者用分层规划——高层用粗粒度地图找大致方向，低层用细粒度地图做局部优化。
动态障碍物怎么处理？ 每次重新规划太慢。我一般用D* Lite算法（后面会讲），它能在局部修改路径，不用全盘重算。
路径不平滑怎么办？ A*出来的路径都是折线。机器人走起来会一顿一顿的。我通常会在路径规划后加一个平滑步骤，比如用B样条曲线拟合。

曾经踩过的坑：有一次我在A*中用了曼哈顿距离作为启发式函数，但地图是八方向移动的。结果h(n)严重低估了实际距离，算法退化成Dijkstra，慢得不行。后来改成切比雪夫距离，速度立刻上来了。记住：启发式函数一定要和移动方式匹配。

4.6 本章小结

好了，这一章我们讲了Dijkstra和A*两个经典算法。说白了，Dijkstra是「老实人」，一步一个脚印；A*是「聪明人」，懂得走捷径。在实际项目中，我建议你先用A*试试，如果发现路径不是最优的，再检查一下启发式函数是否可采纳。