A*寻路算法

一. 引言

在路径规划问题中，我们常常面临这样一个挑战：如何在一个地图上从起点走到终点，既要找到一条可行的路线，又希望这条路线的总成本（距离、时间等）尽可能低。

如果只用广度优先搜索（BFS），虽然一定能找到最短路径，但它会“盲目”地搜索大量无关区域；
如果用 贪心最佳优先搜索（Greedy BFS），它会直接朝着目标冲过去，但不保证最优解；
如果用 Dijkstra 算法，能找到最短路径，但它不区分重要方向与无关方向，效率可能不高。

而 A* 算法（A-star） 结合了 Dijkstra 的全局性和贪心搜索的方向感，它会考虑会综合考虑走了多远和预计还要走多远，从而更有目的性地朝着目标方向前进。是当前最常用、性能与准确性兼顾的路径搜索算法之一，被广泛应用于游戏 AI、地图导航、机器人路径规划等领域。

二. 核心概念

A*算法的核心公式是：f(n) = g(n) + h(n)

• g(n)：从起点到当前节点n的实际代价（已经走过的路的长度）。
• h(n)：启发式函数（heuristic function），估计当前点n到终点的代价。
• f(n)：节点n的总预估成本，即假设从起点经过 n 到终点的总花费。

A*算法搜索过程概览

初始化：把起点加入开放列表（open list）。

循环：

• 从开放列表中取f值最小的节点作为当前节点。
• 如果当前节点是终点 → 搜索结束，回溯路径。
• 否则，生成它的邻居节点：
  • 忽略已在关闭列表（close list）中的节点。
  • 如果邻居不在开放列表，计算g、h、f，加入开放列表。
  • 如果邻居已在开放列表且新的g更小，更新其父节点和g值。
• 把当前节点加入关闭列表。

重复：直到找到终点或开放列表为空（无解）。

(1) 实际代价

• g(n)通常是路径长度，也可以是综合代价（例如地形消耗、能耗等）。
• 如果地图是网格且每一步代价相同，g(n)就是已走步数。
• 如果地图有不同地形（如沙地、沼泽），则 g(n)需要累加具体代价。

(2) 启发式函数的计算

h值的计算是A*算法的重要组成部分，它负责引导搜索方向，让 A* 更快找到终点，而不是像Dijkstra 那样全图遍历。

启发式函数有以下特点：

• 可接受（admissible）：对任意节点n，h(n)不大于从n到目标的真实最短代价。
• 一致/单调（consistent/monotone）：对任意节点n与其后继n′，有h(n) ≤ cost(n, n′) + h(n′)。 一致性保证当节点出列（closed）时，之后不会再找到更优路径，从而能简化实现（无需复杂回溯更新）。

对于四方向移动的网格，使用曼哈顿距离（Manhattan）来计算h值：

h_Manhattan​(a,b)=∣x_a​−x_b​∣+∣y_a​−y_b​ |

注意：如果每一步的代价不是 1，可以乘以直行代价 D。即：D ⋅ (∣d_x∣+∣d_y∣)。

在只允许上下左右的网格中，从点 A 到点 B 至少需要走 ∣dx∣次横向移动和 ∣dy∣次纵向移动，总步数至少是两者之和。由于每步代价为 1（或 D），曼哈顿距离即是最低可能代价，因此计算得到的h值不会高估 。

与之相对的，在允许八方向移动的网格中，通常有：Octile和Euclidean两种做法。

在八方向网格中通常有两种常见的移动代价约定：

1. 直行代价 = 1，斜行代价 = sqrt(2)（这是最常见、物理上合理的设定）；
2. 直行代价 = 1，斜行代价 = 1（如果把对角也当成一格步长，代价相同，则另一套启发式合适）。

网格对角优先Octile（针对直行=1，斜行=sqrt(2)​）的计算公式为：

h_octile​ = min(d_x,d_y) ⋅ sqrt(2) ​+∣dx−dy∣⋅ 1

要从A到B，最多能走min⁡(d_x, d_y)次对角（同时减横和纵），剩下差值用直行完成。所以最少代价就是上面那个式子。它恰好等于在该代价模型下（直行1，斜行sqrt(2)）能达到的最小格子路径代价 —— 因而非常“信息充足”，也是可接受且通常比欧氏更接近真实网格代价。

直线距离Euclidean的计算公式为：

h_euclid​ = sqrt(dx²+dy^2​)

这是两点的直线距离（几何直线）。任意由格子步构成的路径其长度总不小于两点间的直线距离（折线长度 ≥ 直线），因此欧氏距离永远不会高估网格约束下的真实代价——所以它是可接受的。它计算上比 Octile 更“轻”（概念上更直观），但通常比 Octile 更保守（值更小），因此启发信息量较少，搜索会扩展更多节点。

三. 核心代码

(1) 寻路算法的基本逻辑

