3. 全局路径规划A*算法

1.1 从Dijkstra到A*——我踩过的坑

说起路径规划，很多人一上来就撸A*。但我得说句实话：不理解Dijkstra，就别碰A*。

Dijkstra算法是个好同志。它保证能找到最短路径，但代价是什么？它像个无头苍蝇一样，从起点向四周均匀扩散。我当年在做一个仓储机器人项目时，地图有2000×2000个栅格，Dijkstra跑了整整3秒才出结果。机器人早就撞上货架了。

Dijkstra的问题在于——它太“老实”了。它不知道目标在哪边，所以四面八方都探索。而A*算法，说白了就是给Dijkstra装了个“指南针”。这个指南针就是启发函数。

1.2 A*算法的核心公式

A*的决策依据就一个公式：

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

解释一下：

• f(n)：节点n的总代价估计
• g(n)：从起点到节点n的实际代价
• h(n)：从节点n到目标点的估计代价（启发函数）

每次从open list中选f(n)最小的节点来扩展。嗯，这里要注意：g(n)是确定的，h(n)是估计的。两者平衡得好，算法就快；平衡不好，要么慢，要么找不到最优解。

1.3 启发函数设计——成败在此一举

启发函数h(n)的设计，直接决定了A*的表现。我见过太多人随便写个欧几里得距离就交差了，结果算法跑得比Dijkstra还慢。

常用的启发函数有这几种：

启发函数	公式	特点	适用场景
曼哈顿距离	|dx| + |dy|	只允许四方向移动时最优	城市道路、网格地图
欧几里得距离	√(dx² + dy²)	允许任意方向移动时使用	开放空间、无人机
切比雪夫距离	max(|dx|, |dy|)	允许八方向移动时最优	游戏地图、机器人
对角线距离	|dx| + |dy| - (√2-1)×min(|dx|,|dy|)	八方向移动的最精确估计	栅格地图（推荐）

1.4 启发函数的可采纳性与一致性

这里有个关键概念——可采纳性。说白了就是：h(n)永远不能高估到目标的实际代价。

为什么会这样？因为如果h(n)高估了，A*就可能错过最优路径。我刚开始做算法时，有一次把h(n)设成了实际距离的1.5倍，结果算法找到的路径绕了一大圈。排查了半天才发现是启发函数的问题。

可采纳的h(n)保证A*能找到最优解。而一致性（也叫单调性）则保证A*的效率——每个节点只被扩展一次。

1.5 在栅格地图上的实现

好了，理论说完了，咱们直接上代码。这是我在实际项目中用过的实现框架：

def a_star(grid, start, goal):
    """
    grid: 二维栅格地图，0可通行，1障碍物
    start: (x, y) 起点坐标
    goal: (x, y) 终点坐标
    """
    # 初始化
    open_list = []
    closed_set = set()
    
    # 起点加入open list
    start_node = Node(start, g=0, h=heuristic(start, goal))
    heapq.heappush(open_list, (start_node.f, start_node))
    
    # 记录路径
    came_from = {}
    
    while open_list:
        # 取f值最小的节点
        _, current = heapq.heappop(open_list)
        
        # 到达目标
        if current.pos == goal:
            return reconstruct_path(came_from, current)
        
        closed_set.add(current.pos)
        
        # 扩展邻居
        for neighbor_pos in get_neighbors(grid, current.pos):
            if neighbor_pos in closed_set:
                continue
            
            # 计算g值（假设相邻栅格代价为1）
            tentative_g = current.g + 1
            
            # 如果邻居不在open list中，或找到了更优路径
            if neighbor_pos not in [n.pos for n in open_list]:
                neighbor = Node(neighbor_pos, 
                               g=tentative_g, 
                               h=heuristic(neighbor_pos, goal))
                came_from[neighbor_pos] = current.pos
                heapq.heappush(open_list, (neighbor.f, neighbor))
            elif tentative_g < neighbor.g:
                # 更新g值和路径
                neighbor.g = tentative_g
                neighbor.f = neighbor.g + neighbor.h
                came_from[neighbor_pos] = current.pos
    
    return None  # 无路径

1.6 知识体系总览

下面这张图，是我梳理的A*算法知识体系。你跟着这个脉络学，不会走偏：

1.7 性能优化——我踩过的坑

代码写完了，但跑起来慢怎么办？我分享几个实战经验：

1. 用堆优化open list：别用列表排序，每次取最小值用heapq。我见过有人用sorted()，地图一大直接卡死。
2. 预处理障碍物：如果地图不变，提前把障碍物信息存成位图。我做过测试，位图查询比列表查询快10倍以上。
3. 限制搜索范围：如果起点到终点的直线距离是D，搜索范围可以限制在1.5D以内。超出这个范围还没找到路径，基本就是无解了。
4. 使用JPS（Jump Point Search）：在栅格地图上，JPS可以跳过大量对称路径。我有个项目用了JPS后，搜索节点数减少了80%。

1.8 实战中的避坑指南

最后，我总结几个实战中容易踩的坑：

• 启发函数高估：我曾经用欧几里得距离乘以1.2，以为能加速，结果路径质量下降。记住：宁可低估，不要高估。
• 忽略障碍物膨胀：机器人有尺寸，不能贴着障碍物走。我习惯把障碍物膨胀机器人半径+安全距离。否则路径规划出来，机器人直接撞墙。
• open list重复节点：同一个节点可能被多次加入open list。我建议用字典记录每个节点的最佳g值，避免重复计算。
• 死胡同处理：如果地图中有死胡同，A*会探索大量无效节点。我一般会加一个探索节点数上限，超过就返回无解。

好了，A*算法的核心内容就这些。你把这个搞懂了，后面学RRT、D*、Hybrid A*都会轻松很多。记住：算法是死的，场景是活的。多在实际项目中调试，才能真正掌握。