改写A*寻路算法

lage001

已于 2023-12-25 18:48:35 修改

阅读量1k

点赞数 9

分类专栏： unity 文章标签： unity 游戏引擎算法

于 2023-12-25 16:49:37 首次发布

本文链接：https://blog.csdn.net/qq_44979481/article/details/135194312

版权

unity 专栏收录该内容

6 篇文章 0 订阅

订阅专栏

一、问题描述

我在Unity中写A*寻路算法时，遇到这样的问题：没有障碍物的情况下，这个算法可以算的很快，walker都能在获得目标之后立刻行动起来；有障碍物的情况下，walker往往要在原地等算法算很长时间才能动起来。我针对这个问题在A*寻路算法的基础上进行了改写。

二、A*寻路算法

在分析问题之前，我们还是先回顾一下A*寻路算法。

想象一下你现在的视野只有前后左右四个位置，你每次走一步只能看到离你最近的四个位置，你能记得住哪些位置你走过，哪些位置你看到过但还没走过。走过路不再走，每次随机探索没走过的位置，这样一直下去直到走到目的地。每一步迭代，整体的视野向外扩张一圈，就像水面向外扩张的波纹（最后的波纹doge）。以上就是广度优先遍历（BFS）的思想。我们先把这部分逻辑实现一下

// 函数返回一个路径列表 参数包括：起点、终点、判断某位置能否行走的函数 
List<Vector2> FindWay(Vector2 start, Vector2 target, Func<Vector2, bool> Walkable)
{
    Vector2 current = start;
    List<Vector2> toCheck = new List<Vector2>() { current };// 用于存储带探索位置
    List<Vector2> Checked = new List<Vector2>();// 用于存储已探索位置
    Dictionary<Vector2, Vector2> pathDic = new Dictionary<Vector2, Vector2>();// 用于存储路径
    
    while (toCheck.Count > 0)
    {
        List<Vector2> neighbors = GetNeigbors(current);//Func<Vector2,List<Vector2>>
        toCheck.Remove(current);//走过的位置不再探索
        Checked.Add(current);//记录走过的位置

        // 将当前位置的邻居添加到toCheck中
        foreach (var neighbor in neighbors)
        {
            // 如果邻居探索过了，或者不能走就跳过本次循环
            if (Checked.Contains(neighbor) || !Walkable(neighbor))
                continue；
            pathDic[neighbor] = current;//记录路径
            toCheck.Add(neighbor);
            // 找到目标则结束，返回路径
            if (neighbor == target)
            {
                return GetPath(pathDic, neighbor);
            }
        }
    }  
    return null;
}

这样的方法可以找到到达目的地的最小路径，代价就是时间复杂度太高了。

A*寻路算法是对上面方法的优化，在上面算法的基础上添加两个代价

float gCost;//起点到当前位置实际走过的距离
float hCost;//当前位置到终点的估计距离

也就是说每走一步估计总体路径长度（两个代价之和），我们需要选择使得路径最短的那个位置。由于gCost是跟路径有关的，我们将位置以及它的代价封装为节点类

    public class Node
    {
        public Vector2 pos;
        public Vector2 target;

        public float gCost;
        public float hCost;
        public float Cost;

        public bool walkable;
        public Func<Vector2, bool> CheckWalkable;

        public Node(Vector2 pos, Vector2 target, float gCost, Func<Vector2, bool> CheckWalkable)
        {
            this.pos = pos;
            this.target = target;
            this.gCost = gCost;
            this.hCost = GetHCost(this.pos, this.target);
            this.Cost = this.gCost + this.hCost;
            this.CheckWalkable = CheckWalkable;
            this.walkable = CheckWalkable(this.pos);
        }
        
        float GetHCost(Vector2 v1, Vector2 v2)
        {
            return Mathf.Abs(v1.x - v2.x) + Mathf.Abs(v1.y - v2.y);//曼哈顿距离
        }

        // 根据Checked列表和walkable获得当前节点的邻居,计算gCost
        public List<Node> GetNeigbors(List<Vector2> Checked)
        {
            List<Node> nodeList = new List<Node>()
            {
                new Node(pos + Vector2.up, target,gCost + 1, CheckWalkable),
                new Node(pos + Vector2.down, target,gCost + 1, CheckWalkable),
                new Node(pos + Vector2.left, target,gCost + 1, CheckWalkable),
                new Node(pos + Vector2.right, target,gCost + 1, CheckWalkable),
            };

            List<Node> neigbors = new List<Node>();
            foreach (var node in nodeList)
            {
                if (node.walkable && !Checked.Contains(node.pos))
                {
                    neigbors.Add(node);
                }
            }
            return neigbors;
        }
    }

这里顺便把获得邻居的函数封装进去了。

