A*寻路算法，循序渐进，附完整源码

A*寻路算法—循序渐进(附完整源码)

用途

​ A*寻路算法的一般用途即为灵活地寻找初始点到目标点的最短路径。

概述

​ 灵活是A*算法更为注重的特性，可以任意添加障碍物，可以对不同地形的寻路损耗赋予不同的权重，可以设置优先级等等。最短路的算法有很多，Dijkstra，BFS(广度优先搜索)等等，本文着重介绍基于BFS实现的A星寻路算法。

BFS算法原理

​ 在介绍A星算法前，先介绍一下BFS求最短路的方法。广度优先搜索主要用到的是队列，求最短路的基本思路即为，每走一步都遍历以当前格子的九宫格范围内满足条件的格子加入进队列中，再以满足条件的格子为中心继续九宫格向外扩，不断地移除已经到达的节点并新加入可以到达的节点，循环往复直到找到终点即可。注意在此过程中要记录当前节点的上一个节点方便最终查找路径。

这里引用一张其它博主做的动图很清晰BFS 走迷宫过程动态展示_努力中的老周的专栏-CSDN博客

格子总共有三种状态，正在访问，等待访问和已经访问过，对应的正是上方描述的过程。

BFS算法模板

public class Node
{
    public bool passable; //是否可通行
    public int row,col;
    public Node lastNode; //记录上一个节点
    public Node(bool passable,int row,int col)
    {
        this.passable = passable;
        this.row = row;
        this.col = col;
    }
}

public class BFSTest
{
    Node[,] nodeMap;
    List<Node> path = new List<Node>(); //记录路径
    bool[,] used; //记录已经遍历的节点，数组大小和nodeMap保持一致
    int maxWidth,maxHeight; //初始化时需一并记录最大值以判断边界
    //这里省略初始化地图的过程 默认nodeMap已经初始化
    public void InitMap(){} 
    public List<Node> findPath(int startRow,int startCol,int endRow,int endCol)
    {
        //每次寻路前都需要先清空path和used 在此省略
        
        //省略了合法性的检验，一定要验证所给索引的边界问题
        if(BFS(startRow,startCol,endRow,endCol))
        {
            //再根据lastNode构建出路径
            Node node = nodeMap[endRow,endCol];
            while(node!=null)
            {
                path.Add(node);
                node = node.lastNode;
            }
            path.Reverse(); //反转列表
            return path;
        }
        else
        {
            Console.WriteLine("无可行路径");
            return null;
        }
    }
    
    //返回值为是否能到达结尾点 --- 防止死路情况
    private bool BFS(int startRow,int startCol,int endRow,int endCol)
    {
        //定义队列
        Queue<Node> q = new Queue<Node>();
        //始发点入队
        q.Enqueue(nodeMap[startRow,startCol]);
        nodeMap[startRow,startCol].lastNode = null;
        //每次入队都要记录used
        used[startRow,startCol] = true;
        
        while(q.Count != 0)
        {
            //每次都取队首
            Node node = q.Dequeue();
            //构建偏移量 让遍历九宫格更加方便 从左上开始顺时针定义
            int[] dx = {-1,0,1,1,1,0,-1,-1};
            int[] dy = {-1,-1,-1,0,1,1,1,0};
            for(int i = 0; i < 8; i++)
            {
                int row = node.row + dy[i];
                int col = node.col + dx[i];
                //一定记得判断边界合法性  和  used情况
                if(used[row,col] || row < 0 || row >= maxHeight || col < 0 || col >= maxWidth)
                	continue;
                q.Enqueue(nodeMap[row,col]);
                used[row,col] = true;
               	nodeMap[row,col].lastNode = node;
                //到达终点提前结束即可
                if(row == endRow && col == endCol) return true;
            }
        }
        return false;
    }
}

此模板即为实现A星算法的基本模板，利用此算法可以灵活地制定入队条件，达到灵活性的要求，但此算法仍有一个很大的弊端

其会无差别的遍历等待访问的格子，一般是谁先进队先访问谁，对找终点来讲损耗较大

​ 所以A星算法就是在此基础上增加了一种启发式算法，对每个格子求其寻路损耗f，每次访问正在等待访问的格子时，不在无差别的按照先后顺序访问，而是根据f的寻路损耗，选择最低的寻路损耗格子访问，这样就能极大的减少多余格子的访问浪费。

