路径规划A*算法介绍
A*（A-Star）算法是一种常用的寻路算法，用于图形表达的环境中找到起点到目标点的最短路径。
代价函数𝑓(𝑛)由两部分组成：起点沿着已生成的路径到达当前节点的开销𝑔(𝑛)和当前节点到终点的预估开销ℎ(𝑛)。公式表示： 𝑓(𝑛) = 𝑔(𝑛) + ℎ(𝑛)
A*算法的详细原理之定义
基本定义
G: G表示从起点A移动到网格上指定方格的移动耗费(上下左右，下面实例没写沿斜方向移动)
H: H表示从指定方格移动到终点的预计耗费(H方法多种，这里设定忽略障碍物上下左右移动)
F: F=G+H，表示该点的总耗费
4.open列表:一个记录下所有被考虑来寻找最短路径的格子
5. closed列表: 一个记录下不会再被考虑的格子
曼哈顿距离：通过计算两点在空间中沿着网格线（垂直和水平方向）距离来度量它们之间的距离
A*算法结合了广度优先搜索和启发式搜索的思想。它使用一个启发式函数来估计从当前位置到目标位置的代价，通过不断更新并选择代价最低的路径来进行搜索。
下面是A*算法的详细步骤：
1. 初始化
– 将起点放入一个待处理节点列表中，并将其代价设为0。
– 初始化一个节点存储表，用于记录每个节点的父节点、代价等信息。
– 初始化一个节点集合，用于存储已经访问过的节点。
2. 循环直到找到目标点或者遍历完所有可达节点：
– 从待处理节点列表中选择代价最低的节点，作为当前节点。
– 如果当前节点为目标点，算法结束，可以通过节点存储表回溯找到最短路径。
– 将当前节点从待处理节点列表中移除，放入已访问节点集合中。
– 对当前节点的相邻节点进行处理：
– 如果相邻节点已经在已访问节点集合中，跳过该节点。
– 计算相邻节点的代价，包括从起点到该节点的代价和从该节点到目标点的启发式估计代价。
– 如果相邻节点不在待处理节点列表中，将其加入，并更新节点存储表。
– 如果相邻节点已经在待处理节点列表中，并且新的代价更小，则更新节点存储表中的信息。
3. 如果待处理节点列表为空，则表示无法从起点找到目标点，算法结束。
A*算法的关键在于如何选择合适的启发式函数来估计代价。一个好的启发式函数应该满足以下条件：
– 它应该是可计算的，并且能够对任意节点对进行估价。
– 对于目标点，它的估价应该为0。
– 它应该尽可能准确地估计节点到目标点的实际代价
这里使用曼哈顿距离
A*算法在寻路问题中具有较好的性能和效率，在很多实际应用中得到广泛使用。但需要注意的是，A*算法只能保证找到一条最短路径，而不能保证是全局最优的路径。同时，在大规模图形环境中，A*算法的效率可能会受到影响，可以采用一些优化策略来加快搜索速度。
路径规划A*算法实现实例
Node：定义每个结点的属性
using UnityEngine;
using System;
//寻路算法的一个节点
public class Node : IComparable
{
public float nodeTotalCost;//总成本
public float estimatedCost;//估算成本
//estimatedCost=nodeTotalCost+estimatedCost
//f = g + h
public bool bObstacle;//障碍物
public Node parent;//父节点
public Vector3 position;//每个节点的位置
public Node() { //结点信息
this.estimatedCost = 0.0f;
this.nodeTotalCost = 1.0f;
this.bObstacle = false;
this.parent = null;
}
public Node(Vector3 pos) {//结点位置
this.estimatedCost = 0.0f;
this.nodeTotalCost = 1.0f;
this.bObstacle = false;
this.parent = null;
this.position = pos;
}
public void MarkAsObstacle() { //设置障碍物
this.bObstacle= true;
}
public int CompareTo(object obj)//比较节点估算成本的大小
{
Node node = (Node)obj;
if (this.estimatedCost < node.estimatedCost){
return -1;
}
if (this.estimatedCost>node.estimatedCost){
return 1;
}
return 0;
}
}
PriorityQueue：优先级队列
using System.Collections;
using System.Collections.Generic;
using UnityEngine;
//优先级队列
public class PriorityQueue
{
private ArrayList nodes= new ArrayList();//数组列表
public int Length { //获取结点的数量
get { return this.nodes.Count; }
}
public Node First()//获取优先级队列第一个结点
{
if (this.nodes.Count > 0)
{
return (Node)this.nodes[0];
}
return null;
}
public bool Contains(object node) { //检查队列里是否有该结点
return this.nodes.Contains(node);
}
public void Push(Node node) { //存入优先级队列
this.nodes.Add(node);
this.nodes.Sort();//从小到大排序
}
public void Remove(Node node) {//从优先级队列删除
this.nodes.Remove(node);
this.nodes.Sort();
}
}GirdManager：网格管理器，画出参考网格线
网格编号从原点开始，左为X正方向，上位Z轴正方向
using System.Collections;
using UnityEngine;
//网格管理器
public class GirdManager : MonoBehaviour
{
private static GirdManager s_Instance=null;
public static GirdManager instance {//单例
get
{
if (s_Instance == null) {
s_Instance = FindObjectOfType(typeof(GirdManager)) as GirdManager;
if (s_Instance == null)
{
Debug.Log("你在场景中必须要有一个网格管理器");
}
}
return s_Instance;
}
}
private void OnApplicationQuit()
{
s_Instance = null;
}
public int numOfRows;//网格行数
public int numOfColumns;//网格列数
public float gridCellSize;//每一个网格大小
public bool showGrid = true;//画出网格
private GameObject[] obstacleList;//障碍物数组
private Node[,] nodes;//结点数组
private Vector3 origin = new Vector3();
public Vector3 Origin {
get { return origin;}
}
void Awake() {//初始化时必须执行
origin=this.transform.position;
obstacleList = GameObject.FindGameObjectsWithTag("Obstacle");//查找障碍物，障碍物标签标识为Obstacle
CalculateObstacles();
}
private void CalculateObstacles()//计算障碍物属于哪个网格
{
nodes = new Node[numOfColumns,numOfRows];
int index = 0;
for (int i = 0; i<numOfColumns; i++)
{
for (int j = 0; j<numOfRows; j++)
{
Node node = new Node(GetGridCellCenter(index));
nodes[i, j] = node;
index++;
}
}
if (obstacleList != null && obstacleList.Length > 0)
{
foreach (GameObject data in obstacleList)
{
int indexCell = GetGridIndex(data.transform.position);
int col = GetColumn(indexCell);
int row = GetRow(indexCell);
nodes[row, col].MarkAsObstacle();
}
}
}
public int GetGridIndex(Vector3 pos) {//获取网格序号
pos-= Origin;
int col = (int)(pos.x / gridCellSize);
int row = (int)(pos.z / gridCellSize);
return (row * numOfColumns + col);
}
public Vector3 GetGridCellCenter(int index)//获取网格中心点
{
Vector3 cellPosition = GetGridCellPosition(index);
cellPosition.x += (gridCellSize / 2.0f);
cellPosition.z += (gridCellSize / 2.0f);
return cellPosition;
}
public Vector3 GetGridCellPosition(int index) {//获取网格左位置点
int row = GetRow(index);
int col = GetColumn(index);
float xPosInGrid = col * gridCellSize;
float zPosInGrid = row * gridCellSize;
return Origin + new Vector3(xPosInGrid, 0.0f,zPosInGrid);
}
public int GetColumn(int index) {//获取列数
int col = index % numOfColumns;
return col;
}
public int GetRow(int index) {//获取行数
int row= index / numOfColumns;
return row;
}
void OnDrawGizmos()//画出网格
{
if (showGrid) {
DebugDrawGrid(transform.position, numOfRows, numOfColumns, gridCellSize, UnityEngine.Color.blue);
}
Gizmos.DrawSphere(transform.position, 0.5f);//在原点画出一个球，方便查看原点方位
}
private void DebugDrawGrid(Vector3 origin, int numRows,int numColumns,float cellSize, UnityEngine.Color color)
{
float width = numColumns * cellSize;//宽度
float height = numRows * cellSize;//高度
for (int i = 0; i < numRows + 1; i++)
{
Vector3 startPos = origin + i * cellSize * new Vector3(0.0f,0.0f,1.0f);
Vector3 endPos = startPos + width * new Vector3(1.0f,0.0f, 0.0f);
Debug.DrawLine(startPos,endPos, color);
}
for (int i = 0; i < numColumns+1; i++)
{
Vector3 startPos = origin + i * cellSize*new Vector3(1.0f, 0.0f,0.0f);
Vector3 endPos = startPos + height * new Vector3(0.0f,0.0f,1.0f);
Debug.DrawLine(startPos, endPos,color);
}
}
public void GetNeighbors(Node node,ArrayList neighbors)//获取邻居
{
Vector3 neighborPos = node.position;
int neighborIndex = GetGridIndex(neighborPos);
int row=GetRow(neighborIndex);
int column=GetColumn(neighborIndex);
//下
int leftNodeRow = row - 1;
int rightNodeColumn = column;
AssignNeighbor(leftNodeRow, rightNodeColumn, neighbors);
//上
leftNodeRow = row + 1;
rightNodeColumn = column;
AssignNeighbor(leftNodeRow, rightNodeColumn, neighbors);
//右
leftNodeRow = row;
rightNodeColumn = column+1;
AssignNeighbor(leftNodeRow, rightNodeColumn, neighbors);
//左
leftNodeRow = row ;
rightNodeColumn = column-1;
AssignNeighbor(leftNodeRow, rightNodeColumn, neighbors);
}
private void AssignNeighbor(int Row, int Column, ArrayList neighbors)//将结点的行列放入动态数组
{
if (Row != -1 && Column != -1 && Row < numOfRows && Column < numOfColumns)
{
Node nodeToAdd = nodes[Row, Column];
if (!nodeToAdd.bObstacle) {
neighbors.Add(nodeToAdd);
}
}
}
}
AStar算法
using System.Collections;
using UnityEngine;
//AStar算法
public class AStar
{
public static PriorityQueue closedList, openList;
private static float NodeCost(Node a, Node b) {//返回两点之间的距离
Vector3 vecCost = a.position - b.position;
return vecCost.magnitude;
}
public static ArrayList FindPath(Node start, Node goal)//在起始点到终点间寻路
{
openList = new PriorityQueue();
openList.Push(start);//起点存入openList
start.nodeTotalCost = 0.0f;
start.estimatedCost = NodeCost(start, goal);
closedList = new PriorityQueue();
Node node = null;
while (openList.Length != 0)
{
node = openList.First();
if (node.position == goal.position)
{
return CalculatePath(node);
}
ArrayList neighbours = new ArrayList();
GirdManager.instance.GetNeighbors(node, neighbours);
for (int i = 0; i < neighbours.Count; i++) {
Node neighbourNode = (Node)neighbours[i];
if (!closedList.Contains(neighbourNode))
{
float cost = NodeCost(node, neighbourNode);
float totalCost = node.nodeTotalCost + cost;
float neighbourNodeEstCost = NodeCost(neighbourNode, goal);
neighbourNode.nodeTotalCost = totalCost;
neighbourNode.estimatedCost = totalCost + neighbourNodeEstCost;
neighbourNode.parent = node;
if (!openList.Contains(neighbourNode))
{
openList.Push(neighbourNode);
}
}
}
closedList.Push(node);
openList.Remove(node);
}
if (node.position != goal.position)
{
Debug.LogError(" Goal Not Found");
return null;
}
return CalculatePath(node);
}
private static ArrayList CalculatePath(Node node)
{
ArrayList list = new ArrayList();
while (node != null)
{
list.Add(node);
node = node.parent;
}
list.Reverse();
return list;
}
}
TestCode测试：定义一个起点和终点，到达终点时通过遍历返回父节点动态的画出路径
using System.Collections;
using UnityEngine;
//测试
public class TestCode : MonoBehaviour
{
private Transform startPos, endPos;//起点和终点
public Node startNode { get; set; }
public Node goalNode { get; set; }
GameObject objStartCube, objEndCube;
public ArrayList pathArray;
public float intervalTime = 1.0f;
private float elapsedTime = 0.0f;
void Start()
{
objStartCube = GameObject.FindGameObjectWithTag("Start");
objEndCube = GameObject.FindGameObjectWithTag("End");
FindPath();
}
private void FindPath()//查找路径
{
startPos = objStartCube.transform;
endPos = objEndCube.transform;
startNode = new Node(GirdManager.instance.GetGridCellCenter(GirdManager.instance.GetGridIndex(startPos.position)));
goalNode = new Node(GirdManager.instance.GetGridCellCenter(GirdManager.instance.GetGridIndex(endPos.position)));
pathArray = AStar.FindPath(startNode, goalNode);//返回父节点路径
}
void Update()
{
elapsedTime += Time.deltaTime;
if (elapsedTime > intervalTime)
{
elapsedTime = 0.0f;
FindPath();//每隔一秒事件更新一次路径
}
}
void OnDrawGizmos()
{
if (pathArray == null)
{
return;
}
if (pathArray.Count > 0)
{
int index = 1;
foreach (Node node in pathArray)
{
if (index < pathArray.Count)
{
Node nextNode = (Node)pathArray[index];
Debug.DrawLine(node.position, nextNode.position, Color.green);
index++;
}
}
}
}
}注意：设置每个cube的tag
路径实现结果展示
路径规划A*算法实例
学习参考：AStar_demo_1_哔哩哔哩_bilibili
版权声明：本文为博主作者：是否对错原创文章，版权归属原作者，如果侵权，请联系我们删除！
原文链接：https://blog.csdn.net/qq_53077771/article/details/132091355
