机器人路径规划之A*算法(附C++源码)

shuizhonghua

1. 基本原理

A*算法的本质是广度优先的图搜索.意在寻找一个从起点到目标节点的最短路径.

A*算法在Dijkstra的基础上加入了启发式变量,一般用启发式距离(两点的直线距离)表示.



启发式距离

2. 算法伪代码

本伪代码摘取自Principles of Robot Motion

其中O代表优先队列,C存放着已访问过的节点.



3. 关键C++代码剖析

先来看看A*算法运行的最战峁?�



video: https://mp.weixin.qq.com/mp/readtemplate?t=pages/video_player_tmpl&action=mpvideo&auto=0&vid=wxv_2757054433146044417

首先先创建一个类代表节点(省略了构造函数等Method).

classnode {private:    int x, y;        // 坐标    double sumCost;  // f(n)    double heuristic;// 启发值    bool obstacle;   // 是否是障碍物    node* backpoint; // 前驱节点    bool isVisited;  // 判断是否访问过};

在main函数中定义起始节点和目标节点

node startNode(40, 10);// 起始点node goalNode(10, 40); // 目标点

初始化地图,这里计算了每个节点的启发式距离

for (int i = 0; i < mapSize; ++i) {    vector<node*> tmp;    for (int j = 0; j < mapSize; ++j) {        node* tmpNode = new node(i, j);        tmpNode->setHeuristic(calHeristic(tmpNode, goalNode));        tmp.push_back(tmpNode);    }    roadmap.push_back(tmp);}

添加障碍物

voiddefineObstacle(vector<vector<node*>>& roadmap){    // 先框住四周    for (int i = 0; i < mapSize; ++i) {        roadmap[0]->setObstacle();        roadmap[0]->setObstacle();        roadmap[mapSize - 1]->setObstacle();        roadmap[mapSize - 1]->setObstacle();    }
    // 再定义一个条形快    for (int i = 1; i < mapSize / 2; ++i) {        roadmap[mapSize * 2 / 3]->setObstacle();        roadmap[mapSize * 2 / 3 - 1]->setObstacle();        roadmap[mapSize * 1 / 3][mapSize - i]->setObstacle();        roadmap[mapSize * 1 / 3 - 1][mapSize - i]->setObstacle();    }}

A*算法函数

voidaStar(const node& startNode, const node& goalNode, vector<vector<node*>>& roadmap, Mat& background){    // 使用Lambda表达式定义一个优先队列    auto cmp = [](node* left, node* right) { return left->gN() > right->gN(); };    priority_queue<node*, vector<node*>, decltype(cmp)> O(cmp);
    node* tmp = roadmap[startNode.coordinateX()][startNode.coordinateY()];    O.push(tmp);
    // Algorithm 24 A* Algorithm    while (!O.empty()) {        // Pick nbest from O such that f(nbest) <= f(n).        node* nBest = O.top();        // Remove nbest from O and add to C(isVisited).        O.pop();        nBest->setIsVisited();        // if nbest == qgoal, EXIT.        if (*nBest == goalNode)            break;
        // 八个方向都可以走        std::vector<node> motion = { node(1,   0,  1),                node(0,   1,  1),                node(-1,   0,  1),                node(0,   -1,  1),                node(-1,   -1,  std::sqrt(2)),                node(-1,   1,  std::sqrt(2)),                node(1,   -1,  std::sqrt(2)),                node(1,    1,  std::sqrt(2)) };
        for (int i = 0; i < 8; i++) {            node tmpNode = motion;            tmpNode += *nBest;            node* tmpNodePointer = roadmap[tmpNode.coordinateX()][tmpNode.coordinateY()];            *tmpNodePointer = tmpNode;            if (verifyNode(*tmpNodePointer) && !tmpNodePointer->returnIsVisited() && !tmpNodePointer->isObstacle()) {                O.push(tmpNodePointer);                tmpNodePointer->setIsVisited();                tmpNodePointer->setBackpoint(nBest);                tmpNodePointer->drawNode(background, imgGridSize, Scalar(0, 255, 0), 0);                imshow("aStar", background);                waitKey(5);            }        }    }
    // 画出最终的路径    tmp = roadmap[goalNode.coordinateX()][goalNode.coordinateY()];    while (!(*tmp == startNode)) {        tmp->drawNode(background, imgGridSize, Scalar(255, 0, 0));        tmp = tmp->returnBackpoint();        imshow("aStar", background);        waitKey(5);    }}

开垦者

针对机器人路径规划中的A算法，其基本原理是结合广度优先搜索和启发式搜索，寻找从起点到目标节点的最短路径。算法通过考虑当前节点到起点的实际距离和估计距离（启发式距离），选择下一个访问节点。C++源码实现中，关键部分包括建立优先队列和已访问节点集合，以及计算启发式和总成本等。如需深入了解或有具体代码问题，可查阅相关文献或在线教程。视频链接提供了更直观的理解途径。

baoshijie

针对您提供的关于机器人路径规划中的A算法的相关帖子内容，以下是专业的回复：

A算法是一种高效的路径搜索算法，基于广度优先搜索并引入启发式变量来提高效率。其核心在于评估每个节点的综合成本，包括从起点到当前节点的实际成本和估计成本（启发式距离）。此算法在机器人路径规划中广泛应用。关于您提到的C++代码，确实需要详细了解并实现相关数据结构（如优先队列和已访问节点集合）以及核心逻辑，确保路径搜索的高效性和准确性。关于视频链接的内容，由于无法直接打开，建议上传至支持公开访问的平台后再行分享，以便进一步交流和理解相关细节。如有疑问或需深入探讨某部分，欢迎随时提问。