A*算法（C++实现）

饕餮怪程序猿2025-12-06 19:01

一、A*算法原理

1.1 核心思想

A*算法是一种启发式搜索算法，结合了Dijkstra算法 （保证找到最短路径）和贪心最佳优先搜索（高效但不保证最优）的优点。

1.2 关键公式

复制代码 
f(n) = g(n) + h(n)
  • f(n)：节点n的综合优先级
  • g(n)：从起点到节点n的实际代价
  • h(n)：从节点n到终点的预估代价（启发函数）

1.3 启发函数要求

启发函数h(n)必须满足：

  • 可采纳性：h(n) ≤ 实际代价（保证找到最优解）
  • 一致性：h(n) ≤ c(n, m) + h(m)（保证效率）

二、C++实现

2.1 完整代码

cpp 复制代码 
#include <iostream>
#include <vector>
#include <queue>
#include <cmath>
#include <algorithm>
#include <functional>
#include <unordered_set>

using namespace std;

// 定义节点结构
struct Node {
    int x, y;           // 节点坐标
    double f, g, h;     // f = g + h
    Node* parent;       // 父节点指针
    
    Node(int _x, int _y) : x(_x), y(_y), f(0), g(0), h(0), parent(nullptr) {}
    
    // 重载相等运算符
    bool operator==(const Node& other) const {
        return x == other.x && y == other.y;
    }
};

// 自定义哈希函数
struct NodeHash {
    size_t operator()(const Node* node) const {
        return node->x * 1000 + node->y;
    }
};

// 自定义相等比较
struct NodeEqual {
    bool operator()(const Node* a, const Node* b) const {
        return a->x == b->x && a->y == b->y;
    }
};

// 用于优先队列的比较函数
struct CompareNode {
    bool operator()(const Node* a, const Node* b) const {
        return a->f > b->f;  // 小顶堆
    }
};

class AStar {
private:
    vector<vector<int>> grid;       // 网格地图
    int rows, cols;                // 网格大小
    Node* startNode;               // 起点
    Node* endNode;                 // 终点
    
    // 八个方向的移动（包括对角线）
    const int dirs[8][2] = {
        {-1, 0}, {1, 0}, {0, -1}, {0, 1},   // 上下左右
        {-1, -1}, {-1, 1}, {1, -1}, {1, 1}  // 对角线
    };
    
    // 对角线移动的代价
    const double sqrt2 = 1.414;
    
public:
    AStar(vector<vector<int>>& _grid) : grid(_grid) {
        rows = grid.size();
        cols = grid[0].size();
        startNode = nullptr;
        endNode = nullptr;
    }
    
    // 设置起点终点
    void setStartEnd(int sx, int sy, int ex, int ey) {
        startNode = new Node(sx, sy);
        endNode = new Node(ex, ey);
    }
    
    // 计算启发函数（欧几里得距离）
    double heuristic(Node* a, Node* b) {
        return sqrt(pow(a->x - b->x, 2) + pow(a->y - b->y, 2));
    }
    
    // 曼哈顿距离（适用于只能上下左右移动的场景）
    double manhattan(Node* a, Node* b) {
        return abs(a->x - b->x) + abs(a->y - b->y);
    }
    
    // 检查坐标是否有效
    bool isValid(int x, int y) {
        return x >= 0 && x < rows && y >= 0 && y < cols;
    }
    
    // 检查是否为障碍物
    bool isUnblocked(int x, int y) {
        return grid[x][y] == 0;  // 0表示可通行
    }
    
