自动驾驶-决策规划算法十一：A*算法(C++)_a*算法与dijkstra算法对比

自动驾驶-决策规划算法十一：A*算法(C++)

本文介绍无人驾驶决策规划算法中的A*算法，及A*与Dijkstra、BestFS算法的对比。

和之前的算法文章中不同，为了方便表示格栅化地图，本文的地图用二维数组的数据结构表达，而为了方便表达路径点之间的关系，此关系用树（八叉树）的数据结构表达。

语言用的是C++，图形基于easyX库输出。

如有错误或者遗漏之处，欢迎指出。

---

附赠自动驾驶学习资料和量产经验：链接

基本概念：

/*

A*算法：一种启发式搜索，每走一步都要计算代价，选择代价最小的走法；

f = g + h；

f：总代价；

g：起点到中间点已经付出的代价，已知；也可以用上个点到当前点所走过的距离，因为起点到上个点走过的距离都是一样的；

h：中间点到终点的代价，估算；

//有些场合还要考虑权重，加个w表示权重；

h的计算通常有三种：曼哈顿距离和欧式距离；

曼哈顿距离：两个点之间的横坐标距离加上纵坐标距离；

欧式距离：两个点之间的直线距离；如果允许用float或double变量，可以用；

对角距离：就是两个斜着相邻点之间的距离与横纵坐标综合计算出来的距离，介于曼哈顿距离和欧式距离之间；

//h的计算不考虑障碍物；

为了方便，把地图定义为二维地图，然后格栅化，这样地图就由很多个以正方形为最小单位的格子构成；

每个格子可以向周围8个方向移动，也就是可以走直线，也可以走斜线；

为了方便，把每步走直线的代价设定为10，每步走斜线的代价设定为14，这样可以用int类型来计算；实际项目中应根据需要定义；

如果让h始终为0，则f完全等于g，这就是Dijkstra算法了；

如果h(n)始终小于等于节点n到终点的代价，A*算法才能保证一定能够找到最短路径；

如果h(n)的值比节点n到终点的代价要大，则A*算法不能保证找到最短路径；

所以A*算法求出的不一定是最短路径；

如果让g始终为0，f完全等于h，也就是只考虑h，那么这就是最佳优先算法；

最佳优先算法：有时候可以先计算出每个节点到终点的距离，让h成为已知量，这样可以大幅加快算法速度，减少遍历的点数；但是如果起点和终点之间有障碍物，就不一定能找到最短路径；

所以，A*算法可以通过调节h,来控制算法的速度和精确度，这也是A*算法比较灵活的地方；

在一些场合，并不一定要找到最短路径，只是希望能尽快找到一条路径，这时候可以把h设大一点；

而如果需要精确避障、对速度要求不高，则可以把h设小一点；

几种全局路径规划算法比较：

Dijkstra算法和动态规划算法，更合适处理多个城市之间给定起点到终点的最短路径问题，城市与城市之间是否连通、之间的距离都是已知的；

如果要求解任意两点之间最短距离，可以用Floyd算法，是动态规划的一种；

在一些场合，并不一定要找到最短路径，只是希望能尽快找到一条路径，可以用最佳优先算法；

蚁群算法更适合处理多个城市之间TSP商旅问题，城市与城市之间是否连通、之间的距离都是已知的，也就是从给定的起点遍历所有城市后回到起点的最短路径；

A*算法更合适处理一大块区域中给定起点和终点，没有具体的中间结点或城市，需要避开障碍物求解最短路径问题，这块区域通常需要先格栅化，有点类似有限元的思想；

A*算法常用于机器人寻路、游戏中的寻路；

*/

Astar.h

#pragma once
#include<iostream>
#include<graphics.h>
#include<vector>
#include<graphics.h>
#include<string>
#include<ctime>
using namespace std;

