A*算法求解迷宫最短路问题（C++，VScode）

A*算法求解迷宫最短路问题（C++，VScode）

开始正文之前，先介绍一篇对我帮助较大一篇博客： A*算法讲解

一、算法思想及实现思路

1.估价函数：

F = G + H，起点为S，终点为E，从S到E经过节点N，S->N所要耗费的代价为已知代价G，N->E所要耗费的实际代价未知，因为当前在节点N，还未走到终点，所以未知路径，代价也未知，但可以设计启发函数来估计N到E要耗费的代价H，从而得出S->N->E所要耗费的代价的估计值F = G + H。

2.open表，closed表：

起初，将起点放到open表中，对open表初始化，因为可以在地图上斜着走，所以对节点扩展时，设当前正在扩展的节点为N，要沿八个方向按顺序依次访问N的相邻八个节点，设相邻八个节点的某个节点为M，访问N周围节点时会遇到以下三种情况：

（1）节点在之前从未被访问过，即节点既不在open表，也不在closed表，此时节点放到open表中，放入之前，应先计算M的F，G，H值，并把当前正在被扩展的节点N作为它的父节点，M的G值是由父节点的G值加上父节点到M的实际距离（二者相邻），再根据估价函数计算H值，再计算F值，根据F值将M放入到open表的正确位置，open表中排在前面的，估计代价F值就越小。

（2）节点在之前被访问过，但未被扩展，即节点在open表，但不在closed表，此时可能要对M的G，F值更新，H值不变，因为它在地图的位置没变，相对终点位置没变，因为M在之前被访问过，所以它已有父节点，但不是当前正在被扩展的节点N，此时，计算N->M要耗费的代价，再加上N的G值，即为S->N->M所要耗费的实际代价，比较新G值与旧G值，若新<旧，则更新M的节点信息，将N作为它的父节点，不在以之前的父节点继续作为当下的父节点，并将旧G值用新G值覆盖，重新计算F值，然后根据F值调整M在open表中的位置（往前调）。

（3）节点在之前已被扩展过，即节点不在open表，在closed表，此时可以直接忽略M，转到N的下一个相邻节点。

当找到终点，且终点被放到closed表中，即可结束搜索，根据子节点存储的父节点信息，往回走，便能得到最短路。

二、问题求解所用到的方法以及数据结构

1.优先队列

根据open表的特性，优先队列正好能和它相匹配，优先队列里的每一个元素都是一个结构体变量，存储自己的行列，以及F，G，H值，当然，需要对运算符重载，保证入队的元素是根据F值进行排序的。

2.二维结构体数组

（1）作用：用来存储每一个节点的信息：父节点的位置，及自身的F，G，H值；记录最短路径，终点扩展后，根据该数组，便可从终点返回至起点。

（2）使用原因：优先队列每次只能修改或读取队首这一个节点，其他节点无法修改，只能出队才能遍历，很不方便，不能反复地、灵活地对队列中的任意一个节点进行修改或读取，也正是如此，如果队列中地节点信息要更新（说明从其它路径到该节点的G值更小），则直接创建一个新的节点，新旧节点均指向地图上的同一位置，即，某一位置的节点信息已经在队列中后，若要对节点信息修改，由于队列只能取队首，不能取到其他节点，所以，最好的做法便是重新为该位置创建新节点，放到队列中，新节点的F值比旧节点的F值更小，故而拍在队列的前面，排在前面的优先扩展，扩展后的节点所对应的位置，该位置的其他在队列中的节点不在扩展，直接跳过，因为那已经没有利用价值了。

3.估价函数的设计

（1）曼哈顿法：

节点到终点的行之差的绝对值+列之差的绝对值:
$$|r1-en{d}_r|+|c1-en{d}_c|$$

（2）欧氏距离：

两点之间直线距离:
$$sqrt(pow((r1-en{d}_r),2)+pow((c1-en{d}_c),2))$$

（3）对角化距离：

以节点与终点的为对角线构成的矩形，选取最大正方形（顶点含节点），取其对角线，再从节点相对的点出发，横/竖着走到终点：

