OpenCV配置方法：https://1keven1.github.io/2021/01/29/%E3%80%90C-%E3%80%91VS2019%E9%85%8D%E7%BD%AEOpenCV%E5%B9%B6%E8%BF%9B%E8%A1%8C%E6%B5%8B%E8%AF%95/

算法原理

简述

A* 是一种知情搜索算法，或最佳优先搜索，这意味着它是根据加权图制定的：从图的特定起始节点开始，它旨在找到具有最小的给定目标节点的路径成本（最短行驶距离、最短时间等）。它通过维护源自起始节点的路径树并一次一条边扩展这些路径直到满足其终止标准来实现这一点。

最基础的A星算法是2D网格寻路，网格分为可通过与不可通过。

在A星算法中最为重要的是三个Cost：H Cost、G Cost与F Cost。其中H Cost是一个启发式函数，用于估计当前点到目标的最小成本，在这里就是忽略所有障碍物到目标点的距离。G Cost的意思是从起始点到当前点的路径成本，也就是从起始点走多远可以到达当前点。F Cost即为两个Cost的总和，即：

F Cost = G Cost + H Cost

A* 的典型实现使用队列来重复选择最小成本的节点进行扩展。此队列称为开放集或边缘。在算法的每一步循环中，首先会找到具有最低F Cost的节点，将其从队列中移除，并且获取其所有的邻居节点。然后更新每个邻居节点的三个Cost，庵后将这些邻居添加到队列中。该算法继续进行，直到移除的节点是目标节点。如果最终队列为空，即没有到达目标点的路径。

到目前为止描述的算法只给了我们最短路径的长度以及是否可以到达目标点。为了找到实际的路径，我们让每一个节点在被加入队列中的时候设置一个父节点。该算法运行后，从目标点递归寻找父节点，最终回溯出整条路径。

四连通与八连通

四连通与八连通为运动集，对于游戏来说，其的意思为角色可以移动的方向。四连通则只可以向上下左右走，而八连通可以向斜向走。对于A星算法而言，其影响了如何寻找邻居节点。在四连通的情况下，邻居节点只有上下左右四个。而在八连通的情况下，则斜向的四个也算为邻居节点，也就是总共有八个。

在算法逻辑方面四连通与八连通也有一些差别，首先就是距离的问题，四连通因为距离都一样，都可以设为1，但八连通因为斜向距离更远，所以斜向距离需要设置为1.4（根号2的近似）。其次，在八连通的情况下，会出现虽然此节点已经被计算过F Cost，但是从新路径再次到达时其的G Cost会变低的情况，也就是从新路径到达此节点更加便捷，所以八连通的A星算法需要在碰到计算过的节点时不能跳过，而是应该计算F Cost，如果新的F Cost更小，更新此节点。

对于实际应用来说，四连通存有的比较大的缺陷是，在没有障碍物的情况下，其会贴边走直线，如图1。因为对于四连通算法来说途中的两条线路的距离是相同的。但从实际使用考虑，斜线应为更好的选择。

对于四连通，蓝色和粉色两条线路距离相同

教程视频

基本看p1原理就全懂了

伪代码

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
开启队列 // 需要计算的节点的队列
已计算队列 // 已经被计算过的节点的队列

将开始点加入开启队列

在开始队列不为空的时候 循环
	当前节点 = 开始队列中F Cost最小的节点，如果相同则选择H Cost最小的，如果都一样则随意选择
	从开启队列中移除当前节点
	将当前节点加入已计算队列
	
	如果当前节点是目标点
		返回 寻路成功
	
	对于当前节点的每个邻居节点
		如果此邻居节点不可通过或者在已计算队列内
			跳过该节点
		如果此邻居节点不在开启队列中或此路径的G Cost更短
			设置该节点的Cost
			将该节点的父节点设置为当前节点
			如果此邻居节点不在开启队列中
				将此节点加入开启队列
返回 寻路失败

其他前置知识

曼哈顿距离

出租车几何或曼哈顿距离（Manhattan Distance）是由十九世纪的赫尔曼·闵可夫斯基所创词汇 ，是种使用在几何度量空间的几何学用语，用以标明两个点在标准坐标系上的绝对轴距总和。在A星算法中，我使用曼哈顿距离计算两个给定节点之间的最短距离。

在四连通的情况下，曼哈顿距离的计算十分简单。正如上文所述，任意两点之间的最短距离与两点所成矩形的长短边之和相同，其中一个格的距离为Ds，因此曼哈顿距离为：

四连通曼哈顿距离计算公式

在八连通的情况下，因为可以斜向移动，曼哈顿距离的计算复杂了一点。因为斜向移动距离更近，我们需要先计算出斜向最多可以走多少，然后剩下的距离全部由直线移动填充。首先先求出两地两点之间的距离差dx和dy，斜向最多可以走的次数就是这两个数中最小的一个，然后剩下的距离直线到达。其中斜向的距离为Do，因此曼哈顿距离为：