constexpr auto ZXCOST = 10;//每步走直线的代价
constexpr auto XXCOST = 14;//每步走斜线的代价
constexpr auto MAPTYPE = 2;//地图类型
constexpr auto ROWS = 60;//地图行数 //60
constexpr auto COLS = 50;//地图列数 //50
constexpr auto Swidth = 1200;//1200
constexpr auto Sheight = 1400;//1400
constexpr auto ratio = 2;//绘图比例

enum algoType//算法类型
{
	Astar,//Astar算法
	Dijkstra,//Dijkstra算法
	BestFS//最佳优先算法
};

enum direct//方向
{
	p_up,//上
	p_down,//下
	p_left,//左
	p_right,//右
	p_l_up,//左上
	p_r_up,//右上
	p_l_down,//左下
	p_r_down//右下
};

class Point//地图中的最小单元，也就是点
{
public:
	int row;//在地图中的行，不是坐标
	int col;//在地图中的列，不是坐标

	int f = 0;//总代价
	int g = 0;//起点到该点的代价，已知
	int h = 0;//该点到终点的代价，估算
};

class Map//地图
{
public:
	Map();//初始化地图
	void showMap();//打印地图
	void drawMap();//绘制地图
	void drawRec(int row, int col, COLORREF color = WHITE);//根据行数和列数，求矩形四条边位置，然后绘制矩形
	void delay(int time);//延时

public:
	int myMap[ROWS][COLS] = { 0 };//地图用二维数组表示
	Point beginP = { 1, 1 };//{ ROWS - 2, 1 };//{1, 1};//起点
	Point endP = { ROWS - 2,  COLS - 2};//{6, 7};//终点 //这样初始化后，后面的f g h会赋值为0；

	int disX = (Swidth - (COLS - 1) * ZXCOST * ratio) / 2;//左上点离窗口左边的距离，让地图居中
	int disY = (Sheight - (ROWS - 1) * ZXCOST * ratio) / 2;//左上点离窗口上边的距离，让地图居中
};

class treeNode//用树的结构存储点与点之间的关系，这里是树的结点
{
public:
	treeNode(int row, int col);//初始化结点

public:
	Point pos;
	vector<treeNode*> child;//指向孩子的指针，每个结点可以有多个孩子，所以直接用容器表示；
	treeNode* pParent = nullptr;//指向父结点的指针，每个结点只有一个父结点；
};

class tree//用树的结构存储点与点之间的关系：每个点的孩子表示这个点可走的下个点
{
public:
	tree();
	~tree();
	void updateTree();//更新树
	void showResult();//打印结果
	bool bBeginPoint(treeNode* point);//是否是起点
	bool bEndPoint(treeNode* point);//是否是终点

public:
	Map map0;//地图对象
	treeNode* pRoot = nullptr;//根结点
	treeNode* pCurrent = nullptr;//当前结点
	treeNode* pNext = nullptr;//当前结点的相邻结点

	vector<treeNode*> openList;//用来找最小f的数组，存储遍历过的点的指针；
	bool closeList[ROWS][COLS] = { false };//标记走过的点，初始化为没走过
	vector<treeNode*> result;//存储打印数据

	//bool bEnd = false;//是否到达终点
	int cost = 0;//累计代价
	int costFinal = 0;//最终路径的总代价
	int type = Astar;//算法类型
};

class ShortestPath //最短路径
{
public:
	void shortestPath();

public:
	tree tr;
};
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
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111

Astar.cpp

#include"Astar.h"

