算法学习——A*搜索算法

A*算法适用于静态图搜索。

1 A*算法基本原理

1.1 估价函数

$$f(i)=g(i)+h(i)$$
$f(i)$：当前节点的价值估值。
$g(i)$：起始点至当前节点的距离（已经付出的代价）。点在网格可以上下左右移动，一般取横向和纵向移动代价为10，也可以斜方向运动，一般取对角线的移动代价为14（$10\sqrt{2}$）。
$h(i)$：启发式函数，表示当前节点至终点的预计代价，一般用曼哈顿距离计算（无视障碍物）。

曼哈顿距离计算方法如下图所示：

1.2 列表与节点

• Open List
  记录所有被考虑来寻找最短路径的格子。
• Close List
  记录不会再被考虑的格子，即已经走过的节点。
• 父节点
  方格中心的点成为节点。父节点周围的节点均指向该点，且周围点的G值即为它们到父节点的距离加上父节点当前G值。由于搜索过程中父亲节会不断更新，所以节点的F值和G值可能会改变。

1.3 算法流程

1. 将起点加入开放列表中；
2. 寻找起点周围可以到达的方格（最多8个），将它们加入到开放列表，并设置它们的父节点为起点；
3. 从开放列表中删除起点，将其放入封闭列表中；
4. 计算周围方格的F值；
5. 从开放列表中选择F值最小的方格a，将其从开放列表中删除，放入到封闭列表中（已经走过的路径不需要再考虑）；
6. 检查a所有临近并且可达的方格：
   a. 障碍物和封闭列表中的方格不再考虑；
   b. 如果这些方格不在开放列表中，则将它们加入到开放列表，并计算这些方格的F值，将父节点设为a（即这些节点的父指针指向当前节点a）。
   c. 如果当前处理节点a的某相邻方格b已在开放列表，则重新计算新的G值，即从起点经过a到方格b的距离，通过G值大小来检查这条路径是否更优。（方格节点的G值可能会根据搜索路径的不同发生变化。）
    1° 如果新的G值比当前节点a大，不做任何操作。
    2° 如果新的G值比当前节点a小，则将当前处理节点a设为父节点，并重新计算周围节点的F值。
7. 继续从开放列表中找出F值最小的节点，将其从开放列表中删除，添加到关闭列表，再以此节点为中心，继续找出周围可到达的方块，如此循环。
8. 结束判断：
   a. 当开放列表中出现终点时说明路径已找到，结束循环。
   b. 当开放列表中没有数据，说明没有找到合适路径，结束循环。

2 算法实现（C++）

#include <stdio.h>
#include <stdlib.h>
#include <iostream>
#include <vector>
using namespace std;

#define MAP_WIDTH  11  // 图宽 (1 pixel block)
#define MAP_HEIGHT 9   // 图高 (1 pixel block)

#define straight_cost 10  // 直线代价
#define diag_cost 14	  // 斜线代价	

typedef struct
{
	int8_t row;
	int8_t col;
	uint16_t f;
	uint16_t g;
	uint16_t h;
}point_t;

//树的节点类型
struct treeNode   // 结构体可以定义自身类型的指针（指针变量大小可以确定，而定义自身类型的成员则不行，因为编译时成员存储空间大小不确定）
{
	point_t pos;					// 当前点坐标
	struct treeNode* pParent;		// 存储当前点的父节点的指针变量
};

const uint8_t map[MAP_HEIGHT][MAP_WIDTH] = {
	{0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0,},
	{0, 0, 1, 1, 0, 1, 0, 0, 0, 0, 0,},
	{0, 0, 0, 1, 1, 1, 0, 0, 0, 0, 0,},
	{0, 0, 1, 0, 1, 1, 0, 0, 0, 0, 0,},
	{0, 0, 1, 1, 0, 1, 0, 0, 0, 0, 0,},
	{0, 0, 0, 1, 1, 1, 0, 0, 0, 0, 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, 1, 0, 0, 0, 0, 0,},
											};

// 2 辅助地图->标记已走过的点（封闭列表）  false: 没有走过  true: 已走过
bool closeList[MAP_HEIGHT][MAP_WIDTH] = { false }; 

// 3 起点 终点
point_t start_pos = { 2, 2 };
point_t end_pos = { 4, 8 };

enum dircet
{
	up,
	down,
	left,
	right,
	lup,
	ldown,
	rup,
	rdown
};


