A*算法原理及C++实现_c++ a*

A*算法原理

全局路径规划算法，根据给定的起点和终点在全局地图上进行总体路径规划。

导航中使用A*算法计算出机器人到目标位置的最优路线，一般作为规划的参考路线

// 定义地图上的点
struct Point
{
    int x,y; // 栅格行列
    Point(int x, int y):x(x),y(y){}; // 参数列表初始化
    double distance(Point& p)        // 求距离
    {
        return sqrt((x-p.x)*(x-p.x)+(y-p.y)*(y-p.y)); // 欧几里得距离
    }
};
1
2
3
4
5
6
7
8
9
10

// 定义节点
struct Node
{
    Point point; // 栅格点
    double g,h,f;// 代价值,f总价值,g到起点的代价值，h到终点的估计代价（启发式函数）
    Node *parent;// 父节点指针
    Node(Point point, double g, double h, Node* parent = nullptr):point(point), g(g), h(h), f(g+h), parent(parent)
    {}
};
1
2
3
4
5
6
7
8
9

// 定义地图
 vector<vector<int>> gridmap = {
         {0, 1, 0, 0, 0},
         {0, 0, 1, 0, 0},
         {0, 0, 1, 1, 0},
         {0, 0, 1, 0, 0},
         {0, 0, 1, 1, 0}
 };
 // 定义起点和终点
 Point start{0, 0};
 Point goal{4, 4};
1
2
3
4
5
6
7
8
9
10
11

A*算法的寻路原理

A的结束条件

A算法的寻路详细步骤

手撕A*代码

#include <iostream>
#include <vector>
#include <algorithm>
#include <cmath>
using namespace std;
// 定义地图上的点
struct Point
{
    int x,y; // 栅格行列
    Point(int x, int y):x(x),y(y){}; // 参数列表初始化
    double distance(Point& p)        // 求距离
    {
        return sqrt((x-p.x)*(x-p.x)+(y-p.y)*(y-p.y)); // 欧几里得距离
    }
};


// 定义节点
struct Node
{
    Point point; // 栅格点
    double g,h,f;// 代价值,f总价值,g到起点的代价值，h到终点的估计代价（启发式函数）
    Node *parent;// 父节点指针
    Node(Point point, double g, double h, Node* parent = nullptr):point(point), g(g), h(h), f(g+h), parent(parent)
    {}
};

