第四章：路径规划基础：图搜索算法概览、Dijkstra算法原理与实现
各位同学，欢迎来到路径规划的第一课。
说实话，每次讲到图搜索，我都会想起自己刚入行时的一个项目。那时候我负责一个园区物流小车的导航，小车总在岔路口犹豫不决，像个选择困难症患者。后来我才意识到——不是车的问题，是我压根没给它装上一颗会「思考」的脑子。
嗯，这颗脑子，就是图搜索算法。
4.1 图搜索算法概览：从地图到图
我们开车用导航，导航软件怎么知道哪条路最近？
说白了，它把现实世界的地图，抽象成了一张「图」。图里只有两种东西：节点和边。
- 节点：路口、地标、停车点。我习惯叫它「状态」。
- 边：连接两个节点的道路。边上通常带个数字——距离、时间、或者油耗，我们叫它「代价」。
你想想看，路径规划的本质，就是在这张图上，找到一条从起点到终点、总代价最小的路径。
图搜索算法有很多，我按自己的理解把它们分成了几类：
| 类别 | 代表算法 | 核心特点 |
|---|---|---|
| 盲目搜索 | BFS、DFS | 不看代价，只管遍历 |
| 启发式搜索 | A*、D* | 带个「指南针」，直奔目标 |
| 最优搜索 | Dijkstra | 保证找到最短路径，但慢 |
今天咱们先啃最经典的一块骨头——Dijkstra算法。
核心认知：Dijkstra算法是「加权图」上的BFS。BFS只能处理每条边代价一样的情况，而Dijkstra可以处理不同的代价。
4.2 Dijkstra算法原理：贪心的艺术
Dijkstra算法的思路，其实特别朴素。
想象你站在起点，手里拿着一张白纸。你每次只做一件事：从所有「已探索」的节点中，挑一个离起点最近、但还没最终确定的节点，把它「定下来」。
定下来之后，看看它的邻居。如果通过这个节点去邻居，比之前记录的路径更短，就更新邻居的距离。
这个过程，我称之为「贪心+松弛」。
- 贪心：每次都选当前看起来最优的节点。
- 松弛：发现更短的路径，就更新记录。
为什么会这样？因为Dijkstra假设：如果所有边的代价都是非负的，那么当前离起点最近的节点，它的最短路径已经确定了。你想想看，不可能有另一条路比它更短，因为其他路都还没它近呢。
注意：Dijkstra算法要求图中不能有负权边。如果有负权边，这个「贪心」假设就不成立了。我曾经在一个项目中踩过这个坑，地图数据里有个下坡路段的代价标成了负数，结果算法跑出来的路径完全不对。后来排查了半天才发现。
4.3 算法流程与伪代码
我习惯用三个数据结构来理解Dijkstra：
- dist[]：记录从起点到每个节点的当前最短距离。
- visited[]：标记节点是否已经「定下来」。
- prev[]：记录路径中每个节点的前驱节点，方便最后回溯路径。
流程很简单：
初始化：
dist[起点] = 0
dist[其他] = 无穷大
visited[所有] = false
循环 n 次：
1. 在未访问的节点中，找 dist 最小的节点 u
2. 标记 visited[u] = true
3. 对于 u 的每个邻居 v：
如果 visited[v] == false 且 dist[u] + w(u,v) < dist[v]：
更新 dist[v] = dist[u] + w(u,v)
记录 prev[v] = u嗯，这里要注意：找最小dist节点这一步，如果每次都遍历所有节点，复杂度是O(n²)。节点多了会慢得让人抓狂。
优化技巧：我建议用「优先队列（最小堆）」来维护未访问的节点。这样每次取最小节点的时间降到O(log n)，整体复杂度变成O((n+e)log n)。在节点数超过1000时，这个优化效果非常明显。
4.4 代码实现：C++版本
下面是我在实际项目中用过的Dijkstra实现，加了优先队列优化。代码风格偏工程化，注释也写得很详细。
#include <vector>
#include <queue>
#include <limits>
using namespace std;
// 图结构：邻接表
// graph[u] = { (v1, w1), (v2, w2), ... }
vector<vector<pair<int, int>>> graph;
vector<int> dijkstra(int start, int n) {
// 距离数组，初始化为无穷大
vector<int> dist(n, numeric_limits<int>::max());
// 前驱节点数组，用于回溯路径
vector<int> prev(n, -1);
// 优先队列：存储 (距离, 节点编号)
// 默认是大顶堆，用 greater 变成小顶堆
priority_queue<pair<int, int>,
vector<pair<int, int>>,
greater<pair<int, int>>> pq;
dist[start] = 0;
pq.push({0, start});
while (!pq.empty()) {
int d = pq.top().first;
int u = pq.top().second;
pq.pop();
// 如果当前距离大于记录的距离，说明是旧数据，跳过
if (d > dist[u]) continue;
// 遍历邻居
for (auto &edge : graph[u]) {
int v = edge.first;
int w = edge.second;
// 松弛操作
if (dist[u] + w < dist[v]) {
dist[v] = dist[u] + w;
prev[v] = u;
pq.push({dist[v], v});
}
}
}
return dist;
}
// 回溯路径
vector<int> getPath(int start, int end, vector<int>& prev) {
vector<int> path;
for (int v = end; v != -1; v = prev[v]) {
path.push_back(v);
}
reverse(path.begin(), path.end());
return path;
}避坑指南：我曾经在优先队列里忘记加 if (d > dist[u]) continue; 这个判断。结果同一个节点被多次处理，性能直接崩了。记住，优先队列里可能存着过期的旧数据，一定要跳过。
4.5 知识体系与核心逻辑
下面这张图，是我自己梳理的本章知识脉络。你可以把它当作一张「思维导图」来看。
这张图把本章的核心逻辑串起来了。从图结构出发，到算法分类，再到Dijkstra的细节。你可以看到，Dijkstra的每个子节点都是我们刚才讲过的重点。
4.6 实战中的几点体会
最后，分享几个我在项目中用Dijkstra的体会：
- 地图预处理很重要：如果节点数超过10万，Dijkstra的实时计算会吃力。我一般会先做分层处理，或者用A*加启发式函数。
- 路径平滑：Dijkstra找到的路径是「折线」，直接给车用会一顿一顿的。我习惯在后面加一个平滑模块，用B样条曲线处理一下。
- 动态障碍物：Dijkstra是静态算法。如果环境变了，需要重新规划。这时候可以考虑D* Lite或者增量式搜索。
小建议：初学者可以先在网格地图上实现Dijkstra。比如一个10x10的格子，每个格子是一个节点，上下左右移动代价为1。这样调试起来非常直观，你能亲眼看到算法是怎么「一圈一圈」向外扩展的。
好了，图搜索的基础和Dijkstra就讲到这里。算法本身不难，难的是理解它为什么能保证最优。下次遇到路径规划问题，不妨先想想：这张图有没有负权边？节点规模多大？需不需要启发式信息？想清楚了，选算法就不纠结了。