用A*算法实现迷宫寻路的最优解

写在前面

本文是我查阅资料参考了他人的算法思路，然后进行记录总结，鄙人水平有限文章中可能会有错误之处，大家做个参考即可，如若有错误希望大家能够坚定自己的想法，别被鄙人带偏。
参考文章：http://www.gamedev.net/reference/articles/article2003.asp

用A*算法实现迷宫寻路的最优解的算法思路

迷宫问题有很多的解法，通常我们使用这两种解法：一种是深度策略(DFS)，另一种是广度策略(BFS)。这两种策略各有各的优缺点：

BFS:对于解决最短或最少问题特别有效，而且寻找深度小，但缺点是内存耗费量大（需要开大量的数组单元用来存储状态）。
DFS：对于解决遍历和求所有问题有效，对于问题搜索深度小的时候处理速度迅速，然而在深度很大的情况下效率不高。

这里我们给出另一种解决迷宫问题的算法：A*算法。

A*算法是一种静态路网中求解最短路径最有效的直接搜索方法，也是许多其他问题的常用启发式算法，借助该算法我们可以实现迷宫问题的最优解的寻找。在介绍具体的算法思路之前，我们要先引入一个成本的概念，我们这里的成本是指做某件事需要花费的时间，我们通过进行成本的比较来不断探索出一条通向终点的最优解，也就是说我们会计算并记录每个位置距离终点所需要的成本，然后在后续的每次前进中选择成本最小的位置去移动。具体的成本计算是依赖于F=G+H这个数学公式的，其中：

G=从起点的位置到所要移动的位置的距离（移动成本）
H=所要移动的位置到终点的距离（搜索成本）
F=移动成本与搜索成本的总和（总成本）

这里我们以下图中迷宫的例子为例介绍一下此过程的实现：

在这里插入图片描述

路径搜索过程

图中A是起点，B是终点，中间蓝色的部分是墙体。我们使用坐标来描述迷宫中的所有位置并使用二维数组direction [8] [2]来存储探索的8个方向,初始化数组为{0,1},{1,1},{1,0},{1,-1},{0,-1},{-1,-1},{-1,0},{-1,1}，这样是固定了探索方向的顺序是从东开始以顺时针方向旋转。我们还需要初始化两个两个链表：openlist(开放列表)与closelist(封闭列表)。开放列表是为了存储待探索的位置，因为我们会将下一次移动中能到达的所有位置都存储在开放列表中，通俗的说它就是一个待检查的列表；封闭列表则是用于存储那些已经检查过的位置元素，在后续的探索中我们将使用它来避免重复探索某一位置。接下里请看详细的实现过程：

从起点A开始，我们将它加入到openlist中
查看A点周围所有的位置，将所有能探索的位置加入到openlist中（能探索的位置即为周围不是墙体，并且未在closelist中出现的元素），然后将A点设置为这些点的父节点。
将A从openlist中取出，再将其存入closelist中去。

如图所示，A周围的八个位置都为可以到达的有效位置所以将他们存入openlist中，并将他们都指向父节点A，用不同的颜色表示A已经从openlist中出栈，并存入closelist中。

接下来按照我们的算法思路，是一直反复遍历openlist，每一次出取出成本（F）最小的点，将其周围可到达的点存入openlist中去，再将其从openlist剔除并存入closelist。

接下来将上述步骤循环重复，循环退出的条件是在openlist中我们可以查找到终点

可以看到这个过程最主要的是计算每个位置的F、G、H的值，因为F的值是G与H的和，所以我们更需要关注的是G与H的计算。G是移动成本，顾名思义就是当前点到所要移动的点的距离，我们假设每个小正方形的边长为10，那么我们可以得到横向或纵向移动的成本为10，对角线移动的成本为√20，近似为14，这里为了寻路更加快速我们放弃使用相关的数学公式来获得精确的距离，并且为了更好的判断与计算我们也不使用14这个近似值，而是使用当前点与所要移动到的点的横纵坐标之差来代表移动成本G，即在上图中A到对角线的距离我们用10+10=20来表示，这种方式近似是不会影响到路径的选择的，因为无论是选择用还是用√20、14、还是20他都只有一个结果三者都大于横纵的移动成本10；H是搜索成本，是指从当前点到终点的距离，需要注意的是在计算H时我们是忽略墙体对两点之间的距离的影响的，只不过这里还是为了优化时间成本我们不使用数学公式去计算两点之间的直线距离而是计算当前位置到终点的横向距离与纵向距离之和，这种方法被称为Manhattan方法，其核心是只计算横向与纵向不考虑对角线。下图是我们第一步执行后的结果，方格的左上方是F的值，左下方是G的值，右下方是H的值：

