第四章：路径规划基础——图搜索算法与启发式设计

各位同学，欢迎来到路径规划的第一课。说实话，每次讲到这一章，我都会想起自己刚入行时踩过的坑。那时候我天真地以为，路径规划就是“找条路走过去”，结果在实车上跑的时候，车直接怼到了路肩上。嗯，从那以后我才明白——图搜索算法是规划控制的地基，地基不稳，上层算法再花哨也是白搭。

4.1 图搜索算法概述

先聊聊什么是“图”。你想想看，我们开车时面对的道路网络，本质上就是一个图——路口是节点，道路是边。路径规划要做的，就是在这个图里找到一条从起点到终点的最优路径。

我个人习惯把图搜索算法分成两类：

  • 盲目搜索：比如BFS、DFS，它们不看目标在哪，只管往外扩。适合小地图，但大场景下效率堪忧。
  • 启发式搜索：比如A*，它会“猜”一下目标的方向，优先探索有希望的区域。说白了，就是带点“直觉”的搜索。

我在项目中遇到过最典型的场景：用BFS做园区低速泊车，地图只有几十米，BFS完全够用。但一旦上了高速，地图扩大到公里级，BFS的搜索空间会爆炸——这时候就必须上A*了。

核心要点： 图搜索算法的本质是“状态空间遍历”。你选择的算法决定了遍历的顺序和效率。

4.2 Dijkstra算法原理

Dijkstra算法，说白了就是“贪心+动态规划”的结合体。它保证能找到从起点到所有节点的最短路径，前提是边的权重非负。

算法流程其实很简单：

  1. 初始化：起点距离为0，其他节点距离为无穷大。
  2. 每次从未访问的节点中，选距离最小的那个。
  3. 更新它的邻居：如果通过当前节点能走更短的路，就更新距离。
  4. 重复，直到所有节点都被访问过。

我给大家一个伪代码，方便理解：

function Dijkstra(Graph, start):
    dist[start] = 0
    for each node v in Graph:
        if v != start: dist[v] = INF
        visited[v] = False
    
    while 还有未访问节点:
        u = 未访问节点中dist最小的
        visited[u] = True
        for each neighbor v of u:
            new_dist = dist[u] + weight(u, v)
            if new_dist < dist[v]:
                dist[v] = new_dist
                prev[v] = u
    return dist, prev

这里有个坑，我曾经吃过亏——Dijkstra要求所有边的权重必须非负。为什么？因为一旦有负权边，你之前选出的“最小距离节点”可能后面会被更短的路径更新掉，算法就失效了。如果你遇到负权场景，请改用Bellman-Ford。

避坑指南： 我曾经在项目中用Dijkstra做代价地图规划，结果地图里有个“禁止区域”被我设成了负代价，导致路径直接穿墙而过。后来排查了一整天才发现——嗯，Dijkstra真的不能处理负权。

4.3 A*算法原理

A*算法是Dijkstra的“升级版”。它引入了一个启发式函数，让搜索更有方向感。

A*的核心公式就一个：

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

  • g(n)：从起点到当前节点n的实际代价。
  • h(n)：从当前节点n到终点的估计代价（启发式函数）。
  • f(n)：综合代价，A*每次选f最小的节点扩展。

你想想看，Dijkstra只考虑g(n)，所以它像个无头苍蝇一样四面八方乱撞。而A*多了个h(n)，相当于给搜索装了个“指南针”，优先往目标方向走。

伪代码长这样：

function AStar(Graph, start, goal):
    open_list = [start]
    closed_list = []
    g[start] = 0
    f[start] = h(start, goal)
    
    while open_list 不为空:
        current = open_list中f最小的节点
        if current == goal:
            return 重构路径
        
        open_list.remove(current)
        closed_list.add(current)
        
        for each neighbor v of current:
            if v in closed_list: continue
            tentative_g = g[current] + weight(current, v)
            if v not in open_list or tentative_g < g[v]:
                g[v] = tentative_g
                f[v] = g[v] + h(v, goal)
                prev[v] = current
                if v not in open_list:
                    open_list.add(v)
    return 失败

我记得有一次做园区物流机器人，地图有几百个节点。用Dijkstra要算0.5秒，换成A*后直接降到0.05秒。这就是启发式搜索的魅力。

小技巧： 如果你发现A*搜索太慢，可以试试“加权A*”——把f(n)改成 f(n) = g(n) + w * h(n)，w>1。这样算法会更“激进”地往目标冲，虽然不一定最优，但速度快很多。我经常在实时规划中这么干。

4.4 启发式函数设计

启发式函数h(n)的设计，直接决定了A*的表现。我个人总结了三个原则：

  • 可采纳性（Admissible）：h(n) ≤ 真实代价。这样才能保证找到最优路径。
  • 一致性（Consistent）：h(n) ≤ 边代价 + h(m)。满足一致性时，A*不需要重新检查已关闭的节点。
  • 信息性（Informative）：h(n)越接近真实代价，搜索效率越高。

常见的启发式函数有：

场景 推荐启发式 说明
网格地图（四方向） 曼哈顿距离 |dx| + |dy|
网格地图（八方向） 切比雪夫距离 max(|dx|, |dy|)
任意方向移动 欧几里得距离 sqrt(dx² + dy²)
道路网络 直线距离 简单高效，但信息性一般

这里有个细节，我曾经踩过坑——曼哈顿距离在四方向网格中是可采纳的，但在八方向网格中就不一定了。因为八方向允许对角线移动，曼哈顿距离会高估代价，导致A*找不到最优路径。解决办法是用切比雪夫距离或者对角线距离。

经验之谈： 我建议你在设计启发式函数时，先跑一遍Dijkstra得到真实代价，然后对比你的h(n)值。如果h(n)普遍偏大，说明它不可采纳，需要调整。如果h(n)普遍偏小太多，搜索效率会很低，可以适当放大。

最后说一句，启发式函数不是越复杂越好。有时候最简单的欧几里得距离，配合加权策略，效果反而比那些花里胡哨的公式好。我在实际项目中，80%的场景用的都是直线距离或曼哈顿距离——够用，且稳定。

好了，这一章的内容就到这里。下一章我们会深入讨论状态栅格与路径平滑，到时候我会分享一个我在高速场景下做路径优化的实战案例，保证让你有收获。

← 上一章 📖 返回目录 下一章 →