基于 A 星算法的多无人机与移动机器人协同路径规划

多无人机与移动机器人全覆盖路径规划一直是智能机器人领域的关键技术。

A 星算法的基础

A 星算法是一种在图形平面上，有多个节点的路径，求出最低通过成本的算法。它综合考虑了从起点到当前点的实际代价 g(n)，以及从当前点到终点的预估代价 h(n)，通过公式 f(n) = g(n) + h(n) 来评估每个节点的优先级，优先拓展 f(n) 值最小的节点。

简单代码示例（以 C++ 为例）

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

// 定义节点结构体
struct Node {
    int x;
    int y;
    double g;
    double h;
    double f;
    Node* parent;
    Node(int _x, int _y) : x(_x), y(_y), g(0), h(0), f(0), parent(nullptr) {}
};

// 计算启发式函数 h(n)
double heuristic(Node* current, Node* goal) {
    return std::sqrt(std::pow(current->x - goal->x, 2) + std::pow(current->y - goal->y, 2));
}

// A 星算法核心部分
std::vector<Node*>  {
    std::priority_queue<Node*, std::vector<Node*>, [](Node* a, Node* b) {  a->f > b->f; }> openList;
    std::vector<std::vector<>> (map.(), std::<>(map[].(), ));
    openList.(start);
     (!openList.()) {
        Node* current = openList.();
        openList.();
         (current->x == goal->x && current->y == goal->y) {
            std::vector<Node*> path;
             (current != ) {
                path.(current);
                current = current->parent;
            }
             path;
        }
        closedList[current->x][current->y] = ;
        
         ( i = ; i <= ; ++i) {
             ( j = ; j <= ; ++j) {
                 (i ==  && j == ) ;
                 newX = current->x + i;
                 newY = current->y + j;
                 (newX >=  && newX < map.() && newY >=  && newY < map[].() && !map[newX][newY] && !closedList[newX][newY]) {
                    Node* neighbor =  (newX, newY);
                    neighbor->g = current->g + ;
                    neighbor->h = (neighbor, goal);
                    neighbor->f = neighbor->g + neighbor->h;
                    neighbor->parent = current;
                    openList.(neighbor);
                }
            }
        }
    }
     std::<Node*>();
}