基于A*(Astar)算法的自动寻路问题及C代码

飞天霸气猪

已于 2023-10-24 20:49:08 修改

阅读量645

点赞数 5

文章标签：算法 c++ 开发语言 c语言 1024程序员节

于 2023-09-19 17:05:50 首次发布

本文链接：https://blog.csdn.net/DASDN/article/details/132977813

版权

A*算法介绍

A*算法是一种常用的路径搜索算法，用于在图形空间中搜索最短路径。它采用启发式搜索的策略，即根据一定的启发函数来选择最有可能最优解的路径。

A*算法基本原理

A*算法使用两个函数来评估节点的优先级：g(n)表示从起点到节点n的实际代价，h(n)表示从节点n到目标节点的估计代价，即启发函数。每次迭代中，A*算法选择优先级最高的节点进行扩展，并计算其周围节点的代价和优先级。通过不断迭代，直到找到目标节点为止。A*算法的优点在于它可以快速地找到最短路径，并且具有较高的可扩展性。它常用于游戏中的AI路径规划、机器人路径规划等领域。

自动寻路问题

在我们的日常生活中，无论是汽车里的导航系统、游戏里的角色自动寻路系统还是各种路线规划问题，都离不开一个重要的步骤那就是——寻找最短路径，而A*算法无疑是各类路径搜索算法中实用性和稳定性最高的算法之一，很多大家熟知的企业都使用了A*算法或是A*算法的变种进行项目的开发。那么如何使用A*算法解决寻路问题呢？可将寻路问题分解为以下几个步骤：

对已知的地图进行处理，将其各个位置的坐标、参数存储在一个二维数组或其他结构中，用该结构来表示地图上各个位置的状态。
构造启发函数，构造open表、closed表。
获得起点和终点的坐标，通过A*算法进行最佳路径搜索。
找到最佳路径，返回路径坐标。

算法实现

1.地图处理

首先对地图进行处理，对于一个二维平面地图，可以根据各种不同的特征来表示其存在的路径和障碍，比如我们知道可行路径与障碍之间一般存在较大的色彩差异，因此我们可以将地图在色彩上进行灰度处理，然后二值化就可以得到一个01矩阵，可以用1来表示通路，0来表示障碍，如下图所示：

2.程序编译

首先我们定义了节点，每个节点都代表地图矩阵中每个点的状态，而每个节点又存在以自身为中心的8个方向，用来确定路径搜索的方向。

class Node    //定义节点
{
	public:
		int x, y; 
		int F, G, H;
		Node(int a, int b):x(a), y(b){}
		bool operator < (const Node &a) const
		{
			return F > a.F;
		}
}; 
int dir[8][2] = {{-1,-1}, {-1, 0}, {-1, 1}, {0, -1},
                 {0, 1},  {1, -1}, {1, 0},  {1, 1}};    //定义每个节点的八个方向

我们可以用一个优先队列来充当open表，用一个二维数组来充当closed表。

priority_queue<Node>que;// open表
bool visit[M][N]; // closed表

使用曼哈顿距离作为估计代价H。

int Manhuattan(int x, int y, int x1, int y1)    //曼哈顿距离
{
	return (abs(x - x1) + abs(y - y1)) * 10;
}

通过计算节点周围的值是否为0来判断边界。

bool NodeIsLegal(int qp[M][N],int x, int y, int xx, int yy)    //判断边界
{
	if(x < 0 || x >= M  || y < 0 || y >= N)
	return false;
	if(qp[x][y] == 0) 
	return false;
	if(x != xx && y != yy && (qp[x][yy] == 0 || qp[xx][y] == 0)) 
	return false;
	return true;
}

构造A*算法：首先初始化两个表，将起始节点加入open表，若open表非空，则访问该节点，并将其加入closed表，若该店是终点则结束；否则，遍历当前节点周围的8个位置。在遍历过程中，创建一个当前节点周围的节点，若该节点未加入closed表，则计算其估价函数值，若当前节点估价函数值更低或者还未加入open表，则存储该节点的父节点，然后修改该节点对应的估价函数值并加入open表，如此循环，最后便可找到一条最短路径的坐标集合。

void A_star(int qp[M][N],int x0, int y0, int x1, int y1)
{
	Node node(x0, y0);	// 初始化open表 、closed表 
	node.G = 0;
	node.H = Manhuattan(x0, y0, x1, y1);
	node.F = node.G + node.H;
	valF[x0][y0] = node.F;
	que.push(node);
	while(!que.empty())
	{
		Node node_top = que.top();
		que.pop();
		visit[node_top.x][node_top.y] = true;
		if(node_top.x == x1 && node_top.y == y1) // 到达终点
			break;
		for(int i=0; i<8; i++)	// 遍历当前节点周围的8个位置
		{
			Node node_next(node_top.x + dir[i][0], node_top.y + dir[i][1]);
			if(NodeIsLegal(qp,node_next.x, node_next.y, node_top.x, node_top.y) &&     
               !visit[node_next.x][node_next.y])
			{
				node_next.G = node_top.G + int(sqrt(pow(dir[i][0],2)+
                              pow(dir[i][1],2))*10);
				node_next.H = Manhuattan(node_next.x, node_next.y, x1, y1);
				node_next.F = node_next.G + node_next.H;  // 计算其估价函数值 
				if(node_next.F < valF[node_next.x][node_next.y] || valF[node_next.x]    
                  [node_next.y] == 0)
				{
					path[node_next.x][node_next.y][0] = node_top.x; 
					path[node_next.x][node_next.y][1] = node_top.y;
					valF[node_next.x][node_next.y] = node_next.F; 
					que.push(node_next); // 加入open表
				}
			}
		}           //循环 
	}
}

我们还可以设置起点和终点，并通过程序将路径打印出来，可以更好的观察到路径和地图的状态。

void PrintPath(int qp[M][N], int x1, int y1)    //打印路径
{
	if(path[x1][y1][0] == -1 || path[x1][y1][1] == -1)
	{
		cout<<"NO FEASIBLE WAY！"<<endl;
		return;
	}
	int x = x1, y = y1;
	int a, b;
	while(x != -1 || y != -1)
	{
		qp[x][y] = 2;
		a = path[x][y][0];
		b = path[x][y][1];
		x = a;
		y = b;
	}
	for(int i=0; i<M; i++)
	{
		for(int j=0; j<N; j++)
		{
			if(qp[i][j] == 0)
			{
				setcolor(BLACK);
				cout<<"★";
			}
			else if(qp[i][j] == 1)
			{
				setcolor(GREEN);
				cout<<"□";
			}
			else if(qp[i][j] == 2)
			{
				setcolor(BLUE);
				cout<<"█";
				g[z]=i;
				c[z]=j;
				z++;
			}
			else
			continue;
		}
		cout<<endl;
	}
	setcolor(WHITE);
}