A*寻路算法原理

​ A*寻路算法的原理，就如上述所谈到的，其实就是对BFS无差别寻路的一种启发式优化，提供一种寻路损耗变量，每次有依据的选择最小寻路损耗的节点进行访问，可以大幅度减少无效访问次数。

接下来介绍寻路损耗f的计算指标

g：起点到当前节点的欧氏距离

h：当前节点到终点的曼哈顿距离

f：当前节点的寻路损耗 f = g + h

欧式距离即是日常数学中所提到的实际距离，公式为

曼哈顿距离的公式

下面举一个栗子

以上图为例，红色线即为起点到当前点的欧氏距离，一般水平移动一格是单位距离1，斜向移动是根号2，约等于1.4。 曼哈顿距离为黄色箭头，即为横轴和纵轴的差值，只能水平或竖直移动，一次是单位距离1。

g = 1 + 1.4 = 2.4

h = 1 + 1 + 1 = 3

f = 2.4 +3 =5.4

​ 明白了此算法，即可将等待访问的所有节点的f值求出，每次BFS选择时都选择寻路损耗f最小的值进行访问。

A*算法基于BFS的改动点

​ 基本的A*算法就是对BFS的改进，其一般采用一个开启列表和一个关闭列表，开启列表中存有等待访问的结点，关闭列表存有已经访问的结点。

分析相关改动点

针对Node节点的改动

1. 新增加寻路损耗f的属性
2. 提供计算损耗f的方法

针对BFS逻辑的改动

1. 开启列表存有等待访问的节点，即为替代原队列Queue的功能
2. 关闭列表存开启列表中访问完移除的节点，配合开启列表，判断元素是否在这两个列表中替代used数组的判重功能。
3. 每次循环选择的节点不在是队首，而是要选择f值最小的node，可以考虑每次都进行按照f值将开启列表从小到大排序然后取首个元素。(可用优先级队列进行优化)

A*算法的优化

• 将row,col用结构体封装便于传参，逻辑更加清晰
• 列表只存索引，具体Node通过索引去map中查找，节省内存空间，且同一对象的引用只在map唯一，不会造成杂糅现象。
• 采用优先级队列避免每次都要对开启列表进行排序

第三个优化点十分重要，在面对超大的地图时，排序的耗时是十分严重的。优先级队列，也叫二叉堆，实际上就是一个大(小)顶堆。其在存元素时，始终保证队首一定是最大(最小)的。这个性质十分符合A*算法在选择f最小节点时使用，且其利用的是二叉堆的特性，相较于排序，速度有很大的提升，这个优化是很有必要的。

优先级队列在C++等语言中有直接定义好的，priority_queue等，但C#并没有预先定义好的优先级队列，需要自行去实现。

有关优先级队列的知识大家有兴趣可以去自行了解，主要就是堆排序的知识点！下面贴上笔者自行实现的简易C#优先级队列。

PriorityQueue.cs

namespace Common
{
    /// <summary>
    /// 简易优先级队列的实现
    /// </summary>
    public class PriorityQueue<T>
    {
        //用一维数组存储元素
        T[] arr;
        //最大容量
        int capacity;
        public int Length
        {
            get
            {
                return length;
            }
        }
        //当前长度
        int length = 0;
        //自定义比较方式
        Func<T, T, bool> compareHandler;

		//这里没有提供自动扩容的方法，要求定义时必须指明最大容量
        public PriorityQueue(int capacity, Func<T, T, bool> compareHandler)
        {
            arr = new T[capacity];
            this.capacity = capacity;
            this.compareHandler = compareHandler;
        }

        /// <summary>
        /// 加入优先队列
        /// </summary>
        public void insert(T element)
        {
            //判断是否还有空余
            if (length + 1 > capacity) return;
            arr[length] = element;
            ModifyInsertElement(length);
            length++;
        }

        /// <summary>
        /// 调整新插入二叉堆的元素
        /// </summary>
        private void ModifyInsertElement(int index)
        {
            //边界
            if (index < 0) return;
            int father = (index - 1) / 2;
            if (compareHandler(arr[index], arr[father]))
            {
                swap(index, father);
                ModifyInsertElement(father);
            }
        }

