3、路径搜索算法入门：图论基础、Dijkstra算法原理、A*算法原理、启发式函数设计
各位同学，欢迎来到第三章。说实话，路径搜索是整个航线规划系统的「心脏」。你前面学的坐标系、航点，最终都要靠搜索算法把它们串起来。这一章我会把图论基础、Dijkstra、A* 和启发式函数揉碎了讲，结合我这些年踩过的坑，帮你把地基打牢。
3.1 图论基础：把世界抽象成点和线
先问个问题：航线规划里的「地图」到底是什么？
说白了，我们不需要关心地面是草地还是水泥地。我们只关心两个东西——位置和连通关系。这就是图论的核心思想。
图（Graph） = 顶点（Vertex） + 边（Edge）
- 顶点：代表航点、路口、关键位置
- 边：代表可通行的路径，可以带权重（距离、时间、油耗）
我在项目中遇到过最典型的坑：把地图网格直接当图用。其实网格只是视觉表现，真正的图结构是抽象的邻接关系。你想想看，一个 1000x1000 的网格，如果每个格子都连四个邻居，那就是 100 万个顶点、近 400 万条边——搜索起来慢得让人崩溃。
3.1.1 图的存储方式
实际工程里，我推荐两种存储方式：
| 存储方式 | 适用场景 | 空间复杂度 | 查询边耗时 |
|---|---|---|---|
| 邻接矩阵 | 顶点少（< 1000）、稠密图 | O(V²) | O(1) |
| 邻接表 | 顶点多、稀疏图（航线规划典型场景） | O(V+E) | O(度) |
我的习惯：航线规划几乎都是稀疏图——每个航点只连附近几个点。所以邻接表是默认选择。别用邻接矩阵，除非你确定顶点数不超过 500。
3.1.2 有向图 vs 无向图
这个区别很关键。无人机航线里，有些路径是单向的（比如禁飞区边缘只能绕行），有些是双向的。我建议一开始就按有向图建模，双向路径用两条反向边表示。这样后面加约束条件时不用改数据结构。
// 邻接表结构示例（C++风格）
struct Edge {
int to; // 目标顶点编号
double weight; // 边权重（距离/时间）
};
vector<vector<Edge>> graph; // graph[u] 存储从 u 出发的所有边3.2 Dijkstra算法原理：最稳妥的「地毯式搜索」
Dijkstra 算法，我做了这么多年项目，依然觉得它是「最让人放心的算法」。为什么？因为它保证找到最短路径——前提是所有边的权重非负。
3.2.1 核心思想
说白了就一句话：每次从「未确定」的点里，挑一个距离起点最近的点，然后用它去更新邻居。
我刚开始学的时候总觉得这太简单了，能行吗？后来自己手写了一遍才明白，它的巧妙在于「贪心 + 松弛」的组合。
- 初始化：起点距离为 0，其他点为无穷大
- 维护一个优先队列（小顶堆），按当前距离排序
- 每次弹出距离最小的顶点 u，标记为「已确定」
- 遍历 u 的所有邻居 v，如果 dist[u] + w(u,v) < dist[v]，就更新 dist[v]
- 重复直到队列为空
我曾经踩过的坑：有一次用普通队列代替优先队列，结果算法退化成了 O(V²)。在 10 万个顶点的图上跑了 3 分钟没出结果。后来换成优先队列，同样的图只用了 0.3 秒。记住：Dijkstra 必须用优先队列，否则别叫它 Dijkstra。
3.2.2 时间复杂度
| 实现方式 | 时间复杂度 | 备注 |
|---|---|---|
| 普通数组 + 遍历找最小 | O(V²) | 只适合小图 |
| 二叉堆（优先队列） | O((V+E) log V) | 工程中最常用 |
| 斐波那契堆 | O(V log V + E) | 理论最优，实际常数大 |
3.3 A*算法原理：带「方向感」的搜索
Dijkstra 虽然稳，但它有个问题——像个无头苍蝇一样往四面八方搜。你想想看，如果目标在东边，它还会往西边搜半天。A* 算法就是来解决这个问题的。
3.3.1 核心公式
A* 的评估函数就一行：
f(n) = g(n) + h(n)- g(n)：从起点到当前节点 n 的实际代价（和 Dijkstra 一样）
- h(n)：从 n 到目标节点的估计代价（启发式函数）
- f(n)：总估计代价，优先队列按 f(n) 排序
说白了，A* 就是在 Dijkstra 的基础上加了一个「指南针」。这个指南针就是 h(n)。
3.3.2 算法流程
- 把起点加入 open list（优先队列），g(start)=0，f(start)=h(start)
- 从 open list 中弹出 f 值最小的节点 n
- 如果 n 是目标节点，搜索结束
- 否则，把 n 加入 closed list（已处理列表）
- 对 n 的每个邻居 m：
- 如果 m 在 closed list 中，跳过
- 计算 tentative_g = g(n) + w(n, m)
- 如果 m 不在 open list 中，或 tentative_g < g(m)，更新 g(m) 和 f(m)，并把 m 加入 open list
- 重复步骤 2-5
我个人的习惯：open list 用二叉堆实现，closed list 用哈希表（或布尔数组）。别用链表存 closed list，查找太慢。在 10 万节点的图上，哈希表查找是 O(1)，链表是 O(n)，差距巨大。
3.4 启发式函数设计：A* 的灵魂
启发式函数 h(n) 设计得好不好，直接决定 A* 是「智能搜索」还是「比 Dijkstra 还慢的废物」。
3.4.1 可采纳性与一致性
两个关键概念：
- 可采纳性（Admissible）：h(n) 永远不大于实际代价。这样才能保证 A* 找到最优路径。
- 一致性（Consistent）：h(n) ≤ w(n,m) + h(m)。满足一致性时，A* 不需要重新检查 closed list 中的节点，效率更高。
我曾经犯过的错：在一个有障碍物的地图上，用了欧几里得距离作为启发式，但实际路径必须绕障碍物。结果 h(n) 严重低估实际代价，A* 扩展了大量节点，比 Dijkstra 还慢。后来换成曼哈顿距离 + 障碍物惩罚项，才恢复正常。
3.4.2 常用启发式函数
| 函数 | 公式 | 适用场景 | 是否可采纳 |
|---|---|---|---|
| 曼哈顿距离 | |x1-x2| + |y1-y2| | 四方向网格（上下左右） | 是 |
| 欧几里得距离 | √((x1-x2)² + (y1-y2)²) | 任意方向移动 | 是 |
| 切比雪夫距离 | max(|x1-x2|, |y1-y2|) | 八方向网格（可斜走） | 是 |
| 对角线距离 | dx+dy + (√2-2)*min(dx,dy) | 八方向网格，精确代价 | 是 |
3.4.3 工程中的调优技巧
嗯，这里要注意：理论上的可采纳性在工程里往往需要「打折」。我一般会给 h(n) 乘一个略大于 1 的系数（比如 1.01），让搜索更快收敛。虽然这样会损失最优性，但在航线规划里，99% 最优比 100% 最优但慢 10 倍要实用得多。
// 加权 A* 示例
double h_weighted = 1.01 * heuristic(current, goal);
double f = g + h_weighted;3.5 本章知识体系图
下面这张图把本章的核心概念串起来了。我建议你把它存下来，后面学更高级的算法时回来对照。
3.6 本章小结
这一章我们走完了路径搜索的「三件套」：图论是骨架，Dijkstra 是基本功，A* 是进阶武器，启发式函数是调优关键。我个人觉得，初学者最容易犯的错就是跳过图论直接写 A*，结果连邻接表都写不对。万丈高楼平地起，把图论基础打扎实了，后面学 RRT*、Theta* 这些高级算法时才能游刃有余。