Map::Map()//初始化地图
{
	/*for (int i = 0; i < ROWS; i++)
	{
		for (int j = 0; j < COLS; j++)
		{
			myMap[i][j] = 0;
		}
	}*/

	//设置障碍物
	if (MAPTYPE == 0)
	{
		for (int i = 0; i < ROWS; i++)
		{
			for (int j = 0; j < COLS; j++)
			{
				if (j == 20 && i <= 40)
				{
					myMap[i][j] = 1;
				}
			}
		}
	}

	if (MAPTYPE == 1)
	{
		for (int i = 0; i < ROWS; i++)
		{
			for (int j = 0; j < COLS; j++)
			{
				if (j == 4 && i != 6)
				{
					myMap[i][j] = 1;
				}

				if (j == 12 && i != 20)
				{
					myMap[i][j] = 1;
				}

				if (j == 20 && i != 40)
				{
					myMap[i][j] = 1;
				}

				if (j == 35 && i != 20)
				{
					myMap[i][j] = 1;
				}

				if (i == 4 && j >= 2 && j <= 4)
				{
					myMap[i][j] = 1;
				}

				if (i == 3 && j >= 1 && j <= 2)
				{
					myMap[i][j] = 1;
				}

				if (i == ROWS - 10 && j >= COLS - 10 && j < COLS)
				{
					myMap[i][j] = 1;
				}
			}
		}
	}
	else if (MAPTYPE == 2)
	{
		for (int i = 0; i < ROWS; i++)
		{
			for (int j = 0; j < COLS; j++)
			{
				if (j == 3 && i >= 0 && i <= 10)
				{
					myMap[i][j] = 1;
				}

				if (j == 5 && i != 7 && i != 6)
				{
					myMap[i][j] = 1;
				}

				if (j == 11 && i >= 0 && i <= 25)
				{
					myMap[i][j] = 1;
				}

				if (j == COLS - 9 && i >= ROWS - 16)
				{
					myMap[i][j] = 1;
				}

				if (i == 40 && j >= 13 && j < COLS)
				{
					myMap[i][j] = 1;
				}

				if (i == 30 && j >= 5 && j <= 40)
				{
					myMap[i][j] = 1;
				}
			}
		}
	}
	else if (MAPTYPE == 3)
	{
		for (int i = 0; i < ROWS; i++)
		{
			for (int j = 0; j < COLS; j++)
			{
				if (j == 10 && (i >= 0 && /*i <= 20 || i >= 23 &&*/ i <= 50))
				{
					myMap[i][j] = 1;
				}

				if (j == 30 && (i >= 10 && /*i <= 50 || i >= 53 &&*/ i < ROWS))
				{
					myMap[i][j] = 1;
				}
			}
		}
	}

	showMap();
	drawMap();
}

void Map::showMap()//打印地图
{
	cout << "起点位置：(" << beginP.row << ", " << beginP.col << "); 终点位置：(" << endP.row << ", " << endP.col << "), 地图如下：" << endl;

	myMap[beginP.row][beginP.col] = 2;//起点
	myMap[endP.row][endP.col] = 9;//终点

	for (int i = 0; i < ROWS; i++)
	{
		for (int j = 0; j < COLS; j++)
		{
			cout << myMap[i][j] << " ";
		}
		cout << endl;
	}
	cout << endl;
}

void Map::drawMap()//绘制地图
{
	setbkcolor(WHITE);    //设置背景颜色
	setlinecolor(BLACK);    //设置边框颜色
	setlinestyle(PS_SOLID, 2);	// 设置线的样式为实线2px
	cleardevice();

	for (int i = 0; i < ROWS; i++)
	{
		for (int j = 0; j < COLS; j++)
		{	
			if (myMap[i][j] == 1)
			{
				drawRec(i, j, BROWN);//障碍物褐色
			}
			else if (i == beginP.row && j == beginP.col)
			{
				drawRec(i, j, RED);//起点红色
			}
			else if (i == endP.row && j == endP.col)
			{
				drawRec(i, j, BLUE);//终点蓝色
			}
			else
			{
				drawRec(i, j);//默认白色
			}
		}
	}
}

void Map::drawRec(int row, int col, COLORREF color)//根据行数和列数，求矩形四条边位置，然后绘制矩形
{
	setfillcolor(color);//设置填充颜色

	int pointX = disX + col * ZXCOST * ratio;//X对应列
	int pointY = disY + row * ZXCOST * ratio;//Y对应行

	int left = pointX - ZXCOST / 2 * ratio;
	int top = pointY - ZXCOST / 2 * ratio;
	int right = left + ZXCOST * ratio;
	int bottom = top + ZXCOST * ratio;

	fillrectangle(left, top, right, bottom);
}