        /// <summary>
        /// 获取队首元素
        /// </summary>
        public T top()
        {
            if (length == 0) return default(T);
            return arr[0];
        }

        /// <summary>
        /// 出队并获取队首
        /// </summary>
        /// <returns></returns>
        public T pop()
        {
            if (length == 0) return default(T);
            T tmp = arr[0];
            arr[0] = default(T);
            swap(0, length - 1);
            length--;
            ModifyPopElement(0);
            return tmp;
        }

        /// <summary>
        /// 调整出队时的二叉堆(堆顶开始从上至下)
        /// </summary>
        /// <param name="index"></param>
        private void ModifyPopElement(int index)
        {
            if (index >= length) return;
            int left = 2 * index + 1;
            int right = 2 * index + 2;

            if (left >= length && right >= length) return;
            if (left >= length)
            {
                if (compareHandler(arr[right], arr[index]))
                {
                    swap(right, index);
                    ModifyPopElement(right);
                }
                else return;
            }
            if (right >= length)
            {
                if (compareHandler(arr[left], arr[index]))
                {
                    swap(left, index);
                    ModifyPopElement(left);
                }
                else return;
            }

            //三者中取较大，若子节点较大则交换并递归向下继续查询比较
            T maxValue = compareHandler(arr[left], arr[right]) ? arr[left] : arr[right];
            maxValue = compareHandler(maxValue, arr[index]) ? maxValue : arr[index];
            if (maxValue.Equals(arr[index])) return;
            if (maxValue.Equals(arr[left]))
            {
                swap(left, index);
                ModifyPopElement(left);
            }
            if (maxValue.Equals(arr[right]))
            {
                swap(right, index);
                ModifyPopElement(right);
            }

        }

        private void swap(int a, int b)
        {
            T tmp = arr[a];
            arr[a] = arr[b];
            arr[b] = tmp;
        }

        //清空
        public void Clear()
        {
            arr = new T[capacity];
            length = 0;
        }



    }
}

A*算法详细源码

AStarNode.cs node节点类

using System.Collections;
using System.Collections.Generic;
using UnityEngine;

namespace Common.AStar
{
    public struct NodePos
    {
        public int row;
        public int col;
        public NodePos(int row,int col)
        {
            this.row = row;
            this.col = col;
        }
    }

    public enum ANodeType
    {
        //节点可通行
        passable,
        //节点不可通行
        impassable
    }

    /// <summary>
    /// A*寻路系统的逻辑节点
    /// </summary>
    public class AStarNode
    {
        //逻辑位置 （索引行列号)
        public NodePos pos;

        //节点的类型
        public ANodeType nodeType;

        //A*算法的相关计算指标
        public float f;    //寻路消耗 f = g + h
        public float g;    //起点到当前节点的移动距离
        public float h;    //当前节点到终点的曼哈顿距离

        //A*节点的上一步
        public AStarNode lastNode;
        

        public AStarNode(NodePos pos,ANodeType nodeType)
        {
            this.pos = pos;
            this.nodeType = nodeType;
        }

        /// <summary>
        /// 计算寻路消耗,使用前务必设置lastNode
        /// </summary>
        /// <param name="currentPos">当前节点</param>
        public void CalculateF(NodePos endPos)
        {
            //如果是起点 则为空
            if(lastNode == null)
            {
                g = 0;
                h = CalculateMHDis(pos, endPos);
                f = g + h;
                return;
            }
            //判断是否为上下左右 而非斜
            bool isStraight = Mathf.Abs(pos.row - lastNode.pos.row) + Mathf.Abs(pos.col - lastNode.pos.col) == 1;
            //计算g 当前节点到起点的距离
            g = lastNode.g + (isStraight ? 1 : 1.4f);

            //计算h 当前节点到终点的曼哈顿距离
            h = CalculateMHDis(pos, endPos);

            //计算f f = g + h
            f = g + h;
        }

        /// <summary>
        /// 计算当前节点到终点的曼哈顿距离
        /// </summary>
        /// <param name="currentPos">当前节点位置</param>
        /// <param name="endPos">终点位置</param>
        /// <returns></returns>
        private int CalculateMHDis(NodePos currentPos, NodePos endPos)
        {
            return Mathf.Abs(endPos.row - currentPos.row) + Mathf.Abs(endPos.col - currentPos.col);
        }
    }
}

AStarManager.cs A*寻路系统管理器

