3、A星算法原理：代价函数f(n)=g(n)+h(n)详解、Open列表与Close列表、算法伪代码与流程
好，咱们直接进入正题。A星算法，说白了就是「有脑子的广度优先搜索」。你想想看，如果让你在一个迷宫里找出口，你会怎么走？肯定不会每条路都试一遍吧？你会先往看起来离出口近的方向走。A星就是这么干的。
3.1 代价函数 f(n) = g(n) + h(n)
这是A星的核心公式。我当年刚学的时候，觉得这公式简单到不值一提。后来在嵌入式项目里调优，才发现这里面的门道深着呢。
- g(n)：从起点到当前节点n，已经走过的实际代价。说白了就是「已经花了多少力气」。
- h(n)：从当前节点n到终点的估计代价。也就是「预估还要花多少力气」。
- f(n)：总代价。g(n) + h(n)，代表「这条路总共有希望吗」。
嗯，这里要注意：h(n) 是估计值，不是真实值。如果 h(n) 永远小于等于真实代价，那A星保证能找到最短路径。这叫「可采纳性」。我在项目中遇到过有人把 h(n) 设得太大，结果A星直接退化成贪心算法，路径绕了一大圈。
关键点：h(n) 的设计决定了A星的性能。h(n) 越接近真实代价，搜索越快。但绝对不能超过真实代价。
3.2 常用的启发函数 h(n)
不同的地图，适合不同的 h(n)。我整理了一张表，你直接拿去用：
| 地图类型 | 推荐 h(n) | 计算公式 | 特点 |
|---|---|---|---|
| 网格地图（四方向） | 曼哈顿距离 | |dx| + |dy| | 简单、计算快、嵌入式首选 |
| 网格地图（八方向） | 切比雪夫距离 | max(|dx|, |dy|) | 允许对角线移动时用 |
| 任意方向移动 | 欧几里得距离 | sqrt(dx² + dy²) | 精度高，但开方运算慢 |
| 路网/图结构 | 直线距离 | 两点间直线 | 简单粗暴，够用 |
我的经验：在嵌入式平台上，我一般用曼哈顿距离。为什么？因为只需要加减法，没有乘除，更没有开方。你想想看，在STM32上跑一次sqrt()要多少时钟周期？能省则省。
3.3 Open列表与Close列表
这两个列表是A星的「大脑」和「记忆」。
- Open列表：存放「待考察」的节点。也就是已经知道怎么走到这里，但还没确定是不是最优路径的那些节点。
- Close列表：存放「已考察」的节点。也就是已经处理完毕，不会再考虑的节点。
算法每轮从Open列表中取出f值最小的节点，检查它是不是终点。如果不是，就把它放进Close列表，然后把它的邻居加入Open列表。
避坑指南：我曾经在Open列表的实现上吃过亏。用数组+线性查找，节点一多直接卡死。后来改用二叉堆，速度提升了10倍不止。在嵌入式平台上，Open列表的数据结构选择至关重要。
3.4 算法伪代码
直接上干货。这是我在多个项目中验证过的版本：
function AStar(start, goal):
// 初始化
openList = [start]
closeList = []
// 记录代价
g[start] = 0
f[start] = h(start, goal)
// 记录路径
cameFrom[start] = null
while openList is not empty:
// 取出f值最小的节点
current = openList.getLowestF()
// 到达终点？
if current == goal:
return reconstructPath(cameFrom, current)
// 移到Close列表
openList.remove(current)
closeList.add(current)
// 遍历邻居
for each neighbor of current:
if neighbor in closeList:
continue // 已经处理过
tentative_g = g[current] + cost(current, neighbor)
if neighbor not in openList:
openList.add(neighbor)
elif tentative_g >= g[neighbor]:
continue // 这条路更差
// 记录最优路径
cameFrom[neighbor] = current
g[neighbor] = tentative_g
f[neighbor] = g[neighbor] + h(neighbor, goal)
return failure // 无路可走3.5 算法流程详解
咱们一步步拆解：
- 初始化：把起点放入Open列表，g(start)=0，f(start)=h(start, goal)。
- 主循环：只要Open列表不为空，就继续。
- 选节点：从Open列表中取出f值最小的节点。这是A星的核心操作。
- 判终点：如果当前节点就是终点，恭喜，路径找到了。
- 扩展邻居：把当前节点的邻居都检查一遍。
- 跳过已处理：如果邻居已经在Close列表里，直接跳过。
- 计算新g值：从当前节点走到邻居的代价。
- 更新或跳过：如果邻居不在Open列表，加入；如果在但新路径更差，跳过。
- 记录路径：更新cameFrom、g、f值。
- 循环结束：如果Open列表空了还没找到终点，说明无路可走。
核心要点：整个算法的灵魂就在「每次从Open列表中取f值最小的节点」。这保证了A星始终朝着最有希望的方向搜索。说白了，就是「先探索看起来最靠谱的路」。
3.6 嵌入式平台上的注意事项
嗯，这里我要多说几句。在PC上跑A星，你随便写都能跑。但在嵌入式平台上，处处是坑：
- 内存有限：Open列表和Close列表不能无限增长。我建议预先分配最大节点数的数组，用索引管理。
- 计算速度：避免浮点运算。g(n)和h(n)都用整数表示。比如距离乘以100取整。
- 堆操作：Open列表用二叉堆实现，插入和取出都是O(log n)。别用数组遍历，那是O(n)。
- Close列表：可以用位图或者哈希表。节点不多时，直接用数组标记visited状态更快。
我的习惯：在嵌入式平台上，我会把Close列表直接用一个二维数组的标记位来实现。比如grid[10][10]的visited字段。这样查找是O(1)，而且不占额外内存。Open列表则用最小堆，堆的大小预先分配好。
好了，A星的核心原理就这些。下一章咱们会深入Open列表的实现细节，看看二叉堆到底怎么在嵌入式平台上落地。你想想看，一个高效的Open列表，能让你的A星跑得飞起。