A*算法 c++ 路径规划

概念和原理

A*算法是一种启发式的搜索算法，效率比盲目搜索搞很多，在一定条件（即估价函数h（n）<=实际值）下可以保证得到的解是最优解。

搜索算法中常用到的两个结构，open表和closed表

• open表用来记录下一步能走的节点（待扩展节点）算法每一步都会从open表中按某种搜索策略取出一个节点作为下一个扩展节点
• closed表用来记录已经走过的节点（已扩展节点）从open表中扩展后的节点都将移入closed表

A*算法最为核心的是启发函数f(n)=g(n)+h(n),g(n)为目前已经花费的代价，h(n)为估计要花费的代价，f(n)则作为open表中判别下一节点选取的依据。

A算法实际上是Dijkstra’s算法和贪心搜索的组合，只考虑g(n)已经花费的代价令h(n)=0则A退化为Dijkstra’s算法，反之则退化为贪心搜索。

推荐一个能可视化A*算法的学习网站
链接在此

思路

用A*算法实现一个路径规划问题，在一个n×n的地图中有若干个障碍，一个起点一个终点，求一条起点到终点的最短路径。（规定只能上下左右移动）
下面是伪代码

1.	把起始格添加到OPEN表。
2.	 	while OPEN表为空
3.	 		寻找开启列表中F值最低的节点current,把它移动到 CLOSED表
4.			if current就是终点 then
5.				路径被找到
6.	 			 return路径,即从目标格开始,沿着每一格的父节点移动直到回到起始格
7.	
8.	 		for current相邻的4格中的每一个格子i do
9.	 			while i不是障碍且不在 CLOSED表中do
10.	 				If i不在OPEN表中then
11.	 					添加i,把current作为i的父节点。记录i的F,G,和H值
12.	 				else
13.	 				   用G值为参考检查新的路径是否更好。更低的G值意味着更好的路径。
14.	 					如果是这样,  就把i的父节点改成current,并且重新计算i的G和F值
15.	  			end if
16.	 			end while
17.	 		end for
19.		end while
20.	return 失败

因为只能上下左右移动，所以距离函数使用曼哈顿距离；

核心代码展示

可以将calculateCost（）改动可转化为Dijkstra’s算法和贪心搜索

class Node
	{
	public:
		Node(int x, int y)//构造函数
			:x(x), y(y), f(0), g(0), h(0), parent(nullptr),block(false) {}
		int  x, y, f, g, h;
		Node* parent;
		bool block;
		void calculateCost(Node* par,int x2,int y2) {//计算曼哈顿距离f，g，h
			parent = par;
			g = parent->g + 1;
			h = abs(x2 - x ) + abs(y2 - y );
			f = g + h;
		}
	};

	class Astar
	{
	public:
		vector<vector<Node>> nodeMap;//地图矩阵
		list<pair<int, int>> openList;//open表
		list<pair<int, int>> closedList;//closed表

		Astar(int n, int m);//构造函数输入地图矩阵长宽
		void setBlock(initializer_list<pair<int, int>> initList);//设置障碍
		bool search(int x1, int y1, int x2, int y2);//开始搜索
	private:
		int direction[4][2] = { {0,1},{0,-1},{1,0},{-1,0} };//用来生成节点周围上下左右的坐标
		list<pair<int, int>>::iterator findLeastNode();//查找open表中f最小的节点
	};

bool Astar::search(int x1, int y1, int x2, int y2)
{
	openList.push_back({ x1,y1 });//首先将起点输入open表
	while (!openList.empty()) {
		auto currentIter = findLeastNode();//从open表中取出f最小的一个节点
		int cx = currentIter->first, cy = currentIter->second;//cx代表current的x坐标
		auto &currentNode = nodeMap[cy][cx];
		openList.erase(currentIter);//将当前取出的节点从open表中移除
		closedList.push_back({ cx,cy });//加入closed表中

		if (cx == x2 && cy == y2)return true;//取出的节点是终点返回true
		for (auto& d : direction) {//对于上下左右四个方向均尝试探索一次
			int nx = d[0] + cx, ny = d[1] + cy;//nx代表next（探索节点）的x坐标
			if (nx>=nodeMap[0].size() || nx < 0 ||//next节点无意义则跳过
				ny>=nodeMap.size() || ny < 0 )
				continue;
			auto &next = nodeMap[ny][nx];//有意义则找到该节点
			//用来定义STL函数find_if()的Pred的Lambta表达式，定义了如何算相等
			auto preLambta = [&](const pair<int,int>& a) {return nx == a.first && ny == a.second; };
			//若探索节点不为block并且不在closed表中
			if (!next.block && find_if(closedList.begin(), closedList.end(), preLambta) == closedList.end()) {
				//若探索节点不在open表中
				if (find_if(openList.begin(), openList.end(), preLambta) == openList.end()) {
					next.calculateCost(&currentNode, x2, y2);
					openList.push_back({nx,ny});
				}
				else {//若探索节点在open表中
					//当前节点的g+1还小于探索节点的g则重新计算open表中探索节点的g并改父亲节点为当前节点
					if (currentNode.g+1  < next.g) {
						next.calculateCost(&currentNode, x2, y2);
					}
				}
			}
		}
	}
	return false;
}

完整代码

链接