路径规划算法总结：从 Dijkstra 到 A* 与 Hybrid A

1. 路径规划算法

1.1 Dijkstra 算法

Dijkstra 算法是一种广度优先搜索（BFS）的变种，用于在加权图中寻找单源最短路径。

核心思想：

• 维护一个 OpenList（优先队列），按当前已知最短路径 g(n) 排序。
• 每次从 OpenList 取出 g(n) 最小的节点 n，并扩展其所有邻居 m：
  • 若 g(n) + cost(n, m) < g(m)，则更新 g(m) 并重新入队。
• 最终，所有可达节点的 g(n) 即为最短路径。

伪代码：

def dijkstra(graph, start):
    OpenList = PriorityQueue()  # 按 g(n) 排序
    OpenList.push(start, g=0)
    ClosedList = set()          # 已扩展节点
    g = {start: 0}              # 记录各节点的最小 g(n)

    while OpenList:
        n = OpenList.pop()      # 取出 g(n) 最小的节点
        if n in ClosedList:
            continue
        ClosedList.add(n)
        
        for m in graph.neighbors(n):
            new_g = g[n] + graph.cost(n, m)
            if m not in g or new_g < g[m]:
                g[m] = new_g
                OpenList.push(m, g=new_g)
    return g

特点：

• 无启发式，仅依赖 g(n)，搜索范围大，效率低。
• 保证最优解，但计算量高。

1.2 A* 算法

A* 在 Dijkstra 的基础上引入启发函数 h(n)，使搜索更高效。

核心思想：

• 优先级改为 f(n) = g(n) + h(n)，其中 h(n) 是估计的剩余代价（如欧氏距离）。
• 若 h(n) 可采纳（h(n) ≤ h*(n)），则 A* 保证最优解。
• 若 h(n) 一致（h(n) ≤ cost(n, m) + h(m)），则 Closed List 无需重复检查。

伪代码：

def astar(graph, start, goal):
    OpenList = PriorityQueue()  # 按 f(n) = g(n) + h(n) 排序
    OpenList.push(start, f=0 + heuristic(start, goal))
    ClosedList = set()          # 已扩展节点
    g = {start: 0}              # 记录各节点的最小 g(n)

    while OpenList:
        n = OpenList.pop()      # 取出 f(n) 最小的节点
        if n == goal:
            return reconstruct_path(n)
        if n in ClosedList:
            continue
        ClosedList.add(n)
        
        for m in graph.neighbors(n):
            new_g = g[n] + graph.cost(n, m)
            if m not in g or new_g < g[m]:
                g[m] = new_g
                f = new_g + heuristic(m, goal)
                OpenList.push(m, f=f)
    return None

相比 Dijkstra 的优势：

1. 搜索方向更集中，减少不必要的扩展。
2. 计算效率更高，尤其在大规模地图上。
3. 可调节启发函数（如 Weighted A*），平衡最优性与速度。

1.3 混合 A* 算法

Hybrid A* 结合了离散搜索（A*）与连续运动规划，适用于车辆运动学约束（如转向半径）。

核心思想：

• 状态表示为 (x, y, θ)，考虑转向角 δ 和步长 L。
• 使用 Reeds-Shepp 曲线 或 Dubins 路径 作为启发函数。
• 在 OpenList 中存储连续状态，但离散化碰撞检测。

伪代码（简化）：

def hybrid_astar(start, goal, grid):
    OpenList = PriorityQueue()
    OpenList.push(start, f=0 + heuristic(start, goal))
    ClosedList = set()  # 离散化存储 (x, y, θ)

    while OpenList:
        n = OpenList.pop()
        if distance(n, goal) < threshold:
            return path
        if (n.x, n.y, n.θ) in ClosedList:
            continue
        ClosedList.add((n.x, n.y, n.θ))

        for δ in [-π/6, 0, π/6]:  # 可能的转向角
            new_θ = n.θ + (L * tan(δ) / wheelbase)
            new_x = n.x + L * cos(new_θ)
            new_y = n.y + L * sin(new_θ)
            if not collision(new_x, new_y, grid):
                g = n.g + L
                h = reeds_shepp_heuristic((new_x, new_y, new_θ), goal)
                OpenList.push(Node(new_x, new_y, new_θ, g, g + h))
    return None

适用场景：

• 自动驾驶（考虑转向约束）。
• 机器人导航（非完整运动学）。

2. OpenList 与 ClosedList 的作用

2.1 在 Dijkstra 算法中

• OpenList：存储待扩展节点，按 g(n) 排序。
• ClosedList：存储已扩展节点，避免重复计算。

特点：

• 无启发式，OpenList 会均匀扩展所有方向。
• ClosedList 仅用于剪枝，不影响最优性。

2.2 在 A* 算法中

• OpenList：存储待扩展节点，按 f(n) = g(n) + h(n) 排序。
• ClosedList：存储已扩展节点，若 h(n) 一致，则无需重复检查。

特点：

• 启发式引导，OpenList 优先扩展“更可能接近目标”的节点。
• ClosedList 更高效，因为 A* 的扩展范围更小。

3. 启发函数的设计

3.1 基本要求

1. 可采纳性（Admissible）：
   h(n) ≤ h*(n)
   保证 A* 找到最优解。
2. 一致性（Consistent）：
   h(n) ≤ cost(n, m) + h(m)
   保证 Closed List 无需重复检查。

3.2 常见启发函数

3.3 高级启发

• Landmark Heuristic（ALT）：利用地标节点预计算最短路径，提高估计精度。
• 机器学习启发：训练神经网络预测剩余代价（需保证可采纳性）。

总结

关键结论：

1. A* 比 Dijkstra 更高效，因启发式减少了搜索范围。
2. ClosedList 在 A* 中更高效（若 h(n) 一致）。
3. 启发函数决定 A* 的性能，需平衡估计精度与计算成本。