三、源码

1.map数组：用于将结果打印，实现可视化（C++可视化粗糙，暂时未去寻找更好的可视化方法）。

2.q数组：二维结构体数组，作用上面已述。

3.heap：优先队列，实现open表，功能已述。

4.index数组:存储8个方向的增量

5.visited数组：标志位数组，=0，为遍历；=1，已遍历，但未扩展；=2，已扩展；

6.FindWay函数：A*算法主要实现函数

创建障碍物这部分比较简陋，不是算法核心，随便设置了一些。

#include <math.h> 
#include <stdlib.h>
#include "iostream"
#include <queue>
#include<bits/stdc++.h> 
using namespace std;

struct Node{
	int row,col;
	int f,g,h;
	Node(int r,int c,int ff,int gg,int hh):row(r),col(c),f(ff),g(gg),h(hh){}
	//push(int r,int c,int ff,int gg,int hh):row(r),col(c),f(ff),g(gg),h(hh){}
};

typedef struct info{
	int row,col;
	int f,g,h;
}Info;

struct cmp{
	bool operator()(Node a,Node b){
		return a.f > b.f;
	}
};

typedef struct Ind{
	int r,c;
}Index;


#define n 90
#define m 100
priority_queue<Node,vector<Node>,cmp> heap;

void create_Maze(char map[][m+2], int visited[][m+2], int start_r, int start_c, int end_r, int end_c)
{
//	memset(map, 0, sizeof map);
//	memset(visited, 0, sizeof visited);
	for(int i=1,j=1;i<=n;i++)
		{	j=1;
			for(;j<=m;j++)
			{
				visited[i][j] = 0;
				map[i][j] = ':';
			}
		}
	
	for(int j=0;j<m+2;j++)
	{
		map[0][j] = '#';
		visited[0][j] = 2;
		map[n+1][j] = '#';
		visited[n+1][j] = 2;
	}
	for(int i=0;i<n+2;i++)
	{
		map[i][0] = '#';
		visited[i][0] = 2;
		map[i][m+1] = '#';
		visited[i][m+1] = 2;
	}
	visited[start_r][start_c] = 1;
	map[start_r][start_c] = 'S';
	map[end_r][end_c] = 'E';
	// visited[4][4] = 2;
	// visited[4][5] = 2;
	// visited[4][6] = 2;
	//打印 
	// for(int i=0,j=0;i<n+2;i++)
	// 	{
	// 		for(;j<m+2;j++)
	// 		{
	// 			cout<<visited[i][j];
	// 		}
	// 		cout<<endl;
	// 		j=0;
	// 	}
	
	//设置障碍物
	for(int j=4; j<15; j++)
	{
		visited[8][j] = 2;
		map[8][j] = '#';
	} 
	for(int i=8; i<15; i++)
	{
		visited[i][14] = 2;
		map[i][14] = '#';
	}
	for(int i=45;i<60;i++)
	{
		visited[i][35] = 2;
		map[i][35] = '#';
	}
	for(int j=35;j<70;j++)
	{
		visited[60][j] = 2;
		map[60][j] = '#';
	}
	for(int i=50;i<70;i++)
	{
		visited[i][35] = 2;
		map[i][35] = '#';
	}
	for(int j=55;j<77;j++)
	{
		visited[60][j] = 2;
		map[60][j] = '#';
	}
	for(int i=5;i<8;i++)
	{
		visited[i][10] = 2;
		map[i][10] = '#';
	}
	for(int j=60; j<m+2;j++)
	{
		visited[80][j] = 2;
		map[80][j] = '#';
	}
	for(int j=20; j<35; j++)
	{
		visited[60][j] = 2;
		map[60][j] = '#';
	}
}