// 计算预估代价H
uint16_t getH(point_t end_pos, point_t current_pos)
{
	uint16_t x = abs(end_pos.col - current_pos.col);
	uint16_t y = abs(end_pos.row - current_pos.row);

	return (x + y) * straight_cost;
}

bool isAddOpenList(point_t pos, const uint8_t(*pMap)[MAP_WIDTH], bool(*pCloseList)[MAP_WIDTH])
{
	if (pos.row >= MAP_HEIGHT || pos.row < 0 ||
		pos.col >= MAP_WIDTH || pos.col < 0)    // 越界
		return false;

	if (1 == pMap[pos.row][pos.col])  //! 障碍物
		return false;

	if (true == pCloseList[pos.row][pos.col])  //已走过
		return false;

	return true;
}

bool isFindEnd = false;

int main()
{
	//4 标记起点走过
	closeList[start_pos.row][start_pos.col] = true;

	//5 创建一颗树，树的根节点是起点
	treeNode* pRoot = new treeNode;

	//新节点是起点
	pRoot->pos = start_pos;

	//6 用一个动态数组存储用来比较的节点（开放列表）
	vector<treeNode*> openList;
	
	//7 寻路
	treeNode* pCurrent = pRoot;
	treeNode* pChild = NULL;

	vector<treeNode*>::iterator it;
	vector<treeNode*>::iterator itMin;
	while (1) {

		//7.1 找到当前点周围能走的点
		for (uint8_t i = 0; i < 8; i++)
		{
			pChild = new treeNode;
			pChild->pos = pCurrent->pos; 
			switch (i)
			{
			case up:
				pChild->pos.row--;
				pChild->pos.g += straight_cost;
				break;

			case down:
				pChild->pos.row++;
				pChild->pos.g += straight_cost;
				break;

			case dircet::left:
				pChild->pos.col--;
				pChild->pos.g += straight_cost;
				break;

			case dircet::right:
				pChild->pos.col++;
				pChild->pos.g += straight_cost;
				break;

			case lup:
				pChild->pos.row--;
				pChild->pos.col--;
				pChild->pos.g += diag_cost;
				break;
			case ldown:
				pChild->pos.row++;
				pChild->pos.col--;
				pChild->pos.g += diag_cost;
				break;

			case rup:
				pChild->pos.row--;
				pChild->pos.col++;
				pChild->pos.g += diag_cost;
				break;
			case rdown:
				pChild->pos.row++;
				pChild->pos.col++;
				pChild->pos.g += diag_cost;
				break;
			default:
				break;
			}

			//7.2 计算 g h f 值
			pChild->pos.h = getH(end_pos, pChild->pos);
			pChild->pos.f = pChild->pos.g + pChild->pos.h;

			//7.3 入树，入开放列表，标记走过
			if (isAddOpenList(pChild->pos, map, closeList))  // 判断是否满足入树条件
			{
				pChild->pParent = pCurrent;   //父指针指向当前点

				//入开放列表（open_list）
				openList.push_back(pChild);

				//标记已走过
				closeList[pChild->pos.row][pChild->pos.col] = true;
			}
			else {
				delete pChild;
			}
		}

		if (openList.empty())  //! 如果开放列表为空，停止搜索（不会此条件遇到死路时，一直卡在循环不断开辟堆区空间造成内存溢出）
			break;

		//7.4 从开放列表中找到f值最小的节点
		itMin = openList.begin();
		for (it = openList.begin(); it != openList.end(); it++)
		{
			itMin = ((*itMin)->pos.f > (*it)->pos.f) ? it : itMin;
		}
		//7.5 然后变为当前点，再从开放列表中删掉
		pCurrent = *itMin;
		openList.erase(itMin);

		//7.6 判断寻路结束
		if (pCurrent->pos.col == end_pos.col && pCurrent->pos.row == end_pos.row)
		{
			isFindEnd = true;
			break;
		}
	}

	if (isFindEnd == true)
	{
		printf("找到终点了\n");

		while (pCurrent)
		{
			printf("(%d,%d)", (int)pCurrent->pos.row, (int)pCurrent->pos.col);
			if (pCurrent->pos.row == start_pos.row && pCurrent->pos.col == start_pos.col)
				break;
			pCurrent = pCurrent->pParent;   // 通过父指针回溯根节点(起点父指针为空)
		}
	}
	else
		printf("没有找到终点\n");
		
	delete pRoot;
	
	system("pause");
	return 0;
}

运行结果如下：