4、路径搜索基础：图论基础、搜索空间构建、节点与边、代价函数
各位同学，今天我们来聊聊路径搜索的底层逻辑。说白了，无人机怎么从A点飞到B点，不是靠直觉，而是靠数学。我刚开始做航迹规划时，也以为就是画条线，后来被现实狠狠教育了一顿——嗯，咱们一步步来。
4.1 图论基础：把世界抽象成点和线
你想想看，无人机飞行的环境有多复杂？有山、有楼、有禁飞区。但计算机没法直接理解这些。怎么办？抽象。
图论就是干这个的。一个图由两部分组成：顶点（也叫节点）和边。顶点代表位置，边代表路径。我习惯把图想象成一张地铁线路图——每个站是一个节点，轨道是边。
核心概念：
- 无向图：边没有方向，A到B和B到A一样。适合开阔空域。
- 有向图：边有方向，比如单行道。适合有风向约束的场景。
- 加权图：每条边带一个权值，代表距离、时间或能耗。
我在项目中遇到过一个问题：用无向图规划，结果无人机逆风飞回来，能耗直接翻倍。后来我改成有向加权图，把风向作为边的方向属性，才解决。所以，选对图类型，比算法本身更重要。
4.2 搜索空间构建：把连续世界切碎
现实世界是连续的，但计算机只能处理离散数据。怎么办？离散化。
搜索空间，就是所有可能位置的集合。我常用的方法有三种：
| 方法 | 原理 | 适用场景 |
|---|---|---|
| 栅格法 | 把空间切成网格，每个格子是一个节点 | 城市低空、障碍物密集区 |
| 路标法 | 随机撒点，连接可见点 | 开阔野外、山地 |
| Voronoi图 | 以障碍物为中心，生成安全通道 | 避障要求高的场景 |
我个人偏爱栅格法，因为它简单直观。但要注意：栅格大小直接影响搜索效率。我做过一个测试，栅格从1米改成2米，搜索时间从3秒降到0.2秒，但路径长度只增加了5%。所以，别追求极致精度，够用就行。
我的经验：栅格大小取无人机翼展的1.5倍左右。太小了容易卡在缝隙里，太大了又可能撞到障碍物。
4.3 节点与边：路径的基本单元
节点就是位置点，边就是飞行段。听起来简单，但设计时有很多坑。
节点设计要点：
- 每个节点必须包含坐标信息（x, y, z）
- 最好带上高度属性，因为无人机是三维运动
- 可以附加状态信息，比如速度、航向
边设计要点：
- 边必须可飞——不能穿过障碍物
- 边的长度受限于无人机最大航程
- 边的方向要考虑转弯半径约束
我曾经犯过一个错：把两个距离很远的节点直接连起来，结果无人机飞到一半没电了。后来我加了一条规则：边的长度不能超过无人机续航里程的80%。留点余量，心里踏实。
4.4 代价函数：距离、时间、能耗
路径搜索的核心，就是找一条代价最小的路。代价函数就是衡量标准。我常用的有三种：
4.4.1 距离代价
最简单，就是欧氏距离。但实际飞行中，直线距离最短不一定最优。比如绕过一座山，虽然距离长了，但安全。
// 距离代价计算示例
double distanceCost(Node a, Node b) {
return sqrt(pow(b.x - a.x, 2) + pow(b.y - a.y, 2) + pow(b.z - a.z, 2));
}4.4.2 时间代价
时间 = 距离 / 速度。但风速会影响地速。我习惯把风速模型加进去：
// 时间代价计算（考虑风速）
double timeCost(Node a, Node b, Wind w) {
double groundSpeed = airSpeed + w.speed * cos(w.direction - heading(a, b));
return distanceCost(a, b) / groundSpeed;
}你想想看，逆风飞行时，时间代价会变大，算法自然就会选择顺风路径。这就是代价函数的魅力。
4.4.3 能耗代价
这个最复杂，也最实用。无人机能耗主要来自三部分：
- 悬停能耗：与时间成正比
- 平飞能耗：与速度的立方成正比（空气阻力）
- 爬升能耗：与高度变化成正比
综合代价函数（我常用的）：
double totalCost = w1 * distanceCost + w2 * timeCost + w3 * energyCost;其中 w1, w2, w3 是权重系数，根据任务需求调整。比如巡检任务，时间不重要，能耗权重可以设高一些。
避坑指南：我曾经把能耗代价设得太高，结果算法选了一条绕远路的平飞路径，虽然省电，但时间翻了三倍。后来我加了约束：时间不能超过任务时限的1.5倍。多目标优化，别只盯着一个指标。
4.5 知识体系总览
下面这张图，是我自己梳理的路径搜索基础框架。你看一眼，心里就有谱了。
图论是骨架，搜索空间是舞台，节点与边是演员，代价函数是剧本。四者配合好了，路径规划才能演好这出戏。
我的建议：刚开始做路径规划时，别急着调算法。先把这四个基础模块搞清楚。我见过太多人一上来就怼A*算法，结果连代价函数都没定义对——那跑出来的路径，你敢让无人机飞吗？
公众号:蓝海资料掘金营，微信deep3321