📚 课程目录第4章 A*算法实战

4. A*算法实战

各位同学，欢迎来到《复杂避障场景路径规划实战》的第四讲。今天咱们要亲手把A*算法从理论变成代码。说实话，我当年第一次写A*的时候，也踩了不少坑。尤其是8邻域移动和障碍物碰撞检测这块，稍不留神就会出bug。今天我就带着大家，一步步把这些问题都搞定。

4.1 从理论到代码：A*的核心骨架

先回顾一下A*的核心公式：F = G + H。G是起点到当前点的实际代价，H是当前点到终点的预估代价。说白了，A*就是在「已经走过的路」和「还要走的路」之间找平衡。

我个人习惯把A*的实现拆成三个部分：

节点管理：记录每个格子的坐标、G值、H值、父节点
开放列表与关闭列表：open_list存待探索的节点，close_list存已探索的节点
主循环：不断从open_list中取F值最小的节点，扩展它的邻居

嗯，这里要注意：open_list用优先队列（heapq）来实现，效率会高很多。我在项目中曾经用列表手动排序，结果地图一大，程序直接卡死。后来换成heapq，速度提升了十几倍。

4.2 8邻域移动：不只是上下左右

4邻域只能上下左右走，8邻域加上了对角线。你想想看，机器人实际运动时，斜着走往往更自然。但8邻域有个坑：对角线的代价怎么算？

我建议这样处理：

上下左右移动：代价为1
对角线移动：代价为√2，约等于1.414

为什么要用√2？因为斜边长度就是直角边的√2倍。有些同学图省事，对角线也设成1，结果路径看起来歪歪扭扭的。我在做仓储机器人项目时，就因为这个细节，导致机器人走出来的路径总是不平滑。

8邻域移动的邻居生成代码：

def get_neighbors(node, grid):
    neighbors = []
    # 8个方向：上下左右 + 4个对角线
    directions = [
        (-1, 0), (1, 0), (0, -1), (0, 1),  # 上下左右
        (-1, -1), (-1, 1), (1, -1), (1, 1)  # 对角线
    ]
    for dx, dy in directions:
        nx, ny = node.x + dx, node.y + dy
        # 检查边界
        if 0 <= nx < grid.width and 0 <= ny < grid.height:
            # 检查障碍物
            if not grid.is_obstacle(nx, ny):
                # 计算移动代价
                if abs(dx) + abs(dy) == 2:  # 对角线
                    cost = 1.414
                else:  # 直线
                    cost = 1.0
                neighbors.append((nx, ny, cost))
    return neighbors

4.3 障碍物碰撞检测：别让机器人撞墙

障碍物检测听起来简单，但实际做起来有不少门道。最基本的做法是：在生成邻居时，检查目标格子是不是障碍物。但这样够吗？

我曾经遇到过一个情况：机器人贴着障碍物走对角线，结果机器人的「身体」卡在了障碍物的角上。为什么会这样？因为8邻域移动时，对角线路径可能会穿过障碍物的「角」。

避坑指南：我曾经在AGV项目中，因为没做对角线碰撞检测，机器人直接撞上了货架。后来我加了一个规则：走对角线时，必须同时检查相邻的两个直线格子是否都是空地。

举个例子：从(0,0)走到(1,1)，需要检查(1,0)和(0,1)是否都是空地。如果其中一个是障碍物，那这条对角线路径就不能走。

改进后的碰撞检测：

def is_valid_move(current, target, grid):
    # 如果是直线移动，只检查目标格子
    if current.x == target.x or current.y == target.y:
        return not grid.is_obstacle(target.x, target.y)
    
    # 如果是对角线移动，检查两个相邻格子
    if not grid.is_obstacle(current.x, target.y):
        return False
    if not grid.is_obstacle(target.x, current.y):
        return False
    return not grid.is_obstacle(target.x, target.y)

4.4 完整A*算法实现

好了，我们把上面这些知识点串起来，写一个完整的A*算法。我个人习惯把代码写得清晰一些，方便后续调试。

import heapq
import math

class Node:
    def __init__(self, x, y, g=0, h=0, parent=None):
        self.x = x
        self.y = y
        self.g = g
        self.h = h
        self.f = g + h
        self.parent = parent
    
    def __lt__(self, other):
        return self.f < other.f

def astar(grid, start, end):
    # 初始化起点
    start_node = Node(start[0], start[1])
    end_node = Node(end[0], end[1])
    
    open_list = []
    close_set = set()
    
    heapq.heappush(open_list, start_node)
    
    while open_list:
        current = heapq.heappop(open_list)
        
        # 到达终点
        if (current.x, current.y) == (end_node.x, end_node.y):
            path = []
            while current:
                path.append((current.x, current.y))
                current = current.parent
            return path[::-1]
        
        close_set.add((current.x, current.y))
        
        # 扩展邻居
        for dx, dy, cost in get_neighbors(current, grid):
            nx, ny = current.x + dx, current.y + dy
            
            if (nx, ny) in close_set:
                continue
            
            # 计算G值和H值
            g = current.g + cost
            h = math.sqrt((nx - end_node.x)**2 + (ny - end_node.y)**2)
            
            neighbor = Node(nx, ny, g, h, current)
            heapq.heappush(open_list, neighbor)
    
    return None  # 无路径

4.5 核心逻辑流程图

下面我用一张流程图，把A*算法的核心逻辑展示出来。你看着这张图，再对照上面的代码，思路会清晰很多。

4.6 实战中的几个小技巧

代码写完了，我再分享几个实战中总结的小技巧：

启发函数的选择：8邻域移动建议用欧几里得距离，4邻域用曼哈顿距离。混用的话，路径质量会下降。
地图预处理：如果障碍物是静态的，可以提前把障碍物信息存成二维数组，运行时直接查表，比实时计算快得多。
路径平滑A*出来的路径可能有很多折角。我一般会在路径生成后，做一次后处理，去掉不必要的拐点。

调试小技巧：刚开始写A*时，建议把每一步的open_list和close_set都打印出来。我当年就是这么干的，虽然看起来笨，但能快速定位问题。

好了，这一讲的内容就到这里。A*算法的核心就是「启发式搜索」，8邻域移动和碰撞检测是实战中的关键细节。你把这些代码跑一遍，再结合流程图理解，应该就能掌握A*的精髓了。

可可精品资料库