A*算法在Dijkstra的基础上加入了启发式变量，一般用启发式距离（两点的直线距离）表示。
启发式距离
2.算法伪代码
其中O代表优先队列，C存放着已访问过的节点。
先来看看A*算法运行的最终结果吧
首先先创建一个类代表节点（省略了构造函数等Method）。
classnode{
private：
intx，y;//坐标
doublesumCost;//f（n）
doubleheuristic;//启发值
node*backpoint;//前驱节点
};
在main函数中定义起始节点和目标节点
nodestartNode（40，10）;//起始点
nodegoalNode（10，40）;//目标点
初始化地图，这里计算了每个节点的启发式距离
vector《node*》tmp;
for（intj=0;j《mapSize;++j）{
node*tmpNode=newnode（i，j）;
tmpNode-》setHeuristic（calHeristic（tmpNode，goalNode））;
tmp.push_back（tmpNode）;
}
添加障碍物
voiddefineObstacle（vector《vector《node*》》&roadmap）{
//先框住四周
for（inti=0;i《mapSize;++i）{
roadmap［0］［i］-》setObstacle（）;
roadmap［i］［0］-》setObstacle（）;
roadmap［i］［mapSize-1］-》setObstacle（）;
roadmap［mapSize-1］［i］-》setObstacle（）;
//再定义一个条形快
for（inti=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*算法函数
voidaStar（constnode&startNode，constnode&goalNode，vector《vector《node*》》&roadmap，Mat&background）{
//使用Lambda表达式定义一个优先队列
autocmp=［］（node*left，node*right）{returnleft-》gN（）》right-》gN（）;};
priority_queue《node*，vector《node*》，decltype（cmp）》O（cmp）;
node*tmp=roadmap［startNode.coordinateX（）］［startNode.coordinateY（）］;
O.push（tmp）;
//Algorithm24A*Algorithm
while（！O.empty（））{
node*nBest=O.top（）;
//RemovenbestfromOandaddtoC（isVisited）。
O.pop（）;
nBest-》setIsVisited（）;
//ifnbest==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（inti=0;i《8;i++）{
nodetmpNode=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）;
imshow（“aStar”，background）;
//画出最终的路径
tmp=roadmap［goalNode.coordinateX（）］［goalNode.coordinateY（）］;
while（！（*tmp==startNode））{
tmp-》drawNode（background，imgGridSize，Scalar（255，0，0））;
tmp=tmp-》returnBackpoint（）;
waitKey（5）;
4.资源指路
A*算法其中大部分变量和算法过程我都尽量和PrinciplesofMotion中的说明保持一致，源代码已上传github（非工程文件，需自行配置）