void Map::delay(int time) //延时函数，单位ms
{
	clock_t  now = clock();
	while (clock() - now < time)
	{

	}
}

treeNode::treeNode(int row, int col)//初始化结点
{
	pos.row = row;
	pos.col = col;
}

tree::tree()//初始化树
{
	pRoot = new treeNode(map0.beginP.row, map0.beginP.col);//起点放入树中根结点位置
	closeList[map0.beginP.row][map0.beginP.col] = true;//起点初始化为走过
	cout << "根结点：(" << pRoot->pos.row << ", " << pRoot->pos.col << ")" << endl;

	pCurrent = pRoot;//初始化当前结点为根结点
}

tree::~tree()
{
	if (pRoot != nullptr)
	{
		delete pRoot;
		pRoot = nullptr;
	}

	if (pNext != nullptr)
	{
		delete pNext;
		pNext = nullptr;
	}
}

void tree::updateTree()//更新树
{
	while (true)//整个寻路过程
	{
		BeginBatchDraw();
		//openList.clear();//每轮不需要清空，因为后面会erase()；

		for (int i = 0; i < 8; i++)//循环遍历每个点周围的8个点
		{
			pNext = new treeNode(pCurrent->pos.row, pCurrent->pos.col);//下个点初始化为当前点，因为还不知道往哪个方向走；
			//这个要放在for里，不能放在构造函数里，因为每次要从中心点开始走！			
			//不能写成pNext = pCurrent;
			//cout << "pCurrent: " << pNext->pos.row << ", " << pNext->pos.col << endl;

			switch (i)
			{
			case p_up:
			{
				pNext->pos.row--;
				pNext->pos.g += ZXCOST;
				break;
			}
			case p_down:
			{
				pNext->pos.row++;
				pNext->pos.g += ZXCOST;
				break;
			}	
			case p_left:
			{
				pNext->pos.col--;
				pNext->pos.g += ZXCOST;
				break;
			}
			case p_right:
			{
				pNext->pos.col++;
				pNext->pos.g += ZXCOST;
				break;
			}
			case p_l_up:
			{
				pNext->pos.row--;
				pNext->pos.col--;
				pNext->pos.g += XXCOST;
				break;
			}
			case p_r_up:
			{
				pNext->pos.row--;
				pNext->pos.col++;
				pNext->pos.g += XXCOST;
				break;
			}
			case p_l_down:
			{
				pNext->pos.row++;
				pNext->pos.col--;
				pNext->pos.g += XXCOST;
				break;
			}
			case p_r_down:
			{
				pNext->pos.row++;
				pNext->pos.col++;
				pNext->pos.g += XXCOST;
				break;
			}
			default:
				break;
			}

			//cout << "pNext: " << pNext->pos.row << ", " << pNext->pos.col << endl;
			//system("pause");

			if (pNext->pos.row < 0 || pNext->pos.row >= ROWS || pNext->pos.col < 0 || pNext->pos.col >= COLS)
			{
				cout << "超出地图边界" << endl;
				continue;
			}

			//cout << closeList[pNext->pos.row][pNext->pos.col] << ", pNxet: " << pNext->pos.row << ", " << pNext->pos.col << endl;
			if (map0.myMap[pNext->pos.row][pNext->pos.col] == 1 || closeList[pNext->pos.row][pNext->pos.col]) //是障碍物或者走过
			{
				continue;
			}

			//计算h值
			int X = abs(pNext->pos.row - map0.endP.row);
			int Y = abs(pNext->pos.col - map0.endP.col);
			pNext->pos.h = (X + Y) * ZXCOST;//曼哈顿距离

			//计算f值
			if (type == Astar)
			{
				pNext->pos.f = cost + pNext->pos.h;//Astar算法，这里g只能表示单步的代价值，而cost才是从起点到该点的累计代价值
			}
			else if (type == Dijkstra)
			{
				pNext->pos.f = cost;//Dijkstra算法
			}
			else if (type == BestFS)
			{
				pNext->pos.f = pNext->pos.h;//最佳优先算法
			}

			pCurrent->child.push_back(pNext);//存入树中，作为当前点的孩子
			pNext->pParent = pCurrent;//下个点的父结点指向当前点

			bool flag = false;
			for (auto iter = openList.begin(); iter != openList.end(); iter++)
			{
				if ((*iter)->pos.row == pNext->pos.row && (*iter)->pos.col == pNext->pos.col)
				{
					flag = true;//防止重复存入相同的点
					break;
				}
			}
			if (!flag)
			{
				openList.push_back(pNext);//存入openList数组中，用于寻找最小f的点
			}

			//cout << "pNext: " << pNext->pos.row << ", " << pNext->pos.col << endl;
			//system("pause");
		}

		/*for (auto iter = openList.begin(); iter != openList.end(); iter++)
		{
			cout << (*iter)->pos.row << ", " << (*iter)->pos.col << endl;
		}*/
		//system("pause");

		//从openList数组中选取f最小的点
		auto itMin = openList.begin();
		for (auto it = openList.begin(); it != openList.end(); it++)
		{
			if ((*it)->pos.f < (*itMin)->pos.f)
			{
				itMin = it;
			}
		}
		
		pCurrent = *itMin;//走到这个点
		closeList[pCurrent->pos.row][pCurrent->pos.col] = true;//标记为走过
		map0.myMap[pCurrent->pos.row][pCurrent->pos.col] = 5;//显示为5
		cost += (*itMin)->pos.g;//中间试探点的累计路径长度
		cout << "f值最小的点是：(" << (*itMin)->pos.row << ", " << (*itMin)->pos.col << "), f=" << (*itMin)->pos.f << ", g=" << (*itMin)->pos.g << ", h=" << (*itMin)->pos.h << ", 累计代价是：" << cost << endl;
		itMin = openList.erase(itMin);//从openList数组里删除这个点

		/*for (auto iter = openList.begin(); iter != openList.end(); iter++)
		{
			cout << (*iter)->pos.row << ", " << (*iter)->pos.col << endl;
		}*/
		//system("pause");

		/*for (int i = 0; i < ROWS; i++)
		{
			for (int j = 0; j < COLS; j++)
			{
				cout << closeList[i][j] << " ";
			}
			cout << endl;
		}
		cout << endl;*/

		//判断是否找到终点
		if (bEndPoint(pCurrent))
		{
			cout << "已到达终点" << endl << endl;
			showResult();
			break;//到达终点，退出while循环
		}
		else
		{
			map0.drawRec(pCurrent->pos.row, pCurrent->pos.col, YELLOW);//用黄色填充试探的路径点
		}

		if (openList.empty())//判断是否走完了整个地图
		{
			cout << "没有找到终点" << endl << endl;
			break;//走完了整个地图都没找到终点，也退出while循环
		}

		EndBatchDraw();
		//map0.delay(100);//用于实时展示
	}
}

