C#，人工智能，机器人，路径规划，A*（AStar Algorithm）算法、源代码及计算数据可视化

Peter Hart

Nils Nilsson

Bertram Raphael

参考：

C#，人工智能（AI）机器人路径规划（Path Planning）的ARA（Anytime Replanning A Algorithm）算法与源程序https://blog.csdn.net/beijinghorn/article/details/125464754

一、A*算法概述

A*算法最初由斯坦福研究院（Stanford Institute）的 **Peter Hart，Nils Nilsson，Bertram Raphael **发表于1968年，属于Dijkstra算法的拓展之一。

论文原文https://www.cs.auckland.ac.nz/courses/compsci709s2c/resources/Mike.d/astarNilsson.pdf

A*算法是常用的路径规划、最短路径搜索、图遍历算法。

借助於启发函数，A*同时拥有较好的性能与准确度，因而备受人工智能、机器人研究者们的欢迎。

除了 A* ，现在发展了 D，DLite。。。诸多改进的算法，后续都会给出 C# 源代码。

A* 的算法不复杂，属于入门级别的。

二、计算结果的动画演示

三、A* 源代码

1、节点信息 NodeInfo

#define ANIMATE

using System;
using System.IO;
using System.Text;
using System.Linq;
using System.Collections.Generic;
using System.Text.RegularExpressions;

namespace Legalsoft.Truffer.Algorithm
{
    /// <summary>
    /// 节点信息
    /// </summary>
    public class NodeInfo
    {
        /// <summary>
        /// 前一节点
        /// </summary>
        public NodeInfo Last { get; set; } = null;
        /// <summary>
        /// 邻接距离(权值)
        /// </summary>
        public int ManhattanWeight { get; set; } = 0;
        /// <summary>
        /// 曼哈顿距离(权值)
        /// </summary>
        public int RemoteWeight { get; set; } = 0;
        /// <summary>
        /// 综合距离(权值)
        /// </summary>
        public int TotalWeight { get; set; } = 0;
        /// <summary>
        /// 行号
        /// </summary>
        public int Column { get; set; } = 0;
        /// <summary>
        /// 列号
        /// </summary>
        public int Row { get; set; } = 0;
        /// <summary>
        /// 是否为墙(节点)？
        /// </summary>
        public bool IsWall { get; set; } = false;
        /// <summary>
        /// 是否被加入到了close中
        /// </summary>
        public bool InClose { get; set; } = false;
        /// <summary>
        /// 开口节点？
        /// </summary>
        public bool InOpen { get; set; } = false;
        /// <summary>
        /// 路过？
        /// </summary>
        public bool Visited { get; set; } = false;
        /// <summary>
        /// 构造函数
        /// </summary>
        /// <param name="y">行号</param>
        /// <param name="x">列号</param>
        /// <param name="iswall"></param>
        public NodeInfo(int y, int x, bool iswall)
        {
            Row = y;
            Column = x;
            IsWall = iswall;
        }
    }
}

2、地图信息MapInfo

#define ANIMATE

using System;
using System.IO;
using System.Text;
using System.Linq;
using System.Collections.Generic;
using System.Text.RegularExpressions;

namespace Legalsoft.Truffer.Algorithm
{
    /// <summary>
    /// 地图信息（墙1与空0）
    /// </summary>
    public class MapInfo
    {
        /// <summary>
        /// 行数
        /// </summary>
        public int Row { get; set; } = 0;
        /// <summary>
        /// 列数
        /// </summary>
        public int Column { get; set; } = 0;
        /// <summary>
        /// 所有节点
        /// </summary>
        public NodeInfo[,] Nodes { get; set; } = null;

        /// <summary>
        /// 从文本创建地图
        /// </summary>
        /// <param name="buf"></param>
        public void FromBuffer(string buf)
        {
            buf = buf.Replace("\r", "").Trim();
            string[] xlines = buf.Split(new char[] { '\n' }, StringSplitOptions.RemoveEmptyEntries);
            Nodes = new NodeInfo[xlines.Length, xlines[0].Trim().Length];
            Row = Nodes.GetLength(0);
            Column = Nodes.GetLength(1);
            int y = 0;
            foreach (string xu in xlines)
            {
                for (int x = 0; x < xu.Trim().Length; x++)
                {
                    NodeInfo node = new NodeInfo(y, x, Int32.Parse(xu.Trim().Substring(x, 1)) > 0);
                    Nodes[y, x] = node;
                }
                y++;
            }
        }

        /// <summary>
        /// 从文件创建地图
        /// </summary>
        /// <param name="filename"></param>
        /// <exception cref="Exception"></exception>
        public void FromFile(string filename)
        {
            try
            {
                string buf = File.ReadAllText(filename);
                FromBuffer(buf);
            }
            catch (Exception ex)
            {
                throw new Exception("");
            }
        }
        /// <summary>
        /// this重载
        /// </summary>
        /// <param name="y"></param>
        /// <param name="x"></param>
        /// <returns></returns>
        public NodeInfo this[int y, int x]
        {
            set { Nodes[y, x] = value; }
            get { return Nodes[y, x]; }
        }
        /// <summary>
        /// 第一个节点
        /// </summary>
        public NodeInfo First
        {
            get { return Nodes[0, 0]; }
        }
        /// <summary>
        /// 最后一个节点
        /// </summary>
        public NodeInfo Last
        {
            get { return Nodes[Row - 1, Column - 1]; }
        }

