学习路径规划day2

经典算法：A*

1.A*算法概述

用途：A算法广泛应用于路径规划和图遍历的启发式搜索算法。

特点：

• 结合了Dijkstra算法(保证最优性)和贪心最佳优先搜索(保证高效性)的有点
• 引入启发式函数来指导搜索方向，提高搜索效率

典型应用：

        机器人导航、地图路径规划、游戏AI

2.A*算法核心思想

        A*算法的核心是代价函数f(n)的构建：
                                f(n) = g(n) + h(n)

• g(n): 从起点到当前节点的n的实际代价（已知）
• h(n): 当前节点n到目标节点的预估及代价(启发式函数，需满足可采纳性，即不高估实际代价)

关键要求：

• 启发函数h(n)必须为可采纳（例如，曼哈顿距离，欧几里得距离）
• 若h(n)满足一致性，则A*无需重复处理节点，效率更高

3.A*算法实现步骤

1.初始化：

        初始化open_list和close_list

        将起点加入开列表open_list，设置g(start) = 0; f(start) = h(start)

2.循环搜索：

        从open_list中取出f(n)最小的节点n;

        若n为目标节点，回溯路径并结束;

        否则，将n移动到close_list，并遍历其邻居节点m

                若m在close_list中，则跳过;

                若m在open_list中，

                        则计算起点经n到m的g值，若此值g小于原来起点经m的原父节点到m的g值，更新m的父节点为m，重新计算m的移动代价f

                        否则，跳过;

                若邻居节点m不在open_list也不在close_list中，则：

                        设置节点m的parent为节点n；

                        计算节点m的移动代价f=g+h;

                        将节点m加入open_list中；

   

3.终止条件：

        如果open_list为空且未找到目标节点，则路径不存在

   

4.启发式函数h(n)

        常见启发式函数

                曼哈顿距离：适用于网格移动 ----- 只能上下左右

                        h(n) = |x₁ - x₂| + |y₁ - y₂|

                欧几里得距离：适用于自由移动

                        h(n) = √[(x₁ - x₂)² + (y₁ - y₂)²]

                对角线距离：适用于八方向距离

                        h(n) = max(|x₁ - x₂|, |y₁ - y₂|)

5.A*算法代码实现

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

using namespace std;

// 定义节点结构体
struct Node {
    int x, y;       // 坐标
    double g, h, f; // g: 实际代价, h: 启发式代价, f: g + h
    Node* parent;   // 父节点指针

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

    // 重载==运算符，用于unordered_set比较
    bool operator==(const Node& other) const {
        return x == other.x && y == other.y;
    }
};

// 哈希函数，用于unordered_map和unordered_set
struct NodeHash {
    size_t operator()(const Node* node) const {
        return hash<int>()(node->x) ^ hash<int>()(node->y);
    }
};

// 比较函数，用于优先队列（按f值升序）
struct NodeCompare {
    bool operator()(const Node* a, const Node* b) const {
        return a->f > b->f;
    }
};

// 计算曼哈顿距离（启发式函数h）
double manhattanDistance(const Node* a, const Node* b) {
    return abs(a->x - b->x) + abs(a->y - b->y);
}

