機(jī)器人路徑規(guī)劃之A*算法(附C++源碼)

　　1. 基本原理

　　A*算法的本質(zhì)是廣度優(yōu)先的圖搜索。意在尋找一個(gè)從起點(diǎn)到目標(biāo)節(jié)點(diǎn)的最短路徑。

　　A*算法在Dijkstra的基礎(chǔ)上加入了啟發(fā)式變量，一般用啟發(fā)式距離（兩點(diǎn)的直線距離）表示。

　　啟發(fā)式距離

　　2. 算法偽代碼

　　本偽代碼摘取自Principles of Robot Motion

　　其中O代表優(yōu)先隊(duì)列，C存放著已訪問過的節(jié)點(diǎn)。

　　3. 關(guān)鍵C++代碼剖析

　　先來看看A*算法運(yùn)行的最終結(jié)果吧

　　首先先創(chuàng)建一個(gè)類代表節(jié)點(diǎn)（省略了構(gòu)造函數(shù)等Method）。

　　class node {

　　private：

　　int x， y; // 坐標(biāo)

　　double sumCost; // f（n）

　　double heuristic;// 啟發(fā)值

　　bool obstacle; // 是否是障礙物

　　node* backpoint; // 前驅(qū)節(jié)點(diǎn)

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

　　};

　　在main函數(shù)中定義起始節(jié)點(diǎn)和目標(biāo)節(jié)點(diǎn)

　　node startNode（40， 10）;// 起始點(diǎn)

　　node goalNode（10， 40）; // 目標(biāo)點(diǎn)

　　初始化地圖，這里計(jì)算了每個(gè)節(jié)點(diǎn)的啟發(fā)式距離

　　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（）;

　　}

　　// 再定義一個(gè)條形快

　　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*算法函數(shù)

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

　　// 使用Lambda表達(dá)式定義一個(gè)優(yōu)先隊(duì)列

　　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;

　　// 八個(gè)方向都可以走

　　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（非工程文件，需自行配置）

　編輯：黃飛