八连通曼哈顿距离计算公式

C++实现

显示

首先声明一些全局变量

1
2
3
4
5
6
// 全局变量
int windowHeight;		// 屏幕高度
int windowWidth;		// 屏幕宽度
int gridNumX;			// 网格列数
int gridNumY;			// 网格行数
int gridLineWidth;		// 网格边线宽度

声明Grid结构体，并添加成员函数进行绘制，采用帧缓存的方法进行绘制（其中暴露的set和get方法省略）：

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
struct Grid
{
public:
	……
	void Draw(std::vector<cv::Vec3f>& frameBuffer) const
	{
		for (int j = (int)(_centerY - _height * 0.5); j < (int)(_centerY + _height * 0.5); j++)
		{
			for (int i = (int)(_centerX - _width * 0.5); i < (int)(_centerX + _width * 0.5); i++)
			{
				// 防止数组越界
				if (j > windowHeight - 1) i = windowHeight - 1;
				if (i > windowWidth - 1) j = windowWidth - 1;
				// 绘制
				switch (_type)
				{
				case NORMAL:
					if (abs(i - _centerX) > _width * 0.5 - gridLineWidth || abs(j - _centerY) > _height * 0.5 - gridLineWidth)
						frameBuffer[GetFrameBufferNumByCoord(i, j)] = gridLineColor;
					else frameBuffer[GetFrameBufferNumByCoord(i, j)] = normalGridBackgroundColor;
					break;
				case BLOCK:
					if (abs(i - _centerX) > _width * 0.5 - gridLineWidth || abs(j - _centerY) > _height * 0.5 - gridLineWidth)
						frameBuffer[GetFrameBufferNumByCoord(i, j)] = gridLineColor;
					else frameBuffer[GetFrameBufferNumByCoord(i, j)] = blockColor;
					break;
				case START:
					if (abs(i - _centerX) > _width * 0.5 - gridLineWidth || abs(j - _centerY) > _height * 0.5 - gridLineWidth)
						frameBuffer[GetFrameBufferNumByCoord(i, j)] = gridLineColor;
					else frameBuffer[GetFrameBufferNumByCoord(i, j)] = startColor;
					break;
				case GOAL:
					if (abs(i - _centerX) > _width * 0.5 - gridLineWidth || abs(j - _centerY) > _height * 0.5 - gridLineWidth)
						frameBuffer[GetFrameBufferNumByCoord(i, j)] = gridLineColor;
					else frameBuffer[GetFrameBufferNumByCoord(i, j)] = goalColor;
					break;
				case ROUTE:
					if (abs(i - _centerX) > _width * 0.5 - gridLineWidth || abs(j - _centerY) > _height * 0.5 - gridLineWidth)
						frameBuffer[GetFrameBufferNumByCoord(i, j)] = gridLineColor;
					else frameBuffer[GetFrameBufferNumByCoord(i, j)] = routeColor;
				}
			}
		}
	}

private:
	int _x, _y;				// 网格编号
	float _centerX, _centerY;	// 网格中心点屏幕坐标
	float _width, _height;		// 网格长宽
	GridType _type;			// 网格种类
	bool bBlock;			// 网格可通过性

	// A*算法所用变量
	int _gCost = 0;			// 开始点走过来的代价
	int _hCost = 0;			// 距离目标点的距离
	int _fCost = 0;
	Grid* _parent = NULL;	// 父网格
};

然后进行基于OpenCV的可视化显示：

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
// 显示所需变量
std::vector<Grid> grids;			// 所有网格
std::vector<cv::Vec3f> frameBuffer;	// Frame Buffer
Grid* startGrid = NULL;				// 开始点
Grid* goalGrid = NULL;				// 结束点
std::vector<Grid*> route;			// 路径点

……

int main()
{
	int frameCount = 0;

	// 初始化
	Initialization(windowHeight, windowWidth, gridNumX, gridNumY);

	int key = -1;
	while (key != 27) // 如果没有按ESC就一直循环
	{
		// 绘制所有网格
		for (auto& grid : grids)
		{
			grid.Draw(frameBuffer);
		}

		// OpenCV：显示
		cv::Mat window = cv::Mat(windowHeight, windowWidth, CV_32FC3, frameBuffer.data());
		cv::cvtColor(window, window, cv::COLOR_BGR2RGB);
		cv::namedWindow("A*", cv::WINDOW_KEEPRATIO);
		cv::setMouseCallback("A*", mouse_handler, nullptr);

		frameCount++;
		cv::imshow("A*", window);
		key = cv::waitKey(1);

		// 如果寻路完成
	}
	return 0;
}