using System.Collections;
using System.Collections.Generic;
using UnityEngine;
using Common;
using System;

namespace Common.AStar
{
    /// <summary>
    /// A*寻路系统管理器 单例
    /// </summary>
    public class AStarManager : Singleton<AStarManager>
    {
        //逻辑地图 (节点容器)
        public AStarNode[,] nodeMap;

        //开启列表 - 存储当前可以走到的点
        //private List<NodePos> openList; 用优先队列进行优化
        private PriorityQueue<NodePos> openList;
        //关闭列表 - 存储当前已经走完的点
        private List<NodePos> closeList;

        //判重数组
        private bool[,] used;

        //逻辑地图的尺寸 (节点容器大小)
        private int logicWidth;
        private int logicHeight;

        //实际地图节点尺寸 --- 逻辑节点默认 单位长度的正方体
        private float realNodeWidth = 1; //根据实际对地图的切割情况决定 demo默认为1
        private float realNodeHeight = 1;

        /// <summary>
        /// 单例提供的初始化函数
        /// </summary>
        protected override void Init()
        {
            base.Init();
            //初始化相关数据结构 构造小顶堆的优先级队列
            openList = new PriorityQueue<NodePos>(100,(a,b)=>
            {
                return nodeMap[a.row, a.col].f < nodeMap[b.row, b.col].f;
            }
            );
            closeList = new List<NodePos>();

            
        }

        /// <summary>
        /// 初始化节点容器
        /// </summary>
        /// <param name="logicWidth">逻辑宽(容器的列数)</param>
        /// <param name="logicHeight">逻辑高(容器的行数)</param>
        public void InitNodeMap(int logicWidth,int logicHeight)
        {
            //初始化容器
            nodeMap = new AStarNode[logicHeight, logicWidth];
            this.logicWidth = logicWidth;
            this.logicHeight = logicHeight;

            //创建节点并放入容器中
            for(int i = 0; i < logicHeight; i++)
                for(int j = 0; j < logicWidth; j++)
                {
                    //这里使用随机设置节点类型，实际需按地图配置文件进行配置
                    AStarNode node = new AStarNode(new NodePos(i, j),
                        UnityEngine.Random.Range(0,100) > 20 ? ANodeType.passable : ANodeType.impassable );

                    nodeMap[i, j] = node;
                }
            used = new bool[logicHeight, logicWidth];
        }

        /// <summary>
        /// A*核心算法 找寻最短路径
        /// </summary>
        /// <param name="startPos">起点位置(实际位置)</param>
        /// <param name="endPos">终点位置(实际位置)</param>
        /// <returns></returns>
        public List<AStarNode> FindPath(Vector2 startPos,Vector2 endPos)
        {
            //存储最短路径
            List<AStarNode> path = new List<AStarNode>();
            //清空双列表和判重数组
            openList.Clear();
            closeList.Clear();
            used = new bool[logicHeight, logicWidth];

            //实际位置转换到逻辑位置 --这里仍需完善,负值的情况
            int startRow = (int)(startPos.y / realNodeHeight);
            int startCol = (int)(startPos.x / realNodeWidth);
            int endRow = (int)(endPos.y / realNodeHeight);
            int endCol = (int)(endPos.x / realNodeWidth);

            //判断逻辑位置是否合法
            if (startRow < 0 || startRow >= logicHeight
                || startCol < 0 || startCol >= logicWidth
                || endRow < 0 || endRow >= logicHeight
                || endCol < 0 || endCol >= logicWidth)

                return null;

            //特判如果起点或终点有阻挡 直接返回空
            AStarNode startNode = nodeMap[startRow, startCol];
            AStarNode endNode = nodeMap[endRow, endCol];
            if (startNode.nodeType == ANodeType.impassable || endNode.nodeType == ANodeType.impassable) return null;

            //bfs广度优先搜索实现A*寻路算法
            bool isSuccess = bfs(new NodePos(startRow, startCol), 
                new NodePos(endRow, endCol));

            //如果搜索到最短路则返回
            if (isSuccess)
            {
                return CalculatePath(new NodePos(startRow, startCol), new NodePos(endRow, endCol));
            }
            else return null;
        
        }