// A*算法实现
vector<Node*> aStar(Node* start, Node* goal, const vector<vector<int>>& grid) {
    // Open List: 优先队列（按f值最小排序）
    priority_queue<Node*, vector<Node*>, NodeCompare> openList;
    // Close List: 哈希集合
    unordered_set<Node*, NodeHash> closeList;
    // 所有节点的映射（避免重复创建）
    unordered_map<int, unordered_map<int, Node*>> nodeMap;

    // 初始化起点
    start->g = 0;
    start->h = manhattanDistance(start, goal);
    start->f = start->g + start->h;
    openList.push(start);
    nodeMap[start->x][start->y] = start;

    // 定义四个方向的移动（上、右、下、左）
    const int dx[] = {-1, 0, 1, 0};
    const int dy[] = {0, 1, 0, -1};

    while (!openList.empty()) {
        // 取出f值最小的节点
        Node* current = openList.top();
        openList.pop();

        // 找到目标节点，回溯路径
        if (current->x == goal->x && current->y == goal->y) {
            vector<Node*> path;
            while (current != nullptr) {
                path.push_back(current);
                current = current->parent;
            }
            reverse(path.begin(), path.end());
            return path;
        }

        // 将当前节点加入Close List
        closeList.insert(current);

        // 遍历四个方向的邻居
        for (int i = 0; i < 4; ++i) {
            int nx = current->x + dx[i];
            int ny = current->y + dy[i];

            // 检查边界和障碍物（假设grid中1为障碍物）
            if (nx < 0 || nx >= grid.size() || ny < 0 || ny >= grid[0].size() || grid[nx][ny] == 1) {
                continue;
            }

            // 获取或创建邻居节点
            Node* neighbor = nullptr;
            if (nodeMap.count(nx) && nodeMap[nx].count(ny)) {
                neighbor = nodeMap[nx][ny];
            } else {
                neighbor = new Node(nx, ny);
                nodeMap[nx][ny] = neighbor;
            }

            // 跳过Close List中的节点
            if (closeList.find(neighbor) != closeList.end()) {
                continue;
            }

            // 计算从当前节点到邻居的临时g值（假设每一步代价为1）
            double tentativeG = current->g + 1;

            // 检查是否在Open List中
            bool inOpenList = neighbor->parent != nullptr; // 简化判断

            if (!inOpenList || tentativeG < neighbor->g) {
                // 更新父节点和代价
                neighbor->parent = current;
                neighbor->g = tentativeG;
                neighbor->h = manhattanDistance(neighbor, goal);
                neighbor->f = neighbor->g + neighbor->h;

                // 如果不在Open List中，则加入
                if (!inOpenList) {
                    openList.push(neighbor);
                }
            }
        }
    }

    return {}; // 无路径
}

// 打印路径
void printPath(const vector<Node*>& path) {
    for (const Node* node : path) {
        cout << "(" << node->x << ", " << node->y << ") ";
    }
    cout << endl;
}

int main() {
    // 示例网格（0可通行，1为障碍物）
    vector<vector<int>> grid = {
        {0, 0, 0, 0, 0},
        {0, 1, 1, 0, 0},
        {0, 0, 0, 0, 0},
        {0, 0, 1, 1, 0},
        {0, 0, 0, 0, 0}
    };

    // 创建起点和终点
    Node* start = new Node(0, 0);
    Node* goal = new Node(4, 4);

    // 运行A*算法
    vector<Node*> path = aStar(start, goal, grid);

    // 输出结果
    if (!path.empty()) {
        cout << "Path found:" << endl;
        printPath(path);
    } else {
        cout << "No path exists!" << endl;
    }

    // 释放内存（实际应用中需更复杂的资源管理）
    delete start;
    delete goal;
    return 0;
}

6.网格地图寻路案例

考虑一个5x5网格，S为起点，G为目标，X为障碍物：

S . . X .
. X . . .
. . X . .
. X . . .
. . . X G

1. 初始化：将S(0,0)加入开放列表，g(S)=0，h(S)=8（假设用曼哈顿距离）

2. 扩展S：检查相邻节点(0,1)和(1,0)

3. 选择f值较小的节点继续扩展

4. 重复直到找到G或开放列表为空

7.时间复杂度

• 最坏情况：O()O()，其中 b是分支因子，d 是解路径深度（与Dijkstra相同）。

• 优化情况：好的启发函数 h(n)h(n) 能显著减少搜索节点数，接近 O(d)O(d)。

• 空间复杂度：O()（需存储Open_list和Closed_list）。

关键点：A的效率高度依赖启发函数 h(n)的质量。若 h(n)=0，A退化为Dijkstra算法；若 h(n) 精确等于实际代价，则A*直接找到最优路径。