最后添加OpevCV的鼠标事件，右键划动设置网格可通过性，左键点击设置开始点和目标点：

1
2
3
4
5
6
7
8
9
10
11
12
13
14
void mouse_handler(int event, int x, int y, int flags, void* userdata)
{
// 按下鼠标左键
	if (event == cv::EVENT_LBUTTONDOWN) {……}

// 按下鼠标右键
	if (event == cv::EVENT_RBUTTONDOWN) {……}

	// 松开鼠标右键
	if (event == cv::EVENT_RBUTTONUP) {……}

// 鼠标移动
	if (event == cv::EVENT_MOUSEMOVE) {……}
}

A星算法（四连通）

使用曼哈顿距离计算四连通Cost的方法：

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
void SetCost4Way(Grid* const goal)
{
    // GCost
    if (_type == START) // 如果是开始网格
    {
    	_gCost = 0;
    }
    else // 如果不是开始网格
    {
    	_gCost = _parent->GetGCost() + 1;
    }

    // HCost
    int endX, endY;
    std::tie(endX, endY) = goal->GetIndex();
    _hCost = abs(endX - _x) + abs(endY - _y);

    // FCost
    _fCost = _gCost + _hCost;
}

A星算法：

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
bool AStar4Way()
{
	// A*算法所需变量
	std::vector<Grid*> openGrid;	// openList
	std::vector<Grid*> closeGrid;	// closeList
	Grid* current = NULL;			// 现在计算的节点

	std::cout << "开始四连通寻路" << std::endl;
	clock_t start = clock();

	// 将开始节点放入Open List
	startGrid->SetCost4Way(goalGrid);
	openGrid.push_back(startGrid);

	// 在Open List中有元素的时候进行循环
	while (openGrid.size() > 0)
	{
		// 将current网格设置成Open List中FCost最小的，如果FCost一样的话设置成HCost最小的
		int minFCost = std::numeric_limits<int>::max();
		int currentNum = 0;
		int i = 0;
		for (auto grid : openGrid)
		{
			if (grid->GetFCost() < minFCost)
			{
				current = grid;
				minFCost = grid->GetFCost();
				currentNum = i;
			}
			else if (grid->GetFCost() == minFCost && current)
			{
				if (grid->GetHCost() < current->GetHCost())
				{
					current = grid;
					currentNum = i;
				}
			}
			i++;
		}

		// 将current从open list中移除，加入到close list中
		openGrid.erase(openGrid.begin() + currentNum);
		closeGrid.push_back(current);

		// 如果current是目标点则寻路成功
		if (current && current->GetType() == GOAL)
		{
			clock_t end = clock();
			std::cout << "寻路成功，用时：" << end - start << "毫秒" << std::endl;
			return true;
		}

		// 对于每一个邻居网格
		auto neighbors = FindAllNeighbors4Way(current);
		for (auto neighbor : neighbors)
		{
			// 如果在open和close list中或者是墙，则跳过
			if (neighbor->GetBlock() || VectorContainItem(closeGrid, neighbor) || VectorContainItem(openGrid, neighbor))
				continue;
			else
			{
				neighbor->SetParent(current);
				neighbor->SetCost4Way(goalGrid);
				openGrid.push_back(neighbor);
			}
		}

	}
	std::cout << "失败嘞" << std::endl;
	return false;
}

使用递归回溯路径：

1
2
3
4
5
6
7
8
9
10
11
12
void AddAllRouteGridToVector(Grid* const grid)
{
	if (grid->GetParent())
	{
		route.push_back(grid);
		AddAllRouteGridToVector(grid->GetParent());
	}
	else
	{
		return;
	}
}

在主函数中添加判断条件开始寻路：

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
int main()
{
	……

	bool bFinish = false;

	while (key != 27) // 如果没有按ESC就一直循环
	{

		……

		// 如果开始点和结束点都指定了，开始寻路
		if (startGrid && goalGrid)
		{
			bool bSuccess = AStar4Way ();
			if (bSuccess) // 如果寻路成功
			{
				AddAllRouteGridToVector(goalGrid);
				ShowRoute();
			}

			// 绘制
			for (auto& grid : grids)
			{
				grid.Draw(frameBuffer);
			}
			bFinish = true;
		}

		……

		// 如果寻路完成
		if (bFinish)
		{
			system("pause");
			// 按下任意键，重新开始
			Restart();
			frameCount = 0;
			bFinish = false;
		}
	}
	return 0;
}

A星算法（八连通）

