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

1
2
3
4
5
6
7
8
9
class node {
private:
    int x, y;        // 坐标
    double sumCost;  // f(n)
    double heuristic;// 启发值
    bool obstacle;   // 是否是障碍物
    node* backpoint; // 前驱节点
    bool isVisited;  // 判断是否访问过
};

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

1
2
3
4
5
6
7
8
9
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);
}

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
void defineObstacle(vector<vector<node*>>& roadmap) {
    // 先框住四周
    for (int i = 0; i < mapSize; ++i) {
        roadmap[0][i]->setObstacle();
        roadmap[i][0]->setObstacle();
        roadmap[i][mapSize - 1]->setObstacle();
        roadmap[mapSize - 1][i]->setObstacle();
    }


    // 再定义一个条形快
    for (int i = 1; i < mapSize / 2; ++i) {
        roadmap[mapSize * 2 / 3][i]->setObstacle();
        roadmap[mapSize * 2 / 3 - 1][i]->setObstacle();
        roadmap[mapSize * 1 / 3][mapSize - i]->setObstacle();
        roadmap[mapSize * 1 / 3 - 1][mapSize - i]->setObstacle();
    }
}

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
void aStar(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[i];
            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);
    }
}