在这里插入图片描述

通过这张图我们可以直观的理解F、G、H的计算规则，与A横纵相邻的位置G值均为10，对角线的位置上的G值均为20，以A右侧的位置为例，他到B的H为0+30=30。

然后我们继续搜索下一个成本最小的位置，在上图中我们可以很明显的看到A右边的位置的F是最小的，这就是我们上一步进行完之后所需要的移动到的位置，现在以移动后的该点为基础探索他周围的八个方向。

从当前位置S以东开始，顺时针方向依次探索八个方向
- 若探索的位置为墙体或该位置在closelist中则什么也不做，继续探索下一个位置。
- 若探索的位置不在openlist中则将该位置添加到openlist中，继续探索下一个位置。
- 若探索的位置在openlist中则比较该点到点S的距离（G）与该点到其父节点的距离（G’），若G<G’则将点S设置为探索位置的父节点，并将当前位置的G的值与F的值重新计算再存入该位置中；若G>=G’则什么也不做。继续探索下一个位置。
将当前位置S从openlist中剔除，加入到closelist中去。
遍历openlist，找到F最小的点将其设置为S
- 若有多个位置的F相同，我们比较这些位置的H,在这写总成本相同（F）的位置中寻找移动成本（G）最小的点，并将其记为S。
- 若有多个点的F、G均相同则将其这些点中最后一个存入openlist的点设为S。

一直重复上述过程直到终点B可以在openlist中找到即找到了最短路径。最后从终点开始，按着父节点的指向，一步一步你就会顺着一条路径被带回到起点，而这条路径就是我们搜索出来的最优解。

下面是关于F最小的位置的寻找的详细解释：

S是我们当前的位置，我们用灰色表示该位置在closelist中，前面说到蓝色表示的墙体。
在这里插入图片描述
这里有个问题要注意一下，在你将S移动到下一个位置时，他会按照上面的过程遍历周围的八个位置，这就可能会使得周围这八个位置的成本和父节点发生改变。比如当S移动到当前位置后，实际上我们得到的演示图应该是下图这样：
在这里插入图片描述

通过上图我们可以发现探索完当前位置S后有两个位置的F与G的值是相同的，按照上面的思路我们选择S正上方的位置（因为他是后加入openlist中的）。如图：

在这里插入图片描述

后面的步骤都是按照上述所描述的过程进行，最后我们会得到这样一条路径：
在这里插入图片描述

A*算法实现迷宫寻路最优解思路总结

将起点存入openlist中，将起点设为F最小的点min
对min的八个方向进行搜索，符合要求则将min设为他们的父节点并存入openlist，将min从openlist中取出，存入closelist。在探索方向的过程中要注意：
- 若探索的位置为墙体或该位置在closelist中则什么也不做，继续探索下一个位置。
- 若探索的位置不在openlist中则将该位置添加到openlist中，继续探索下一个位置。
- 若探索的位置在openlist中则比较该点到点S的距离（G）与该点到其父节点的距离（G’），若G<G’则将点S设置为探索位置的父节点，并将当前位置的G的值与F的值重新计算再存入该位置中；若G>=G’则什么也不做。继续探索下一个位置。
遍历openlist将其中F最小的值赋给min
遍历openlist查看终点是否在其中
- 若终点不在openlist中则跳转到第二步。
- 若终点在openlist中，路径已找到，顺着终点一直向前寻找父节点即可当得到最优路径。
- 若openlist为空时还没有找到终点，则查找失败，没有路径通向终点。

代码实现

下面是我用C语言写出的代码，鄙人水平有限仅供参考，有错误之处和不足之处希望大家指出！

#include<stdio.h>
#include<stdlib.h>
#include<math.h>

struct Maze
{
    
    
	int x;
	int y;
	int g;
	int h;
	int f;//f=g+h
	struct Maze* next;
	struct Maze* father;
};

struct MAZE_STRU
{
    
    
	int size;
	int** data;
};

typedef struct Maze* PMaze;
typedef struct Maze* LinkStack;
typedef struct MAZE_STRU MAZE;//定义存储迷宫的数组

LinkStack SetNullStack_link()
{
    
    
	LinkStack top = (LinkStack)malloc(sizeof(struct Maze));
	if (top != NULL)
		top->next = NULL;
	else
		printf("Alloc failure");
	return top;

}

