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

发布时间:2024年01月17日

Peter Hart?

Nils Nilsson?

Bertram Raphael?

参考:

C#,人工智能(AI)机器人路径规划(Path Planning)的ARA*(Anytime Replanning A* Algorithm)算法与源程序icon-default.png?t=N7T8https://blog.csdn.net/beijinghorn/article/details/125464754

一、A*算法概述

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

论文原文icon-default.png?t=N7T8https://www.cs.auckland.ac.nz/courses/compsci709s2c/resources/Mike.d/astarNilsson.pdf

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

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

除了 A* ,现在发展了 D*,D*Lite。。。诸多改进的算法,后续都会给出 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

文章来源:https://blog.csdn.net/beijinghorn/article/details/124119531
本文来自互联网用户投稿,该文观点仅代表作者本人,不代表本站立场。本站仅提供信息存储空间服务,不拥有所有权,不承担相关法律责任。