3、路径规划基础：图搜索算法（Dijkstra、A*）、路径平滑与优化

各位同学，今天我们来聊聊路径规划里最核心的一块——图搜索算法。说白了，就是怎么让无人机在地图上找到一条从起点到终点的路。

我刚开始做飞控的时候，觉得路径规划不就是找条最短路径吗？后来真上了项目才发现，事情远没那么简单。地图怎么建？节点怎么选？遇到障碍物怎么绕？这些都是坑。

3.1 图搜索的基本概念

在讲具体算法之前，我们先统一一下概念。路径规划里的「图」，不是图片，而是由节点和边组成的网络结构。

• 节点（Node）：代表空间中的一个位置点。比如经纬度坐标、栅格中心点。
• 边（Edge）：连接两个节点的通路。边可以有长度、时间、能耗等权重。
• 代价（Cost）：从当前节点移动到相邻节点需要付出的「成本」。距离是最常见的代价。

嗯，这里要注意：图可以是二维的，也可以是三维的。无人机在三维空间里飞，节点就是三维坐标点。不过为了讲解方便，我们先拿二维图来举例。

3.2 Dijkstra 算法：最朴素的搜索

Dijkstra 算法是图搜索的「老大哥」。它的思路很直白：从起点开始，一层一层往外扩散，每次都选当前代价最小的节点继续扩展，直到找到终点。

我记得第一次在无人机上跑 Dijkstra，地图是 100x100 的栅格。结果呢？跑了快 3 秒才出路径。对于实时飞控来说，这太慢了。但它的优点是保证能找到最短路径。

算法流程

1. 初始化：起点代价为 0，其他节点代价为无穷大。
2. 从待处理节点中选代价最小的节点 u。
3. 遍历 u 的所有邻居 v，计算新代价 = u.代价 + 边(u,v) 的权重。
4. 如果新代价小于 v 的当前代价，更新 v 的代价和父节点。
5. 重复步骤 2-4，直到终点被选中或所有节点处理完毕。

代码示例（简化版）

def dijkstra(graph, start, goal):
    open_set = {start: 0}
    came_from = {}
    cost_so_far = {start: 0}
    
    while open_set:
        current = min(open_set, key=open_set.get)
        if current == goal:
            break
        
        del open_set[current]
        for neighbor, weight in graph[current].items():
            new_cost = cost_so_far[current] + weight
            if neighbor not in cost_so_far or new_cost < cost_so_far[neighbor]:
                cost_so_far[neighbor] = new_cost
                came_from[neighbor] = current
                open_set[neighbor] = new_cost
    
    return reconstruct_path(came_from, start, goal)

3.3 A* 算法：带方向的搜索

A* 算法是 Dijkstra 的「升级版」。它加了一个启发函数，让搜索更有方向性。说白了，就是告诉算法：「往终点方向走，别瞎转悠」。

我曾经在一个项目中对比过 Dijkstra 和 A*。同样的地图，Dijkstra 扩展了 8000 多个节点，A* 只扩展了 2000 多个。速度差了 4 倍，路径长度几乎一样。

A* 的核心公式

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

• g(n)：从起点到当前节点 n 的实际代价。
• h(n)：从节点 n 到终点的估计代价（启发函数）。
• f(n)：总估计代价，算法按 f(n) 最小的节点优先扩展。

常用启发函数

距离类型	公式	适用场景
曼哈顿距离	|x1-x2| + |y1-y2|	四方向移动（上下左右）
欧几里得距离	√((x1-x2)² + (y1-y2)²)	任意方向移动（无人机常用）
切比雪夫距离	max(|x1-x2|, |y1-y2|)	八方向移动

对于无人机来说，我建议用欧几里得距离。因为它最接近实际飞行路径的直线距离，而且计算量也不大。

A* 与 Dijkstra 对比

特性	Dijkstra	A*
搜索方向	无方向，均匀扩散	有方向，朝向终点
扩展节点数	多	少
计算速度	慢	快
路径最优性	保证最优	保证最优（h 可采纳时）
适用场景	小地图、静态环境	中大地图、实时规划

3.4 路径平滑与优化

图搜索出来的路径有个问题——它是一段一段的折线。无人机飞这种路径，每到一个拐点就得急转弯，不仅不舒服，还费电。

我做过一个测试：同样的航线，折线路径比平滑路径多耗电 15%。所以路径平滑不是锦上添花，而是必须做的优化。

常见的平滑方法

• B 样条曲线：用控制点生成平滑曲线。优点是局部可控，修改一个点不影响整体。
• 贝塞尔曲线：通过起点、终点和控制点生成曲线。适合短距离平滑。
• 梯度下降优化：把路径点看作变量，用优化算法最小化曲率和长度。

平滑前后的对比

指标	平滑前	平滑后
路径长度	1000 米	1020 米（略长）
最大转弯角	90°	15°
飞行时间	120 秒	115 秒（更快）
能耗	100%	85%

你看，虽然路径长了 20 米，但飞行时间反而缩短了，能耗也降低了。这就是平滑的价值。

3.5 本章知识体系

下面这张图展示了本章的核心知识结构，方便你整体把握。

3.6 实际项目中的选择建议

说了这么多，到底该用哪个？我根据实际项目经验，给你几个建议：

• 小地图（100x100 以内）：用 Dijkstra，简单可靠，路径最优。
• 中大地图（100x100 以上）：用 A*，速度快，路径质量好。
• 动态环境（有移动障碍物）：用 A* 加局部重规划，或者用 D* Lite。
• 需要平滑飞行：A* 找路径 + B 样条曲线平滑，这是最稳妥的方案。