C++ 实现A*算法-EW帮帮网

A* 算法详解：路径规划中的“黄金标准”

在众多路径搜索算法中，A* 算法（A-star）因其高效性与灵活性，被广泛应用于游戏开发、机器人导航、地图路径规划等领域。本文将带你深入了解这个经典算法的原理与应用。

一、A* 算法简介

A* 是一种启发式搜索算法，它在 Dijkstra 算法的基础上引入了估价函数，通过更聪明地选择路径节点，以更快地找到目标路径。其核心思想是：在保证最短路径的同时，尽量减少搜索空间。

公式结构

A* 算法的核心是一个评价函数：

[
f(n) = g(n) + h(n)
]

g(n)：从起点到当前节点 n 的实际代价。
h(n)：从节点 n 到目标的启发式估价（Heuristic）。
f(n)：当前路径的总代价估算。

二、适用场景

A* 算法的强大之处在于可调节性，只需替换启发式函数 h(n)，即可适配不同需求：

游戏AI路径寻路：精确且快速避开障碍物。
机器人路径规划：可结合物理空间地图进行实时路径规划。
交通路线计算：如 GPS 导航系统，估算时间/距离混合代价。

三、A* 与其他算法的对比

算法	是否启发式	是否保证最短路径	性能表现
Dijkstra	否	是	较慢，搜索广泛
BFS	否	是（在无权图中）	节点多时低效
DFS	否	否	可能陷入死路
A*	是	是	速度与精度兼顾

四、启发式函数的设计

A* 成败的关键在于 h(n) 的设计。启发函数需估价准确且不能高估（即必须“可采”/admissible），常见设计包括：

曼哈顿距离：适用于网格地图（只能上下左右移动）：
[
h(n) = |x_1 - x_2| + |y_1 - y_2|
]
欧几里得距离：适用于可斜向移动的场景：
[
h(n) = \sqrt{(x_1 - x_2)^2 + (y_1 - y_2)^2}
]
对角线距离：适用于八方向移动：
[
h(n) = \max(|x_1 - x_2|, |y_1 - y_2|)
]

五、A* 的常见陷阱与优化

常见陷阱

启发函数不合适：如采用了过高的估价，导致搜索路径偏离最优解。
未考虑障碍物或代价变化：导致路径穿越不可行区域。
数据结构低效：如 open list 用线性数组处理会严重拖慢效率。

优化建议

使用 优先队列（如堆） 来管理 open list。
双向 A*：同时从起点与终点搜索，加速收敛。
动态启发式函数结合实际代价进行调整。
针对稀疏图使用 Jump Point Search 等优化手段。

算法原理

代码实现是基于下面这篇博客。

https://blog.csdn.net/hitwhylz/article/details/23089415