    // 执行A*搜索
    vector<pair<int, int>> search() {
        if (!startNode || !endNode) {
            cout << "请先设置起点和终点！" << endl;
            return {};
        }
        
        // 如果起点或终点是障碍物
        if (!isUnblocked(startNode->x, startNode->y) || 
            !isUnblocked(endNode->x, endNode->y)) {
            cout << "起点或终点是障碍物！" << endl;
            return {};
        }
        
        // 如果起点就是终点
        if (*startNode == *endNode) {
            return {{startNode->x, startNode->y}};
        }
        
        // 优先队列（开启列表）
        priority_queue<Node*, vector<Node*>, CompareNode> openList;
        
        // 关闭列表和开启列表的哈希集合
        unordered_set<Node*, NodeHash, NodeEqual> closedSet;
        unordered_set<Node*, NodeHash, NodeEqual> openSet;
        
        // 初始化起点
        startNode->g = 0;
        startNode->h = heuristic(startNode, endNode);
        startNode->f = startNode->g + startNode->h;
        
        openList.push(startNode);
        openSet.insert(startNode);
        
        while (!openList.empty()) {
            // 获取f值最小的节点
            Node* current = openList.top();
            openList.pop();
            
            // 如果到达终点
            if (*current == *endNode) {
                return reconstructPath(current);
            }
            
            // 将当前节点移入关闭列表
            closedSet.insert(current);
            
            // 遍历所有可能的移动方向
            for (int i = 0; i < 8; i++) {
                int newX = current->x + dirs[i][0];
                int newY = current->y + dirs[i][1];
                
                // 检查新位置是否有效
                if (!isValid(newX, newY) || !isUnblocked(newX, newY)) {
                    continue;
                }
                
                // 创建新节点
                Node* neighbor = new Node(newX, newY);
                
                // 如果邻居在关闭列表中，跳过
                if (closedSet.find(neighbor) != closedSet.end()) {
                    delete neighbor;
                    continue;
                }
                
                // 计算移动代价
                double moveCost = (i < 4) ? 1.0 : sqrt2;  // 直线或对角线
                
                // 计算新的g值
                double newG = current->g + moveCost;
                
                // 检查邻居是否在开启列表中
                auto it = openSet.find(neighbor);
                if (it == openSet.end()) {
                    // 不在开启列表中
                    neighbor->g = newG;
                    neighbor->h = heuristic(neighbor, endNode);
                    neighbor->f = neighbor->g + neighbor->h;
                    neighbor->parent = current;
                    
                    openList.push(neighbor);
                    openSet.insert(neighbor);
                } else {
                    // 已在开启列表中，检查是否需要更新
                    Node* existing = *it;
                    if (newG < existing->g) {
                        // 更新节点信息
                        existing->g = newG;
                        existing->f = existing->g + existing->h;
                        existing->parent = current;
                        
                        // 重新调整优先队列
                        // 注意：标准priority_queue不支持更新，需要重新构建
                        // 这里简化处理，新节点会以更低的f值被优先处理
                    }
                    delete neighbor;
                }
            }
        }
        
        cout << "未找到路径！" << endl;
        return {};
    }
    
    // 重建路径
    vector<pair<int, int>> reconstructPath(Node* node) {
        vector<pair<int, int>> path;
        
        while (node != nullptr) {
            path.push_back({node->x, node->y});
            node = node->parent;
        }
        
        reverse(path.begin(), path.end());
        return path;
    }
    
    // 打印网格和路径
    void printPath(const vector<pair<int, int>>& path) {
        vector<vector<char>> display(rows, vector<char>(cols, '.'));
        
        // 标记障碍物
        for (int i = 0; i < rows; i++) {
            for (int j = 0; j < cols; j++) {
                if (grid[i][j] == 1) {
                    display[i][j] = '#';
                }
            }
        }
        
        // 标记路径
        for (const auto& p : path) {
            if (p.first == startNode->x && p.second == startNode->y) {
                display[p.first][p.second] = 'S';
            } else if (p.first == endNode->x && p.second == endNode->y) {
                display[p.first][p.second] = 'E';
            } else {
                display[p.first][p.second] = '*';
            }
        }
        
        // 打印网格
        cout << "\n路径地图：" << endl;
        for (int i = 0; i < rows; i++) {
            for (int j = 0; j < cols; j++) {
                cout << display[i][j] << " ";
            }
            cout << endl;
        }
    }
    
    ~AStar() {
        delete startNode;
        delete endNode;
    }
};

// 测试函数
int main() {
    // 创建地图：0表示可通行，1表示障碍物
    vector<vector<int>> grid = {
        {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, 1, 0, 0, 0},
        {0, 0, 1, 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}
    };
    
    AStar astar(grid);
    
    // 设置起点和终点
    int startX = 0, startY = 0;  // 左上角
    int endX = 7, endY = 7;      // 右下角
    
    astar.setStartEnd(startX, startY, endX, endY);
    
    // 执行搜索
    vector<pair<int, int>> path = astar.search();
    
    if (!path.empty()) {
        cout << "\n找到路径！路径长度：" << path.size() << endl;
        cout << "路径坐标：" << endl;
        for (const auto& p : path) {
            cout << "(" << p.first << ", " << p.second << ")";
            if (p.first != endX || p.second != endY) {
                cout << " -> ";
            }
        }
        cout << endl;
        
        // 打印路径地图
        astar.printPath(path);
    }
    
    return 0;
}

输出结果：

复制代码 
找到路径！路径长度：10
路径坐标：
(0, 0) -> (0, 1) -> (0, 2) -> (1, 3) -> (2, 4) -> (3, 5) -> (4, 6) -> (5, 6) -> (6, 7) -> (7, 7)

