4. 双向A*路径规划

任务说明

本案例实现双向A*路径规划算法。与单向A*从起点单向搜索至终点不同，双向A*同时从起点（正向搜索）和终点（反向搜索）扩展节点，当两个搜索方向的前沿相遇时即找到路径。在栅格地图上，双向A*通常能显著减少扩展节点数量，从而提升搜索效率。本案例将给出完整的Python实现，包含地图定义、启发函数、双向搜索主循环以及路径重建，并提供可视化验证方法。

算法实现

<code>import heapq
import math

class BidirectionalAStar:
    def __init__(self, grid, start, goal):
        """
        grid: 2D list, 0=可通行, 1=障碍物
        start: (row, col)
        goal:  (row, col)
        """
        self.grid = grid
        self.rows = len(grid)
        self.cols = len(grid[0])
        self.start = start
        self.goal = goal

        # 正向搜索(从起点出发)的数据结构
        self.open_f = []          # 优先队列 (f, g, node)
        self.g_f = {start: 0}     # 起点到该节点的实际代价
        self.parent_f = {}        # 正向搜索的父节点记录

        # 反向搜索(从终点出发)的数据结构
        self.open_b = []          # 优先队列 (f, g, node)
        self.g_b = {goal: 0}      # 终点到该节点的实际代价(反向)
        self.parent_b = {}        # 反向搜索的父节点记录

        # 相遇节点
        self.meeting_node = None

        # 初始化优先队列
        heapq.heappush(self.open_f, (self._heuristic(start, goal), 0, start))
        heapq.heappush(self.open_b, (self._heuristic(goal, start), 0, goal))

    def _heuristic(self, a, b):
        """欧几里得距离启发函数"""
        return math.sqrt((a[0]-b[0])**2 + (a[1]-b[1])**2)

    def _neighbors(self, node):
        """返回4邻域可通行邻居"""
        r, c = node
        for dr, dc in [(-1,0),(1,0),(0,-1),(0,1)]:
            nr, nc = r+dr, c+dc
            if 0 <= nr < self.rows and 0 <= nc < self.cols:
                if self.grid[nr][nc] == 0:
                    yield (nr, nc)

    def _expand(self, open_heap, g_dict, parent_dict, other_g_dict, forward):
        """
        扩展一个节点。
        forward=True 表示正向搜索，False 表示反向搜索。
        返回True表示两方向相遇，搜索结束。
        """
        if not open_heap:
            return False

        f, g, node = heapq.heappop(open_heap)

        # 如果当前节点已经被另一方向搜索到，则相遇
        if node in other_g_dict:
            self.meeting_node = node
            return True

        for nb in self._neighbors(node):
            new_g = g + 1  # 每步代价为1
            if nb not in g_dict or new_g < g_dict[nb]:
                g_dict[nb] = new_g
                if forward:
                    # 正向搜索：f = g(start→nb) + h(nb→goal)
                    h = self._heuristic(nb, self.goal)
                    parent_dict[nb] = node
                else:
                    # 反向搜索：f = g(goal→nb) + h(nb→start)
                    h = self._heuristic(nb, self.start)
                    parent_dict[nb] = node
                heapq.heappush(open_heap, (new_g + h, new_g, nb))
        return False

    def search(self):
        """执行双向A*搜索，返回路径节点列表；无路径返回None"""
        while self.open_f and self.open_b:
            # 正向扩展一步
            if self._expand(self.open_f, self.g_f, self.parent_f,
                            self.g_b, forward=True):
                break
            # 反向扩展一步
            if self._expand(self.open_b, self.g_b, self.parent_b,
                            self.g_f, forward=False):
                break

        if self.meeting_node is None:
            return None

        # 重建路径：从起点到相遇点 + 从相遇点到终点
        path_f = []
        node = self.meeting_node
        while node is not None:
            path_f.append(node)
            node = self.parent_f.get(node)

        path_f.reverse()  # 现在是从起点到相遇点

        path_b = []
        node = self.parent_b.get(self.meeting_node)
        while node is not None:
            path_b.append(node)
            node = self.parent_b.get(node)

        # path_b是从相遇点下一节点到终点的顺序
        return path_f + path_b

# ========== 测试用例 ==========
if __name__ == "__main__":
    # 定义10x10栅格地图，0=空地，1=障碍
    grid = [
        [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,0,0,0,0,0],
        [0,0,0,0,1,0,0,0,0,0],
        [0,0,0,0,1,0,0,0,0,0],
        [0,0,0,0,1,0,0,0,0,0],
        [0,0,0,0,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],
    ]

    start = (0, 0)
    goal = (9, 9)

    algo = BidirectionalAStar(grid, start, goal)
    path = algo.search()

    if path:
        print(f"找到路径，共 {len(path)} 个节点:")
        print(" -> ".join(f"({r},{c})" for r, c in path))
    else:
        print("未找到路径")

    # 统计扩展节点数(用于效率对比)
    expanded = set(algo.g_f.keys()) | set(algo.g_b.keys())
    print(f"双向A*扩展节点总数: {len(expanded)}")

关键实现说明

双向A*的核心在于两个独立维护的开放列表：open_f从起点正向扩展，open_b从终点反向扩展。正向搜索的启发函数计算当前节点到终点的估计代价，反向搜索则计算到起点的估计代价。每次迭代交替从两个方向各扩展一个f值最小的节点。当某一方向扩展的节点出现在另一方向的已访问集合（g_dict）中时，即判定两方向相遇，搜索终止。路径重建时，从相遇点沿正向父指针回溯得到起点到相遇点的路径，再沿反向父指针回溯得到相遇点到终点的路径，拼接即为完整路径。上述代码中每步移动代价固定为1，启发函数采用欧几里得距离，保证可采纳性与一致性。

运行与验证

直接运行上述Python脚本即可测试。代码中已包含一个10×10栅格地图，其中第2至第6列（索引2~6）的第4行（索引3）至第8行（索引7）为垂直障碍墙，起点为左上角(0,0)，终点为右下角(9,9)。预期输出应显示一条绕开障碍的路径，路径节点数约为19个（绕行障碍墙），并打印双向A*扩展节点总数。为验证算法正确性，可对比单向A*在相同地图上的扩展节点数——双向A*通常扩展更少节点。若将起点或终点置于障碍物内，或地图完全被障碍分隔，程序应输出“未找到路径”。

验证判定标准

1. 程序输出路径中相邻节点在地图上均上下左右相邻且无穿越障碍；
2. 路径首节点为(0,0)，尾节点为(9,9)；
3. 路径长度（节点数减1）等于路径中每一步的代价值之和，且不存在更短的可行路径（可通过穷举或与A*结果对比确认）；
4. 扩展节点总数小于单向A*的扩展节点数（在多数地图布局下成立，障碍墙越宽效果越明显）。

上图为双向A*在测试地图上的搜索过程示意。蓝色半透明区域表示从起点出发的正向搜索扩展范围，绿色半透明区域表示从终点出发的反向搜索扩展范围，红色方块为障碍墙，橙色折线为最终规划路径，S与G分别标记起点和终点，M标记两方向搜索的相遇位置。可见两方向搜索在障碍墙左侧相遇，避免了单向搜索需要探索整个右侧区域的浪费。