代码实现

    #include <vector>
    #include <queue>
    #include <algorithm>
    #include <cmath>

    // 直线和对角线代价（常用 A* 设定）
    const int kCostStraight = 10;
    const int kCostDiagonal = 14;

    // 节点结构
    struct Node {
        int x, y;
        int g, h;
        Node* parent;

        Node(int x_, int y_, int g_ = 0, int h_ = 0, Node* parent_ = nullptr)
            : x(x_), y(y_), g(g_), h(h_), parent(parent_) {}

        int f() const { return g + h; }

        bool operator==(const Node& other) const {
            return x == other.x && y == other.y;
        }
    };

    // 启发函数：曼哈顿距离
    int Heuristic(int x1, int y1, int x2, int y2) {
        return 10 * (std::abs(x1 - x2) + std::abs(y1 - y2));
    }

    // 判断是否为合法格子
    bool IsValid(int x, int y, const std::vector<std::vector<int>>& map) {
        return x >= 0 && x < map.size() &&
               y >= 0 && y < map[0].size() &&
               map[x][y] == 0;
    }

    // 获取邻居节点
    std::vector<Node*> GetNeighbors(Node* current, const std::vector<std::vector<int>>& map, const Node& end, bool is_cross_coner) {
        std::vector<Node*> neighbors;
        int rows = map.size();
        int cols = map[0].size();

        for (int dx = 1; dx >= -1; --dx) {
            for (int dy = -1; dy <= 1; ++dy) {
                if (dx == 0 && dy == 0) continue;

                int new_x = current->x + dx;
                int new_y = current->y + dy;

                // 地图边界检查
                if (!IsValid(new_x, new_y, map)) continue;
                if (map[new_x][new_y] == 1) continue;  // 障碍物

                // 如果不允许穿过角落，处理斜向移动的合法性
                if (!is_cross_coner && dx != 0 && dy != 0) {
                    int adj1_x = current->x + dx;
                    int adj1_y = current->y;
                    int adj2_x = current->x;
                    int adj2_y = current->y + dy;

                    if (!IsValid(adj1_x, adj1_y, map) || !IsValid(adj2_x, adj2_y, map)) continue;
                    if (map[adj1_x][adj1_y] == 1 || map[adj2_x][adj2_y] == 1) continue;
                }

                int cost = (dx == 0 || dy == 0) ? kCostStraight : kCostDiagonal;
                int g = current->g + cost;
                int h = Heuristic(new_x, new_y, end.x, end.y);

                neighbors.push_back(new Node(new_x, new_y, g, h, current));
            }
        }
        return neighbors;
    }


    std::vector<Node> AStar(const std::vector<std::vector<int>>& map, const Node& start, const Node& end) {
        std::vector<Node*> open_list;
        std::vector<Node*> closed_list;

        auto start_node = new Node(start.x, start.y, 0, Heuristic(start.x, start.y, end.x, end.y));
        open_list.push_back(start_node);

        int step = 0;

        while (!open_list.empty()) {

            auto current_it = std::min_element(open_list.begin(), open_list.end(),
                [](Node* a, Node* b) { return a->f() < b->f(); });
            Node* current = *current_it;

            std::cout << "Step " << ++step << ": 当前处理节点 (" << current->x << ", " << current->y << ")"
                      << " g=" << current->g << " h=" << current->h << " f=" << current->f() << "\n";

            // 打印 Open List
            std::cout << "Open List: \n";
            for (auto node : open_list) {
                std::cout << "  (" << node->x << "," << node->y << ") g=" << node->g
                          << " h=" << node->h << " f=" << node->f() << "\n";
            }

            // 如果到达终点
            if (*current == end) {
                std::vector<Node> path;
                while (current) {
                    path.push_back(*current);
                    current = current->parent;
                }
                std::reverse(path.begin(), path.end());
                for (auto node : open_list) delete node;
                for (auto node : closed_list) delete node;
                return path;
            }

            open_list.erase(current_it);
            closed_list.push_back(current);
            std::cout<<" 已处理节点：\n";
            for (auto &t:closed_list) {
                std::cout << "    (" << t->x << "," << t->y << ") "
                          << "g=" << t->g << " h=" << t->h << " f=" << t->f() << "\n";
            }
            auto neighbors = GetNeighbors(current, map, end, false);

            std::cout << "  邻居节点:\n";
            for (auto neighbor : neighbors) {
                std::cout << "    (" << neighbor->x << "," << neighbor->y << ") "
                          << "g=" << neighbor->g << " h=" << neighbor->h << " f=" << neighbor->f() << "\n";
            }

            for (auto neighbor : neighbors) {
                // 如果在 Closed List 中跳过
                bool in_closed = std::any_of(closed_list.begin(), closed_list.end(),
                                             [neighbor](Node* n) { return *n == *neighbor; });
                if (in_closed) {
                    delete neighbor;
                    continue;
                }

                auto open_it = std::find_if(open_list.begin(), open_list.end(),
                                            [neighbor](Node* n) { return *n == *neighbor; });

                if (open_it == open_list.end()) {
                    // 新节点加入
                    std::cout << "  添加到Open_list: (" << neighbor->x << "," << neighbor->y << ") "
                              << "g=" << neighbor->g << " h=" << neighbor->h << " f=" << neighbor->f() << "\n";
                    open_list.push_back(neighbor);
                } else {
                    if (neighbor->g < (*open_it)->g) {
                        (*open_it)->g = neighbor->g;
                        (*open_it)->parent = current;
                        std::cout << "  更新节点 (" << (*open_it)->x << "," << (*open_it)->y << ") 的 g 值为 " <<
                            (*open_it)->g <<" 父节点： ("<<current->x<<","<<current->y<<")"<< "\n";
                    }
                    delete neighbor;
                }
            }

            std::cout << "---------------------------\n";
        }

        for (auto node : open_list) delete node;
        for (auto node : closed_list) delete node;
        return {};
    }



    // 打印路径
    void PrintPath(const std::vector<Node>& path, const std::vector<std::vector<int>>& map) {
        auto display_map = map;
        for (const auto& node : path) {
            display_map[node.x][node.y] = 8;
        }
        for (int i = 0; i < display_map.size(); ++i) {
            for (int j = 0; j < display_map[0].size(); ++j) {
                if (display_map[i][j] == 1)
                    std::cout << "█  ";
                else if (display_map[i][j] == 8)
                    std::cout << "*  ";
                else
                    std::cout << ".  ";
            }
            std::cout << "\n";
        }
    }

    // 主函数
    int main() {
        std::vector<std::vector<int>> map = {
            {0, 0, 0, 0, 0, 0, 0, 0},
            {0, 0, 0, 0, 1, 0, 0, 0},
            {0, 0, 0, 0, 1, 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},
        };

        Node start(2, 2), end(2, 6);
        auto path = AStar(map, start, end);

        if (!path.empty()) {
            std::cout << "找到路径，共 " << path.size() << " 步:\n";
            PrintPath(path, map);
        } else {
            std::cout << "未找到路径。\n";
        }

        return 0;
    }

C++ 实现A*算法