void create_Ind(Index index[])
{
	index[0].r = 0;index[0].c = 1;//右
	index[1].r = 1;index[1].c = 1;//右下
	index[2].r = 1;index[2].c = 0;//下
	index[3].r = 1;index[3].c = -1;//左下
	index[4].r = 0;index[4].c = -1;//左
	index[5].r = -1;index[5].c = -1;//左上
	index[6].r = -1;index[6].c = 0;//上
	index[7].r = -1;index[7].c = 1;//右上
}

void FindWay(Index index[], Info q[][m+2], int end_r, int end_c, char map[][m+2], int visited[][m+2])
{

	while(visited[end_r][end_c] < 2 && !heap.empty())
	{
		
		for(int i=0; i<8 && visited[heap.top().row][heap.top().col]<2; i++)
		{
			//cout<<i<<' '<<heap.top().row<<' '<<heap.top().col<<' '<<heap.top().f<<' '<<heap.top().g<<' '<<heap.top().h<<endl;
			//cout<<i;
			//if( visited[heap.top().row+index[i].r][heap.top().col+index[i].c] == 2); 
			if( visited[heap.top().row+index[i].r][heap.top().col+index[i].c] == 1)
			{
				int f,g,h;
				if(index[i].r*index[i].c == 0)
					g = heap.top().g + 10;//横着走或竖着走，g值加10 
				else g = heap.top().g +14;//斜着走，g值加14 
				
				if(g < q[heap.top().row+index[i].r][heap.top().col+index[i].c].g)
				{//更新已遍历过的子节点信息 
					h = q[heap.top().row+index[i].r][heap.top().col+index[i].c].h;
					f = g + h;
					heap.push( Node(heap.top().row+index[i].r, heap.top().col+index[i].c, f, g, h) );
					q[heap.top().row+index[i].r][heap.top().col+index[i].c].g = g;
					q[heap.top().row+index[i].r][heap.top().col+index[i].c].f = f;
					q[heap.top().row+index[i].r][heap.top().col+index[i].c].row = heap.top().row;
					q[heap.top().row+index[i].r][heap.top().col+index[i].c].col = heap.top().col;
				}
			}
			
			if( visited[heap.top().row+index[i].r][heap.top().col+index[i].c] == 0)
			{
				int f,g,h;
				if(index[i].r*index[i].c == 0)
					g = heap.top().g + 10;//横着走或竖着走，g值加10 
				else g = heap.top().g +14;//斜着走，g值加14
				//h = abs(end_r - (heap.top().row+index[i].r))*10 + abs(end_c - (heap.top().col+index[i].c))*10; //曼哈顿距离
				//h = 10*sqrt((end_r - (heap.top().row+index[i].r))*(end_r - (heap.top().row+index[i].r)) + (end_c - (heap.top().col+index[i].c))*(end_c - (heap.top().col+index[i].c)));//两点间直线距离
				h = abs((heap.top().row+index[i].r)-end_r)==abs((heap.top().col+index[i].c)-end_c)?(14*abs((heap.top().col+index[i].c)-end_c)):(abs((heap.top().row+index[i].r)-end_r)>abs((heap.top().col+index[i].c)-end_c)?(4*abs((heap.top().col+index[i].c)-end_c)+10*abs((heap.top().row+index[i].r)-end_r)):(4*abs((heap.top().row+index[i].r)-end_r)+10*abs((heap.top().col+index[i].c)-end_c)));//对角化距离
				f = g + h; 
				heap.push(Node(heap.top().row+index[i].r, heap.top().col+index[i].c, f, g, h));//子节点入队 
				//q[heap.top().row+index[i].r][heap.top().col+index[i].c].push();
				//将子节点的信息保存到二维结构体数组q中 
				q[heap.top().row+index[i].r][heap.top().col+index[i].c].row = heap.top().row;
				q[heap.top().row+index[i].r][heap.top().col+index[i].c].col = heap.top().col;
				q[heap.top().row+index[i].r][heap.top().col+index[i].c].f = f;
				q[heap.top().row+index[i].r][heap.top().col+index[i].c].g = g;
				q[heap.top().row+index[i].r][heap.top().col+index[i].c].h = h;
				//if (i==5) cout<<i<<index[i].r<<' '<<index[i].c<<' '<<' '<<heap.top().row+index[i].r<<' '<<heap.top().col+index[i].c<<' '<<q[heap.top().row+index[i].r][heap.top().col+index[i].c].f<<endl;
				visited[heap.top().row+index[i].r][heap.top().col+index[i].c] = 1; //表示子节点已遍历，进入队列 
				map[heap.top().row+index[i].r][heap.top().col+index[i].c] = '=';
			}
		}
		//cout<<heap.top().row<<' '<<heap.top().col<<endl;
		visited[heap.top().row][heap.top().col] = 2; //当前节点已扩展，该弹出； 
		heap.pop();
		//cout<<heap.top().row<<' '<<heap.top().col<<endl;
	}
}

