机器人路径规划之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);    }}