A*算法在BFS的基础上在每次遍历可行走位置时进行了一个选择，即选择代价最小的那个位置，所以在原来的基础上每次遍历会进行一个基于代价的排序。代码如下

    public static List<Vector2> AStarFindWay(Vector2 start, Vector2 target, Func<Vector2, bool> Walkable)
    {
        Node currentNode = new Node(start, target, 0, Walkable);
        List<Node> toCheck = new List<Node>() { currentNode };
        List<Vector2> Checked = new List<Vector2> { };
        Dictionary<Vector2, Vector2> pathDic = new Dictionary<Vector2, Vector2>() { };

        while (toCheck.Count > 0)
        {
            List<Node> neighbors = currentNode.GetNeigbors(Checked);
            toCheck.Remove(currentNode);
            Checked.Add(currentNode.pos);

            // check neigbors of current node
            if (neighbors.Count > 0)
            {
                foreach (var neighbor in neighbors)
                {
                    pathDic[neighbor.pos] = currentNode.pos;
                    toCheck.Add(neighbor);
                    if (neighbor.pos == target)
                    {
                        //return neighbor.path;
                        return GetPath(pathDic, neighbor.pos);
                    }
                }
            }
            //这里添加了一个排序
            foreach (var node in toCheck)
            {
                //选择Cost小的，如果Cost相等，选择离目标近的(hCost小的)
                if (node.Cost < currentNode.Cost || node.Cost == currentNode.Cost && node.hCost < currentNode.hCost)
                {
                    currentNode = node;
                }
            }
        }
        return null;
    }

二、问题分析

当我添加障碍物的时候，往往walker要经过一个先远离目标再接近目标的过程，远离目标的过程就和A*算法的本质相悖了，所以这种情况下又回归到BFS了，会消耗大量时间

三、针对问题进行该写

A*算法的优势是能找到最短路径，但是代价是付出大量时间。现在我们为了加快速度，只好牺牲准确度，不必找到最短路径。简单来说就是有路就走，遇到死路再回溯（有点类似于深度优先遍历的思想），但这里并不是盲目的行走，因为我们还会保留代价函数，所以walker是有方向的在行走。总结来说，要实现下面两点

1.当前节点有邻居的话，就在邻居里找代价最小的下一个节点

2.当前节点没有邻居的话，就在toCheck里寻找代价最小的下一节点

下面是代码：

    public static List<Vector2> AStarFindWay(Vector2 start, Vector2 target, Func<Vector2, bool> Walkable)
        {
            Node currentNode = new Node(start, target, 0, Walkable);
            List<Node> toCheck = new List<Node>() { currentNode };
            List<Vector2> Checked = new List<Vector2> {};
            Dictionary<Vector2, Vector2> pathDic = new Dictionary<Vector2, Vector2>() { };
            
            while (toCheck.Count > 0)
            {
                List<Node> neighbors = currentNode.GetNeigbors(Checked);
                toCheck.Remove(currentNode);
                Checked.Add(currentNode.pos);

                // 判断当前节点是否有邻居
                if (neighbors.Count > 0)
                {
                    //有邻居就在邻居里找下一节点
                    Node node = neighbors[0];
                    foreach (var neighbor in neighbors)
                    {
                        if (neighbor.Cost < node.Cost || neighbor.Cost == node.Cost && neighbor.hCost < node.hCost)
                        {
                            node = neighbor;
                        }
                        pathDic[neighbor.pos] = currentNode.pos; 
                        toCheck.Add(neighbor);
                        if (neighbor.pos == target)
                        {
                            //return neighbor.path;
                            return GetPath(pathDic, neighbor.pos);
                        }
                    }
                    currentNode = node;
                }
                else
                {
                    //没有邻居就在全局搜索下一节点
                    currentNode = toCheck[0];
                    foreach (var node in toCheck)
                    {
                        if (node.Cost < currentNode.Cost || node.Cost == currentNode.Cost && node.hCost < currentNode.hCost)
                        {
                            currentNode = node;
                        }
                    }
                }
            }
            return null;
        }

这样确实大大缩短了找到目标的时间，当然不一定能以最快路径找到目标，但是大体上是向目标方向前进的。

四、后续问题

遇到Cost和hCost均相等的情况，walker有可能会往目标方向的反方向行走，绕一圈才能找到目标，针对这种情况我又添加了行走方向的约束，在选择下一邻居时，再加一个条件

if (neighbor.Cost < node.Cost
    ||neighbor.Cost == node.Cost && neighbor.hCost < node.hCost
    || neighbor.Cost == node.Cost && neighbor.hCost == node.hCost && neighbor.dCost > node.dCost)
{
    node = neighbor;
}

我希望每次行走的方向接近目标方向，所以我选择邻居节点时会考虑更接近目标方向的下一节点，其中dCost表示的是当前行走方向与目标方向的余弦值

$v1 = nextnode.pos-currentnode.pos\\ v2 = targetnode.pos-currentnode.pos\\ dCost = cos<v1,v2> = \frac{v1\cdot{v2}}{|v1||v2|}$

dCost越接近1说明夹角越小，dCost越接近-1说明夹角越大，所以我们选择dCost更大的下一节点

改到这里，对于开放型的地图，这个算法已经可以不那么笨的快速找到目标了，但是遇到半封闭式的地图比如只有一个门的屋子，如果选择目标方向在门的反方向，那么这个算发还是会很笨的搜索完整个屋子才发现要先走到门那里，再朝目标方向走。听说过基于射线的A*寻路算法，还有标记拐点的方法等等，还没有去学，我也不是专门研究这个东西，所以A*算法暂时告一段落了。