算法介绍
A*（念做：A Star）算法是一种很常用的路径查找和图形遍历算法。它有较好的性能和准确度。本文在讲解算法的同时也会提供Python语言的代码实现，并会借助matplotlib库动态的展示算法的运算过程。
A*算法最初发表于1968年，由Stanford研究院的Peter Hart, Nils Nilsson以及Bertram Raphael发表。它可以被认为是Dijkstra算法的扩展。
由于借助启发函数的引导，A*算法通常拥有更好的性能。
A*算法通过下面这个函数来计算每个节点的优先级。
其中：
f(n)是节点n的综合优先级。当我们选择下一个要遍历的节点时，我们总会选取综合优先级最高（值最小）的节点。
g(n) 是节点n距离起点的代价。
h(n)是节点n距离终点的预计代价，这也就是A*算法的启发函数。关于启发函数我们在下面详细讲解。
A*算法在运算过程中，每次从优先队列中选取f(n)值最小（优先级最高）的节点作为下一个待遍历的节点。
另外，A*算法使用两个集合来表示待遍历的节点，与已经遍历过的节点，这通常称之为 open_set 和close_set。
完整的A*算法描述如下：
* 初始化open_set和close_set；
* 将起点加入open_set中，并设置优先级为0（优先级最高）；
* 如果open_set不为空，则从open_set中选取优先级最高的节点n：
* 如果节点n为终点，则：
* 从终点开始逐步追踪parent节点，一直达到起点；
* 返回找到的结果路径，算法结束；
* 如果节点n不是终点，则：
* 将节点n从open_set中删除，并加入close_set中；
* 遍历节点n所有的邻近节点：
* 如果邻近节点m在close_set中，则：
* 跳过，选取下一个邻近节点
* 如果邻近节点m也不在open_set中，则：
* 设置节点m的parent为节点n
* 计算节点m的优先级
* 将节点m加入open_set中
启发函数
上面已经提到，启发函数会影响A*算法的行为。
在极端情况下，当启发函数h(n)始终为0，则将由g(n)决定节点的优先级，此时算法就退化成了Dijkstra算法。
如果h(n)始终小于等于节点n到终点的代价，则A*算法保证一定能够找到最短路径。但是当h(n)的值越小，算法将遍历越多的节点，也就导致算法越慢。
如果h(n)完全等于节点n到终点的代价，则A*算法将找到最佳路径，并且速度很快。可惜的是，并非所有场景下都能做到这一点。因为在没有达到终点之前，我们很难确切算出距离终点还有多远。
如果h(n)的值比节点n到终点的代价要大，则A*算法不能保证找到最短路径，不过此时会很快。
在另外一个极端情况下，如果h()n相较于g(n)大很多，则此时只有h(n)产生效果，这也就变成了最佳优先搜索。
关于距离
曼哈顿距离
如果图形中只允许朝上下左右四个方向移动，则启发函数可以使用曼哈顿距离
计算曼哈顿距离的函数如下，这里的D是指两个相邻节点之间的移动代价，通常是一个固定的常数。
function heuristic(node) =
dx = abs(node.x - goal.x)
dy = abs(node.y - goal.y)
return D * (dx + dy)
对角距离
如果图形中允许斜着朝邻近的节点移动，则启发函数可以使用对角距离。它的计算方法如下：
function heuristic(node) =
dx = abs(node.x - goal.x)
dy = abs(node.y - goal.y)
return D * (dx + dy) + (D2 - 2 * D) * min(dx, dy)
欧几里得距离
如果图形中允许朝任意方向移动，则可以使用欧几里得距离。
欧几里得距离是指两个节点之间的直线距离，因此其计算方法也是我们比较熟悉的：
function heuristic(node) =
dx = abs(node.x - goal.x)
dy = abs(node.y - goal.y)
return D * sqrt(dx * dx + dy * dy)
完整的A*代码
以下是unity中实现A*寻路的代码，由C#编写
public class Point
{
public int x,y;//点的坐标
public int F, G, H; //F=G+H;
public Point parent; //父节点
public Point(int x,int y)
{
this.x = x;
this.y = y;
F = 0;
G = 0;
H = 0;
}
}
public class AStar
{
private int kCost1 = 10; //直移动一个单位
private int kCost2 = 14; //斜着移动一个单位
int[,] maze = new int[8, 8];
List<Point> openList = new List<Point>(); //开启列表
List<Point> closeList = new List<Point>(); //关闭列表
public void initAstar(int[,] _maze)
{
maze = _maze;
/*for (int i = 0; i < 20; i++)
{
for (int j = 0; j < 20; j++)
{
std::cout << maze[i][j] << " ";
}
std::cout << std::endl;
}*/
}
int calcG(Point temp_start, Point point)
{
int extraG = (Math.Abs(point.x - temp_start.x) + Math.Abs(point.y - temp_start.y)) == 1 ? kCost1 : kCost2;
int parentG = point.parent == null ? 0 : point.parent.G; //如果是初始节点，则其父节点是空
return parentG + extraG;
}
int calcH(Point point, Point end)
{
return (Math.Abs(end.x - point.x) + Math.Abs(end.y - point.y)) * kCost1;
}
int calcF(Point point)
{
return point.G + point.H;
}
//获取开启列表中最小F
Point getLeastFpoint()
{
if (openList.Count!=0)
{
Point resPoint = openList[0];
foreach(Point point in openList)
if (point.F < resPoint.F)
resPoint = point;
return resPoint;
}
return null;
}
//寻找路径
Point findPath(Point startPoint, Point endPoint, bool isIgnoreCorner)
{
openList.Add(new Point(startPoint.x, startPoint.y)); //置入起点,拷贝开辟一个节点，内外隔离
while (openList.Count!=0)
{
Point curPoint = getLeastFpoint(); //找到F值最小的点
openList.Remove(curPoint); //从开启列表中删除
closeList.Add(curPoint); //放到关闭列表
//1,找到当前周围八个格中可以通过的格子
List<Point> surroundPoints = getSurroundPoints(curPoint, isIgnoreCorner);
foreach (var target in surroundPoints)
{
//2,对某一个格子，如果它不在开启列表中，加入到开启列表，设置当前格为其父节点，计算F G H
if (isInList(openList, target)==null)
{
target.parent = curPoint;
target.G = calcG(curPoint, target);
target.H = calcH(target, endPoint);
target.F = calcF(target);
openList.Add(target);
}
//3，对某一个格子，它在开启列表中，计算G值, 如果比原来的大, 就什么都不做, 否则设置它的父节点为当前点,并更新G和F
else
{
//int tempG = calcG(curPoint, target);
Point tempPoint = isInList(openList, target);
int tempG = calcG(curPoint, target);
if (tempG < tempPoint.G)
{
//cout << "x:" << target->x << "y:" << target->y << "改变后" << tempG << "改变前:" << tempPoint->G <<"CurrPoint:"<<curPoint->x<<curPoint->y<< endl;
target.parent = curPoint;
//cout << curPoint->x << ":" << curPoint->y << endl;
target.G = tempG;
target.F = calcF(target);
/*tempPoint->parent = curPoint;
cout << curPoint->x << ":" << curPoint->y << endl;
tempPoint->G = tempG;
tempPoint->F = calcF(tempPoint);*/
}
}
Point resPoint = isInList(openList, endPoint);
if (resPoint!=null)
return resPoint; //返回列表里的节点指针，不要用原来传入的endpoint指针，因为发生了深拷贝
}
}
return null;
}
//开始寻路
public List<Point> GetPath(Point startPoint, Point endPoint, bool isIgnoreCorner)
{
Point result = findPath(startPoint, endPoint, isIgnoreCorner);
List<Point> path = new List<Point>();
//返回路径，如果没找到路径，返回空链表
while (result!=null)
{
path.Add(result);
result = result.parent;
}
path.Reverse();
openList.Clear();
closeList.Clear();
return path;
}
//是否在开启列表内
Point isInList(List<Point> list, Point point)
{
//判断某个节点是否在列表中，这里不能比较指针，因为每次加入列表是新开辟的节点，只能比较坐标
foreach (var p in list)
if (p.x == point.x&&p.y == point.y)
return p;
return null;
}
//是否可以到达target
bool isCanreach( Point point, Point target, bool isIgnoreCorner)
{
if (target.x<0 || target.x> 8 - 1
|| target.y<0 || target.y> 8 - 1
|| maze[target.x,target.y] == 1
|| target.x == point.x&&target.y == point.y
|| isInList(closeList, target)!=null) //如果点与当前节点重合、超出地图、是障碍物、或者在关闭列表中，返回false
return false;
else
{
if (Math.Abs(point.x - target.x) + Math.Abs(point.y - target.y) == 1) //非斜角可以
return true;
else
{
//斜对角要判断是否绊住
if (maze[point.x,target.y] == 0 && maze[target.x,point.y] == 0)
return true;
else
return isIgnoreCorner;
}
}
}
//获取周围8个格子，可以到达的格子，并且没有在关闭列表中
List<Point> getSurroundPoints(Point point, bool isIgnoreCorner)
{
List<Point> surroundPoints = new List<Point>();
for (int x = point.x - 1; x <= point.x + 1; x++)
for (int y = point.y - 1; y <= point.y + 1; y++)
if (isCanreach(point, new Point(x, y), isIgnoreCorner))
surroundPoints.Add(new Point(x, y));
return surroundPoints;
}
}
我们在unity中制作一个简单的地图
给小球添加脚本
public int[,] maze =
{
{ 0,0,0,0,0,0,0,0},
{ 0,0,0,0,0,0,0,0},
{ 0,1,0,1,0,0,1,0},
{ 0,0,0,1,0,1,0,0},
{ 0,0,0,1,0,0,0,0},
{ 0,0,0,1,0,1,0,0},
{ 0,1,0,0,0,0,0,0},
{ 0,0,0,0,0,0,0,0}
};
int num = 0;
bool isFindPath = false; //是否在寻路
List<Point> path = new List<Point>();
public AStar astar = new AStar();
public GameObject red_click;
//设置起始和结束点
Point start = new Point(0, 0);
Point end = new Point(7, 7);
// Start is called before the first frame update
private void Awake()
{
astar.initAstar(maze);
A*算法找寻路径
//path = astar.GetPath(start, end, false);
//isFindPath = true;
//foreach (var p in path)
//Debug.Log("(" + p.x + "," + p.y + ")");
}
void Start()
{
}
// Update is called once per frame
void Update()
{
if(Input.GetMouseButtonDown(0))
{
Ray ray = Camera.main.ScreenPointToRay(Input.mousePosition);
RaycastHit hit;
bool collider = Physics.Raycast(ray, out hit);
if (collider)
{
if (hit.collider.tag == "Plane")
{
path.Clear();
num = 0;
start = new Point((int)Mathf.Round(transform.position.z), (int)Mathf.Round(transform.position.x));
end = new Point((int)Mathf.Round(hit.point.z), (int)Mathf.Round(hit.point.x));
isFindPath = true;
path = astar.GetPath(start, end, false);
//Debug.Log((int)hit.point.z+":"+ (int)hit.point.x);
Instantiate(red_click, new Vector3((int)Mathf.Round(hit.point.x), 0, (int)Mathf.Round(hit.point.z)), Quaternion.identity);
}
}
}
if (!isFindPath) return;
if(num>=path.Count)
{
num = 0;
path.Clear();
isFindPath = false;
return;
}
if(transform.position.x==path[num].y&&transform.position.z==path[num].x)
{
num++;
}
else
{
transform.position = Vector3.MoveTowards(transform.position, new Vector3(path[num].y, 0, path[num].x), 1.3f*Time.deltaTime);
}
}
2367
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