void tree::showResult()//打印结果
{
	while (pCurrent)//循环遍历树，直到pCurrent为空，也就是到根结点为止
	{
		BeginBatchDraw();
		//cout << "(" << pCurrent->pos.row << ", " << pCurrent->pos.col << ") ";//这样直接打印是逆序的，从终点到起点；
		result.insert(result.begin(), pCurrent);//头插存入容器，不然只能逆序打印
		costFinal += pCurrent->pos.g;//累计路径长度

		if (!bBeginPoint(pCurrent) && !bEndPoint(pCurrent))//如果不是起点和终点
		{
			map0.myMap[pCurrent->pos.row][pCurrent->pos.col] = 6;//用6标记最终的路径点
			map0.drawRec(pCurrent->pos.row, pCurrent->pos.col, GREEN);//用绿色填充最终的路径点
		}
		
		pCurrent = pCurrent->pParent;//指针向上移动到父结点
		EndBatchDraw();
	}
	map0.showMap();

	string typeName;
	if (type == Astar)
	{
		typeName = "Astar";
	}
	else if (type == Dijkstra)
	{
		typeName = "Dijkstra";
	}
	else if (type == BestFS)
	{
		typeName = "BestFS";
	}
	cout << typeName << "算法求得的最短路径是：" << endl;
	for (auto it = result.begin(); it != result.end(); it++)
	{
		cout << "(" <<(*it)->pos.row << ", " << (*it)->pos.col << ")";
		if (!bEndPoint(*it))//不是终点就打印箭头
		{
			cout << " -> ";
		}
	}
	cout << endl;

	cout << "总路径长度是：" << costFinal << endl;
}