void printWay(Info q[][m+2], int end_r, int end_c, int start_r, int start_c, char map[][m+2])
{
	int r = end_r;
	int c = end_c;
	int i = r;
	int j = c;
	//cout<<q[3][4].row<<' '<<q[3][4].col<<endl;
	//cout<<q[4][5].row<<' '<<q[4][5].col<<endl;
	//cout<<"M\n";
	cout<<"最短路（从终点返回起点）：";
	while(r!=start_r || c!=start_c)
	{
		cout<<r<<' '<<c<<endl;
		i = q[r][c].row;
		j = q[r][c].col;
		map[q[r][c].row][q[r][c].col] = '*';
		r = i;
		c = j;

	}
	map[start_r][start_c] = 'S';
	map[end_r][end_c] = 'E';
	//cout<<'T';
	cout<<r<<' '<<c<<endl;
	for(int i=0; i<n+2; i++)
	{
		for(int j=0; j<=m+2; j++)
		{
			cout<<map[i][j];
		}
		cout<<endl;
	}
}

int main()
{	
	//cout<<"输入尺寸"; 
	//cin>>n>>m;
	
	//
	int start_r,start_c;//起点 
	int end_r,end_c;//终点位置
	cout<<"输入起点位置：";
	cin>>start_r>>start_c;
	cout<<"输入终点位置：";
	cin>>end_r>>end_c;
	// int c1 = abs(start_r - end_r)*10;//曼哈顿法
	// int c2 = abs(start_c - end_c)*10;
	//int h = 10*sqrt((start_r - end_r)*(start_r - end_r) + (start_c - end_c)*(start_c - end_c));//两点间直线距离
	int h = abs(start_r - end_r)==abs(start_c - end_c)?(abs(start_c - end_c)*14):(abs(start_r - end_r)>abs(start_c - end_c)?(4*abs(start_c - end_c)+10*abs(start_r - end_r)):(4*abs(start_r - end_r)+10*abs(start_c - end_c)));//对角化距离
	//int h = c1 + c2;
	heap.push(Node(start_r,start_c,h,0,h));
	Index index[8];
	char map[n+2][m+2];
	int visited[n+2][m+2];//p_row[n+2][m+2],p_col[n+2][m+2];
	Info q[n+2][m+2];
	q[start_r][start_c].row = -1;
	q[start_r][start_c].col = -1;
	q[start_r][start_c].f = h;
	q[start_r][start_c].g = 0;
	q[start_r][start_c].h = h;

	//
	create_Maze(map,visited,start_r,start_c,end_r,end_c);
	create_Ind(index);
	//cout<<heap.top().row<<' '<<heap.top().col<<endl<<'v';
	FindWay(index,q,end_r,end_c,map,visited); 
	//cout<<'z';
	printWay(q,end_r,end_c,start_r,start_c,map);
	system("pause");
}

四、运行结

图中S是起点，E是终点。

:是未遍历的点，#是墙，*是路径，=遍历过的点，也是搜索空间。

路径也有打印，只是篇幅太长，没有截图。

五、写在最后：

发表一篇博客，以来记录自己的大二下课设。

整篇代码比较简陋，为了能实现A*算法，没有考虑太多的时空复杂度，用树来实现或许会更好，但限于本人水平有限，用的是数组。