int IsNullStack_link(LinkStack top)
{
    
    
	if (top->next == NULL)
		return 1;
	else
		return 0;
}

void Push_link(LinkStack top,PMaze p)//入栈
{
    
    
	p->next = top->next;
	top->next = p;
}

void POP(LinkStack top,PMaze precur)
{
    
    //当前位置出栈（将上一个记录在openlist中的min出栈）
	PMaze pre,p;
	pre = top;
	p = top;
	if (p->next != NULL)
	{
    
    
		while (p != NULL)
		{
    
    
			if (p == precur)
			{
    
    
				pre->next = p->next;
				//return 0;
			}
			pre = p;
			p = p->next;
		}
	}
	else {
    
    
		printf("openlist已空!\n");
	}
}

LinkStack Top_link(LinkStack sstack)//取栈顶元素
{
    
    
	if (IsNullStack_link(sstack))
	{
    
    
		printf("it is empty");
		return 0;
	}
	else
		return sstack->next;
}

PMaze InputCoordinate(int x,int y,int g,int h,int f)//将坐标,成本存入指向当前位置的节点
{
    
    
	LinkStack top = (LinkStack)malloc(sizeof(struct Maze));
	if (top != NULL)
	{
    
    
		top->x = x;
		top->y = y;
		top->g = g;
		top->h = h;
		top->f = f;
		top->next = NULL;
	}
	else
		printf("Alloc failure");
	return top;
}

int Exist(LinkStack head,PMaze p)
{
    
    
	PMaze q=head->next;
	while (q != NULL)
	{
    
    
		if ((q->x == p->x) && (q->y == p->y)) return 1;
		q = q->next;
	}
	return 0;
}

PMaze min_pmaze(LinkStack top)
{
    
    
	PMaze q, p = top->next;
	q = p;
	while (p != NULL)
	{
    
    
		if (q->f > p->f)
		{
    
    
			q = p;
		}
		if (q->f == p->f)
		{
    
    
			if (q->h > p->h)
			{
    
    
				q = p;
			}
		}
		p = p->next;
	}
	return q;
}


int maze_A(int entryX, int entryY, int exitX, int exitY, struct MAZE_STRU* MAZE)
{
    
    
	//A*算法探索迷宫
	//定义了八个方向
	int direction[8][2] = {
    
     {
    
    0,1},{
    
    1,1},{
    
    1,0},{
    
    1,-1},{
    
    0,-1},{
    
    -1,-1},{
    
    -1,0},{
    
    -1,1} };
	int posX=entryX, posY=entryY;//临时变量，储存坐标（x,y）
	int preposX = entryX, preposY = entryY;//用于存储前一个位置的坐标（x,y）
	//int i, j;//循环变量
	int mov;//移动的方向
	int g, h, f;//临时存储g,h

	PMaze cur;//指向当前位置
	PMaze precur;//存储前一个位置
	PMaze min;//用于存储具有最小的f的位置

	LinkStack openlist = SetNullStack_link();//定义开放列表
	LinkStack closelist = SetNullStack_link();//定义封闭列表

	g = abs(posX - preposX) + abs(posY - preposY);
	h = abs(exitX - posX) + abs(exitY - posY);
	f = g + h;
	cur = InputCoordinate(entryX, entryY, g, h, f);
	cur->father = NULL;
	Push_link(openlist, cur);
	min = cur;//赋予初始位置为起点位置
	 
	while (!IsNullStack_link(openlist))
	{
    
    
		precur = min;
		//printf("((%d %d)g:%d h:%d f:%d)\n", min->x, min->y,min->g,min->h,min->f);
		//上一条注释是用于调试时来查看每一个最小成本的位置的具体信息
		preposX = precur->x;//记录当前位置的X坐标（下一个位置的前一个位置）
		preposY = precur->y;//记录当前位置的Y坐标（下一个位置的前一个位置）
		POP(openlist, precur);//出栈,入closelist
		Push_link(closelist, precur);
		mov = 0;//设置初始探索方向
		while(mov<8)
		{
    
    
			posX = preposX + direction[mov][0];
			posY = preposY + direction[mov][1];

			if (posX == exitX && posY == exitY)//openlist中有出口，则表示到达终点
			{
    
    
				g = abs(posX - preposX) + abs(posY - preposY);
				h = abs(exitX - posX) + abs(exitY - posY);
				f = g + h;
				cur = InputCoordinate(exitX, exitY, g, h, f);
				cur->father = precur;
				Push_link(openlist, cur);//进入openlist
				printf("\nA*算法搜索出的最优解为:\n");
				PMaze temp = Top_link(openlist);
				while (temp->father!=NULL)
				{
    
    
					printf("(%d %d) ", temp->x, temp->y);
					temp = temp->father;
				}
				printf("(%d %d) ", entryX, entryY);
				return 1;
			}
			g = abs(posX - preposX) + abs(posY - preposY);
			h = abs(exitX - posX) + abs(exitY - posY);
			f = g + h;
			cur = InputCoordinate(posX, posY, g, h, f);
			if (!(MAZE->data[posX][posY])  && !Exist(closelist, cur) ) //判断是否是墙体,是否在closelist中
			 {
    
    
				if (!Exist(openlist, cur))//不在则将当前位置存入openlist
				{
    
    
					cur->father = precur;
					Push_link(openlist, cur);//进栈
				}
				else
				{
    
    //若当前点到min的位置短于到原父亲节点的位置的距离，则将当前位置的父节点更改为min
					if (cur->g < precur->g)
					{
    
    
						cur->father = min;
					}
				}//否则什么也不做
			}
			mov++;
		} 
		min = min_pmaze(openlist);// 每次遍历找f最小存入min
	}
	return 0;
}
/*    定义创建迷宫的函数    */
MAZE* InitMaze(int size)//创建迷宫
{
    
    
	int i;
	MAZE* maze = (MAZE*)malloc(sizeof(struct MAZE_STRU));
	maze->size = size;
	//给迷宫分配空间
	maze->data = (int**)malloc(sizeof(int*) * maze->size);
	for (i = 0; i < maze->size; i++)
	{
    
    
		maze->data[i] = (int*)malloc(sizeof(int) * maze->size);
	}
	return maze;
}

