路径规划A*(A-Star)算法-Toy模板网

这篇具有很好参考价值的文章主要介绍了路径规划A*(A-Star)算法。希望对大家有所帮助。如果存在错误或未考虑完全的地方，请大家不吝赐教，您也可以点击"举报违法"按钮提交疑问。

路径规划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轴正方向

a*算法路径规划,算法,数据结构

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