A*算法(A-star algorithm)是一种广泛应用于路径规划和图搜索的启发式搜索算法。它结合了广度优先搜索的全面性和深度优先搜索的效率,通过估计当前路径代价和到达目标的预估代价,来找到从起点到目标的最短路径。A算法在游戏开发、机器人导航和人工智能等领域中非常常见,因为它可以有效地处理复杂的搜索空间并找到最优解。
在A*算法中,节点有三个重要参数:
- g值:从起点到当前节点的实际代价。
- h值:从当前节点到目标节点的预估代价,通常通过启发式函数(如曼哈顿距离或欧几里得距离)来计算。
- f值:f = g + h,用于评估当前节点的优先级,f值越小,节点越有可能在接下来的搜索中被探索。
通过构建一个优先队列,A*算法每次从未探索的节点中选择f值最小的节点进行扩展,直至找到目标节点或遍历完整个搜索空间。
现在我们可以开始用C++实现A*算法,基于这个背景编写代码。(先把代码放上来)
#include <iostream>
#include <vector>
#include <queue>
#include <cmath>
#include <cstring>
#include <unordered_set>
using namespace std;#define ROWS 7 // 地图行数
#define COLS 7 // 地图列数
#define ZXDJ 10 // 正交方向移动代价
#define XXDJ 14 // 对角线方向移动代价// 枚举方向
enum dirent { p_up, p_down, p_left, p_right, p_lup, p_ldown, p_rdown, p_rup
};// 表示坐标的结构体
struct MyPoint {int row, col; // 行和列int f, g, h; // A* 算法中的 f = g + hbool operator==(const MyPoint &other) const {return row == other.row && col == other.col;}
};// 哈希函数,用于在 unordered_set 中使用 MyPoint 作为键
struct MyPointHash {size_t operator()(const MyPoint &p) const {return p.row * 31 + p.col; // 简单的哈希函数}
};// 树节点结构体,用于存储节点位置和父节点
struct treeNode {MyPoint pos; // 节点位置treeNode *pParent; // 指向父节点的指针
};// 比较函数,优先队列中比较 f 值大小
struct compare {bool operator()(treeNode* a, treeNode* b) {return a->pos.f > b->pos.f; // 小顶堆,f 值小的优先}
};// 创建树节点函数
treeNode* createTreeNode(int row, int col) {treeNode* pNew = new treeNode; // 动态分配内存memset(pNew, 0, sizeof(treeNode)); // 清空结构体pNew->pos.row = row;pNew->pos.col = col;return pNew;
}// 计算 H 值(曼哈顿距离)
int getH(MyPoint pos, MyPoint endpos) {// 曼哈顿距离公式:|x1 - x2| + |y1 - y2|return ZXDJ * (abs(endpos.row - pos.row) + abs(endpos.col - pos.col));
}int main() {// 0 表示可通行,1 表示障碍物int map[ROWS][COLS] = {{0,0,0,0,0,0,0},{0,0,0,0,1,0,0},{0,0,0,0,1,0,0},{0,0,0,0,1,0,0},{0,0,0,0,0,0,0},{0,0,0,0,0,0,0,},{0,0,0,0,0,0,0},};MyPoint beginpos = { 2, 2, 0, 0, 0 }; // 起点MyPoint endpos = { 2, 6, 0, 0, 0 }; // 终点// openList: 优先队列存储待处理节点,按照 f 值排序priority_queue<treeNode*, vector<treeNode*>, compare> openList;// closedList: 哈希集合,存储已访问的节点,避免重复处理unordered_set<MyPoint, MyPointHash> closedList;// 初始化起点节点,设置 h 和 f 值treeNode* pRoot = createTreeNode(beginpos.row, beginpos.col);pRoot->pos.h = getH(beginpos, endpos); // 计算起点到终点的 h 值pRoot->pos.f = pRoot->pos.h; // 起点的 f = g + h,其中 g = 0openList.push(pRoot); // 将起点放入 openListbool isFindend = false; // 标记是否找到终点treeNode* pCurrent = nullptr; // 当前处理的节点// A* 算法主循环while (!openList.empty()) {// 从 openList 中取出 f 值最小的节点pCurrent = openList.top();openList.pop(); // 如果到达终点,退出循环if (pCurrent->pos.row == endpos.row && pCurrent->pos.col == endpos.col) {isFindend = true;break;}// 将当前节点加入 closedListclosedList.insert(pCurrent->pos);// 遍历 8 个方向的邻居节点for (int i = 0; i < 8; ++i) {int newRow = pCurrent->pos.row;int newCol = pCurrent->pos.col;int newG = pCurrent->pos.g; // 继承父节点的 g 值// 根据不同方向更新坐标和移动代价switch (i) {case p_up: newRow--; newG += ZXDJ; break;case p_down: newRow++; newG += ZXDJ; break;case p_left: newCol--; newG += ZXDJ; break;case p_right: newCol++; newG += ZXDJ; break;case p_lup: newRow--; newCol--; newG += XXDJ; break;case p_ldown: newRow++; newCol--; newG += XXDJ; break;case p_rdown: newRow++; newCol++; newG += XXDJ; break;case p_rup: newRow--; newCol++; newG += XXDJ; break;}// 边界检查,确保新位置在地图范围内if (newRow < 0 || newRow >= ROWS || newCol < 0 || newCol >= COLS)continue;// 障碍物检查,如果新位置为障碍物,跳过if (map[newRow][newCol] != 0)continue;// 创建新节点,计算其 g, h, f 值MyPoint newPoint = { newRow, newCol };// 如果新节点已在 closedList 中,跳过if (closedList.count(newPoint))continue;newPoint.g = newG;newPoint.h = getH(newPoint, endpos); // 计算新的 h 值newPoint.f = newPoint.g + newPoint.h; // f = g + h// 创建新的子节点treeNode* pChild = createTreeNode(newRow, newCol);pChild->pos.g = newG;pChild->pos.h = newPoint.h;pChild->pos.f = newPoint.f;pChild->pParent = pCurrent; // 设置父节点为当前节点// 检查是否在 openList 中存在更优路径bool isBetter = true;priority_queue<treeNode*, vector<treeNode*>, compare> tempQueue;while (!openList.empty()) {treeNode* existingNode = openList.top();openList.pop();// 如果发现相同位置的节点if (existingNode->pos.row == newRow && existingNode->pos.col == newCol) {// 如果当前路径的 g 值更小,更新节点if (newG < existingNode->pos.g) {existingNode->pos.g = newG;existingNode->pos.f = existingNode->pos.g + existingNode->pos.h;existingNode->pParent = pCurrent;}isBetter = false; // 已找到更优路径}tempQueue.push(existingNode); // 重新将节点放回队列}openList = tempQueue; // 更新 openList// 如果新路径更好,将子节点放入 openListif (isBetter) {openList.push(pChild);} else {delete pChild; // 否则释放内存}}}// 输出结果,如果找到终点,则回溯输出路径if (isFindend) {cout << "找到终点!" << endl;while (pCurrent) {cout << "(" << pCurrent->pos.row << "," << pCurrent->pos.col << ")" << endl;pCurrent = pCurrent->pParent; // 回溯路径}} else {cout << "没有找到通往终点的路径。" << endl;}system("pause");return 0;
}
找到终点!(2,6)(3,5)(4,4)(3,3)(2,2)
接下来一步步分析
1、枚举是方便switch,并且明白switch(数字)的含义
enum dirent {
p_up, p_down, p_left, p_right,
p_lup, p_ldown, p_rdown, p_rup
}
2、表示坐标的结构体,属性就是该点的行、列、F值
代码中的 operator==
函数是 MyPoint
结构体的一个重载运算符,它用于比较两个 MyPoint
对象是否相等。具体来说,它比较两个点的行和列坐标是否相同。如果两个点的行和列坐标都相同,那么这两个点就被认为是相等的。
这个重载运算符的意义在于:
-
唯一性检查:在哈希表(如
unordered_set
)中,需要判断元素是否已经存在。通过重载operator==
,可以直接使用MyPoint
对象作为unordered_set
的键,因为unordered_set
在插入元素前需要判断元素是否已经存在。 -
简化比较逻辑:在代码的其他部分,你可能需要比较两个
MyPoint
对象是否相等。通过重载operator==
,可以直接使用==
运算符进行比较,而不需要手动比较每个字段,这样代码更加简洁易读。
在这段代码中,MyPoint
结构体被用作 unordered_set
的键,以存储已经访问过的节点。如果没有重载 operator==
,unordered_set
将无法判断两个 MyPoint
对象是否相等,也就无法正确地存储和检查节点。
此外,代码中还定义了一个 MyPointHash
结构体,这是一个哈希函数,用于为 MyPoint
对象生成哈希值。这是因为 unordered_set
需要哈希值来快速定位元素。MyPointHash
结构体通过行和列坐标计算出一个整数作为哈希值,这个哈希值用于在哈希表中存储和查找 MyPoint
对象。
struct MyPoint {
int row, col; // 行和列
int f, g, h; // A* 算法中的 f = g + h
bool operator==(const MyPoint &other) const {
return row == other.row && col == other.col;
}
};
// 哈希函数,用于在 unordered_set 中使用 MyPoint 作为键
struct MyPointHash {
size_t operator()(const MyPoint &p) const {
return p.row * 31 + p.col; // 简单的哈希函数
}
};
3、treeNode
结构体被用来表示在 A* 算法中搜索路径树的节点。每个 treeNode
实例存储了以下信息:
-
pos
:这是一个MyPoint
类型的成员,它包含了当前节点在地图上的位置信息,包括行(row
)、列(col
)坐标,以及 A* 算法中用于路径搜索的g
、h
、f
值。g
值是从起点到当前节点的成本,h
值是从当前节点到终点的估计成本,f
值是g
和h
的和,代表从起点到终点通过当前节点的总成本。 -
pParent
:这是一个指向treeNode
类型的指针,它指向当前节点的父节点。在路径搜索树中,每个节点(除了根节点)都有一个父节点,指向它在树中的直接上级。这个指针用于重建从起点到终点的路径,一旦找到终点,可以通过追踪这些父节点指针回溯到起点。
struct treeNode {
MyPoint pos; // 节点位置
treeNode *pParent; // 指向父节点的指针
};
treeNode
结构体的使用场景如下:
- 在 A* 算法的执行过程中,每个待考虑的位置都会创建一个
treeNode
实例。 - 每个节点都会被评估,以确定它是否是终点,或者是否值得进一步探索其邻居节点。
- 如果一个节点不是终点,算法会生成该节点的所有可能的子节点(即邻居),并将这些子节点加入到开放列表(
openList
)中,以便后续处理。 - 每个节点都会保留指向其父节点的引用,这样一旦找到终点,就可以通过这些引用回溯到起点,从而重建整个路径。
4、创造树节点函数
treeNode* createTreeNode(int row, int col) {
treeNode* pNew = new treeNode; // 动态分配内存
memset(pNew, 0, sizeof(treeNode)); // 清空结构体
pNew->pos.row = row;
pNew->pos.col = col;
return pNew;
}
-
treeNode* createTreeNode(int row, int col)
:这是函数的声明,它说明函数的返回类型是treeNode
类型的指针,接收两个整数参数row
和col
,分别代表要创建的树节点在地图上的行和列坐标。 -
treeNode* pNew = new treeNode;
:这行代码使用new
操作符动态分配了一块内存,用于存储一个新的treeNode
实例。new
操作符会调用treeNode
的默认构造函数(如果存在),并返回一个指向这块内存的指针。 -
memset(pNew, 0, sizeof(treeNode));
:这行代码使用memset
函数将新分配的内存块初始化为零。memset
是一个标准库函数,用于将一块内存中的所有字节设置为特定的值。在这里,它将pNew
指向的内存块中的所有字节都设置为 0。这样做的目的是确保treeNode
实例中的所有成员变量都被初始化为零值,避免使用未初始化的内存。 -
pNew->pos.row = row;
和pNew->pos.col = col;
:这两行代码分别设置新创建的treeNode
实例的pos
成员的row
和col
字段。这两个字段分别代表节点在地图上的行和列坐标。 -
return pNew;
:这行代码返回指向新创建的treeNode
实例的指针。这样,调用createTreeNode
函数的代码就可以通过返回的指针来访问和操作这个新创建的树节点。
5、计算 H 值(曼哈顿距离)
int getH(MyPoint pos, MyPoint endpos) {
// 曼哈顿距离公式:|x1 - x2| + |y1 - y2|
return ZXDJ * (abs(endpos.row - pos.row) + abs(endpos.col - pos.col));
}
6、主函数分析
6.1 创建地图、初始化起点与终点
int map[ROWS][COLS] = {
{0,0,0,0,0,0,0},
{0,0,0,0,1,0,0},
{0,0,0,0,1,0,0},
{0,0,0,0,1,0,0},
{0,0,0,0,0,0,0},
{0,0,0,0,0,0,0,},
{0,0,0,0,0,0,0},
};
MyPoint beginpos = { 2, 2, 0, 0, 0 }; // 起点
MyPoint endpos = { 2, 6, 0, 0, 0 }; // 终点
6.2 创建openList和closedList
-
priority_queue
是 C++ 标准库中的一个容器适配器,它提供了一个优先队列的功能,其中元素自动按照某种优先级顺序排列。 -
treeNode*
是队列中存储的元素类型,这意味着队列中的每个元素都是指向treeNode
结构体的指针。 -
vector<treeNode*>
是内部容器类型,它指定了priority_queue
将使用vector
(向量)来存储其元素。vector
是一个动态数组,可以根据需要自动调整大小。 -
compare
是一个比较函数对象(或称为比较器),它定义了队列中元素的排序规则。在这个例子中,compare
结构体定义了如何比较两个treeNode
指针的f
值,以确定它们的优先级顺序。由于compare
结构体中的operator()
实现为a->pos.f > b->pos.f
,这意味着优先队列是一个小顶堆,f
值较小的元素会被认为优先级更高,并且会排在队列的前面。
// openList: 优先队列存储待处理节点,按照 f 值排序
priority_queue<treeNode*, vector<treeNode*>, compare> openList;
// closedList: 哈希集合,存储已访问的节点,避免重复处理
unordered_set<MyPoint, MyPointHash> closedList;
-
unordered_set
是 C++ 标准库中的一个容器,它基于哈希表实现。它存储唯一的元素,并且查找、插入和删除操作的平均时间复杂度为 O(1)。 -
MyPoint
是unordered_set
存储的元素类型。这意味着unordered_set
中的每个元素都将是一个MyPoint
对象。 -
MyPointHash
是一个自定义的哈希函数对象,它必须为MyPoint
类型的对象提供一个哈希值。这个哈希函数对象需要定义一个operator()
,该操作符接受一个MyPoint
对象作为参数,并返回一个size_t
类型的哈希值。 -
closedList
是这个unordered_set
实例的名称,可以使用这个名字来操作这个集合,比如添加元素、检查元素是否存在等。
6.3 初始化起始节点,将起始节点放入openlist中,此时没有处理的节点
// 初始化起点节点,设置 h 和 f 值
treeNode* pRoot = createTreeNode(beginpos.row, beginpos.col);
pRoot->pos.h = getH(beginpos, endpos); // 计算起点到终点的 h 值
pRoot->pos.f = pRoot->pos.h; // 起点的 f = g + h,其中 g = 0
openList.push(pRoot); // 将起点放入 openList
bool isFindend = false; // 标记是否找到终点
treeNode* pCurrent = nullptr; // 当前处理的节点
6.4 A*算法
- 首先判断
openList
是否存在值,即是否有待处理的节点。 - 将当前处理的节点设为起点,并从
openList
中移除该节点(此时起点是openList
中的最小元素)。 - 判断当前处理节点是否为终点:
- 如果是终点,则直接跳出
while
循环。 - 如果不是终点,则将该节点加入
closedList
,表示该节点已经被探索过。
- 如果是终点,则直接跳出
- 开始遍历当前节点周围的八个节点:
- 如果遍历的节点不在地图范围内、是障碍物、或者已经在
closedList
中存在,则直接进入下一次for
循环迭代。
- 如果遍历的节点不在地图范围内、是障碍物、或者已经在
- 对于每个有效的遍历节点,计算其
F
值,并设置这些节点的父节点为当前处理节点。 - 检查
openList
中是否存在更优路径:- 从
openList
的最小元素开始,判断是否存在具有相同位置但g
值更小的节点。 - 如果找到更小的
g
值,则更新该节点的父节点为当前遍历的节点,并更新其f
值。 - 循环检查
openList
中所有节点,直到完成对所有节点的检查。
- 从
- 根据是否存在该点,决定是否将当前遍历节点加入
openList
:- 如果存在该点,则更新
openList
中相应节点的信息。 - 如果不存在,则将当前遍历节点加入
openList
。
- 如果存在该点,则更新
- 重复步骤 3 至 7,直到
openList
为空或找到终点。
while (!openList.empty()) {
// 从 openList 中取出 f 值最小的节点
pCurrent = openList.top();
openList.pop();
// 如果到达终点,退出循环
if (pCurrent->pos.row == endpos.row && pCurrent->pos.col == endpos.col) {
isFindend = true;
break;
}
// 将当前节点加入 closedList
closedList.insert(pCurrent->pos);
// 遍历 8 个方向的邻居节点
for (int i = 0; i < 8; ++i) {
int newRow = pCurrent->pos.row;
int newCol = pCurrent->pos.col;
int newG = pCurrent->pos.g; // 继承父节点的 g 值
// 根据不同方向更新坐标和移动代价
switch (i) {
case p_up: newRow--; newG += ZXDJ; break;
case p_down: newRow++; newG += ZXDJ; break;
case p_left: newCol--; newG += ZXDJ; break;
case p_right: newCol++; newG += ZXDJ; break;
case p_lup: newRow--; newCol--; newG += XXDJ; break;
case p_ldown: newRow++; newCol--; newG += XXDJ; break;
case p_rdown: newRow++; newCol++; newG += XXDJ; break;
case p_rup: newRow--; newCol++; newG += XXDJ; break;
}
// 边界检查,确保新位置在地图范围内
if (newRow < 0 || newRow >= ROWS || newCol < 0 || newCol >= COLS)
continue;
// 障碍物检查,如果新位置为障碍物,跳过
if (map[newRow][newCol] != 0)
continue;
// 创建新节点,计算其 g, h, f 值
MyPoint newPoint = { newRow, newCol };
// 如果新节点已在 closedList 中,跳过
if (closedList.count(newPoint))
continue;
对于
unordered_set<MyPoint, MyPointHash> closedList;
这样的声明,count
方法的工作方式如下:
- 你传递一个
MyPoint
对象给count
方法。count
方法会使用MyPointHash
哈希函数来计算这个MyPoint
对象的哈希值。- 然后,它会在内部的哈希表中查找具有相同哈希值的元素。
- 最后,它会检查这些具有相同哈希值的元素中是否有任何一个与传递给
count
方法的MyPoint
对象相等。这是通过比较操作符(在这个例子中是MyPoint
结构体中定义的operator==
)来完成的。如果
count
方法找到了至少一个与传递的MyPoint
对象相等的元素,它会返回1
(因为unordered_set
只存储唯一的元素,所以不可能有多个相等的元素)。如果没有找到相等的元素,它会返回0
。newPoint.g = newG;
newPoint.h = getH(newPoint, endpos); // 计算新的 h 值
newPoint.f = newPoint.g + newPoint.h; // f = g + h
// 创建新的子节点
treeNode* pChild = createTreeNode(newRow, newCol);
pChild->pos.g = newG;
pChild->pos.h = newPoint.h;
pChild->pos.f = newPoint.f;
pChild->pParent = pCurrent; // 设置父节点为当前节点
// 检查是否在 openList 中存在更优路径,isBetter还可以判断该点是否已经存在
bool isBetter = true;
priority_queue<treeNode*, vector<treeNode*>, compare> tempQueue;
while (!openList.empty()) {
treeNode* existingNode = openList.top();
openList.pop();
// 如果发现相同位置的节点
if (existingNode->pos.row == newRow && existingNode->pos.col == newCol) {
// 如果当前路径的 g 值更小,更新节点
if (newG < existingNode->pos.g) {
existingNode->pos.g = newG;
existingNode->pos.f = existingNode->pos.g + existingNode->pos.h;
existingNode->pParent = pCurrent;
}
isBetter = false; //已找到这个节点,即不用再加入openList
}
tempQueue.push(existingNode); // 重新将节点放回队列
}
openList = tempQueue; // 更新 openList
// 如果该位置的节点不存在,将子节点放入 openList
if (isBetter) {
openList.push(pChild);
} else {
delete pChild; // 否则释放内存
}
}
}
6.5 回溯路径
if (isFindend) {
cout << "找到终点!" << endl;
while (pCurrent) {
cout << "(" << pCurrent->pos.row << "," << pCurrent->pos.col << ")" << endl;
pCurrent = pCurrent->pParent; // 回溯路径
}
} else {
cout << "没有找到通往终点的路径。" << endl;
}
system("pause");
return 0;