        private bool bfs(NodePos startPos,NodePos endPos)
        {
            //每次从开启队列中移除最优的一个节点加入关闭列表，将移除节点周围的新节点加入开启列表
            //初始状态 先初始化起点相关 在加入开启列表
            
            
            AStarNode startNode = nodeMap[startPos.row, startPos.col];
            startNode.lastNode = null; //起点设为空 因为寻路可能多次 此步不能省略
            startNode.CalculateF(endPos);
            openList.insert(startPos);
            used[startPos.row, startPos.col] = true;

            while (openList.Length > 0)
            {
                //对openList进行排序 每次取f最小的移除
                //openList.Sort(CustomSortFunc);  优化成优先队列后无需进行排序

                //取出List的首位
                NodePos topPos = openList.pop();
                AStarNode currentNode = nodeMap[topPos.row,topPos.col];
                closeList.Add(topPos);

                //定义偏移量 从左下开始顺时针
                int[] dx = { -1, -1, -1, 0, 1, 1, 1, 0 };
                int[] dy = { -1, 0, 1, 1, 1, 0, -1, -1 };

                for(int i = 0; i < 8; i++)
                {
                    int row = currentNode.pos.row + dx[i];
                    int col = currentNode.pos.col + dy[i];
                    //根据边界进行筛选
                    if (row < 0 || row >= logicHeight || col < 0 || col >= logicWidth) 
                        continue;
                    //根据openList和closeList判断重复 --- 单独使用used判重 空间换时间
                    //if (openList.Contains(new NodePos(row, col)) || closeList.Contains(new NodePos(row, col)))
                    //continue;
                    //判重
                    if (used[row, col]) continue;

                    AStarNode node = nodeMap[row, col];
                    //还需判断节点是否可通过
                    if (node.nodeType == ANodeType.impassable) continue;

                    //筛选成功的节点设置lastNode 计算f 加入openList
                    node.lastNode = currentNode;
                    node.CalculateF(endPos);
                    //添加进开启列表
                    openList.insert(node.pos);
                    used[row, col] = true;
                    //如果结尾添加进来 可提前结束
                    if (node.pos.Equals(endPos)) return true;

                }
            }

            return false;
            
        }

        private int CustomSortFunc(NodePos x, NodePos y)
        {
            AStarNode nodeX = nodeMap[x.row, x.col];
            AStarNode nodeY = nodeMap[y.row, y.col];

            if (nodeX.f > nodeY.f) return 1;
            else if (nodeX.f == nodeY.f) return 1;
            else return -1;
        }

        
        
        
        /// <summary>
        /// 根据关闭列表计算路径
        /// </summary>
        /// <param name="startPos">起始逻辑位置</param>
        /// <param name="endPos">终点逻辑位置</param>
        /// <returns></returns>
        private List<AStarNode> CalculatePath(NodePos startPos,NodePos endPos)
        {
            List<AStarNode> path = new List<AStarNode>();
            AStarNode endNode = nodeMap[endPos.row, endPos.col];
            AStarNode startNode = nodeMap[startPos.row, endPos.col];
            AStarNode node = endNode;
            while(node!=null && !node.Equals(startNode))
            {
                path.Add(node);
                node = node.lastNode;
            }
            //翻转列表
            path.Reverse();

            return path;
        }



    }
}

需要注意的地方

1. 本文提到的单位距离1等，均为逻辑距离，实际地图尺寸等需要根据实际地图数据进行加载，最终将实际距离转换到逻辑距离上进行计算，结果再转换回去使用。
2. 地图的初始化直接是随机初始化，实际开发需要根据地图数据进行初始化和生成。
3. 测试本模块前，需要先调用一次InitNodeMap方法才能使用FindPath功能。
4. 本管理器还用到了单例模式，有兴趣的读者可以自行查阅，下面也贴出单例模式的代码

namespace Common
{
    /// <summary>
    /// 饿汉式单例模式
    /// </summary>
    /// <typeparam name="T"></typeparam>
    public class Singleton<T> where T : Singleton<T> //注意此约束为T必须为其本身或子类
    {
        protected Singleton() { }
        public static T Instance { get; private set; }

        static Singleton()
        {
            Instance = Activator.CreateInstance(typeof(T), true) as T;
            Instance.Init(); //初始化一次
        }

        /// <summary>
        /// 可选初始化函数
        /// </summary>
        protected virtual void Init()
        {

        }
    }
}