A*寻路算法(二)：从理论到代码实践

bit_beat
127 阅读5分钟

上一篇文章已经详细介绍了A*的算法思想，即靠实际代价G与预估代价H为待搜索节点划分搜索优先级，从而有向地遍历节点。

比如要在这么一张图中快速获取最短路径: image.png

绿色：路径节点

灰色：关闭集合中的节点

蓝色：开放集合中的节点

1. 核心数据结构准备

重点内容：

  •   节点(Node)类的定义
  •   位置表示方法
  •   优先级队列的选择

关键代码：

// 节点类定义
struct Node {
    Vector2 position;   // 位置坐标
    Node* parent;       // 父节点指针
    float g;            // 起点到当前节点的代价
    float h;            // 当前节点到终点的启发值
    float f() const { return g + h; } // 综合评估值
};

// 位置结构体
struct Vector2 {
    int x, y;
    bool operator==(const Vector2& other) const {
        return x == other.x && y == other.y;
    }
};

2. 算法初始化

实现步骤：

  1.  创建开放列表(openList)和关闭列表(closedList)
  2.  将起点加入开放列表
  3.  设置起点的G值和H值

关键代码：

// 初始化开放列表（优先队列）
auto cmp = [](Node* a, Node* b) {
     if(a->f() != b->f())
         return a->f() > b->f(); 
     return a->h > b->h; 
};

priority_queue<Node*, vector<Node*>, decltype(cmp)> openList(cmp);

// 初始化起点节点
Node* startNode = new Node{startPos, nullptr, 0, heuristic(startPos, endPos)};
openList.push(startNode);

为什么这样排序？

  • 保证最优性
    • F = G + H 是A*算法的核心公式，确保在启发式函数可采纳（即H ≤ 真实代价）时能找到最短路径。
    • 始终扩展F值最小的节点，相当于在"当前已知的最优路径"上推进。
  • 引导搜索方向
    •   H值 作为"指南针"，使搜索优先朝向目标方向。
    •   当F值相同时，选择H值更小的节点能更快接近终点。
  • 避免无效搜索**
    •   若仅按G值排序（如Dijkstra），会盲目均匀扩散。
    •   若仅按H值排序（如最佳优先搜索），可能找到非最优路径。

3. 主循环逻辑

实现步骤：

  1.  从开放列表取出F值最小的节点
  2.  检查是否到达终点
  3.  将当前节点加入关闭列表
  4.  遍历所有邻居节点

关键代码：

while (!openList.empty()) {
    // 获取F值最小的节点
    Node* current = openList.top();
    openList.pop();

    // 检查是否到达终点
    if (current->position == endPos) {
        return reconstructPath(current);
    }

    // 加入关闭列表
    closedSet.insert(current->position);

    // 遍历邻居节点
    for (auto& neighborPos : getNeighbors(current->position)) {
        // ...处理邻居节点
    }
}

4. 邻居节点处理

实现步骤：

  1.  跳过关闭列表中的节点
  2.  跳过障碍物
  3.  计算新的路径代价
  4.  更新或添加节点到开放列表

关键代码：

// 跳过已处理的节点
if (closedSet.find(neighborPos) != closedSet.end())
    continue;

// 跳过障碍物
if (isObstacle(neighborPos))
    continue;

// 计算新G值
float newG = current->g + getMoveCost(current->position, neighborPos);

// 检查节点是否在开放列表中
if (openSet.find(neighborPos) == openSet.end() || newG < existingNode->g) {
    // 创建或更新节点
    Node* neighbor = new Node{
        neighborPos,
        current,
        newG,
        heuristic(neighborPos, endPos)
    };

    // 加入开放列表
    openList.push(neighbor);//无需重新建堆，直接压入新的节点
    openSet[neighborPos] = neighbor;
}

