机器人路径规划之A*算法(附C++源码)

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

　编辑：黄飞