void ReadMaze(MAZE* maze)
{
    
    
	int i, j;
	printf("请用矩阵的形式输入迷宫的结构：\n");
	for (i = 0; i < maze->size; i++)
	{
    
    
		for (j = 0; j < maze->size; j++)
			scanf_s("%d", &maze->data[i][j]);
	}
}

void WriteMaze(MAZE* maze)
{
    
    
	int i, j;
	printf("迷宫的结构如下：\n");
	for (i = 0; i < maze->size; i++)
	{
    
    
		for (j = 0; j < maze->size; j++)
			printf("%5d", maze->data[i][j]);
		printf("\n");
	}
}

int main()
{
    
    
	MAZE* maze;
	int A;//用于存储函数调用返回值，借此来判断迷宫是否有路径通向终点
	int mazeSize;
	int entryX, entryY, exitX, exitY;
	printf("请输入迷宫大小：\n");
	scanf_s("%d", &mazeSize);
	maze = InitMaze(mazeSize);
	ReadMaze(maze);
	printf("请输入入口点与出口点的坐标：\n");
	scanf_s("%d%d%d%d", &entryX, &entryY, &exitX, &exitY);
	WriteMaze(maze);//打印迷宫
	A = maze_A(entryX, entryY, exitX, exitY, maze);//调用A*
	if (A == 0) printf("\n该迷宫没有路径通向终点！\n");
	return 0;
}

个人给出的测试用例，鼓励大家多使用其他的测试用例来检验代码的正确性。

/*测试输入
9
1 1 1 1 1 1 1 1 1
1 0 0 1 1 0 1 1 1
1 1 0 0 0 0 0 0 1
1 0 1 0 0 1 1 1 1
1 0 1 1 1 0 0 1 1
1 1 0 0 1 0 0 0 1
1 0 1 1 0 0 0 1 1
1 1 1 1 1 1 1 0 1
1 1 1 1 1 1 1 1 1
1 1 7 7
------------------
8
1 1 1 1 1 1 1 1
1 0 0 0 0 0 0 1
1 0 0 0 0 1 0 1
1 0 0 0 0 1 0 1
1 0 0 0 0 1 0 1
1 0 0 0 0 0 0 1
1 0 0 0 0 0 0 1
1 1 1 1 1 1 1 1
3 2 3 6
*/

结尾

还是希望大家能够自己独立的完成代码的设计，这也是锻炼自己的机会，在不断练习中把所学到的知识融会贯通。代码中若有错误的地方欢迎大家指正！

用A*算法实现迷宫寻路的最优解

目录

写在前面

用A*算法实现迷宫寻路的最优解的算法思路

A*算法实现迷宫寻路最优解思路总结

代码实现

结尾

猜你喜欢