开放列表更新问题：修改节点G值后需重新调整堆结构(对优先队列重建堆），本文章的做法是直接加入新创建的节点。

5. 路径回溯

实现步骤：

  1.  从终点节点开始回溯
  2.  沿着父节点指针逆向收集路径点
  3.  反转路径顺序

关键代码：

vector<Vector2> reconstructPath(Node* endNode) {
    vector<Vector2> path;
    Node* current = endNode;

    while (current != nullptr) {
        path.push_back(current->position);
        current = current->parent;
    }

    reverse(path.begin(), path.end());
    return path;
}

6. 关键辅助函数实现

(1) 启发式函数

int Node::CalculateHCost(const Node& end) {//计算预估代价H
    return heuristic(position, end.position);//采用曼哈顿距离
}
int Node::CalculateGCost(const Node& end) {//计算实际代价G
    return euclidean(position, end.position);//采用对角线距离
}

如上所示，对H（启发式）使用曼哈顿距离，而对G（实际代价）使用欧式距离，采用了混合策略设计

H用曼哈顿距离的原因

  • 可采纳性保证：曼哈顿距离在网格中永远 ≤ 真实代价（满足A*的最优解条件）。
    • 例如：在2D网格中，真实对角线距离是√2≈1.414，而曼哈顿距离是1+1=2（高估了，但若用对角线距离则需调整）
  • 计算效率：仅需加减法，比欧式距离的平方和开方快10倍以上。

(2) 移动代价计算

// 曼哈顿距离
float heuristic(Vector2 a, Vector2 b) {
    return 10 * abs(a.x - b.x) + abs(a.y - b.y);
}

// 欧几里得距离
float euclidean(Vector2 a, Vector2 b) {
    int dx = abs(a.x - b.x);
    int dy = abs(a.y - b.y);
    return 10 * sqrt(dx*dx + dy*dy); // 精确但慢
}

// 对角线距离
float diagonalHeuristic(Vector2 a, Vector2 b) {
    int dx = abs(a.x - b.x);
    int dy = abs(a.y - b.y);
    return 10 * (dx + dy) + 6 * min(dx, dy);//水平移动代价为10，沿对角线移动代价为20
}

选择建议

  • 精度：欧式距离 > 对角线距离
  • 性能：对角线距离 > 欧式距离（快3-5倍）
  • 推荐：在网格游戏中使用对角线距离，在物理仿真中使用欧式距离。二者的选择本质是精度与性能的权衡。

(3) 邻居节点获取

vector<Vector2> getNeighbors(Vector2 pos) {
    vector<Vector2> neighbors;
    // 八方向移动
    const vector<Vector2> directions = = {  
        {1,0}, {0,1}, {-1,0}, {0,-1},  // 四方向  
        {1,1}, {1,-1}, {-1,1}, {-1,-1} // 对角线  
    };  
    for (auto& dir : directions) {
        Vector2 neighbor = {pos.x + dir.x, pos.y + dir.y};
        neighbors.push_back(neighbor);
    }
    
    return neighbors;
}

7. 完整流程整合

伪代码：

function findPath(start, end):
    initOpenList(start)
    initClosedList()

    while openList not empty:
        current = popMinFNode()

        if current == end: return buildPath(current)
        markAsClosed(current)

        for each neighbor of current:
            if inClosed(neighbor) or isObstacle(neighbor): skip

            newG = calculateNewCost(current, neighbor)
            if neighbor not in openList or newG < oldG:
                updateOrAddNode(neighbor, current, newG)

    return emptyPath // 未找到路径

graph TD
A[初始化起点] --> B{开放列表空?}  
B -->|是| C[寻路失败]  
B -->|否| D[取F最小节点]  
D --> E{是终点?}  
E -->|是| F[回溯路径]  
E -->|否| G[加入关闭集]  
G --> H[遍历邻居]  
H --> I{可访问?}  
I -->|是| J{需更新?}  
J -->|是| K[更新节点]  
J -->|否| H  
K --> H

效果：

pathfind.gif

avatar
文章
阅读
粉丝