1.算法概述
A*算法也叫做A星(A star)算法，A*算法是之前提过的Dijkstra最短路径的一个扩展和改进，大体思路是通过一个评估方法来估计一个节点到终点的估计值，并不断通过选择拥有最优估计值的节点，最终到达终点，这个过程也称作启发式寻找，当然，因为节点上可能会放置障碍物等无法通过的节点，所以这个值不一定够准确，所以才称这个值为估计值。
与Dijkstra最短路最大的区别是，Dijkstra最短路会计算一个起点到所有节点的最短路径，这就意味着Dijkstra算法将遍历所有节点，而当我们只想知道一个起点到一个终点的最短路径时，Dijkstra算法虽然能找到，但也遍历了所有节点。而A*算法通过估计值进行启发式寻找只会遍历较少的节点就找到最短路径，所以A*更适合寻找一个点到一个点的最短路径。
![[算法笔记]A*寻路算法_第1张图片](http://img.e-com-net.com/image/info10/c9582ac74ec642279c46ef7cca0dc0c6.gif)
![[算法笔记]A*寻路算法_第2张图片](http://img.e-com-net.com/image/info10/ed9667b3e1e6425faafa0e448dc37b12.gif)
从上图可以很明显的看出来二者的差异，Dijkstra算法因为是求起点到所有节点的最短距离，因为包含起点到所有节点的最短距离，所以我们也可以知道我们想知道的起点到任何一点的最短距离，但是也遍历了大部分的节点。而A*算法只会通过估计值不断寻找拥有最优估计值的节点来靠近终点，所以只遍历了较少的节点，同时也找到了最短路径。
2.算法工作原理
A*算法的前面的步骤与Dijkstra算法类似，通过给定一个起点，然后获取起点附近的节点，先计算当前所在节点到附近每个节点的距离，我们称这个距离为g，然后在计算附近每个节点到终点的估计值(可以理解为表示到终点的大概距离)，我们称这个值为h，最后附近每个节点到终点的评估值用h+g得到，我们称这个值为f，最终我们可以得到评估方法的计算方式为:f(n) = g(n) + h(n)，评估值的值有很多种方式可以得到，后面会介绍几个常用的。
当得到附近每个节点到终点的评估值后，我们从目前已经得到的节点里面，根据评估值找到一个最优最靠近终点的节点，然后将这个节点设为当前节点，并把之前的当前节点设为已关闭，之后只要遇到该节点则跳过(包括从获取附近节点的方式遇到)，不断重复前面的过程，直到找到终点为止。
3.伪码实现
// A* Search Algorithm
1. Initialize the open list //初始化open列表，表示当前找到的最优节点集合(不断的遍历过程中该列表会变化)
2. Initialize the closed list //初始化closed列表，表示以及遍历并处理过的节点，后面不会在考虑
put the starting node on the open //将起点初始化放入到open列表中作为初始点开始后面的流程
list (you can leave its f at zero)
3. while the open list is not empty //不断遍历open列表中的节点，直到找到终点
a) find the node with the least f on //从当前open列表中找到评估值最优的一共节点，称之位q
the open list, call it "q"
b) pop q off the open list //将q节点从open列表中移除
c) generate q's 4 successors and set their //我们的网格图表只有4个临近点(上、下、左、右)，获得当前q节点附近的4个节点
parents to q
//处理找到的每个附近节点
d) for each successor
i) if successor is the goal, stop search //如果该节点是终点，则停止查找
successor.g = q.g + distance between //计算当前节点q到附近节点successor的距离g
successor and q
successor.h = distance from goal to //计算附近节点successor到终点goal的估计值h
successor (This can be done using many
ways, we will discuss three heuristics-
Manhattan, Diagonal and Euclidean
Heuristics)
successor.f = successor.g + successor.h //计算附近节点successor的评估值f，f=g+h
/*
* 如果有一个节点和这个附近节点(successor )拥有相同位置并存在于open列表中，
* 而且有更低的评估值f，则跳过这个附近节点的处理。
*/
ii) if a node with the same position as
successor is in the OPEN list which has a
lower f than successor, skip this successor
/*
* 如果有一个节点和这个附近节点(successor )拥有相同位置并存在于closed列表中，
* 而且有更低的评估值f，则跳过这个附近节点的处理。除此之外，都添加到open列表中
*/
iii) if a node with the same position as
successor is in the CLOSED list which has
a lower f than successor, skip this successor
otherwise, add the node to the open list
end (for loop)
e) push q on the closed list
end (while loop)
4.C++代码实现
我们首先定义一个类表示节点:
class GraphNode {
public:
GraphNode(int row_index, int col_index) :
is_obstacle_(false),
is_closed_(false),
row_index_(row_index),
col_index_(col_index),
f(UINT_MAX),
g(UINT_MAX),
h(UINT_MAX),
display_mark_("-"),
pervNode(nullptr)
{
}
GraphNode() :
is_obstacle_(false),
is_closed_(false),
row_index_(0),
col_index_(0),
f(UINT_MAX),
g(UINT_MAX),
h(UINT_MAX),
display_mark_("-"),
pervNode(nullptr)
{
}
bool is_obstacle_; //表示该节点是否是障碍物
bool is_closed_; //表示该节点是否已经访问并不再考虑了
int row_index_; //节点在二维数组中的行索引
int col_index_; //节点在二维数组中的列索引
double f; //当前节点到终点的评估值(g + h)
double g; //目前为止到该节点所需的距离
double h; //当前节点到终点的估计值(估计距离)
string display_mark_; //当前节点在打印时显示的字符
GraphNode *pervNode; //通过哪个节点到达该节点(最短路径)
bool operator < (const GraphNode &right) const {
return this->f < right.f;
}
};
最后我们还重载了操作符<，这是因为我们将要遍历的节点都放在一个set中，而自定义类型则需要重载该运算符来让set排序以及去重要放进来的节点。
最后我们需要用一个类来表示一个可自定义大小的表格(网格图)，该类原型如下:
class Graph {
public:
//默认构造函数
Graph() ;
//初始化指定长宽的网格
Graph(int rows, int cols) ;
//析构函数，示范动态分配的网格内存
~Graph();
//打印该网格
void Print();
//根据节点指定的行，列防止该节点到指定位置上
void PutNode(GraphNode *graphNode);
//放置一个节点到指定位置上
void PutNode(GraphNode *graphNode, int row_index, int col_index);
//计算currentNode节点到dstNode节点的估计值(估计距离)
double estimatedDistance(GraphNode *currentNode, GraphNode *dstNode);
//A*寻路算法主实现
vector *FindPath(GraphNode *srcNode, GraphNode *dstNode);
//获取给定一个节点附近的节点列表
vector *GetNeighborsByNode(GraphNode *node);
//按照给定的行，列索引获取指定位置上的节点对象
GraphNode *FindByIndex(int row_index, int col_index);
//检查要进行操作的行，列索引示范合法(越界等情况...)
bool CheckIndexIsValid(int row_index, int col_index);
//获取该表格一共有多少行
int GetRows();
//获取该表格一共有多少列
int GetCols();
private:
int rows_; //该表格一共有多少行
int cols_; //该表格一共有多少列
GraphNode **graph_; //一次性分配的表格二维数组，每个元素都指向一个动态分配的Node对象指针
//一共指定行，列索引位置的二维数组元素值
void SetLocation(GraphNode *graphNode, int row_index, int col_index);
//获取二维数组指定位置上的元素
GraphNode **GetLoaction(int row_index, int col_index);
因为我们的的表对象可以支持各个大小的网格图，所以是一次性动态分配的内存，这一块代码理解起来会比较绕，但属于指针操作的基础知识，可以暂时先不理解通过指针操作二维数组的这块代码，只用知道该类提供了PutNode、FindByIndex成员方法来操作该二维数组即可。
首先我们先实现获取给定节点附近节点列表的方法，该成员方法实现如下:
vector *GetNeighborsByNode(GraphNode *node)
{
vector filter;
vector *result = new vector;
//左
if (node->col_index_ > 0) {
GraphNode *leftNode = this->FindByIndex(node->row_index_, node->col_index_ - 1);
filter.push_back(leftNode);
}
//右
if (node->col_index_ < cols_ - 1) {
GraphNode *rightNode = this->FindByIndex(node->row_index_, node->col_index_ + 1);
filter.push_back(rightNode);
}
//上
if (node->row_index_ > 0) {
GraphNode *topNode = this->FindByIndex(node->row_index_ - 1, node->col_index_);
filter.push_back(topNode);
}
//下
if (node->row_index_ < rows_ - 1) {
GraphNode *bottomNode = this->FindByIndex(node->row_index_ + 1, node->col_index_);
filter.push_back(bottomNode);
}
for (auto iter = filter.begin(); iter != filter.end(); iter++)
{
GraphNode *iterNode = *iter;
if (nullptr != iterNode &&
!iterNode->is_closed_ &&
!iterNode->is_obstacle_) {
result->push_back(iterNode);
}
}
return result;
}
和我们之前实现的Dijkstra算法获取一个节点附近的节点一样，只不过最后我们只返回有效的附近节点，排除了已经关闭的节点以及障碍物节点。
我们还需要实现一个核心的成员方法，计算指定节点到终点节点的评估值，计算评估值(估计距离)的方法常用的有下面几种:
1.曼哈顿距离(Manhattan Distanc):
h = abs (current_cell.x – goal.x) +
abs (current_cell.y – goal.y)
2.对角线距离(Diagonal Distance):
h = max { abs(current_cell.x – goal.x),
abs(current_cell.y – goal.y) }
3.欧几里得距离(Euclidean Distance):
h = sqrt ( (current_cell.x – goal.x)2 +
(current_cell.y – goal.y)2 )
这里我们用最简单最直观理解的曼哈顿距离来计算评估值，所以我们计算评估值的成员方法实现如下:
double estimatedDistance(GraphNode *currentNode, GraphNode *dstNode)
{
double cur_row_index = currentNode->row_index_;
double cur_col_index = currentNode->col_index_;
double dst_row_index = dstNode->row_index_;
double dst_col_index = dstNode->col_index_;
return std::abs(cur_row_index - dst_row_index) + std::abs(cur_col_index - dst_col_index);
}
当算法主题需要的两个方法都实现后，我们就可以来实现A*寻路算法的主逻辑了，主逻辑实现如下:
//A* serach
vector *FindPath(GraphNode *srcNode, GraphNode *dstNode)
{
bool reached = false; //是否已经到达终点了
//因为我们set存放的是动态分配的对象指针，所以需要定义一共比较两个指针对象的lambda方法
auto setCompFunction = [](const GraphNode *left, const GraphNode *right) -> bool {
return left->f < right->f;
};
//用set存放我们要遍历的节点对象
set openList(setCompFunction);
//初始化起点的f，g，h都为0，然后添加到set中等待遍历
srcNode->f = 0;
srcNode->g = 0;
srcNode->h = 0;
openList.insert(srcNode);
while (!openList.empty())
{
//总是获取set中的第一个节点然后在set中移除
GraphNode *currentNode = *openList.begin();
openList.erase(openList.begin());
//标记该节点已经关闭，不再考虑
currentNode->is_closed_ = true;
//检查该节点是否是终点，如果是标记为已到达并结束循环
if (currentNode->col_index_ == dstNode->col_index_ &&
currentNode->row_index_ == dstNode->row_index_) {
reached = true;
break;
}
//获取当前节点附近的节点
vector *neighbors = this->GetNeighborsByNode(currentNode);
if (!neighbors->empty()) {
//如果附近节点列表不是空的，则处理列表中每个节点
for (auto iter = neighbors->begin(); iter != neighbors->end(); iter++)
{
GraphNode *nNode = *iter;
double nG = currentNode->g + 1; //计算当前节点到该附近节点的距离，因为是网格，所以各个节点直接间隔距离都是1
double nH = this->estimatedDistance(nNode, dstNode);//计算该附近节点到终点的估计值(估计距离)
double nF = nG + nH; //计算评估值
//如果当前节点到该附近节点的评估值小于之前附近节点的评估值，则更新该附近节点的各个值属性(代表找到最短距离)
if (nNode->f > nF) {
nNode->f = nF;
nNode->g = nG;
nNode->h = nH;
nNode->pervNode = currentNode;
//如果找到了最短距离则添加到set中等待遍历处理
openList.insert(nNode);
}
}
}
delete neighbors;
}
//如果找到了终点，则从终点节点，利用pervNode成员属性向后反推到达该终点经过的节点
vector *pathList = new vector;
if (reached) {
stack pathStack;
GraphNode *iterNode = dstNode;
while (nullptr != iterNode)
{
pathStack.push(iterNode);
iterNode = iterNode->pervNode;
}
while (!pathStack.empty())
{
pathList->push_back(pathStack.top());
pathStack.pop();
}
}
return pathList;
}
下面分开讲解一下上面的各个点，首先是set的比较方法:
auto setCompFunction = [](const GraphNode *left, const GraphNode *right) -> bool {
return left->f < right->f;
};
set openList(setCompFunction);
我们之前在GraphNode类中重载了<操作符，为了让set能够比较去重元素，而当我们要存放的类型是动态分配对象的指针，则set就不能正常调用到我们的重载的<操作符方法了，而变成比较指针值了，这会导致set不能正常按照我们的逻辑去重了。所以我们定义了一个lambda方法来比较f(评估值)，并当传给了set类，这样set类就能正常按照对象中的f来去重以及比较该对象的顺序了。
然后是在while中的主流程:
while (!openList.empty())
{
//总是获取set中的第一个节点然后在set中移除
GraphNode *currentNode = *openList.begin();
openList.erase(openList.begin());
//标记该节点已经关闭，不再考虑
currentNode->is_closed_ = true;
//检查该节点是否是终点，如果是标记为已到达并结束循环
if (currentNode->col_index_ == dstNode->col_index_ &&
currentNode->row_index_ == dstNode->row_index_) {
reached = true;
break;
}
//获取当前节点附近的节点
vector *neighbors = this->GetNeighborsByNode(currentNode);
if (!neighbors->empty()) {
//如果附近节点列表不是空的，则处理列表中每个节点
for (auto iter = neighbors->begin(); iter != neighbors->end(); iter++)
{
GraphNode *nNode = *iter;
double nG = currentNode->g + 1; //计算当前节点到该附近节点的距离，因为是网格，所以各个节点直接间隔距离都是1
double nH = this->estimatedDistance(nNode, dstNode);//计算该附近节点到终点的估计值(估计距离)
double nF = nG + nH; //计算评估值
//如果当前节点到该附近节点的评估值小于之前附近节点的评估值，则更新该附近节点的各个值属性(代表找到最短距离)
if (nNode->f > nF) {
nNode->f = nF;
nNode->g = nG;
nNode->h = nH;
nNode->pervNode = currentNode;
//如果找到了最短距离则添加到set中等待遍历处理
openList.insert(nNode);
}
}
}
delete neighbors;
}
因为现在set能够正常去重以及比较我们的节点对象了，所以当我们先计算当前节点和它附近节点的评估值f，然后比较当前节点和它附近节点的评估值f，如果小于，则代表当前这个节点到它附近节点路径更短，我们就将这个附近节点添加到set中，所以这个set总会存放着每一次循环中的最优节点，最后不断重复这个过程，并不断通过每次找到的最优点去扩展寻找更优的节点并直到到达终点。
最后的逻辑很简单，当找到了终点，我们则利用pervNode成员属性去反推出完整的最短路径:
vector *pathList = new vector;
if (reached) {
stack pathStack;
GraphNode *iterNode = dstNode;
while (nullptr != iterNode)
{
pathStack.push(iterNode);
iterNode = iterNode->pervNode;
}
while (!pathStack.empty())
{
pathList->push_back(pathStack.top());
pathStack.pop();
}
}
因为pervNode成员属性总是记录到达该节点最短路径的上一个节点，所以我们可以利用该成员属性反推出完整路径，因为是从终点开始反推，所以整个路径是反过来的(终点->起点)，所以我们利用栈(stack)这个结构，先入后出的特性，每反推出一个节点就压入栈，最后不断pop并添加到list中，最终出来就是正确顺序(起点->终点)的完整路径了。
完整代码实现:https://github.com/ZhiyangLeeCN/algorithms/blob/master/A-Star%20Search%20Algorithm/main.cpp
参考资料:
https://en.wikipedia.org/wiki/A*_search_algorithm
https://www.geeksforgeeks.org/a-search-algorithm/
https://brilliant.org/wiki/a-star-search/