4. 全局路径规划算法（A*）：A*算法原理、启发函数设计、栅格地图实现
各位同学，今天我们来啃一块硬骨头——A*算法。说实话，我在刚入行那会儿，觉得路径规划就是“找条路走过去”，直到第一次在产线上调试AGV，车子在货架中间转圈圈，我才意识到：没有好的全局路径规划，局部控制再牛也白搭。
A*算法，说白了就是“有脑子的广度优先”。它不像Dijkstra那样傻傻地四面八方扩散，也不像贪心算法那样只顾眼前。它结合了实际代价和预估代价，每次扩展都挑最有可能通往终点的节点。嗯，这个思路很符合我们工程师的直觉——干活前先评估一下，哪条路性价比最高。
4.1 A*算法核心原理
A*算法的核心公式就一个：
f(n) = g(n) + h(n)
- f(n)：节点n的总评估代价
- g(n)：从起点到节点n的实际代价
- h(n)：从节点n到终点的启发式预估代价
我习惯把A*的搜索过程想象成“带着指南针的盲人”。g(n)是已经走过的路，h(n)是指南针指的方向。盲人每走一步，都会摸摸地面（实际代价），再抬头看看指南针（预估代价），然后决定下一步往哪走。
算法维护两个关键列表：
- Open List：待评估的节点集合。每次从中取出f值最小的节点进行扩展。
- Closed List：已经评估过的节点集合。已经处理过的节点不再重复考虑。
基本流程是这样的：
- 把起点加入Open List
- 从Open List中取出f值最小的节点，记为当前节点
- 如果当前节点是终点，搜索结束，回溯路径
- 否则，将当前节点移入Closed List，扩展它的邻居节点
- 对每个邻居，计算g、h、f值，更新Open List
- 重复步骤2-5，直到找到终点或Open List为空
我的经验：Open List的实现方式直接影响性能。小地图用普通列表+遍历没问题，大地图（比如1000x1000的栅格）一定要用优先队列（最小堆）。我曾经在一个项目里用列表硬扛，结果搜索时间从毫秒级变成了秒级，被客户当场吐槽。
4.2 启发函数设计——A*的灵魂
启发函数h(n)的设计，直接决定了A*算法的搜索效率和最优性。说白了，h(n)就是你对终点的“直觉”。直觉越准，搜索越快；直觉太离谱，要么找不到最优解，要么搜索范围爆炸。
常用的启发函数有三种：
| 启发函数 | 公式 | 适用场景 | 特点 |
|---|---|---|---|
| 曼哈顿距离 | |dx| + |dy| | 四方向移动（上下左右） | 计算快，可采纳，最常用 |
| 欧几里得距离 | √(dx² + dy²) | 八方向移动或自由移动 | 更精确，但计算开销大 |
| 切比雪夫距离 | max(|dx|, |dy|) | 八方向移动（对角代价相同） | 介于两者之间 |
避坑指南：我曾经在一个AGV项目里用了欧几里得距离做启发函数，结果搜索出来的路径全是斜线，AGV在货架通道里根本走不了。后来换成曼哈顿距离，路径规规矩矩地沿着通道走。记住：启发函数必须与机器人的实际运动模型匹配。
关于启发函数的可采纳性，这里有个关键点：如果h(n) 永远不大于从节点n到终点的实际最小代价，那么A*算法保证能找到最优解。这样的启发函数称为可采纳的。
你想想看，如果h(n)高估了距离，算法可能会错过最优路径，因为它觉得某条路“看起来太远”就不去探索了。嗯，这就像你凭直觉觉得某条路堵车就不走，结果那条路其实一路畅通。
4.3 栅格地图实现
在实际项目中，我们通常用栅格地图来离散化环境。每个栅格要么是可通行（0），要么是障碍物（1）。
我习惯用二维数组来表示栅格地图：
// 栅格地图定义
// 0: 可通行, 1: 障碍物
int map[10][10] = {
{0, 0, 0, 0, 0, 0, 0, 0, 0, 0},
{0, 0, 0, 0, 0, 0, 0, 0, 0, 0},
{0, 0, 0, 0, 0, 1, 1, 0, 0, 0},
{0, 0, 0, 0, 0, 1, 1, 0, 0, 0},
{0, 0, 0, 0, 0, 0, 0, 0, 0, 0},
{0, 0, 0, 0, 0, 0, 0, 0, 0, 0},
{0, 0, 0, 0, 0, 0, 0, 0, 0, 0},
{0, 0, 0, 0, 0, 0, 0, 0, 0, 0},
{0, 0, 0, 0, 0, 0, 0, 0, 0, 0},
{0, 0, 0, 0, 0, 0, 0, 0, 0, 0}
};节点结构体设计：
typedef struct {
int x, y; // 栅格坐标
int g; // 起点到当前节点的代价
int h; // 当前节点到终点的启发式代价
int f; // 总代价 f = g + h
struct Node* parent; // 父节点指针，用于回溯路径
} Node;邻居扩展时，需要考虑四方向还是八方向：
- 四方向：上下左右，代价为1
- 八方向：上下左右+四个对角，直行代价1，对角代价√2≈1.414
我的建议：对于大多数工业AGV场景，用四方向+曼哈顿距离就够了。八方向虽然路径更短，但AGV在狭窄通道里走对角线容易撞到货架。除非你的机器人是全向移动的，否则别贪图那点路径长度。
下面我用SVG画一张A*算法的完整流程图，方便大家理解整个搜索过程：
最后，我给大家一个完整的A*算法伪代码，可以直接照着实现：
function AStar(start, goal, map):
openList = PriorityQueue() // 按f值排序
closedList = Set()
start.g = 0
start.h = heuristic(start, goal)
start.f = start.g + start.h
openList.add(start)
while openList is not empty:
current = openList.pop() // 取出f值最小的节点
if current == goal:
return reconstructPath(current)
closedList.add(current)
for each neighbor in getNeighbors(current, map):
if neighbor in closedList:
continue
if neighbor is obstacle:
continue
tentative_g = current.g + distance(current, neighbor)
if neighbor not in openList:
neighbor.g = tentative_g
neighbor.h = heuristic(neighbor, goal)
neighbor.f = neighbor.g + neighbor.h
neighbor.parent = current
openList.add(neighbor)
elif tentative_g < neighbor.g:
// 找到更优路径，更新
neighbor.g = tentative_g
neighbor.f = neighbor.g + neighbor.h
neighbor.parent = current
// 更新优先队列中的位置
return null // 无路径关键点总结：
- 启发函数h(n)必须可采纳（不高估），才能保证最优解
- Open List用优先队列实现，别用列表硬扛
- 邻居扩展方式（四方向/八方向）要与机器人运动模型匹配
- 栅格地图的分辨率直接影响路径质量——太粗路径不精确，太细计算量爆炸
嗯，A*算法就讲到这里。说实话，这个算法看起来简单，但真正用好它，需要在启发函数设计和工程实现上多下功夫。我当年第一次在嵌入式平台上跑A*，内存爆了三次才调好。你们回去后，建议先在小地图上手动模拟一遍搜索过程，把Open List和Closed List的变化画出来。相信我，画一遍比看十遍代码都管用。