路径搜索算法——A*算法_a*搜索伪代码-CSDN博客

本文链接：https://blog.csdn.net/swaggytiger/article/details/149026889

路径搜索算法——A*算法

一、A*算法简介

A* 是一种用于图搜索的启发式搜索算法（Heuristic Search Algorithm），常用于求解从起点到目标点的最短路径问题。它结合了 Dijkstra 算法的最短路径特性与贪心算法的目标导向性，既高效又能找到最优解。

二、算法核心思想

每个节点的总评估代价：

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

$f (n) ：从起点到终点经过当前点 n 的总代价估计；$
$g (n) ：从起点到当前点 n 的实际代价；$
$h (n) ：从当前点 n 到目标点的启发函数（ He u r i s t i c f u n c t i o n ），即代价估计。$

在这里插入图片描述

代价函数分为两部分，一部分是到目前点的代价，另外一部分是通过启发函数 $h (n)$ 计算到目标点的距离

三、启发函数 $h (n)$

启发函数是算法效率的关键。常用的启发函数包括：

启发函数	适用场景	说明
曼哈顿距离（Manhattan）	仅水平垂直移动的栅格地图	$h (n) =∣ x 1 - x 2 ∣ + ∣ y 1 - y 2 ∣$
欧几里得距离（Euclidean）	可以斜着走的平面图	$\sqrt{(x_1 - x_2)^2 + (y_1 - y_2)^2}$
对角线距离	允许八方向移动	$h (n) = ma x (∣ x 1 - x 2 ∣, ∣ y 1 - y 2 ∣)$

如果 $h (n)$ 永远不超过真实的最短代价，则称它是可接受的（admissible），A* 就能保证找到最优路径。

四、A* 算法流程（伪代码）

1. 将起点加入开放列表 open_list
2. 初始化关闭列表 closed_list 为空

3. 当 open_list 不为空时：
   a. 从 open_list 中选出 f 最小的节点 current
   b. 如果 current 是目标节点，则回溯返回路径
   c. 将 current 加入 closed_list
   d. 遍历 current 的所有相邻节点 neighbor：
       - 如果 neighbor 在 closed_list 中，则跳过
       - 计算 neighbor 的 g、h、f 值
       - 如果 neighbor 不在 open_list 中，则加入
       - 如果在 open_list 中，且新的 g 更小，则更新其 g 和父节点

五、Python 简化实现示例

以下为二维栅格地图上的 A* 实现（仅上下左右四邻接）：

import heapq  # 导入堆队列模块，用作优先队列（open list）

def astar(grid, start, goal):
    # 获取地图大小
    rows, cols = len(grid), len(grid[0])

    # 启发函数：使用曼哈顿距离作为估算从当前位置到目标点的代价
    def h(n):
        return abs(n[0] - goal[0]) + abs(n[1] - goal[1])

    # open_list 使用最小堆结构，每个元素是 (f, g, 当前节点坐标, 当前路径)
    open_list = [(h(start), 0, start, [])]

    # visited 用于记录已经访问过的节点，避免重复访问
    visited = set()

    # A* 主循环，只要 open_list 非空就一直搜索
    while open_list:
        # 弹出 f 最小的节点（f = g + h）
        f, g, node, path = heapq.heappop(open_list)

        # 如果这个节点已经访问过，则跳过
        if node in visited:
            continue
        visited.add(node)

        # 将当前节点加入路径轨迹
        path = path + [node]

        # 如果当前节点是目标点，返回完整路径
        if node == goal:
            return path

        # 遍历当前节点的四个相邻方向（上、下、左、右）
        for dx, dy in [(-1,0), (1,0), (0,-1), (0,1)]:
            x, y = node[0] + dx, node[1] + dy

            # 判断是否越界，是否为障碍物（1表示障碍，0表示可通行）
            if 0 <= x < rows and 0 <= y < cols and grid[x][y] == 0:
                # 新的 g = 当前 g + 1（每步代价为 1）
                new_g = g + 1
                # 新的 f = g + h
                new_f = new_g + h((x, y))
                # 将该邻居节点加入 open_list，路径为当前路径加当前节点
                heapq.heappush(open_list, (new_f, new_g, (x, y), path))

    # 如果搜索结束都没找到目标，则返回 None
    return None

# 示例地图：0 为可通行区域，1 为障碍物
grid = [
    [0, 0, 0, 0],
    [1, 1, 0, 1],
    [0, 0, 0, 0],
    [0, 1, 1, 0],
    [0, 0, 0, 0]
]

# 定义起点和终点
start = (0, 0)
goal = (4, 3)

# 调用 A* 函数进行路径搜索
path = astar(grid, start, goal)

# 打印搜索结果
if path:
    print("找到路径：")
    print(path)
else:
    print("无可行路径")

六、C++ 简化实现示例