bool tree::bBeginPoint(treeNode* point)//是否是起点
{
	if (point->pos.row == map0.beginP.row && point->pos.col == map0.beginP.col)
	{
		return true;
	}
	return false;
}

bool tree::bEndPoint(treeNode* point)//是否是终点
{
	if (point->pos.row == map0.endP.row && point->pos.col == map0.endP.col)
	{
		return true;
	}
	return false;
}

void ShortestPath::shortestPath()//求解路径
{
	system("pause");
	tr.updateTree();
}





int main()
{
	initgraph(Swidth, Sheight, EW_SHOWCONSOLE);// easyx初始化，同时创建图形窗口和命令窗口	

	ShortestPath sPath;
	sPath.shortestPath();

	system("pause");
	closegraph();
	return 0;
}
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
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
181
182
183
184
185
186
187
188
189
190
191
192
193
194
195
196
197
198
199
200
201
202
203
204
205
206
207
208
209
210
211
212
213
214
215
216
217
218
219
220
221
222
223
224
225
226
227
228
229
230
231
232
233
234
235
236
237
238
239
240
241
242
243
244
245
246
247
248
249
250
251
252
253
254
255
256
257
258
259
260
261
262
263
264
265
266
267
268
269
270
271
272
273
274
275
276
277
278
279
280
281
282
283
284
285
286
287
288
289
290
291
292
293
294
295
296
297
298
299
300
301
302
303
304
305
306
307
308
309
310
311
312
313
314
315
316
317
318
319
320
321
322
323
324
325
326
327
328
329
330
331
332
333
334
335
336
337
338
339
340
341
342
343
344
345
346
347
348
349
350
351
352
353
354
355
356
357
358
359
360
361
362
363
364
365
366
367
368
369
370
371
372
373
374
375
376
377
378
379
380
381
382
383
384
385
386
387
388
389
390
391
392
393
394
395
396
397
398
399
400
401
402
403
404
405
406
407
408
409
410
411
412
413
414
415
416
417
418
419
420
421
422
423
424
425
426
427
428
429
430
431
432
433
434
435
436
437
438
439
440
441
442
443
444
445
446
447
448
449
450
451
452
453
454
455
456
457
458
459
460
461
462
463
464
465
466
467
468
469
470
471
472
473
474
475
476
477
478
479
480
481
482
483
484
485
486
487
488
489
490
491
492
493
494
495
496
497
498
499
500
501
502
503
504
505
506
507
508
509

测试结果：

红色点代表起点，蓝色点代表终点，褐色点代表障碍物，黄色点代表搜索过程中试探到的点，绿色点代表此算法求出的最终路径点；

简单地图：

较复杂地图：

更复杂的地图：

可以看出，在各种地图下，只要从起点到终点存在可到达的路径，三种算法都有解；

算法速度方面，Dijkstra算法速度通常最慢，BestFS算法速度通常最快，而A*算法速度通常介于两者之间；

算法精确度方面，Dijkstra算法通常能准确地求出最短路径，而BestFS算法和A*算法求得的不一定是最短路径，但A*算法求出的路径通常会比BestFS算法的路径长度更短；

算法遍历格栅数方面，Dijkstra算法遍历到的格栅数通常最多，几乎要遍历整个地图，因此速度相对最慢；而BestFS算法遍历到的格栅数通常最少，因此最快；A*算法通常介于两者之间；

所以，在实际问题中，应根据实际情况选取合适的算法、选取合适的h值（启发函数），来综合考虑到算法的精确度和速度。