        /// <summary>
        /// 清除路径
        /// </summary>
        public void ClearPath()
        {
            for (int i = 0; i < Row; i++)
            {
                for (int j = 0; j < Column; j++)
                {
                    this[i, j].Visited = false;
                }
            }
        }
    }
}

3、核心代码 AStar Kernal

#define ANIMATE

using System;
using System.IO;
using System.Text;
using System.Linq;
using System.Collections.Generic;
using System.Text.RegularExpressions;

namespace Legalsoft.Truffer.Algorithm
{
    public class AStar
    {
        /// <summary>
        /// 开放列表
        /// </summary>
        private List<NodeInfo> openList { get; set; } = new List<NodeInfo>();
        /// <summary>
        /// 当前访问的节点
        /// </summary>
        private NodeInfo curNode { get; set; } = null;
        /// <summary>
        /// 起始节点
        /// </summary>
        private NodeInfo startNode { get; set; } = null;
        /// <summary>
        /// 终止节点
        /// </summary>
        private NodeInfo endNode { get; set; } = null;
        /// <summary>
        /// 地图
        /// </summary>
        public MapInfo map { get; set; } = new MapInfo();

        /// <summary>
        /// 构造函数
        /// </summary>
        public AStar()
        {
        }

        /// <summary>
        /// 节点加入列表，并计算相关权值
        /// </summary>
        /// <param name="node"></param>
        private void AddToList(NodeInfo node)
        {
            if (node.IsWall) return;
            if (node.InClose) return;
            if (node.InOpen) return;
            openList.Add(node);
            node.InOpen = true;
            node.ManhattanWeight = curNode.ManhattanWeight + WeightOf(node, curNode);
            node.RemoteWeight = WeightOf(node, endNode);
            node.TotalWeight = node.ManhattanWeight + node.RemoteWeight;
            node.Last = curNode;
        }

        /// <summary>
        /// 路径规划A*算法(最短路径A*算法)
        /// 默认左上角(0,0)为起点；右下角为终点；
        /// </summary>
        /// <param name="start">起点</param>
        /// <param name="end">终点</param>
        public void Tracing(NodeInfo start = null, NodeInfo end = null)
        {
            List<int[]> steps = new List<int[]>() {
                new int[2] {  1, 1 },
                new int[2] {  0, 1 },
                new int[2] {  1, 0 },
                new int[2] {  0,-1 },
                new int[2] { -1, 0 },
                new int[2] {  1,-1 },
                new int[2] { -1, 1 },
                new int[2] { -1,-1 },
            };

            startNode = (start == null) ? map.First : start;
            endNode = (end == null) ? map.Last : end;

            // 将起点加入到开放列表中
            openList.Add(startNode);
            while (true)
            {
                openList.Sort(SortByCost);

                curNode = openList[0];
                openList.Remove(curNode);
                curNode.InOpen = false;
                curNode.InClose = true;

                // 终点?
                if (curNode == endNode)
                {
                    return;
                }

                MarkPath();
                map.ClearPath();
            }
        }

        /// <summary>
        /// 节点1到节点2的Weight
        /// </summary>
        /// <param name="a">节点1</param>
        /// <param name="b">节点2</param>
        /// <returns></returns>
        private int WeightOf(NodeInfo a, NodeInfo b)
        {
            // 直行步长
            double straightDistance = 1.0;
            // 斜行步长
            double diagonalDistance = 1.414;

            int deltaX = Math.Abs(a.Column - b.Column);
            int deltaY = Math.Abs(a.Row - b.Row);
            if (deltaX == deltaY)
            {
                return (int)(deltaX * diagonalDistance);
            }
            else if (deltaX < deltaY)
            {
                return (int)(deltaX * diagonalDistance + (deltaY - deltaX) * straightDistance);
            }
            else
            {
                return (int)(deltaY * diagonalDistance + (deltaX - deltaY) * straightDistance);
            }
        }

        /// <summary>
        /// 排序的委托函数
        /// </summary>
        /// <param name="a">节点1</param>
        /// <param name="b">节点2</param>
        /// <returns></returns>
        private int SortByCost(NodeInfo a, NodeInfo b)
        {
            if (a.TotalWeight.CompareTo(b.TotalWeight) != 0)
            {
                return (a.TotalWeight.CompareTo(b.TotalWeight));
            }
            else if (a.RemoteWeight.CompareTo(b.RemoteWeight) != 0)
            {
                return (a.RemoteWeight.CompareTo(b.RemoteWeight));
            }
            else
            {
                return 1;
            }
        }

        /// <summary>
        /// 标记当前获取的路径
        /// </summary>
        public void MarkPath()
        {
            NodeInfo cd = curNode;
            while (cd != null)
            {
                if (cd == startNode)
                {
                    break;
                }
                cd.Visited = true;
                cd = cd.Last;
            }
        }
    }
}

POWER BY TRUFFER.CN