4. 全局路径规划之A*算法：启发式函数设计、算法流程、与Dijkstra的对比、工程优化技巧
说到全局路径规划，A*算法绝对是绕不开的经典。我个人觉得，它就像路径规划界的“瑞士军刀”——功能强大，但用得好不好，全看你会不会磨刀。今天咱们就把它拆开揉碎了讲，从原理到实战，把那些坑都填上。
4.1 启发式函数设计：A*的灵魂所在
A*算法之所以比Dijkstra快，关键就在于这个启发式函数。说白了，它就像给搜索过程装了个“指南针”，告诉算法往哪个方向走更有希望。
启发式函数h(n)的设计原则
- 可采纳性（Admissible）：h(n) ≤ 实际代价。这是保证找到最优解的前提。我刚开始学的时候，总觉得这个条件太苛刻，后来才明白，这是数学上的严谨要求。
- 一致性（Consistent）：h(n) ≤ c(n, m) + h(m)。这个条件更强，但能保证算法效率更高。
- 信息度（Informed）：h(n)越接近真实代价，搜索越快。但计算h(n)本身也要花时间，这是个权衡。
常见的启发式函数
| 地图类型 | 推荐启发式 | 公式 | 特点 |
|---|---|---|---|
| 网格地图（四方向） | 曼哈顿距离 | |dx| + |dy| | 简单高效，适合城市道路 |
| 网格地图（八方向） | 切比雪夫距离 | max(|dx|, |dy|) | 允许对角线移动时使用 |
| 任意方向 | 欧几里得距离 | sqrt(dx² + dy²) | 最精确，但计算开销大 |
| 带权重的场景 | 加权曼哈顿 | w * (|dx| + |dy|) | w > 1时更快，但可能非最优 |
4.2 算法流程：从原理到代码
A*的核心思想其实很简单：维护两个列表——Open List（待探索节点）和Closed List（已探索节点）。每次从Open List中取出f(n)=g(n)+h(n)最小的节点，然后扩展它的邻居。
标准流程
- 将起点加入Open List，设置g(start)=0，h(start)按启发式函数计算
- 循环直到Open List为空或找到终点：
- 从Open List中取出f值最小的节点n
- 如果n是终点，回溯路径
- 将n移入Closed List
- 对n的每个邻居m：
- 如果m在Closed List中，跳过
- 计算临时g值：g_temp = g(n) + cost(n, m)
- 如果m不在Open List中，加入并设置g(m)=g_temp
- 如果m在Open List中且g_temp < g(m)，更新g(m)和父节点
- 如果Open List为空，说明无路可走
// 伪代码示例
function AStar(start, goal):
openList = PriorityQueue()
closedList = Set()
openList.push(start, f(start))
g[start] = 0
parent[start] = null
while not openList.isEmpty():
current = openList.pop()
if current == goal:
return reconstructPath(parent, current)
closedList.add(current)
for neighbor in getNeighbors(current):
if neighbor in closedList:
continue
tentative_g = g[current] + cost(current, neighbor)
if neighbor not in openList:
g[neighbor] = tentative_g
f[neighbor] = g[neighbor] + heuristic(neighbor, goal)
parent[neighbor] = current
openList.push(neighbor, f[neighbor])
elif tentative_g < g[neighbor]:
g[neighbor] = tentative_g
f[neighbor] = g[neighbor] + heuristic(neighbor, goal)
parent[neighbor] = current
// 更新优先级队列中的位置
return null // 无路径4.3 与Dijkstra的对比：谁更胜一筹？
Dijkstra算法可以看作是A*的特例——当h(n)=0时，A*就退化成Dijkstra。你想想看，没有启发式信息的引导，Dijkstra只能像“水波扩散”一样均匀向外搜索，效率自然低。
| 对比维度 | Dijkstra | A* |
|---|---|---|
| 搜索策略 | 广度优先 + 贪心 | 启发式引导 + 贪心 |
| 最优性 | 保证最优 | h(n)可采纳时保证最优 |
| 搜索空间 | 全向扩散，大 | 指向目标，小 |
| 适用场景 | 无启发式信息、小地图 | 有明确目标、大地图 |
| 时间复杂度 | O(V²) 或 O(E log V) | 通常优于Dijkstra |
说白了，如果你知道目标在哪，就用A*；如果不知道（比如探索未知环境），那就只能用Dijkstra。我在做扫地机器人项目时，建图阶段用Dijkstra，路径规划阶段用A*，各取所长。
4.4 工程优化技巧：让A*跑得更快
理论讲完了，咱们来点干货。这些技巧都是我在实际项目中踩坑踩出来的。
技巧一：使用二叉堆优化Open List
Open List的插入和取出操作是A*的性能瓶颈。用二叉堆实现优先级队列，插入和取出都是O(log n)。我见过有人用普通数组，地图一大直接卡死。
// 二叉堆实现示例（Python风格伪代码）
class MinHeap:
def __init__(self):
self.heap = []
def push(self, node, f_value):
self.heap.append((f_value, node))
self._sift_up(len(self.heap) - 1)
def pop(self):
if not self.heap:
return None
min_node = self.heap[0]
last = self.heap.pop()
if self.heap:
self.heap[0] = last
self._sift_down(0)
return min_node[1]
def _sift_up(self, idx):
while idx > 0:
parent = (idx - 1) // 2
if self.heap[parent][0] <= self.heap[idx][0]:
break
self.heap[parent], self.heap[idx] = self.heap[idx], self.heap[parent]
idx = parent
def _sift_down(self, idx):
size = len(self.heap)
while True:
smallest = idx
left = 2 * idx + 1
right = 2 * idx + 2
if left < size and self.heap[left][0] < self.heap[smallest][0]:
smallest = left
if right < size and self.heap[right][0] < self.heap[smallest][0]:
smallest = right
if smallest == idx:
break
self.heap[idx], self.heap[smallest] = self.heap[smallest], self.heap[idx]
idx = smallest技巧二：跳跃点搜索（Jump Point Search）
对于网格地图，JPS能大幅减少扩展节点数。它的核心思想是：在开阔区域，很多节点是“对称”的，可以跳过。我曾在1000x1000的网格上测试，JPS比标准A*快了10倍以上。
技巧三：分层A*（Hierarchical A*）
先在高分辨率地图上规划粗路径，再在局部细化。这就像你先看城市地图规划路线，再看街道地图找具体位置。我在AGV项目中用过，规划时间从秒级降到了毫秒级。
技巧四：动态加权
当搜索陷入死胡同时，动态调整h(n)的权重。比如，如果连续N步没有进展，就把权重从1.0提高到1.5，让算法更“激进”地朝目标前进。当然，这可能会牺牲最优性，但很多时候我们更在乎“能不能找到路”而不是“是不是最短”。
好了，这一章的内容就到这里。下一章我们会讲D* Lite算法——一种适用于动态环境的增量式路径规划算法。到时候我会分享一个我在无人机避障项目中的实战案例，保证精彩。