// 自定义Node*排序规则
struct NodeCompare{
    bool operator()(Node* n1, Node* n2){
        return (n1->f) < (n2->f); // 表示升序排列
    }
};
// 基于栅格地图的路径规划A*算法,返回由Point组成的路径path,输入为地图，起点和终点
vector<Point> AstarPathPlanning(vector<vector<int>> &gridmap, Point& start, Point& goal)
{
    // 获取地图参数
    int row = gridmap.size(); // 行，表示地图的宽度
    int col = gridmap[0].size(); // 列，表示地图的长
    // 定义openlist, closelist
    vector<Node *> openlist; // openlist 表示待搜索的节点
    vector<Node *> closelist;// closelist表示已搜索的节点
    openlist.push_back(new Node(start, start.distance(start), start.distance(goal))); // 将起点加入openlist中，作为初始化
    int count1 = 1;
    // 进入循环，开始搜索,搜索到终点则返回路径
    vector<Point> path;
    while (!openlist.empty()) // 当openlist为空，表示所有可搜索节点已经被搜索，此时循环结束
    {
        // 获取当前搜索节点current,即openlist中f最小节点
        sort(openlist.begin(), openlist.end(), NodeCompare{}); // 先对openlist排序，这里自定义排序规则(从小到大)
        Node* current = *openlist.begin(); // *openlist.begin()排序后即为f最小的迭代器位置
        // 将current对应的元素从openlist中删除
        openlist.erase(openlist.begin());
        // 将current加入到closelist中
        closelist.push_back(current);
        // 对当前搜索节点current进行分类讨论
        // 1-current是终点，则返回路径，表示找到路径
        if (current->point.x == goal.x && current->point.y == goal.y)
        {
            while (current != nullptr) // 利用父节点，从终点向起点回溯最短路径,因为起点没有父节点，所以起点current父节点为nullptr
            {
                path.push_back(current->point);
                current = current->parent;
            }
            reverse(path.begin(), path.end()); // 路径是反的，翻转路径
            int count2 = 0; // delete 次数
            for (auto o : openlist)
            {
                delete o;
                count2++;
            }
            for (auto c : closelist)
            {
                delete c;
                count2++;
            }
            cout << "new times: " << count1 << endl;
            cout << "delete times: " << count2 << endl;
            return path;
        }
        // 2-current 不是终点，需要讨论其邻近的节点neighbors
        int x = current->point.x;
        int y = current->point.y;
        vector<Point> neighbors = { // 8个邻近节点的坐标
                {x-1,y-1}, {x-1,y}, {x-1,y+1},
                {x,y-1},     {x,y+1},
                {x+1,y-1}, {x+1,y}, {x+1,y+1}
        };
        // 遍历所有的临近节点,每一个邻近节点n必须满足在地图范围内同时不是障碍物
        for (auto n : neighbors)
        {
            if ((n.x >= 0 && n.x < row) && (n.y >= 0 && n.y < col) && gridmap[n.x][n.y]==0)
            {
                // 1 n在closelist中，表示已经搜索过了，此时直接跳过
                bool incloselist = false;
                for (auto c : closelist)
                {
                    if (c->point.x == n.x && c->point.y == y)
                    {
                        incloselist = true;
                        break;
                    }
                }
                if (incloselist)
                {
                    continue;
                }
                // 2 n是否在openlist中进行讨论
                bool inopenlist = false;
                for (auto o : openlist)
                {
                    if (o->point.x == n.x && o->point.y == n.y)
                    {
                        inopenlist = true;
                        // n 在openlist中，对比f值，更新代价值和父节点parent
                        double g = current->g + n.distance(current->point); // 临近节点n到起点的距离 = 当前搜索节点current到起点的距离 + 当前搜索节点current到邻近节点n距离
                        double h = n.distance(goal); // 临近节点n到终点的估计距离代价
                        double f = g + h;
                        if (f < (o->f))
                        {
                            o->f = f;
                            o->parent = current;
                        }
                        break;
                    }
                }
                if (!inopenlist) // n不在openlist中，对比f值，计算代价值，添加到openlist中，下次备选
                {
                    double g = current->g + n.distance(current->point); // 临近节点n到起点的距离 = 当前搜索节点current到起点的距离 + 当前搜索节点current到邻近节点n距离
                    double h = n.distance(goal); // 临近节点n到终点的估计距离代价
                    double f = g + h;
                    openlist.push_back(new Node(n,g,h,current));
                    count1++;
                }
            }
        }

    }


    // 搜索完成没有路径，表示路径规划失败,此时返回空路径
    return path;
}
int main()
{
    // 定义地图
    vector<vector<int>> gridmap = {
            {0, 1, 0, 0, 0},
            {0, 0, 1, 0, 0},
            {0, 0, 1, 1, 0},
            {0, 0, 1, 0, 0},
            {0, 0, 1, 1, 0}
    };
    // 定义起点和终点
    Point start{0, 0};
    Point goal{4, 4};
    vector<Point> path = AstarPathPlanning(gridmap, start, goal);
    cout << path.size() << endl;
    for (auto p : path)
    {
        if (p.x == goal.x && p.y == goal.y)
        {
            cout << "(" << p.x << ',' << p.y << ")" << endl;
        }
        else
        {
            cout << "(" << p.x << ',' << p.y << ")" << "->";
        }
    }
    return 0;
}
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
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171

A*算法总结

把起点加入 open list 。
重复如下过程：
a. 遍历 open list ，查找 F 值最小的节点，把它作为当前要处理的节点。
b. 把这个节点移到 close list 。
c. 对当前方格的 8 个相邻方格的每一个方格？
◆ 如果它是不可抵达的或者它在 close list 中，忽略它。否则，做如下操作。
◆ 如果它不在 open list 中，把它加入 open list ，并且把当前方格设置为它的父亲，记录该方格的 F ， G 和 H 值。
◆ 如果它已经在 open list 中，检查这条路径 ( 即经由当前方格到达它那里 ) 是否更好，用 G 值作参考。更小的 G 值表示这是更好的路径。如果是这样，把它的父亲设置为当前方格，并重新计算它的 G 和 F 值。如果你的 open list 是按 F 值排序的话，改变后你可能需要重新排序。
d. 停止，当你
◆ 把终点加入到了 open list 中，此时路径已经找到了，或者
◆ 查找终点失败，并且 open list 是空的，此时没有路径。
3.保存路径。从终点开始，每个方格沿着父节点移动直至起点，这就是你的路径。