曼哈顿距离计算八连通Cost的方法：

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
void SetCost8Way(Grid* const goal)
{
	// GCost
	if (_type == START) // 如果是开始网格
	{
		_gCost = 0;
	}
	else // 如果不是开始网格
	{
		int parentX, parentY;
		std::tie(parentX, parentY) = _parent->GetIndex();
		if (parentX == _x || parentY == _y)
		{
			_gCost = _parent->GetGCost() + 10;
		}
		else
		{
			_gCost = _parent->GetGCost() + 14;
		}
	}

	// HCost
	int endX, endY;
	std::tie(endX, endY) = goal->GetIndex();
	int dx = abs(endX - _x);
	int dy = abs(endY - _y);
	_hCost = std::min(dx, dy) * 14 + abs(dx - dy) * 10;

	// FCost
	_fCost = _gCost + _hCost;
}

八连通A星算法大部分与四连通相同，只有对于邻居节点的操作不同：

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
bool AStar8Way()
{
	……

	while (openGrid.size() > 0)
	{
		……

		// 对于每一个邻居网格
		auto neighbors = FindAllNeighbors8Way(current);
		for (auto neighbor : neighbors)
		{
			if (neighbor->GetBlock() || VectorContainItem(closeGrid, neighbor))
				continue;

			// 如果该路径FCost更低的话更新该节点的Cost
			int newFCost = CalculateFCost(neighbor, current, goalGrid);
			if ((newFCost < neighbor->GetFCost()) || (!VectorContainItem(openGrid, neighbor)))
			{
				neighbor->SetParent(current);
				neighbor->SetCost8Way(goalGrid);
				if (!VectorContainItem(openGrid, neighbor))
				{
					openGrid.push_back(neighbor);
				}
			}
		}
	}
	std::cout << "寻路失败" << std::endl;
	return false;
}

结果

下方两张图片是同样地图的情况下四连通和八连通A星寻路的结果，其中白色是墙，绿色是开始点，粉色是结束点，深黄色是路径。可以看出，八连通更加接近于真实情况下的最短路径。

四连通和八连通寻路

在代码中我还进行了计时，在上面的情况下，四连通A星寻路需要花费2毫秒完成寻路，八连通需要3毫秒，总的来说相差不大。而且由于自带函数只能精确到毫秒，此数据并不十分精确。

其他测试：用时1ms

其他测试

其他优化

因为测试需要，需要多次更改网格数量，每次都改代码重新编译非常费时间，所以引入了一个config文本文件，加入了外部读入数据的功能。且为了提升代码的鲁棒性，对读入的每个数据都会进行验证再读入，如果没有通过验证或者数据不足会报错，程序退出。

不足

  1. 算法效率问题

    在A星算法中，每个循环都需要进行一个获得F Cost最小的节点的操作，在这里应该使用排序队列，在每次插入的时候进行排序，保证队列为有序，这样每次可以直接取第一个元素即为最小，不需要每个循环都遍历所有元素取最小。这样算法的执行效率会快很多。

  2. 地图问题

    在实际使用中，地图与算法是分开的，需要获得地图可通过性数据，传入网格然后进行算法计算。如果场景中出现可移动的物体，还需要实时更改地图数据。

总结

总的来说，A星算法是一个十分高效的寻路算法，虽然结果并不十分完美，但是在大部分游戏中，尤其是2D游戏中已经完全足够了，极高的运行效率也让其可以放心大胆的在各种低性能设备上使用。

A星算法是一个基础，在他的基础上还衍生出了更多更高级的算法，在理解了A星算法的最佳优先搜索以及加权的思想之后，再学习高级寻路算法将会更加得心应手。

算法很大程度上是一种思想，同一种思想可以延伸出很多算法。虽然如今的计算机性能日益提高，但是对于游戏这种实时渲染的情况来说还是远远不够的。在越来越多的计算资源被分配到画面渲染上的如今，我们需要使用高性能算法在其他地方追求极致高效，算法的重要性可见一斑。

后期计划

  1. 添加显示寻路过程功能
  2. 添加保存地图、外部读取地图功能
  3. 优化八连通算法，防止从墙壁缝隙中斜着走过去

希望有生之年能做完

资源

开源项目网址：https://github.com/1keven1/AStar

参考文献

  1. Russell, Stuart J. (2018). Artificial intelligence a modern approach. Norvig, Peter (4th ed.). Boston: Pearson. ISBN 978-0134610993. OCLC 1021874142.
  2. Martelli, Alberto (1977). “On the Complexity of Admissible Search Algorithms”. Artificial Intelligence. 8 (1): 1–13. doi:10.1016/0004-3702(77)90002-9.
  3. Ferguson, Dave; Likhachev, Maxim; Stentz, Anthony (2005). A Guide to Heuristic-based Path Planning (PDF). Proc. ICAPS Workshop on Planning under Uncertainty for Autonomous Systems.
  4. Zeng, W.; Church, R. L. (2009). “Finding shortest paths on real road networks: the case for A*”. International Journal of Geographical Information Science. 23 (4): 531–543.