一、介绍A*算法
学过数据结构的人都知道,图的搜索算法主要有深度优先算法和广度优先算法。广度优先是从初始节点一层一层向下找,直到找到目标为止。深度优先是按照一定的顺序前查找完节点的一个分支,再查找另一个分支,以至找到目标为止。在广度优先中,整个搜索好似一个圆形向外展开,直到到达目标节点,这样求解虽然一定能找到最优解,但节点展开的数量是和距离成级数增加的, 这对计算机内存和CPU资源要求非常大,运算效率太低,可以说这个算法并不实用。
这时可考虑启发式搜索算法。而 A* 算法实际就是一种启发式搜索。
所谓启发式搜索,就是利用一个估价函数评估每次的的决策的价值,决定先尝试哪一种方案, 这样可以极大的优化普通的广度优先搜索。举例来说,从出发点(A)到目的地(B)的最短距离是固定的,我们可以写一个函数 Judge() 估计(A)到(B)的最短距离,如果程序已经尝试着从出发点(A) 沿着某条路线移动到了(C) 点,那么我们认为这个方案的(A)到(B)间的估计距离为(A) 到(C) 实际已经行走了的距离(H)加上用 Judge() 估计出的(C) 到(B) 的距离。 如此,无论我们的程序搜索展开到哪一步,都会算出一个评估值, 每一次决策后,将评估值和等待处理的方案一起排序,然后挑出待处理的各个方案中最有可能是最短路线的一部分的方案展开到下一步,一直循环到对象移动到目的地,或所有方案都尝试过却没有找到一条通向目的地的路径则结束。
由此可见,在启发式搜索中,对距离的估价是十分重要的。采用了不同的估价策略可以有不同的效果。我们先看看估价是如何表示的。
启发中的估价是用估价函数表示的,如:
f(n) = g(n) + h(n)
其中f(n)是节点n的估价函数,g(n)实在状态空间中从初始节点到n节点的实际代价,h(n)是从n到目标节点最佳路径的估计代价。在这里主要是h(n)体现了搜索的启发信息,因为g(n)是已知的。如果说详细点,g(n)代表了搜索的广度的优先趋势。
在启发式搜索中的估价函数非常的重要,如何保证一定能找到最短路径呢?充要条件是, 估价函数算出的两点间的距离必须小于等于实际距离。这个可以从数学上严格证明。有如果你的估价函数不满足这点,就只能叫做 A 算法,并不能保证最后的结果是最优的,但它可能速度非常的快,所以在不一定要求最优解的问题中,也有实用性。 但无疑,满足上叙充要条件的 A* 算法中,估计值越接近真实值的估价函数就做的越好。
二、A*算法伪代码
如图有如下的状态空间:(起始位置是A,目标位置是P,字母后的数字表示节点的估价值)
搜索过程中设置两个表:OPEN和CLOSED。OPEN表保存了所有已生成而未考察的节点,CLOSED表中记录已访问过的节点。算法中有一步是根据估价函数重排OPEN表。这样循环中的每一步只考虑OPEN表中状态最好的节点。具体搜索过程如下:
1)初始状态:
OPEN=[A5];CLOSED=[];
2)估算A5,取得搜有子节点,并放入OPEN表中;
OPEN=[B4,C4,D6];CLOSED=[A5]
3)估算B4,取得搜有子节点,并放入OPEN表中;
OPEN=[C4,E5,F5,D6];CLOSED=[B4,A5]
4)估算C4;取得搜有子节点,并放入OPEN表中;
OPEN=[H3,G4,E5,F5,D6];CLOSED=[C4,B4,A5]
5)估算H3,取得搜有子节点,并放入OPEN表中;
OPEN=[O2,P3,G4,E5,F5,D6];CLOSED=[H3,C4,B4,A5]
6)估算O2,取得搜有子节点,并放入OPEN表中;
OPEN=[P3,G4,E5,F5,D6];CLOSED=[O2,H3,C4,B4,A5]
7)估算P3,已得到解;
看了具体的过程,再看看伪程序吧。算法的伪程序如下:
Best_First_Search()
{
Open = [起始节点];
Closed = [];
while (Open表非空)
{
从Open中取得一个节点X,并从OPEN表中删除。
if (X是目标节点)
{
求得路径PATH;
返回路径PATH;
}
for (每一个X的子节点Y)
{
if (Y不在OPEN表和CLOSE表中)
{
求Y的估价值;
并将Y插入OPEN表中;
}
//还没有排序
else if (Y在OPEN表中)
{
if (Y的估价值小于OPEN表的估价值)
更新OPEN表中的估价值;
}
else //Y在CLOSE表中
{
if (Y的估价值小于CLOSE表的估价值)
{
更新CLOSE表中的估价值;
从CLOSE表中移出节点,并放入OPEN表中;
}
}
将X节点插入CLOSE表中;
按照估价值将OPEN表中的节点排序;
}
}
}
三、A*算法实现(一)
void AstarPathfinder::FindPath(int sx, int sy, int dx, int dy)
{
NODE *Node, *BestNode;
int TileNumDest;
//得到目标位置,作判断用
TileNumDest = TileNum(sx, sy);
//生成Open和Closed表
OPEN = ( NODE* )calloc(1,sizeof( NODE ));
CLOSED=( NODE* )calloc(1,sizeof( NODE ));
//生成起始节点,并放入Open表中
Node=( NODE* )calloc(1,sizeof( NODE ));
Node->g = 0;
//这是计算h值
// should really use sqrt().
Node->h = (dx-sx)*(dx-sx) + (dy-sy)*(dy-sy);
//这是计算f值,即估价值
Node->f = Node->g+Node->h;
Node->NodeNum = TileNum(dx, dy);
Node->x = dx; Node->y = dy;
// make Open List point to first node
OPEN->NextNode=Node;
for (;;)
{
//从Open表中取得一个估价值最好的节点
BestNode=ReturnBestNode();
//如果该节点是目标节点就退出
// if we've found the end, break and finish break;
if (BestNode->NodeNum == TileNumDest)
//否则生成子节点
GenerateSuccessors(BestNode,sx,sy);
}
PATH = BestNode;
}
//生成子节点函数:
void AstarPathfinder::GenerateSuccessors(NODE *BestNode, int dx, int dy)
{
int x, y;
//哦!依次生成八个方向的子节点,简单!
// Upper-Left
if ( FreeTile(x=BestNode->x-TILESIZE, y=BestNode->y-TILESIZE) )
GenerateSucc(BestNode,x,y,dx,dy);
// Upper
if ( FreeTile(x=BestNode->x, y=BestNode->y-TILESIZE) )
GenerateSucc(BestNode,x,y,dx,dy);
// Upper-Right
if ( FreeTile(x=BestNode->x+TILESIZE, y=BestNode->y-TILESIZE) )
GenerateSucc(BestNode,x,y,dx,dy);
// Right
if ( FreeTile(x=BestNode->x+TILESIZE, y=BestNode->y) )
GenerateSucc(BestNode,x,y,dx,dy);
// Lower-Right
if ( FreeTile(x=BestNode->x+TILESIZE, y=BestNode->y+TILESIZE) )
GenerateSucc(BestNode,x,y,dx,dy);
// Lower
if ( FreeTile(x=BestNode->x, y=BestNode->y+TILESIZE) )
GenerateSucc(BestNode,x,y,dx,dy);
// Lower-Left
if ( FreeTile(x=BestNode->x-TILESIZE, y=BestNode->y+TILESIZE) )
GenerateSucc(BestNode,x,y,dx,dy);
// Left
if ( FreeTile(x=BestNode->x-TILESIZE, y=BestNode->y) )
GenerateSucc(BestNode,x,y,dx,dy);
}
void AstarPathfinder::GenerateSucc(NODE *BestNode,int x, int y, int dx, int dy)
{
int g, TileNumS, c = 0;
NODE *Old, *Successor;
//计算子节点的 g 值
// g(Successor)=g(BestNode)+cost of getting from BestNode to Successor
g = BestNode->g+1;
// identification purposes
TileNumS = TileNum(x,y);
//子节点再Open表中吗?
// if equal to NULL then not in OPEN list, else it returns the Node in Old
if ( (Old=CheckOPEN(TileNumS)) != NULL )
{
//若在
for( c = 0; c < 8; c++)
// Add Old to the list of BestNode's Children (or Successors).
if( BestNode->Child[c] == NULL )
break;
BestNode->Child[c] = Old;
//比较Open表中的估价值和当前的估价值(只要比较g值就可以了)
// if our new g value is < Old's then reset Old's parent to point to BestNode
if ( g < Old->g )
{
//当前的估价值小就更新Open表中的估价值
Old->Parent = BestNode;
Old->g = g;
Old->f = g + Old->h;
}
}
else
//在Closed表中吗?
// if equal to NULL then not in OPEN list, else it returns the Node in Old
if ( (Old=CheckCLOSED(TileNumS)) != NULL )
{
//若在
for( c = 0; c< 8; c++)
// Add Old to the list of BestNode's Children (or Successors).
if ( BestNode->Child[c] == NULL )
break;
BestNode->Child[c] = Old;
//比较Closed表中的估价值和当前的估价值(只要比较g值就可以了)
// if our new g value is < Old's then reset Old's parent to point to BestNode
if ( g < Old->g )
{
//当前的估价值小就更新Closed表中的估价值
Old->Parent = BestNode;
Old->g = g;
Old->f = g + Old->h;
//再依次更新Old的所有子节点的估价值
// Since we changed the g value of Old, we need
// to propagate this new value downwards, i.e.
// do a Depth-First traversal of the tree!
PropagateDown(Old);
}
}
//不在Open表中也不在Close表中
else
{
//生成新的节点
Successor = ( NODE* )calloc(1,sizeof( NODE ));
Successor->Parent = BestNode;
Successor->g = g;
// should do sqrt(), but since we don't really
Successor->h = (x-dx)*(x-dx) + (y-dy)*(y-dy);
// care about the distance but just which branch looks
Successor->f = g+Successor->h;
// better this should suffice. Anyayz it's faster.
Successor->x = x;
Successor->y = y;
Successor->NodeNum = TileNumS;
//再插入Open表中,同时排序。
// Insert Successor on OPEN list wrt f
Insert(Successor);
for( c =0; c < 8; c++)
// Add Old to the list of BestNode's Children (or Successors).
if ( BestNode->Child[c] == NULL )
break;
BestNode->Child[c] = Successor;
}
}
四、A*算法实现(二)
/* 云风的求解最短路径代码 (Cloud Wu's Pathfinding code)*/
#define MAPMAXSIZE 100 //地图面积最大为 100x100
#define MAXINT 8192 //定义一个最大整数, 地图上任意两点距离不会超过它
#define STACKSIZE 65536 //保存搜索节点的堆栈大小
#define tile_num(x,y) ((y)*map_w+(x)) //将 x,y 坐标转换为地图上块的编号
#define tile_x(n) ((n)%map_w) //由块编号得出 x,y 坐标
#define tile_y(n) ((n)/map_w)
// 树结构, 比较特殊, 是从叶节点向根节点反向链接
typedef struct node *TREE;
struct node {
int h;
int tile;
TREE father;
} ;
typedef struct node2 *LINK;
struct node2 {
TREE node;
int f;
LINK next;
};
LINK queue; // 保存没有处理的行走方法的节点
TREE stack[STACKSIZE]; // 保存已经处理过的节点 (搜索完后释放)
int stacktop;
unsigned char map[MAPMAXSIZE][MAPMAXSIZE]; //地图数据
int dis_map[MAPMAXSIZE][MAPMAXSIZE]; //保存搜索路径时,中间目标地最优解
int map_w,map_h; //地图宽和高
int start_x,start_y,end_x,end_y; //地点,终点坐标
// 初始化队列
void init_queue()
{
queue=(LINK)malloc(sizeof(*queue));
queue->node=NULL;
queue->f=-1;
queue->next=(LINK)malloc(sizeof(*queue));
queue->next->f=MAXINT;
queue->next->node=NULL;
queue->next->next=NULL;
}
// 待处理节点入队列, 依靠对目的地估价距离插入排序
void enter_queue(TREE node,int f)
{
LINK p=queue,father,q;
while(f>p->f) {
father=p;
p=p->next;
assert(p);
}
q=(LINK)malloc(sizeof(*q));
assert(queue);
q->f=f,q->node=node,q->next=p;
father->next=q;
}
// 将离目的地估计最近的方案出队列
TREE get_from_queue()
{
TREE bestchoice=queue->next->node;
LINK next=queue->next->next;
free(queue->next);
queue->next=next;
stack[stacktop++]=bestchoice;
assert(stacktop<STACKSIZE);
return bestchoice;
}
// 释放栈顶节点
void pop_stack()
{
free(stack[--stacktop]);
}
// 释放申请过的所有节点
void freetree()
{
int i;
LINK p;
for (i=0;i<stacktop;i++)
free(stack[i]);
while (queue) {
p=queue;
free(p->node);
queue=queue->next;
free(p);
}
}
// 估价函数,估价 x,y 到目的地的距离,估计值必须保证比实际值小
int judge(int x,int y)
{
int distance;
distance=abs(end_x-x)+abs(end_y-y);
return distance;
}
// 尝试下一步移动到 x,y 可行否
int trytile(int x,int y,TREE father)
{
TREE p=father;
int h;
if (map[y][x]!=' ') return 1; // 如果 (x,y) 处是障碍,失败
while (p) {
if (x==tile_x(p->tile) && y==tile_y(p->tile)) return 1; //如果 (x,y) 曾经经过,失败
p=p->father;
}
h=father->h+1;
if (h>=dis_map[y][x]) return 1; // 如果曾经有更好的方案移动到 (x,y) 失败
dis_map[y][x]=h; // 记录这次到 (x,y) 的距离为历史最佳距离
// 将这步方案记入待处理队列
p=(TREE)malloc(sizeof(*p));
p->father=father;
p->h=father->h+1;
p->tile=tile_num(x,y);
enter_queue(p,p->h+judge(x,y));
return 0;
}
// 路径寻找主函数
void findpath(int *path)
{
TREE root;
int i,j;
stacktop=0;
for (i=0;i<map_h;i++)
for (j=0;j<map_w;j++)
dis_map[i][j]=MAXINT;
init_queue();
root=(TREE)malloc(sizeof(*root));
root->tile=tile_num(start_x,start_y);
root->h=0;
root->father=NULL;
enter_queue(root,judge(start_x,start_y));
for (;;) {
int x,y,child;
TREE p;
root=get_from_queue();
if (root==NULL) {
*path=-1;
return;
}
x=tile_x(root->tile);
y=tile_y(root->tile);
if (x==end_x && y==end_y) break; // 达到目的地成功返回
child=trytile(x,y-1,root); //尝试向上移动
child&=trytile(x,y+1,root); //尝试向下移动
child&=trytile(x-1,y,root); //尝试向左移动
child&=trytile(x+1,y,root); //尝试向右移动
if (child!=0)
pop_stack(); // 如果四个方向均不能移动,释放这个死节点
}
// 回溯树,将求出的最佳路径保存在 path[] 中
for (i=0;root;i++) {
path[i]=root->tile;
root=root->father;
}
path[i]=-1;
freetree();
}
void printpath(int *path)
{
int i;
for (i=0;path[i]>=0;i++) {
gotoxy(tile_x(path[i])+1,tile_y(path[i])+1);
cprintf("/xfe");
}
}
int readmap()
{
FILE *f;
int i,j;
f=fopen("map.dat","r");
assert(f);
fscanf(f,"%d,%d/n",&map_w,&map_h);
for (i=0;i<map_h;i++)
fgets(&map[i][0],map_w+1,f);
fclose(f);
start_x=-1,end_x=-1;
for (i=0;i<map_h;i++)
for (j=0;j<map_w;j++) {
if (map[i][j]=='s') map[i][j]=' ',start_x=j,start_y=i;
if (map[i][j]=='e') map[i][j]=' ',end_x=j,end_y=i;
}
assert(start_x>=0 && end_x>=0);
return 0;
}
void showmap()
{
int i,j;
clrscr();
for (i=0;i<map_h;i++) {
gotoxy(1,i+1);
for (j=0;j<map_w;j++)
if (map[i][j]!=' ') cprintf("/xdb");
else cprintf(" ");
}
gotoxy(start_x+1,start_y+1);
cprintf("s");
gotoxy(end_x+1,end_y+1);
cprintf("e");
}
int main()
{
int path[MAXINT];
readmap();
showmap();
getch();
findpath(path);
printpath(path);
getch();
return 0;
}