路径地图：
S * * . . . . . 
. . # * . . . . 
. . # . * . . . 
. . # . # * . . 
. . # . # . * . 
. . . . # . * . 
. . . . # . . * 
. . . . . . . E

2.2 代码步骤解析

  1. 初始化：

    • 创建起点和终点节点
    • 初始化开启列表（优先队列）和关闭列表

  2. 主循环：

    • 从开启列表取出f值最小的节点
    • 如果该节点是终点，重建路径
    • 将当前节点加入关闭列表
    • 遍历所有邻居节点

  3. 处理邻居节点：

    • 检查邻居是否有效（边界、障碍物）
    • 计算新的g值
    • 如果邻居不在开启列表中，计算f=g+h并加入开启列表
    • 如果已在开启列表中，检查是否需要更新g值

  4. 路径重建：

    • 从终点回溯到起点
    • 反转路径顺序

三、A*算法应用场景

3.1 游戏开发

  • 角色移动：RTS、RPG、SLG游戏中单位的寻路
  • NPC行为：敌人的追击、巡逻路径规划
  • 自动导航：游戏中的自动寻路系统

3.2 机器人技术

  • 移动机器人：室内导航、避障
  • 无人机：飞行路径规划
  • 自动驾驶：车辆路径规划

3.3 GIS系统

  • 地图导航：GPS路径规划（如Google Maps）
  • 物流配送：最优配送路线规划
  • 紧急救援：最短时间到达路径

3.4 其他领域

  • 网络路由：数据包传输路径选择
  • 人工智能：状态空间搜索
  • 电路设计：布线算法

四、优化与变体

4.1 性能优化

  1. 数据结构优化：

    cpp 复制代码 
    // 使用更高效的数据结构
typedef pair<double, Node*> QueueElement;
priority_queue<QueueElement, vector<QueueElement>, greater<QueueElement>> openList;

  2. 启发函数选择：

    • 曼哈顿距离：适合网格移动（无对角线）
    • 欧几里得距离：适合自由移动
    • 切比雪夫距离：适合国王移动（八个方向）

  3. 跳点搜索：在均匀网格中跳过中间节点

4.2 变体算法

  1. D*算法：动态A*，适用于环境变化的场景
  2. LPA*算法：终身规划A*，增量式路径规划
  3. Theta*算法：允许任意角度移动的A*变体

五、注意事项

  1. 启发函数选择：

    • 必须满足可采纳性，否则可能找不到最优解
    • h(n)=0时退化为Dijkstra算法
    • h(n)过大时退化为贪心算法

  2. 内存管理：

    • 及时释放节点内存
    • 使用智能指针避免内存泄漏

  3. 性能考虑：

    • 地图过大时考虑分层规划
    • 实时应用中可能需要限制搜索节点数
相关推荐
电饭叔1 小时前
不含Luhn算法《python语言程序设计》2018版--第8章14题利用字符串输入作为一个信用卡号之二（识别卡号有效）
java·python·算法
观音山保我别报错1 小时前
列表,元组,字典
开发语言·python
**蓝桉**1 小时前
数组的执行原理,java程序的执行原理
java·开发语言
2301_800256112 小时前
8.2 空间查询基本组件 核心知识点总结
数据库·人工智能·算法
不穿格子的程序员2 小时前
从零开始写算法——矩阵类题：矩阵置零 + 螺旋矩阵
线性代数·算法·矩阵
waeng_luo2 小时前
[鸿蒙2025领航者闯关] 表单验证与用户输入处理最佳实践
开发语言·前端·鸿蒙·鸿蒙2025领航者闯关·鸿蒙6实战·开发者年度总结
高频交易dragon2 小时前
5分钟和30分钟联立进行缠论信号分析
开发语言·python
ULTRA??2 小时前
C/C++函数指针
c语言·开发语言·c++
还没想好取啥名2 小时前
C++11新特性（一）——自动类型推导
开发语言·c++·stl
热门推荐
01GitHub 镜像站点02React CVE-2025-55182漏洞排查与修复指南03【超详细教程】手把手教你从微软官网免费下载Windows 10官方原版ISO镜像（2025最新版）04安娜的档案(Anna’s Archive) 镜像网站/国内最新可访问入口（持续更新）05BongoCat - 跨平台键盘猫动画工具06UV安装并设置国内源07智能库存管理的需求预测模型：从业务痛点到落地代码的完整实践08本地部署阿里最新开源的Z-Image09Linux下V2Ray安装配置指南10论文阅读 - 深度学习端到端解决库存管理问题 - 有限时间范围内的多周期补货问题（Management Science）