public List<Vector2Int> FindPath(Vector2Int start, Vector2Int goal)
    {
        List<Node> openList = new List<Node>();
        HashSet<Vector2Int> closedSet = new HashSet<Vector2Int>();

        Node startNode = new Node(start);
        startNode.g = 0;
        startNode.h = Heuristic(start, goal);
        openList.Add(startNode);

        while (openList.Count > 0)
        {
            // 取 f 最小的节点
            Node current = openList[0];
            foreach (var node in openList)
                if (node.f < current.f) current = node;

            // 找到终点
            if (current.pos == goal)
                return ReconstructPath(current);

            openList.Remove(current);
            closedSet.Add(current.pos);

            // 遍历邻居
            foreach (var neighborPos in GetNeighbors(current.pos))
            {
                if (!walkable[neighborPos.x, neighborPos.y]) continue;
                if (closedSet.Contains(neighborPos)) continue;

                float tentativeG = current.g + 1; // 相邻代价为 1
                Node neighbor = openList.Find(n => n.pos == neighborPos);

                if (neighbor == null)
                {
                    neighbor = new Node(neighborPos);
                    neighbor.g = tentativeG;
                    neighbor.h = Heuristic(neighborPos, goal);
                    neighbor.parent = current;
                    openList.Add(neighbor);
                }
                else if (tentativeG < neighbor.g)
                {
                    neighbor.g = tentativeG;
                    neighbor.parent = current;
                }
            }
        }

        return null; // 没有路径
    }

(2) 使用曼哈顿距离计算四方向网格的启发式函数：

private float HeuristicManhattan(Vector2Int a, Vector2Int b, float straightCost = 1f)
{
    int dx = Mathf.Abs(a.x - b.x);
    int dy = Mathf.Abs(a.y - b.y);
    return straightCost * (dx + dy);
}

举例：

给定起点坐标 (0, 0), 目标坐标(3, 4)，可以计算出：h = 3 + 4 = 7。在只能走四方向且直行代价为1的情形下，这是必须的最少步数。

(3) 使用Octile计算八方向网格的启发式函数：

private float HeuristicOctile(Vector2Int a, Vector2Int b, float straightCost = 1f, float diagonalCost = 1.41421356f)
{
    int dx = Mathf.Abs(a.x - b.x);
    int dy = Mathf.Abs(a.y - b.y);
    int min = Mathf.Min(dx, dy);
    int diff = Mathf.Abs(dx - dy);
    return min * diagonalCost + diff * straightCost;
}

举例：

给定起点坐标 (0, 0), 目标坐标(3, 4)，可以计算出：min = min(dx = 3, dy = 4)，diff = 1，最终可以求出h = 3 ∗ 2 ​+ 1 ∗ 1 ≈ 4.24264 + 1 = 5.24264。

(4) 使用Euclidean计算八方向网格的启发式函数：

private float HeuristicEuclidean(Vector2Int a, Vector2Int b)
{
    int dx = a.x - b.x;
    int dy = a.y - b.y;
    return Mathf.Sqrt(dx * dx + dy * dy);
}

举例：

给定起点坐标 (0, 0), 目标坐标(3, 4)，可以计算出：h = sqrt(32 + 42) = 5.0。

由此可见：该方法比Octile得到的值更小，说明它更加保守。

(5) 基于八方向网格的GetNeighbors函数

private List<Vector2Int> GetNeighbors(Vector2Int pos)
{
    List<Vector2Int> neighbors = new List<Vector2Int>();

    Vector2Int[] dirs = {
        new Vector2Int(0, 1),   // 上
        new Vector2Int(0, -1),  // 下
        new Vector2Int(-1, 0),  // 左
        new Vector2Int(1, 0),   // 右
        new Vector2Int(-1, 1),  // 左上
        new Vector2Int(1, 1),   // 右上
        new Vector2Int(-1, -1), // 左下
        new Vector2Int(1, -1)   // 右下
    };

    foreach (var dir in dirs)
    {
        Vector2Int newPos = pos + dir;
        if (newPos.x >= 0 && newPos.x < width && newPos.y >= 0 && newPos.y < height)
            neighbors.Add(newPos);
    }

    return neighbors;
}

(6) 使用结构体定义一个节点Node

public class Node
{
    public Vector2Int pos;
    public float g; // 起点到当前点的代价
    public float h; // 启发式
    public float f => g + h;
    public Node parent;

    public Node(Vector2Int pos) { this.pos = pos; }
}

四. 优化点

(1) 把OpenList的线性扫描换成优先队列（Priority Queue）

当前实现每次从OpenList中寻找f值最小的节点时间复杂度是 O(n)。改为使用Priority Queue可以将其时间复杂度降为O(logn)，对大地图速度提升非常显著。

代码实现：

var open = new PriorityQueue<Node,float>();
open.Enqueue(startNode, startNode.f);
var cur = open.Dequeue();