手撕A*算法（详解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)); // 欧几里得距离}
};

// 定义节点
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){}
};

// 定义地图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};

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, closelistvector<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 不是终点，需要讨论其邻近的节点neighborsint 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值，更新代价值和父节点parentdouble 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;
}

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.保存路径。从终点开始，每个方格沿着父节点移动直至起点，这就是你的路径。