第一章 路径规划基础：图搜索算法

大家好，我是你们的老朋友。今天咱们聊聊路径规划里最基础的东西——图搜索算法。

说实话，我刚入行那会儿，觉得路径规划不就是找条路嘛，有啥难的？后来真上了车，才发现事情没那么简单。你想想看，一个城市道路网，节点成千上万，怎么在几十毫秒内找到最优路径？这就是图搜索算法要解决的问题。

1.1 图的基本概念

先说说图是什么。图由节点和边组成。节点代表位置，边代表连接关系。比如十字路口是节点，路段是边。

图分两种：有向图和无向图。有向图里，边有方向，比如单行道。无向图里，边没方向，比如双向车道。

还有一种叫加权图。每条边带一个权重，代表距离、时间或代价。咱们做路径规划，基本都在加权图上干活。

1.2 BFS与DFS：最朴素的搜索

BFS（广度优先搜索）和DFS（深度优先搜索）是最基础的两种搜索策略。

BFS：一层一层往外扩。像水波一样，从起点开始，先搜所有距离为1的节点，再搜距离为2的，以此类推。

DFS：一条路走到黑。从起点出发，沿着一条路径一直往下走，走不通了再回头。

我刚开始学的时候，总觉得DFS更聪明，因为它能快速找到一条路径。但后来发现，BFS在路径规划里更实用。为什么？因为BFS找到的第一条路径就是最短路径（在无权图上）。

代码实现其实不复杂：

// BFS伪代码
function BFS(start, goal):
    queue = [start]
    visited = {start}
    parent = {}
    
    while queue is not empty:
        current = queue.pop(0)
        if current == goal:
            return reconstruct_path(parent, start, goal)
        
        for neighbor in current.neighbors:
            if neighbor not in visited:
                visited.add(neighbor)
                parent[neighbor] = current
                queue.append(neighbor)
    
    return null  // 没找到路径

1.3 Dijkstra算法：带权重的BFS

BFS只能处理无权图。但现实世界里的路，每条都有长度、时间、拥堵程度。这时候就需要Dijkstra算法了。

Dijkstra算法的核心思想：维护一个距离表，记录从起点到每个节点的最短距离。每次选距离最小的节点展开，更新邻居的距离。

说白了，Dijkstra就是BFS的升级版。BFS用队列，Dijkstra用优先队列（最小堆）。

代码实现：

// Dijkstra伪代码
function Dijkstra(start, goal):
    dist = {start: 0}
    parent = {}
    pq = priority_queue()
    pq.push((0, start))
    
    while pq is not empty:
        current_dist, current = pq.pop()
        
        if current == goal:
            return reconstruct_path(parent, start, goal)
        
        if current_dist > dist[current]:
            continue
        
        for neighbor, weight in current.neighbors:
            new_dist = current_dist + weight
            if new_dist < dist.get(neighbor, INF):
                dist[neighbor] = new_dist
                parent[neighbor] = current
                pq.push((new_dist, neighbor))
    
    return null

我曾经在一个项目中，用Dijkstra做全局路径规划。路网有5000多个节点，跑一次大概要200毫秒。对于全局规划来说，这个速度还行。但要是做局部规划，每100毫秒就要更新一次路径，Dijkstra就有点吃力了。

1.4 A*算法：启发式搜索

A*算法是Dijkstra的改进版。它引入了一个启发式函数，告诉算法「往哪个方向走更有可能找到终点」。

A*的代价函数：f(n) = g(n) + h(n)

• g(n)：从起点到节点n的实际代价
• h(n)：从节点n到终点的估计代价（启发式函数）

启发式函数怎么设计？最常用的是欧几里得距离和曼哈顿距离。

启发式函数	适用场景	特点
欧几里得距离	自由空间（如空地、空中）	直线距离，计算简单
曼哈顿距离	网格地图（如城市道路）	只允许上下左右移动
对角线距离	允许对角线移动的网格	比曼哈顿更精确

代码实现：

// A*伪代码
function AStar(start, goal):
    open_set = {start}
    closed_set = {}
    g = {start: 0}
    f = {start: heuristic(start, goal)}
    parent = {}
    
    while open_set is not empty:
        current = node in open_set with lowest f
        
        if current == goal:
            return reconstruct_path(parent, start, goal)
        
        open_set.remove(current)
        closed_set.add(current)
        
        for neighbor in current.neighbors:
            if neighbor in closed_set:
                continue
            
            tentative_g = g[current] + distance(current, neighbor)
            
            if neighbor not in open_set:
                open_set.add(neighbor)
            elif tentative_g >= g[neighbor]:
                continue
            
            parent[neighbor] = current
            g[neighbor] = tentative_g
            f[neighbor] = g[neighbor] + heuristic(neighbor, goal)
    
    return null

1.5 三种算法的对比

算法	适用场景	时间复杂度	空间复杂度	是否保证最优
BFS	无权图，小规模	O(V+E)	O(V)	是（无权图）
Dijkstra	加权图，中等规模	O((V+E)logV)	O(V)	是
A*	加权图，大规模	取决于启发式函数	O(V)	是（可采纳启发式）

1.6 工程实现中的坑

嗯，这里要注意几个容易踩坑的地方。

第一个坑：启发式函数设计不当。我曾经用曼哈顿距离做城市道路的启发式，结果A*跑得比Dijkstra还慢。后来发现，城市道路有斜街，曼哈顿距离低估了实际距离，导致搜索范围过大。

第二个坑：优先队列的实现。用二叉堆没问题，但要注意重复节点的处理。我见过有人把同一个节点多次入队，导致内存爆炸。

第三个坑：实时性要求。自动驾驶里，路径规划必须在几十毫秒内完成。如果地图太大，可以考虑分层规划——先粗粒度规划，再细粒度优化。

1.7 小结

这一章我们聊了图搜索的基础。BFS是最朴素的，Dijkstra是带权重的BFS，A*是带方向的Dijkstra。

说白了，这三兄弟一个比一个聪明，但一个比一个复杂。实际项目中，要根据场景选择合适的算法。

下一章，咱们聊聊更高级的路径规划算法——RRT和PRM。这些算法能处理高维空间和复杂约束，是自动驾驶里常用的技术。

记住，算法是工具，不是目的。理解每个算法的优缺点，才能在工程中做出正确的选择。

公众号:蓝海资料掘金营，微信deep3321