a*算法进行路径规划

canyuemanyue

已于 2024-07-25 10:22:24 修改

阅读量107

点赞数 2

文章标签：算法 c++ 开发语言

于 2024-07-24 18:17:07 首次发布

本文链接：https://blog.csdn.net/canyuemanyue/article/details/140670083

版权

一、节点信息建立

struct Node {
    int x, y;
    double g, h, f;
    Node* parent;
    int index; // 唯一的索引

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

    bool operator<(const Node& other) const {
        return f < other.f;
    }
};

x,y分别为坐标点，其中index为该节点的唯一标识。

二、曼哈顿距离计算

double heuristic(const Node& a, const Node& b) {
    return std::abs(a.x - b.x) + std::abs(a.y - b.y);
}

三、获取临近节点

std::vector<Node*> getNeighbors(Node* current, const std::vector<std::vector<char>>& grid) {
    std::vector<Node*> neighbors;
    int dx[] = { -1, 1, 0, 0 };
    int dy[] = { 0, 0, -1, 1 };

    for (int i = 0; i < 4; ++i) {
        int nx = current->x + dx[i];
        int ny = current->y + dy[i];
        // 边界判断、障碍物判断
        if (nx >= 0 && nx < grid.size() && ny >= 0 && ny < grid[0].size() && grid[nx][ny] == '0') {
            Node* neighbor = new Node(nx, ny);
            neighbor->index = id++;
            neighbor->g = 1;
            neighbors.push_back(neighbor);
        }
    }
    return neighbors;
}

四、a*算法处理

Node* aStar(Node* start, Node* end, const std::vector<std::vector<char>>& grid) {
    std::priority_queue<Node*, std::vector<Node*>, Compare> openList;  // compare函数自定义排序从大到小排序
    std::unordered_set<int> openSet;
    std::unordered_map<int, Node*> closedMap;

    start->g = 0;
    start->h = heuristic(*start, *end);
    start->f = start->g + start->h;
    start->index = 0;
    openList.push(start);
    openSet.insert(start->index);

    while (!openList.empty()) {
        Node* current = openList.top();
        openList.pop();
        //openSet.erase(current->index);

        if (current->x == end->x && current->y == end->y) {
            return current;
        }

        closedMap[current->index] = current;
        std::vector<Node*> neighbors = getNeighbors(current, grid);
        for (Node* neighbor : neighbors) {
            int neighborIndex = neighbor->index;
            if (closedMap.find(neighborIndex) != closedMap.end()) continue;
            neighbor->g = neighbor->g + 1;
            neighbor->h = heuristic(*neighbor, *end);
            neighbor->f = neighbor->g + neighbor->h;
            if (openSet.find(neighborIndex) == openSet.end()  ) {
                neighbor->parent = current;
                openList.push(neighbor);
                openSet.insert(neighborIndex);
            }
        }
    }
    return nullptr;
}

五、主函数调用

int main() {
    const int GRID_SIZE = 10;
    // 初始化网格
    std::vector<std::vector<char>> grid(GRID_SIZE, std::vector<char>(GRID_SIZE, '0'));
    //grid[1][1] = '1';
    //grid[2][2] = '1';
    //grid[3][3] = '1';
    //grid[1][7] = '1';
    //grid[2][3] = '1';
    //grid[3][7] = '1';
    // 打印网格
    for (int i = 0; i < GRID_SIZE; ++i) {
        for (int j = 0; j < GRID_SIZE; ++j) {
            std::cout << grid[i][j] << " ";
        }
        std::cout << std::endl;
    }


    Node* start = new Node(0, 0);
    Node* end = new Node(6, 3);

    Node* path = aStar(start, end, grid);

    if (path) {
        Node* step = path;
        std::cout << "Path from (" << start->x << ", " << start->y << ") to (" << end->x << ", " << end->y << "):\n";
        while (step) {
            std::cout << "(" << step->x << ", " << step->y << ") -> ";
            step = step->parent;
        }
        std::cout << "(end)" << std::endl;
    }
    else {
        std::cout << "No path found." << std::endl;
    }

    // 清理内存
    delete start;
    delete end;
    return 0;
}

