修复C#中带转向惩罚的A*寻路路径不连贯问题

我实现了一个不允许斜向移动的A*寻路函数，为转向操作增加了F值惩罚，但发现生成的路径并不连贯，时常跳回随机方格。例如起点为(10, 4)、终点为(0, 13)时，预期路径应为(10, 4)→(0, 4)→(0, 13)，但代码返回的点列表是(0,4), (1, 3), (0, 5)。以下是我的寻路函数及F值计算代码：

public void checkPath(Points location, Points endGoal, List<Points> obstacles, List<string> Path){
            List<Points> openList = new List<Points>();
            List<Points> closedList = obstacles;
            List<Points> pointPath = new List<Points>();
            List<double> openFscore = new List<double>();
            openList.Add(location);
            List<Points> adjacent;
            double locationFscore = Fscore(location, location, endGoal, pointPath);
            openFscore.Add(locationFscore);

            while(contains.Containers(pointPath, endGoal) == false){
                double Closest = openFscore.Min();
                Points Current = openList[openFscore.LastIndexOf(Closest)];
                pointPath.Add(Current);
                closedList.Add(Current);
                openFscore.Remove(openFscore[openList.IndexOf(Current)]);
                openList.Remove(Current);
                adjacent = [Current.northPoint(), Current.southPoint(), Current.eastPoint(), Current.westPoint()];

                foreach (Points square in adjacent){
                    if (contains.Containers(closedList, square) == true){
                        continue;
                    }
                    else if(contains.Containers(openList, square) == false){
                        openList.Add(square);
                        openFscore.Add(Fscore(location, square, endGoal, pointPath));
                    }
                }
            }

            for(int i = 0; i < pointPath.Count; i++){
                string direction = "("+ pointPath[i].X + ", " + pointPath[i].Y + ")";
                Path.Add(direction);
}

static double Gscore(Points location, Points current){
                double score = Math.Abs(location.X - current.X) + Math.Abs(location.Y - current.Y);
                return score;
            }

            static double Hscore(Points current, Points endGoal){
                double score = Math.Abs(endGoal.X - current.X) + Math.Abs(endGoal.Y - current.Y); 
                return score;
            }

            static double Fscore(Points location, Points current, Points endGoal, List<Points> pointPath){
                double score = Gscore(location, current) + Hscore(current, endGoal);

                if (pointPath.Count >= 2){
                    int length = pointPath.Count - 1;
                    int Xdir = Math.Abs(pointPath[length].X - pointPath[length - 1].X);
                    int Ydir = Math.Abs(pointPath[length].Y - pointPath[length - 1].Y); 

                    if (Xdir == 0 && Math.Abs(pointPath[length].X - current.X) != 0){
                        score *= 1.1;
                        return score;
                    }

                    if (Ydir == 0 && Math.Abs(pointPath[length].Y - current.Y) != 0){
                        score *= 1.1;
                        return score;
                    }
                    else{
                        return score;
                    }
                }
                else {
                    return score;
                }
            }

核心问题分析 #

代码存在多处A*算法的实现错误，导致路径逻辑混乱：

• 路径记录逻辑错误：pointPath被错误用作已访问节点列表，而非回溯路径，导致转向惩罚计算完全偏离实际路径走向
• G值计算错误：当前G值是起点到当前节点的曼哈顿距离，未考虑实际移动的累积成本，无法正确结合转向惩罚
• OpenList管理错误：未处理节点已在OpenList中但需要更新更优F值的情况，导致旧的高F值节点被优先选择
• 转向惩罚逻辑错误：基于错误的pointPath计算转向，而非当前节点到父节点的实际移动方向

具体修复步骤 #

1. 重构节点数据结构，记录父节点与实际路径成本
   新增包含父节点、G值的节点类，避免用分离的列表管理F值：

public class PathNode
{
    public Points Position { get; set; }
    public PathNode Parent { get; set; }
    public double G { get; set; } // 从起点到当前节点的实际累积成本
    public double H { get; set; } // 到终点的曼哈顿距离
    public double F => G + H;
}

2. 修正G值与转向惩罚计算逻辑
   转向惩罚应基于当前节点的父节点移动方向，而非错误的pointPath：

static double CalculateG(PathNode parentNode, Points currentPos)
{
    double baseCost = 1.0; // 每步基础成本
    // 检查是否转向
    if (parentNode.Parent != null)
    {
        // 父节点的移动方向：从祖父到父节点
        int parentDirX = parentNode.Position.X - parentNode.Parent.Position.X;
        int parentDirY = parentNode.Position.Y - parentNode.Parent.Position.Y;
        // 当前移动方向：从父节点到当前节点
        int currentDirX = currentPos.X - parentNode.Position.X;
        int currentDirY = currentPos.Y - parentNode.Position.Y;
        
        // 方向不同则增加惩罚
        if (parentDirX != currentDirX || parentDirY != currentDirY)
        {
            baseCost *= 1.1;
        }
    }
    return parentNode.G + baseCost;
}

3. 修复OpenList与ClosedList的管理逻辑

• 禁止直接将obstacles赋值给closedList，应创建新列表避免修改原障碍物数据
• 处理节点已在OpenList中的情况：如果新路径的G值更低，则更新父节点与G值

public void checkPath(Points start, Points endGoal, List<Points> obstacles, List<string> pathOutput)
{
    List<PathNode> openList = new List<PathNode>();
    HashSet<Points> closedList = new HashSet<Points>(obstacles); // 用HashSet提升查找效率
    PathNode startNode = new PathNode { Position = start, G = 0, H = Hscore(start, endGoal) };
    openList.Add(startNode);

    while (openList.Count > 0)
    {
        // 找到F值最小的节点
        PathNode currentNode = openList.OrderBy(n => n.F).First();
        openList.Remove(currentNode);
        closedList.Add(currentNode.Position);

        // 到达终点，回溯路径
        if (currentNode.Position.Equals(endGoal))
        {
            List<Points> pointPath = new List<Points>();
            PathNode temp = currentNode;
            while (temp != null)
            {
                pointPath.Add(temp.Position);
                temp = temp.Parent;
            }
            pointPath.Reverse(); // 反转得到从起点到终点的路径
            foreach (Points p in pointPath)
            {
                pathOutput.Add($"({p.X}, {p.Y})");
            }
            return;
        }

        // 获取四个方向相邻节点
        List<Points> adjacent = new List<Points>
        {
            currentNode.Position.northPoint(),
            currentNode.Position.southPoint(),
            currentNode.Position.eastPoint(),
            currentNode.Position.westPoint()
        };

        foreach (Points adjPos in adjacent)
        {
            if (closedList.Contains(adjPos))
                continue;

            // 计算新的G值
            double newG = CalculateG(currentNode, adjPos);
            // 检查是否已在OpenList中
            PathNode existingNode = openList.FirstOrDefault(n => n.Position.Equals(adjPos));
            if (existingNode == null)
            {
                PathNode newNode = new PathNode
                {
                    Position = adjPos,
                    Parent = currentNode,
                    G = newG,
                    H = Hscore(adjPos, endGoal)
                };
                openList.Add(newNode);
            }
            else if (newG < existingNode.G)
            {
                // 找到更优路径，更新节点
                existingNode.Parent = currentNode;
                existingNode.G = newG;
            }
        }
    }

    // 未找到路径的情况处理
    pathOutput.Clear();
}

4. 保留原H值计算逻辑
   曼哈顿距离作为H值是合适的，无需修改：

static double Hscore(Points current, Points endGoal)
{
    return Math.Abs(endGoal.X - current.X) + Math.Abs(endGoal.Y - current.Y);
}

关键优化点 #

• 使用HashSet存储已访问节点，提升节点查找效率（原contains.Containers方法可替换为HashSet的Contains）
• 路径回溯改为从终点反向遍历父节点，避免pointPath的错误使用
• 转向惩罚仅在连续两次移动方向不同时触发，符合实际路径走向逻辑

内容的提问来源于stack exchange，提问作者user25197772