1. 基本原理

　　A*算法的本質是廣度優先的圖搜索。意在尋找一個從起點到目標節點的最短路徑。

　　A*算法在Dijkstra的基礎上加入了啟發式變量，一般用啟發式距離（兩點的直線距離）表示。

　　啟發式距離

　　2. 算法偽代碼

　　本偽代碼摘取自Principles of Robot Motion

　　其中O代表優先隊列，C存放著已訪問過的節點。

　　3. 關鍵C++代碼剖析

　　先來看看A*算法運行的最終結果吧

　　首先先創建一個類代表節點（省略了構造函數等Method）。

　　class node {

　　private：

　　int x， y; // 坐標

　　double sumCost; // f（n）

　　double heuristic;// 啟發值

　　bool obstacle; // 是否是障礙物

　　node* backpoint; // 前驅節點

　　bool isVisited; // 判斷是否訪問過

　　};

　　在main函數中定義起始節點和目標節點

　　node startNode（40， 10）;// 起始點

　　node goalNode（10， 40）; // 目標點

　　初始化地圖，這里計算了每個節點的啟發式距離

　　for （int i = 0; i 《 mapSize; ++i） {

　　vector《node*》 tmp;

　　for （int j = 0; j 《 mapSize; ++j） {

　　node* tmpNode = new node（i， j）;

　　tmpNode-》setHeuristic（calHeristic（tmpNode， goalNode））;

　　tmp.push_back（tmpNode）;

　　}

　　roadmap.push_back（tmp）;

　　}

　　添加障礙物

　　void defineObstacle（vector《vector《node*》》& roadmap） {

　　// 先框住四周

　　for （int i = 0; i 《 mapSize; ++i） {

　　roadmap［0］［i］-》setObstacle（）;

　　roadmap［i］［0］-》setObstacle（）;

　　roadmap［i］［mapSize - 1］-》setObstacle（）;

　　roadmap［mapSize - 1］［i］-》setObstacle（）;

　　}

　　// 再定義一個條形快

　　for （int i = 1; i 《 mapSize / 2; ++i） {

　　roadmap［mapSize * 2 / 3］［i］-》setObstacle（）;

　　roadmap［mapSize * 2 / 3 - 1］［i］-》setObstacle（）;

　　roadmap［mapSize * 1 / 3］［mapSize - i］-》setObstacle（）;

　　roadmap［mapSize * 1 / 3 - 1］［mapSize - i］-》setObstacle（）;

　　}

　　}

　　A*算法函數

　　void aStar（const node& startNode， const node& goalNode， vector《vector《node*》》& roadmap， Mat& background） {

　　// 使用Lambda表達式定義一個優先隊列

　　auto cmp = ［］（node* left， node* right） { return left-》gN（） 》 right-》gN（）; };

　　priority_queue《node*， vector《node*》， decltype（cmp）》 O（cmp）;

　　node* tmp = roadmap［startNode.coordinateX（）］［startNode.coordinateY（）］;

　　O.push（tmp）;

　　// Algorithm 24 A* Algorithm

　　while （！O.empty（）） {

　　// Pick nbest from O such that f（nbest） 《= f（n）。

　　node* nBest = O.top（）;

　　// Remove nbest from O and add to C（isVisited）。

　　O.pop（）;

　　nBest-》setIsVisited（）;

　　// if nbest == qgoal， EXIT.

　　if （*nBest == goalNode）

　　break;

　　// 八個方向都可以走

　　std：：vector《node》 motion = { node（1， 0， 1），

　　node（0， 1， 1），

　　node（-1， 0， 1），

　　node（0， -1， 1），

　　node（-1， -1， std：：sqrt（2）），

　　node（-1， 1， std：：sqrt（2）），

　　node（1， -1， std：：sqrt（2）），

　　node（1， 1， std：：sqrt（2）） };

　　for （int i = 0; i 《 8; i++） {

　　node tmpNode = motion［i］;

　　tmpNode += *nBest;

　　node* tmpNodePointer = roadmap［tmpNode.coordinateX（）］［tmpNode.coordinateY（）］;

　　*tmpNodePointer = tmpNode;

　　if （verifyNode（*tmpNodePointer） && ！tmpNodePointer-》returnIsVisited（） && ！tmpNodePointer-》isObstacle（）） {

　　O.push（tmpNodePointer）;

　　tmpNodePointer-》setIsVisited（）;

　　tmpNodePointer-》setBackpoint（nBest）;

　　tmpNodePointer-》drawNode（background， imgGridSize， Scalar（0， 255， 0）， 0）;

　　imshow（“aStar”， background）;

　　waitKey（5）;

　　}

　　}

　　}

　　// 畫出最終的路徑

　　tmp = roadmap［goalNode.coordinateX（）］［goalNode.coordinateY（）］;

　　while （！（*tmp == startNode）） {

　　tmp-》drawNode（background， imgGridSize， Scalar（255， 0， 0））;

　　tmp = tmp-》returnBackpoint（）;

　　imshow（“aStar”， background）;

　　waitKey（5）;

　　}

　　}

　　4. 資源指路

　　A*算法其中大部分變量和算法過程我都盡量和Principles of Motion中的說明保持一致，源代碼已上傳github（非工程文件，需自行配置）

　編輯：黃飛