C++启发式搜索算法详解（从零开始掌握A*路径规划）

A*算法核心思想

A*算法维护两个关键集合：

• openSet：待探索的节点集合（通常用优先队列实现）
• closedSet：已探索的节点集合

每一步，A*从 openSet 中取出 f 值最小的节点进行扩展，并更新其邻居节点的 g 值。如果某个邻居未被访问过或找到了更优路径，则将其加入 openSet。

C++实现A*算法（完整代码）

下面我们用 C++ 实现一个简单的 A* 算法，用于在二维网格地图中寻找起点到终点的最短路径。假设地图中 0 表示可通行，1 表示障碍物。

#include <iostream>#include <vector>#include <queue>#include <cmath>#include <algorithm>using namespace std;// 定义节点结构struct Node {    int x, y;    double g, h, f;        Node(int x_, int y_, double g_ = 0, double h_ = 0)         : x(x_), y(y_), g(g_), h(h_), f(g_ + h_) {}        // 用于优先队列排序（f值小的优先）    bool operator<(const Node& other) const {        return f > other.f; // 注意：priority_queue 是最大堆，所以用 >    }};// 曼哈顿距离作为启发函数double heuristic(int x1, int y1, int x2, int y2) {    return abs(x1 - x2) + abs(y1 - y2);}// A* 主函数vector<pair<int, int>> aStar(    const vector<vector<int>>& grid,    pair<int, int> start,    pair<int, int> goal) {    int rows = grid.size();    int cols = grid[0].size();        // 初始化 gScore 和 visited    vector<vector<double>> gScore(rows, vector<double>(cols, 1e9));    vector<vector<bool>> visited(rows, vector<bool>(cols, false));        // 父节点记录路径    vector<vector<pair<int, int>>> parent(rows, vector<pair<int, int>>(cols, {-1, -1}));        priority_queue<Node> openSet;        gScore[start.first][start.second] = 0;    openSet.push(Node(start.first, start.second, 0, heuristic(start.first, start.second, goal.first, goal.second)));        // 四个方向移动    vector<pair<int, int>> directions = {{0,1}, {1,0}, {0,-1}, {-1,0}};        while (!openSet.empty()) {        Node current = openSet.top();        openSet.pop();                if (visited[current.x][current.y]) continue;        visited[current.x][current.y] = true;                // 到达终点        if (current.x == goal.first && current.y == goal.second) {            // 回溯路径            vector<pair<int, int>> path;            pair<int, int> curr = {goal.first, goal.second};            while (curr.first != -1) {                path.push_back(curr);                curr = parent[curr.first][curr.second];            }            reverse(path.begin(), path.end());            return path;        }                // 探索邻居        for (auto& dir : directions) {            int nx = current.x + dir.first;            int ny = current.y + dir.second;                        if (nx < 0 || nx >= rows || ny < 0 || ny >= cols || grid[nx][ny] == 1 || visited[nx][ny])                continue;                        double tentative_g = current.g + 1; // 假设每步代价为1                        if (tentative_g < gScore[nx][ny]) {                gScore[nx][ny] = tentative_g;                double h = heuristic(nx, ny, goal.first, goal.second);                openSet.push(Node(nx, ny, tentative_g, h));                parent[nx][ny] = {current.x, current.y};            }        }    }        return {}; // 无路径}// 测试主函数int main() {    vector<vector<int>> grid = {        {0, 0, 0, 0, 1},        {1, 1, 0, 0, 0},        {0, 0, 0, 1, 0},        {0, 1, 0, 0, 0},        {0, 0, 0, 1, 0}    };        pair<int, int> start = {0, 0};    pair<int, int> goal = {4, 4};        vector<pair<int, int>> path = aStar(grid, start, goal);        if (!path.empty()) {        cout << "找到路径：\n";        for (auto& p : path) {            cout << "(" << p.first << ", " << p.second << ") ";        }        cout << endl;    } else {        cout << "未找到路径！" << endl;    }        return 0;}

代码说明与优化建议

上述代码实现了基本的 A* 算法，使用曼哈顿距离作为启发函数（适用于只能上下左右移动的网格）。如果你的游戏或应用允许对角线移动，可以改用欧几里得距离：

double heuristic(int x1, int y1, int x2, int y2) {    return sqrt((x1 - x2)*(x1 - x2) + (y1 - y2)*(y1 - y2));}

此外，为了提升性能，你可以考虑以下优化：

• 使用更高效的 openSet 数据结构（如 Fibonacci 堆）
• 提前终止条件（如限制搜索深度）
• 动态调整启发函数权重（Weighted A*）

总结

通过本教程，你已经掌握了如何用 C++ 实现 A* 启发式搜索算法。这项技能在路径规划算法领域非常实用，无论是开发游戏 AI、机器人导航，还是解决迷宫问题，都能派上用场。

记住，启发式函数的选择直接影响算法效率。只要 h(n) ≤ 实际代价（即满足“可采纳性”），A* 就能保证找到最优解。

现在，动手试试吧！修改地图、更换启发函数，观察路径变化，加深理解。祝你在C++启发式搜索算法的学习之旅中收获满满！