六、测试函数

#include <iostream>
#include <vector>
#include <queue>
#include <cmath>
#include <unordered_set>
#include <unordered_map>
#include <functional>
using namespace std;
static int id = 1; 

struct Node {
    int x, y;
    double g, h, f;
    Node* parent;
    int index; // 唯一的索引

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

    bool operator<(const Node& other) const {
        return f < other.f;
    }
};

struct Compare {
    bool operator()(Node* a, Node* b) {
        return a->f > b->f;
    }
};

double heuristic(const Node& a, const Node& b) {
    return std::abs(a.x - b.x) + std::abs(a.y - b.y);
}

double distance(const Node& a, const Node& b) {
    return std::abs(a.x - b.x) + std::abs(a.y - b.y);
}

std::vector<Node*> getNeighbors(Node* current, const std::vector<std::vector<char>>& grid) {
    std::vector<Node*> neighbors;
    int dx[] = { -1, 1, 0, 0 };
    int dy[] = { 0, 0, -1, 1 };

    for (int i = 0; i < 4; ++i) {
        int nx = current->x + dx[i];
        int ny = current->y + dy[i];
        // 边界判断、障碍物判断
        if (nx >= 0 && nx < grid.size() && ny >= 0 && ny < grid[0].size() && grid[nx][ny] == '0') {
            Node* neighbor = new Node(nx, ny);
            neighbor->index = id++;
            neighbor->g = 1;
            neighbors.push_back(neighbor);
        }
    }
    return neighbors;
}

Node* aStar(Node* start, Node* end, const std::vector<std::vector<char>>& grid) {
    std::priority_queue<Node*, std::vector<Node*>, Compare> openList;  // compare函数自定义排序从大到小排序
    std::unordered_set<int> openSet;
    std::unordered_map<int, Node*> closedMap;

    start->g = 0;
    start->h = heuristic(*start, *end);
    start->f = start->g + start->h;
    start->index = 0;
    openList.push(start);
    openSet.insert(start->index);

    while (!openList.empty()) {
        Node* current = openList.top();
        openList.pop();
        //openSet.erase(current->index);

        if (current->x == end->x && current->y == end->y) {
            return current;
        }

        closedMap[current->index] = current;
        std::vector<Node*> neighbors = getNeighbors(current, grid);
        for (Node* neighbor : neighbors) {
            int neighborIndex = neighbor->index;
            if (closedMap.find(neighborIndex) != closedMap.end()) continue;
            neighbor->g = neighbor->g + 1;
            neighbor->h = heuristic(*neighbor, *end);
            neighbor->f = neighbor->g + neighbor->h;
            if (openSet.find(neighborIndex) == openSet.end() ) {
                neighbor->parent = current;
                openList.push(neighbor);
                openSet.insert(neighborIndex);
            }
        }
    }
    return nullptr;
}


int main() {
    const int GRID_SIZE = 10;
    // 初始化网格
    std::vector<std::vector<char>> grid(GRID_SIZE, std::vector<char>(GRID_SIZE, '0'));
    //grid[1][1] = '1';
    //grid[2][2] = '1';
    //grid[3][3] = '1';
    //grid[1][7] = '1';
    //grid[2][3] = '1';
    //grid[3][7] = '1';
    // 打印网格
    for (int i = 0; i < GRID_SIZE; ++i) {
        for (int j = 0; j < GRID_SIZE; ++j) {
            std::cout << grid[i][j] << " ";
        }
        std::cout << std::endl;
    }


    Node* start = new Node(0, 0);
    Node* end = new Node(6, 3);

    Node* path = aStar(start, end, grid);

    if (path) {
        Node* step = path;
        std::cout << "Path from (" << start->x << ", " << start->y << ") to (" << end->x << ", " << end->y << "):\n";
        while (step) {
            std::cout << "(" << step->x << ", " << step->y << ") -> ";
            step = step->parent;
        }
        std::cout << "(end)" << std::endl;
    }
    else {
        std::cout << "No path found." << std::endl;
    }

    // 清理内存
    delete start;
    delete end;
    return 0;
}