c++ A*搜索算法

算法原理

A*算法通过以下公式计算节点的优先级：

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

f(n)：节点 n的总优先级，表示从起点通过节点 n 到目标点的估计代价。
g(n)：从起点到节点 n 的实际代价。
h(n)：从节点 n到目标点的启发式估计代价。

A*的核心是选择具有最低 f(n)值的节点进行扩展，从而在保证正确性的同时提高效率。

启发函数（Heuristic Function）

启发函数 h(n)决定了算法的效率和准确性：

一致性（Consistency）：如果 h(n)满足三角不等式，即 h(n)≤c(n,n′)+h(n′)，算法可以保证最优性。
常用启发函数：
- 曼哈顿距离（适用于网格地图）：
```
h(n)=∣x1−x2∣+∣y1−y2∣
```
- 欧几里得距离（适用于几何地图）：
- 自定义启发函数：针对特定场景设计的函数。

算法流程

初始化：

将起点加入开放列表（Open List）。
关闭列表（Closed List）为空。

迭代搜索：

从开放列表中选择 f(n)最小的节点 n。
如果 n 是目标节点，终止并返回路径。
将 n 从开放列表移除，并加入关闭列表。
扩展节点 n 的所有邻居节点：
- 如果邻居不在关闭列表中：
  - 计算邻居的 g(n)、h(n)和 f(n)。
  - 如果邻居不在开放列表中，将其加入开放列表。
  - 如果邻居已在开放列表中且新的 g(n) 更优，更新邻居的值。

返回结果：

如果开放列表为空，表示无解。
否则返回路径。

代码实现

#include <iostream>
#include <vector>
#include <queue>
#include <cmath>
#include <unordered_map>
using namespace std;

// 定义网格节点
struct Node {
    int x, y;           // 坐标
    double g, h, f;     // 代价
    Node* parent;       // 父节点

    Node(int x, int y, double g = 0, double h = 0, Node* parent = nullptr)
        : x(x), y(y), g(g), h(h), f(g + h), parent(parent) {}

    bool operator>(const Node& other) const {
        return f > other.f; // 优先队列按 f 值排序
    }
};

// 启发函数：曼哈顿距离
double heuristic(int x1, int y1, int x2, int y2) {
    return abs(x1 - x2) + abs(y1 - y2);
}

// 检查点是否在网格内
bool isValid(int x, int y, int rows, int cols) {
    return x >= 0 && x < rows && y >= 0 && y < cols;
}

// A* 搜索
vector<pair<int, int>> aStarSearch(vector<vector<int>>& grid, pair<int, int> start, pair<int, int> goal) {
    int rows = grid.size();
    int cols = grid[0].size();

    priority_queue<Node, vector<Node>, greater<Node>> openList; // 最小堆
    unordered_map<int, bool> closedList; // 关闭列表（标记访问的点）

    auto encode = [&](int x, int y) { return x * cols + y; }; // 坐标哈希

    // 初始节点
    Node* startNode = new Node(start.first, start.second, 0, heuristic(start.first, start.second, goal.first, goal.second));
    openList.push(*startNode);

    // 定义方向向量
    vector<pair<int, int>> directions = {{-1, 0}, {1, 0}, {0, -1}, {0, 1}};

    while (!openList.empty()) {
        Node current = openList.top();
        openList.pop();

        // 检查是否到达目标
        if (current.x == goal.first && current.y == goal.second) {
            vector<pair<int, int>> path;
            Node* temp = &current;
            while (temp) {
                path.emplace_back(temp->x, temp->y);
                temp = temp->parent;
            }
            reverse(path.begin(), path.end());
            return path;
        }

        // 标记当前点为访问过
        closedList[encode(current.x, current.y)] = true;

        // 扩展邻居节点
        for (auto& dir : directions) {
            int nx = current.x + dir.first;
            int ny = current.y + dir.second;

            if (isValid(nx, ny, rows, cols) && grid[nx][ny] == 0 && !closedList[encode(nx, ny)]) {
                double g = current.g + 1; // 假设代价为1
                double h = heuristic(nx, ny, goal.first, goal.second);
                openList.push(Node(nx, ny, g, h, new Node(current.x, current.y, current.g, current.h, current.parent)));
            }
        }
    }

    return {}; // 无解
}

int main() {
    vector<vector<int>> grid = {
        {0, 0, 0, 0, 0},
        {0, 1, 1, 1, 0},
        {0, 0, 0, 1, 0},
        {0, 1, 0, 0, 0},
        {0, 0, 0, 0, 0},
    };

    pair<int, int> start = {0, 0};
    pair<int, int> goal = {4, 4};

    vector<pair<int, int>> path = aStarSearch(grid, start, goal);

    if (!path.empty()) {
        cout << "Path found:\n";
        for (auto& p : path) {
            cout << "(" << p.first << ", " << p.second << ") -> ";
        }
        cout << "Goal\n";
    } else {
        cout << "No path found.\n";
    }

    return 0;
}

时间复杂度