1.算法概述
A*算法也叫做A星(A star)算法，A*算法是之前提過的Dijkstra最短路徑的一個擴展和改進，大體思路是通過一個評估方法來估計一個節點到終點的估計值，并不斷通過選擇擁有最優估計值的節點，最終到達終點，這個過程也稱作啟發式尋找，當然，因為節點上可能會放置障礙物等無法通過的節點，所以這個值不一定夠準確，所以才稱這個值為估計值。
與Dijkstra最短路最大的區別是，Dijkstra最短路會計算一個起點到所有節點的最短路徑，這就意味著Dijkstra算法將遍歷所有節點，而當我們只想知道一個起點到一個終點的最短路徑時，Dijkstra算法雖然能找到，但也遍歷了所有節點。而A*算法通過估計值進行啟發式尋找只會遍歷較少的節點就找到最短路徑，所以A*更適合尋找一個點到一個點的最短路徑。
從上圖可以很明顯的看出來二者的差異，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<GraphNode *> *FindPath(GraphNode *srcNode, GraphNode *dstNode);
//獲取給定一個節點附近的節點列表
vector<GraphNode *> *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<GraphNode *> *GetNeighborsByNode(GraphNode *node)
{
vector<GraphNode *> filter;
vector<GraphNode *> *result = new vector<GraphNode *>;
//左
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<GraphNode *> *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<GraphNode *, decltype(setCompFunction)> 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<GraphNode *> *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<GraphNode *> *pathList = new vector<GraphNode *>;
if (reached) {
stack<GraphNode *> 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<GraphNode *, decltype(setCompFunction)> 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<GraphNode *> *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<GraphNode *> *pathList = new vector<GraphNode *>;
if (reached) {
stack<GraphNode *> 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/
