无人机路径规划算法代码C++

在C++中，无人机路径规划算法可以基于多种原理编写，例如A*搜索算法、Dijkstra算法或者RRT（快速随机树）算法。这里是一个简单的A*搜索算法示例，适用于二维平面上的目标点到达问题：

#include <iostream>
#include <vector>
#include <queue>

const int INF = INT_MAX;
struct Node {
    int x, y;
    int cost;
    bool operator<(const Node& other) const { return cost < other.cost; }
};

std::vector<std::vector<int>> grid; // 二维网格地图

// 节点状态转移函数
int heuristic(Node a, Node b) {
    return abs(a.x - b.x) + abs(a.y - b.y);
}

// A*搜索算法
Node* astar(int start_x, int start_y, int goal_x, int goal_y) {
    std::priority_queue<Node*, std::vector<Node*>, Node> open_set;
    Node start_node{start_x, start_y, 0};
    Node* closed_set[grid.size()][grid[0].size()]; // 状态已访问标记

    open_set.push(&start_node);

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

        if (current->x == goal_x && current->y == goal_y)
            return current;

        if (closed_set[current->x][current->y])
            continue;

        closed_set[current->x][current->y] = true;

        for (int dx = -1; dx <= 1; ++dx) {
            for (int dy = -1; dy <= 1; ++dy) {
                if (dx == 0 && dy == 0) continue; // 防止走自身
                int nx = current->x + dx;
                int ny = current->y + dy;
                if (nx < 0 || ny < 0 || nx >= grid.size() || ny >= grid[0].size())
                    continue;

                Node* neighbor = &grid[nx][ny];
                if (neighbor->cost == INF)
                    neighbor->cost = current->cost + 1;

                neighbor->parent = current;
                neighbor->heuristic = heuristic(neighbor, {goal_x, goal_y});
                neighbor->total_cost = neighbor->cost + neighbor->heuristic;

                if (open_set.empty() || neighbor->total_cost < open_set.top()->total_cost)
                    open_set.push(neighbor);
            }
        }
    }

    return nullptr; // 如果无法到达目标，则返回nullptr
}