下面是一个用 C++ 实现 A* 算法 的简化示例，支持二维栅格地图（四方向移动），使用曼哈顿距离作为启发函数。

#include <iostream>
#include <vector>
#include <queue>
#include <cmath>
#include <unordered_set>
#include <unordered_map>
#include <algorithm>

using namespace std;

// 节点结构体，包含坐标、代价信息和父节点指针
struct Node {
    int x, y;           // 节点坐标
    int g, h;           // g: 起点到当前点的代价；h: 启发函数估值
    Node* parent;       // 父节点指针，用于路径回溯

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

    int f() const { return g + h; }  // 总代价 f = g + h

    // 重载运算符，用于 priority_queue 比较，f 值小的优先
    bool operator>(const Node& other) const {
        return f() > other.f();
    }
};

// 哈希函数：允许 unordered_map/set 使用 pair<int, int> 作为 key
struct PairHash {
    size_t operator()(const pair<int, int>& p) const {
        return hash<int>()(p.first) ^ hash<int>()(p.second << 1);
    }
};

// 检查位置 (x, y) 是否在地图范围内且不是障碍物
bool isValid(int x, int y, const vector<vector<int>>& grid) {
    return x >= 0 && x < grid.size() && y >= 0 && y < grid[0].size() && grid[x][y] == 0;
}

// 启发函数：曼哈顿距离，用于估计当前点到目标点的代价
int manhattan(int x1, int y1, int x2, int y2) {
    return abs(x1 - x2) + abs(y1 - y2);
}

// 回溯路径：从终点向父节点递归，生成完整路径
vector<pair<int, int>> reconstructPath(Node* node) {
    vector<pair<int, int>> path;
    while (node) {
        path.emplace_back(node->x, node->y);
        node = node->parent;
    }
    reverse(path.begin(), path.end()); // 起点在前
    return path;
}

// A* 主算法函数
vector<pair<int, int>> astar(const vector<vector<int>>& grid,
                              pair<int, int> start,
                              pair<int, int> goal) {
    int m = grid.size(), n = grid[0].size();

    // open list：优先队列，按 f 值升序排列
    priority_queue<Node, vector<Node>, greater<Node>> open;

    // 记录所有创建过的节点，用于避免重复生成
    unordered_map<pair<int, int>, Node*, PairHash> allNodes;

    // 记录已访问节点（closed list）
    unordered_set<pair<int, int>, PairHash> closed;

    // 创建起点节点，g=0，h=曼哈顿距离
    Node* startNode = new Node(start.first, start.second,
                               0, manhattan(start.first, start.second, goal.first, goal.second));
    open.push(*startNode);
    allNodes[start] = startNode;

    // 允许移动的方向（上、下、左、右）
    vector<pair<int, int>> directions = {{-1,0}, {1,0}, {0,-1}, {0,1}};

    // 开始搜索
    while (!open.empty()) {
        Node current = open.top();
        open.pop();
        pair<int, int> currPos = {current.x, current.y};

        // 如果已经访问过该节点，则跳过
        if (closed.count(currPos)) continue;
        closed.insert(currPos);

        // 如果到达目标点，则回溯路径
        if (currPos == goal) {
            return reconstructPath(allNodes[currPos]);
        }

        // 遍历当前节点的相邻节点
        for (auto [dx, dy] : directions) {
            int nx = current.x + dx;
            int ny = current.y + dy;
            pair<int, int> neighbor = {nx, ny};

            // 如果不合法或已经访问过，跳过
            if (!isValid(nx, ny, grid) || closed.count(neighbor)) continue;

            // 计算新 g 和 h
            int g_new = current.g + 1; // 每移动一步代价为 1
            int h_new = manhattan(nx, ny, goal.first, goal.second);

            // 如果是新节点或找到更短路径，更新信息并加入 open list
            if (!allNodes.count(neighbor) || g_new < allNodes[neighbor]->g) {
                Node* nextNode = new Node(nx, ny, g_new, h_new, allNodes[currPos]);
                open.push(*nextNode);
                allNodes[neighbor] = nextNode;
            }
        }
    }

    // 未找到路径，返回空
    return {};
}

// 主函数：测试 A* 算法
int main() {
    // 地图：0 可通行，1 是障碍物
    vector<vector<int>> grid = {
        {0, 0, 0, 0},
        {1, 1, 0, 1},
        {0, 0, 0, 0},
        {0, 1, 1, 0},
        {0, 0, 0, 0}
    };

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

    auto path = astar(grid, start, goal);

    // 打印路径结果
    if (path.empty()) {
        cout << "未找到可行路径。" << endl;
    } else {
        cout << "路径如下：" << endl;
        for (auto [x, y] : path) {
            cout << "(" << x << ", " << y << ") ";
        }
        cout << endl;
    }

    return 0;
}