1.算法概述
A*算法也叫做A星(A star)算法，A*算法是之前提過的Dijkstra最短路徑的一個(gè)擴(kuò)展和改進(jìn)，大體思路是通過一個(gè)評估方法來估計(jì)一個(gè)節(jié)點(diǎn)到終點(diǎn)的估計(jì)值，并不斷通過選擇擁有最優(yōu)估計(jì)值的節(jié)點(diǎn)，最終到達(dá)終點(diǎn)，這個(gè)過程也稱作啟發(fā)式尋找，當(dāng)然，因?yàn)楣?jié)點(diǎn)上可能會放置障礙物等無法通過的節(jié)點(diǎn)，所以這個(gè)值不一定夠準(zhǔn)確，所以才稱這個(gè)值為估計(jì)值。
與Dijkstra最短路最大的區(qū)別是，Dijkstra最短路會計(jì)算一個(gè)起點(diǎn)到所有節(jié)點(diǎn)的最短路徑，這就意味著Dijkstra算法將遍歷所有節(jié)點(diǎn)，而當(dāng)我們只想知道一個(gè)起點(diǎn)到一個(gè)終點(diǎn)的最短路徑時(shí)，Dijkstra算法雖然能找到，但也遍歷了所有節(jié)點(diǎn)。而A*算法通過估計(jì)值進(jìn)行啟發(fā)式尋找只會遍歷較少的節(jié)點(diǎn)就找到最短路徑，所以A*更適合尋找一個(gè)點(diǎn)到一個(gè)點(diǎn)的最短路徑。
從上圖可以很明顯的看出來二者的差異，Dijkstra算法因?yàn)槭乔笃瘘c(diǎn)到所有節(jié)點(diǎn)的最短距離，因?yàn)榘瘘c(diǎn)到所有節(jié)點(diǎn)的最短距離，所以我們也可以知道我們想知道的起點(diǎn)到任何一點(diǎn)的最短距離，但是也遍歷了大部分的節(jié)點(diǎn)。而A*算法只會通過估計(jì)值不斷尋找擁有最優(yōu)估計(jì)值的節(jié)點(diǎn)來靠近終點(diǎn)，所以只遍歷了較少的節(jié)點(diǎn)，同時(shí)也找到了最短路徑。
2.算法工作原理
A*算法的前面的步驟與Dijkstra算法類似，通過給定一個(gè)起點(diǎn)，然后獲取起點(diǎn)附近的節(jié)點(diǎn)，先計(jì)算當(dāng)前所在節(jié)點(diǎn)到附近每個(gè)節(jié)點(diǎn)的距離，我們稱這個(gè)距離為g，然后在計(jì)算附近每個(gè)節(jié)點(diǎn)到終點(diǎn)的估計(jì)值(可以理解為表示到終點(diǎn)的大概距離)，我們稱這個(gè)值為h，最后附近每個(gè)節(jié)點(diǎn)到終點(diǎn)的評估值用h+g得到，我們稱這個(gè)值為f，最終我們可以得到評估方法的計(jì)算方式為:f(n) = g(n) + h(n)，評估值的值有很多種方式可以得到，后面會介紹幾個(gè)常用的。
當(dāng)?shù)玫礁浇總€(gè)節(jié)點(diǎn)到終點(diǎn)的評估值后，我們從目前已經(jīng)得到的節(jié)點(diǎn)里面，根據(jù)評估值找到一個(gè)最優(yōu)最靠近終點(diǎn)的節(jié)點(diǎn)，然后將這個(gè)節(jié)點(diǎn)設(shè)為當(dāng)前節(jié)點(diǎn)，并把之前的當(dāng)前節(jié)點(diǎn)設(shè)為已關(guān)閉，之后只要遇到該節(jié)點(diǎn)則跳過(包括從獲取附近節(jié)點(diǎn)的方式遇到)，不斷重復(fù)前面的過程，直到找到終點(diǎn)為止。
3.偽碼實(shí)現(xiàn)
// A* Search Algorithm
1. Initialize the open list //初始化open列表，表示當(dāng)前找到的最優(yōu)節(jié)點(diǎn)集合(不斷的遍歷過程中該列表會變化)
2. Initialize the closed list //初始化closed列表，表示以及遍歷并處理過的節(jié)點(diǎn)，后面不會在考慮
put the starting node on the open //將起點(diǎn)初始化放入到open列表中作為初始點(diǎn)開始后面的流程
list (you can leave its f at zero)
3. while the open list is not empty //不斷遍歷open列表中的節(jié)點(diǎn)，直到找到終點(diǎn)
a) find the node with the least f on //從當(dāng)前open列表中找到評估值最優(yōu)的一共節(jié)點(diǎn)，稱之位q
the open list, call it "q"
b) pop q off the open list //將q節(jié)點(diǎn)從open列表中移除
c) generate q's 4 successors and set their //我們的網(wǎng)格圖表只有4個(gè)臨近點(diǎn)(上、下、左、右)，獲得當(dāng)前q節(jié)點(diǎn)附近的4個(gè)節(jié)點(diǎn)
parents to q
//處理找到的每個(gè)附近節(jié)點(diǎn)
d) for each successor
i) if successor is the goal, stop search //如果該節(jié)點(diǎn)是終點(diǎn)，則停止查找
successor.g = q.g + distance between //計(jì)算當(dāng)前節(jié)點(diǎn)q到附近節(jié)點(diǎn)successor的距離g
successor and q
successor.h = distance from goal to //計(jì)算附近節(jié)點(diǎn)successor到終點(diǎn)goal的估計(jì)值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 //計(jì)算附近節(jié)點(diǎn)successor的評估值f，f=g+h
/*
* 如果有一個(gè)節(jié)點(diǎn)和這個(gè)附近節(jié)點(diǎn)(successor )擁有相同位置并存在于open列表中，
* 而且有更低的評估值f，則跳過這個(gè)附近節(jié)點(diǎn)的處理。
*/
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
/*
* 如果有一個(gè)節(jié)點(diǎn)和這個(gè)附近節(jié)點(diǎn)(successor )擁有相同位置并存在于closed列表中，
* 而且有更低的評估值f，則跳過這個(gè)附近節(jié)點(diǎn)的處理。除此之外，都添加到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++代碼實(shí)現(xiàn)
我們首先定義一個(gè)類表示節(jié)點(diǎn):
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_; //表示該節(jié)點(diǎn)是否是障礙物
bool is_closed_; //表示該節(jié)點(diǎn)是否已經(jīng)訪問并不再考慮了
int row_index_; //節(jié)點(diǎn)在二維數(shù)組中的行索引
int col_index_; //節(jié)點(diǎn)在二維數(shù)組中的列索引
double f; //當(dāng)前節(jié)點(diǎn)到終點(diǎn)的評估值(g + h)
double g; //目前為止到該節(jié)點(diǎn)所需的距離
double h; //當(dāng)前節(jié)點(diǎn)到終點(diǎn)的估計(jì)值(估計(jì)距離)
string display_mark_; //當(dāng)前節(jié)點(diǎn)在打印時(shí)顯示的字符
GraphNode *pervNode; //通過哪個(gè)節(jié)點(diǎn)到達(dá)該節(jié)點(diǎn)(最短路徑)
bool operator < (const GraphNode &right) const {
return this->f < right.f;
}
};
最后我們還重載了操作符<，這是因?yàn)槲覀儗⒁闅v的節(jié)點(diǎn)都放在一個(gè)set中，而自定義類型則需要重載該運(yùn)算符來讓set排序以及去重要放進(jìn)來的節(jié)點(diǎn)。
最后我們需要用一個(gè)類來表示一個(gè)可自定義大小的表格(網(wǎng)格圖)，該類原型如下:
class Graph {
public:
//默認(rèn)構(gòu)造函數(shù)
Graph() ;
//初始化指定長寬的網(wǎng)格
Graph(int rows, int cols) ;
//析構(gòu)函數(shù)，示范動(dòng)態(tài)分配的網(wǎng)格內(nèi)存
~Graph();
//打印該網(wǎng)格
void Print();
//根據(jù)節(jié)點(diǎn)指定的行，列防止該節(jié)點(diǎn)到指定位置上
void PutNode(GraphNode *graphNode);
//放置一個(gè)節(jié)點(diǎn)到指定位置上
void PutNode(GraphNode *graphNode, int row_index, int col_index);
//計(jì)算currentNode節(jié)點(diǎn)到dstNode節(jié)點(diǎn)的估計(jì)值(估計(jì)距離)
double estimatedDistance(GraphNode *currentNode, GraphNode *dstNode);
//A*尋路算法主實(shí)現(xiàn)
vector<GraphNode *> *FindPath(GraphNode *srcNode, GraphNode *dstNode);
//獲取給定一個(gè)節(jié)點(diǎn)附近的節(jié)點(diǎn)列表
vector<GraphNode *> *GetNeighborsByNode(GraphNode *node);
//按照給定的行，列索引獲取指定位置上的節(jié)點(diǎn)對象
GraphNode *FindByIndex(int row_index, int col_index);
//檢查要進(jìn)行操作的行，列索引示范合法(越界等情況...)
bool CheckIndexIsValid(int row_index, int col_index);
//獲取該表格一共有多少行
int GetRows();
//獲取該表格一共有多少列
int GetCols();
private:
int rows_; //該表格一共有多少行
int cols_; //該表格一共有多少列
GraphNode **graph_; //一次性分配的表格二維數(shù)組，每個(gè)元素都指向一個(gè)動(dòng)態(tài)分配的Node對象指針
//一共指定行，列索引位置的二維數(shù)組元素值
void SetLocation(GraphNode *graphNode, int row_index, int col_index);
//獲取二維數(shù)組指定位置上的元素
GraphNode **GetLoaction(int row_index, int col_index);
因?yàn)槲覀兊牡谋韺ο罂梢灾С指鱾€(gè)大小的網(wǎng)格圖，所以是一次性動(dòng)態(tài)分配的內(nèi)存，這一塊代碼理解起來會比較繞，但屬于指針操作的基礎(chǔ)知識，可以暫時(shí)先不理解通過指針操作二維數(shù)組的這塊代碼，只用知道該類提供了PutNode、FindByIndex成員方法來操作該二維數(shù)組即可。
首先我們先實(shí)現(xiàn)獲取給定節(jié)點(diǎn)附近節(jié)點(diǎn)列表的方法，該成員方法實(shí)現(xiàn)如下:
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;
}
和我們之前實(shí)現(xiàn)的Dijkstra算法獲取一個(gè)節(jié)點(diǎn)附近的節(jié)點(diǎn)一樣，只不過最后我們只返回有效的附近節(jié)點(diǎn)，排除了已經(jīng)關(guān)閉的節(jié)點(diǎn)以及障礙物節(jié)點(diǎn)。
我們還需要實(shí)現(xiàn)一個(gè)核心的成員方法，計(jì)算指定節(jié)點(diǎn)到終點(diǎn)節(jié)點(diǎn)的評估值，計(jì)算評估值(估計(jì)距離)的方法常用的有下面幾種:
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 )
這里我們用最簡單最直觀理解的曼哈頓距離來計(jì)算評估值，所以我們計(jì)算評估值的成員方法實(shí)現(xiàn)如下:
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);
}
當(dāng)算法主題需要的兩個(gè)方法都實(shí)現(xiàn)后，我們就可以來實(shí)現(xiàn)A*尋路算法的主邏輯了，主邏輯實(shí)現(xiàn)如下:
//A* serach
vector<GraphNode *> *FindPath(GraphNode *srcNode, GraphNode *dstNode)
{
bool reached = false; //是否已經(jīng)到達(dá)終點(diǎn)了
//因?yàn)槲覀僺et存放的是動(dòng)態(tài)分配的對象指針，所以需要定義一共比較兩個(gè)指針對象的lambda方法
auto setCompFunction = [](const GraphNode *left, const GraphNode *right) -> bool {
return left->f < right->f;
};
//用set存放我們要遍歷的節(jié)點(diǎn)對象
set<GraphNode *, decltype(setCompFunction)> openList(setCompFunction);
//初始化起點(diǎn)的f，g，h都為0，然后添加到set中等待遍歷
srcNode->f = 0;
srcNode->g = 0;
srcNode->h = 0;
openList.insert(srcNode);
while (!openList.empty())
{
//總是獲取set中的第一個(gè)節(jié)點(diǎn)然后在set中移除
GraphNode *currentNode = *openList.begin();
openList.erase(openList.begin());
//標(biāo)記該節(jié)點(diǎn)已經(jīng)關(guān)閉，不再考慮
currentNode->is_closed_ = true;
//檢查該節(jié)點(diǎn)是否是終點(diǎn)，如果是標(biāo)記為已到達(dá)并結(jié)束循環(huán)
if (currentNode->col_index_ == dstNode->col_index_ &&
currentNode->row_index_ == dstNode->row_index_) {
reached = true;
break;
}
//獲取當(dāng)前節(jié)點(diǎn)附近的節(jié)點(diǎn)
vector<GraphNode *> *neighbors = this->GetNeighborsByNode(currentNode);
if (!neighbors->empty()) {
//如果附近節(jié)點(diǎn)列表不是空的，則處理列表中每個(gè)節(jié)點(diǎn)
for (auto iter = neighbors->begin(); iter != neighbors->end(); iter++)
{
GraphNode *nNode = *iter;
double nG = currentNode->g + 1; //計(jì)算當(dāng)前節(jié)點(diǎn)到該附近節(jié)點(diǎn)的距離，因?yàn)槭蔷W(wǎng)格，所以各個(gè)節(jié)點(diǎn)直接間隔距離都是1
double nH = this->estimatedDistance(nNode, dstNode);//計(jì)算該附近節(jié)點(diǎn)到終點(diǎn)的估計(jì)值(估計(jì)距離)
double nF = nG + nH; //計(jì)算評估值
//如果當(dāng)前節(jié)點(diǎn)到該附近節(jié)點(diǎn)的評估值小于之前附近節(jié)點(diǎn)的評估值，則更新該附近節(jié)點(diǎn)的各個(gè)值屬性(代表找到最短距離)
if (nNode->f > nF) {
nNode->f = nF;
nNode->g = nG;
nNode->h = nH;
nNode->pervNode = currentNode;
//如果找到了最短距離則添加到set中等待遍歷處理
openList.insert(nNode);
}
}
}
delete neighbors;
}
//如果找到了終點(diǎn)，則從終點(diǎn)節(jié)點(diǎn)，利用pervNode成員屬性向后反推到達(dá)該終點(diǎn)經(jīng)過的節(jié)點(diǎn)
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;
}
下面分開講解一下上面的各個(gè)點(diǎn)，首先是set的比較方法:
auto setCompFunction = [](const GraphNode *left, const GraphNode *right) -> bool {
return left->f < right->f;
};
set<GraphNode *, decltype(setCompFunction)> openList(setCompFunction);
我們之前在GraphNode類中重載了<操作符，為了讓set能夠比較去重元素，而當(dāng)我們要存放的類型是動(dòng)態(tài)分配對象的指針，則set就不能正常調(diào)用到我們的重載的<操作符方法了，而變成比較指針值了，這會導(dǎo)致set不能正常按照我們的邏輯去重了。所以我們定義了一個(gè)lambda方法來比較f(評估值)，并當(dāng)傳給了set類，這樣set類就能正常按照對象中的f來去重以及比較該對象的順序了。
然后是在while中的主流程:
while (!openList.empty())
{
//總是獲取set中的第一個(gè)節(jié)點(diǎn)然后在set中移除
GraphNode *currentNode = *openList.begin();
openList.erase(openList.begin());
//標(biāo)記該節(jié)點(diǎn)已經(jīng)關(guān)閉，不再考慮
currentNode->is_closed_ = true;
//檢查該節(jié)點(diǎn)是否是終點(diǎn)，如果是標(biāo)記為已到達(dá)并結(jié)束循環(huán)
if (currentNode->col_index_ == dstNode->col_index_ &&
currentNode->row_index_ == dstNode->row_index_) {
reached = true;
break;
}
//獲取當(dāng)前節(jié)點(diǎn)附近的節(jié)點(diǎn)
vector<GraphNode *> *neighbors = this->GetNeighborsByNode(currentNode);
if (!neighbors->empty()) {
//如果附近節(jié)點(diǎn)列表不是空的，則處理列表中每個(gè)節(jié)點(diǎn)
for (auto iter = neighbors->begin(); iter != neighbors->end(); iter++)
{
GraphNode *nNode = *iter;
double nG = currentNode->g + 1; //計(jì)算當(dāng)前節(jié)點(diǎn)到該附近節(jié)點(diǎn)的距離，因?yàn)槭蔷W(wǎng)格，所以各個(gè)節(jié)點(diǎn)直接間隔距離都是1
double nH = this->estimatedDistance(nNode, dstNode);//計(jì)算該附近節(jié)點(diǎn)到終點(diǎn)的估計(jì)值(估計(jì)距離)
double nF = nG + nH; //計(jì)算評估值
//如果當(dāng)前節(jié)點(diǎn)到該附近節(jié)點(diǎn)的評估值小于之前附近節(jié)點(diǎn)的評估值，則更新該附近節(jié)點(diǎn)的各個(gè)值屬性(代表找到最短距離)
if (nNode->f > nF) {
nNode->f = nF;
nNode->g = nG;
nNode->h = nH;
nNode->pervNode = currentNode;
//如果找到了最短距離則添加到set中等待遍歷處理
openList.insert(nNode);
}
}
}
delete neighbors;
}
因?yàn)楝F(xiàn)在set能夠正常去重以及比較我們的節(jié)點(diǎn)對象了，所以當(dāng)我們先計(jì)算當(dāng)前節(jié)點(diǎn)和它附近節(jié)點(diǎn)的評估值f，然后比較當(dāng)前節(jié)點(diǎn)和它附近節(jié)點(diǎn)的評估值f，如果小于，則代表當(dāng)前這個(gè)節(jié)點(diǎn)到它附近節(jié)點(diǎn)路徑更短，我們就將這個(gè)附近節(jié)點(diǎn)添加到set中，所以這個(gè)set總會存放著每一次循環(huán)中的最優(yōu)節(jié)點(diǎn)，最后不斷重復(fù)這個(gè)過程，并不斷通過每次找到的最優(yōu)點(diǎn)去擴(kuò)展尋找更優(yōu)的節(jié)點(diǎn)并直到到達(dá)終點(diǎn)。
最后的邏輯很簡單，當(dāng)找到了終點(diǎn)，我們則利用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();
}
}
因?yàn)閜ervNode成員屬性總是記錄到達(dá)該節(jié)點(diǎn)最短路徑的上一個(gè)節(jié)點(diǎn)，所以我們可以利用該成員屬性反推出完整路徑，因?yàn)槭菑慕K點(diǎn)開始反推，所以整個(gè)路徑是反過來的(終點(diǎn)->起點(diǎn))，所以我們利用棧(stack)這個(gè)結(jié)構(gòu)，先入后出的特性，每反推出一個(gè)節(jié)點(diǎn)就壓入棧，最后不斷pop并添加到list中，最終出來就是正確順序(起點(diǎn)->終點(diǎn))的完整路徑了。
完整代碼實(shí)現(xiàn):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/
