【规划】A*算法以及c++实现
A*算法
A*算法的核心在于估价函数的设计上，如下式所示：
其中 
称为耗散函数，表示从起始节点 
到节点 n 的实际代价； 
称为启发函数，表示节点 到目标节点 的估计代价； 表示从起始节点经由节点 到目标节点的估计代价。
同Dijkstra算法类似，A*算法也维持一个Open表。Open表中节点的优先级是依据 的大小排列的， 值越小，被搜索到的优先级越高。为保证能搜索到最优解，启发函数 不能太大，不能大于节点 到目标节点的实际代价；但如果 ，则A*算法退化为Dijkstra算法，虽能保证得到最优路径，但算法效率低；如果 恰好等于节点 到目标节点的实际代价，则A*算法探索的节点恰好就是最优路径上的节点。所以 的取值直接影响算法的速度和精确度，常见的 的取值有两点之间的欧几里得距离(Euclidean Distance)和曼哈顿距离(Manhattan Distance)等。
Über 每个方格左上角是f(n)，左下角是g(n)，右下角是h(n)。schrift
- 从起点 开始，把它就加入到Open List中，这个Open List有点像一个购
物单，实际上是一个待检查的方格列表。 - 忽略墙壁和Close List中的方格，将与起点 相邻并且可到达的(Reachable)
方格也加入到Open List中。把起点 设置为这些方格的父节点。 - 把 从Open List中移除，加入到Close List中，表示不再关注。
- 把新得到的 值最小的方格 从Open List里取出，放到Close List中。
- 忽略墙壁和Close List中的方格，检查所有与 相邻的方格，把它们加入到Open List中，并把我们选定的方格设置为它们的父节点。
- 如果某个与 相邻的方格 已经在Open List中，则检查它的 值是否更小。如果没有，不做任何操作。相反，如果 更小，则把 的父节点设为当前方格，然后重新计算 的 值和 值
c++实现
Astar.h
#ifndef ASTAR_H
#define ASTAR_H
#include
#include
#include
#include
#include
using namespace std;
typedef struct Node
{
int x,y;
int g; //起始点到当前点实际代价
int h;//当前节点到目标节点最佳路径的估计代价
int f;//估计值
Node* father;
Node(int x,int y)
{
this->x = x;
this->y = y ;
this->g = 0;
this->h = 0;
this->f = 0;
this->father = NULL;
}
Node(int x,int y,Node* father)
{
this->x = x;
this->y = y ;
this->g = 0;
this->h = 0;
this->f = 0;
this->father = father;
}
}Node;
class Astar{
public:
Astar();
~Astar();
void search(Node* startPos,Node* endPos);
void checkPoit(int x,int y,Node* father,int g);
void NextStep(Node* currentPoint);
int isContains(vector* Nodelist ,int x,int y);
void countGHF(Node* sNode,Node* eNode,int g);
static bool compare(Node* n1,Node* n2);
bool unWalk(int x,int y);
void printPath(Node* current);
void printMap();
vector openList;
vector closeList;
Node *startPos;
Node *endPos;
static const int WeightW = 10;// 正方向消耗
static const int WeightWH = 14;//打斜方向的消耗
static const int row = 6;
static const int col = 8;
};
#endif Astar.cpp
#include "stdafx.h"
#include "Astar.h"
int map[101][101] =
{
{0,0,0,1,0,1,0,0,0},
{0,0,0,1,0,1,0,0,0},
{0,0,0,0,0,1,0,0,0},
{0,0,0,1,0,1,0,1,0},
{0,0,0,1,0,1,0,1,0},
{0,0,0,1,0,0,0,1,0},
{0,0,0,1,0,0,0,1,0}
};
Astar::Astar()
{
}
Astar::~Astar()
{
}
void Astar::search( Node* startPos,Node* endPos )
{
if (startPos->x < 0 || startPos->x > row || startPos->y < 0 || startPos->y >col
||
endPos->x < 0 || endPos->x > row || endPos->y < 0 || endPos->y > col)
return ;
Node* current;
this->startPos = startPos;
this->endPos = endPos;
openList.push_back(startPos);
//主要是这块，把开始的节点放入openlist后开始查找旁边的8个节点，如果坐标超长范围或在closelist就return 如果已经存在openlist就对比当前节点到遍历到的那个节点的G值和当前节点到原来父节点的G值 如果原来的G值比较大 不用管 否则重新赋值G值 父节点 和f 如果是新节点 加入到openlist直到opellist为空或找到终点
while(openList.size() > 0)
{
current = openList[0];
if (current->x == endPos->x && current->y == endPos->y)
{
cout<<"find the path"< row || y < 0 || y > col)
return;
if (this->unWalk(x,y))
return;
if (isContains(&closeList,x,y) != -1)
return;
int index;
if ((index = isContains(&openList,x,y)) != -1)
{
Node *point = openList[index];
if (point->g > father->g + g)
{
point->father = father;
point->g = father->g + g;
point->f = point->g + point->h;
}
}
else
{
Node * point = new Node(x,y,father);
countGHF(point,endPos,g);
openList.push_back(point);
}
}
void Astar::NextStep( Node* current )
{
checkPoit(current->x - 1,current->y,current,WeightW);//左
checkPoit(current->x + 1,current->y,current,WeightW);//右
checkPoit(current->x,current->y + 1,current,WeightW);//上
checkPoit(current->x,current->y - 1,current,WeightW);//下
checkPoit(current->x - 1,current->y + 1,current,WeightWH);//左上
checkPoit(current->x - 1,current->y - 1,current,WeightWH);//左下
checkPoit(current->x + 1,current->y - 1,current,WeightWH);//右下
checkPoit(current->x + 1,current->y + 1,current,WeightWH);//右上
}
int Astar::isContains(vector* Nodelist, int x,int y )
{
for (int i = 0;i < Nodelist->size();i++)
{
if (Nodelist->at(i)->x == x && Nodelist->at(i)->y == y)
{
return i;
}
}
return -1;
}
void Astar::countGHF( Node* sNode,Node* eNode,int g)
{
int h = abs(sNode->x - eNode->x) * WeightW + abs(sNode->y - eNode->y) * WeightW;
int currentg = sNode->father->g + g;
int f = currentg + h;
sNode->f = f;
sNode->h = h;
sNode->g = currentg;
}
bool Astar::compare( Node* n1,Node* n2 )
{
//printf("%d,%d",n1->f,n2->f);
return n1->f < n2->f;
}
bool Astar::unWalk( int x,int y)
{
if (map[x][y] == 1)
return true;
return false;
}
void Astar::printPath( Node* current )
{
if (current->father != NULL)
printPath(current->father);
map[current->x][current->y] = 6;
printf("(%d,%d)",current->x,current->y);
}
void Astar::printMap()
{
for(int i=0;i<=row;i++){
for(int j=0;j<=col;j++){
printf("%d ",map[i][j]);
}
printf("\n");
}
}
main.cpp
// Astar.cpp : Definiert den Einstiegspunkt für die Konsolenanwendung.
//
#include "stdafx.h"
#include "Astar.h"
int _tmain(int argc, _TCHAR* argv[])
{
Astar astar;
Node *startPos = new Node(5,1);
Node *endPos = new Node(3,8);
//astar.printMap();
astar.search(startPos,endPos);
cout<最后的输出：
6的数字就是最终的路径。
参考资料：
[C++]:A*——A Star算法简介
路径规划——A*算法
A*算法 c++实现