devbox
笔触狂放9
这个屌丝很懒，什么也没留下！
关注作者
热门标签
热门文章
当前位置:   article > 正文

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

作者：笔触狂放9 | 2024-03-10 21:08:35

赞

踩

c# 人工智能

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

  1. #define ANIMATE
  2.  
  3. using System;
  4. using System.IO;
  5. using System.Text;
  6. using System.Linq;
  7. using System.Collections.Generic;
  8. using System.Text.RegularExpressions;
  9.  
  10. namespace Legalsoft.Truffer.Algorithm
  11. {
  12.     /// <summary>
  13.     /// 节点信息
  14.     /// </summary>
  15.     public class NodeInfo
  16.     {
  17.         /// <summary>
  18.         /// 前一节点
  19.         /// </summary>
  20.         public NodeInfo Last { get; set; } = null;
  21.         /// <summary>
  22.         /// 邻接距离(权值)
  23.         /// </summary>
  24.         public int ManhattanWeight { get; set; } = 0;
  25.         /// <summary>
  26.         /// 曼哈顿距离(权值)
  27.         /// </summary>
  28.         public int RemoteWeight { get; set; } = 0;
  29.         /// <summary>
  30.         /// 综合距离(权值)
  31.         /// </summary>
  32.         public int TotalWeight { get; set; } = 0;
  33.         /// <summary>
  34.         /// 行号
  35.         /// </summary>
  36.         public int Column { get; set; } = 0;
  37.         /// <summary>
  38.         /// 列号
  39.         /// </summary>
  40.         public int Row { get; set; } = 0;
  41.         /// <summary>
  42.         /// 是否为墙(节点)？
  43.         /// </summary>
  44.         public bool IsWall { get; set; } = false;
  45.         /// <summary>
  46.         /// 是否被加入到了close中
  47.         /// </summary>
  48.         public bool InClose { get; set; } = false;
  49.         /// <summary>
  50.         /// 开口节点？
  51.         /// </summary>
  52.         public bool InOpen { get; set; } = false;
  53.         /// <summary>
  54.         /// 路过？
  55.         /// </summary>
  56.         public bool Visited { get; set; } = false;
  57.         /// <summary>
  58.         /// 构造函数
  59.         /// </summary>
  60.         /// <param name="y">行号</param>
  61.         /// <param name="x">列号</param>
  62.         /// <param name="iswall"></param>
  63.         public NodeInfo(int y, int x, bool iswall)
  64.         {
  65.             Row = y;
  66.             Column = x;
  67.             IsWall = iswall;
  68.         }
  69.     }
  70. }

2、地图信息MapInfo

  1. #define ANIMATE
  2.  
  3. using System;
  4. using System.IO;
  5. using System.Text;
  6. using System.Linq;
  7. using System.Collections.Generic;
  8. using System.Text.RegularExpressions;
  9.  
  10. namespace Legalsoft.Truffer.Algorithm
  11. {
  12.     /// <summary>
  13.     /// 地图信息（墙1与空0）
  14.     /// </summary>
  15.     public class MapInfo
  16.     {
  17.         /// <summary>
  18.         /// 行数
  19.         /// </summary>
  20.         public int Row { get; set; } = 0;
  21.         /// <summary>
  22.         /// 列数
  23.         /// </summary>
  24.         public int Column { get; set; } = 0;
  25.         /// <summary>
  26.         /// 所有节点
  27.         /// </summary>
  28.         public NodeInfo[,] Nodes { get; set; } = null;
  29.  
  30.         /// <summary>
  31.         /// 从文本创建地图
  32.         /// </summary>
  33.         /// <param name="buf"></param>
  34.         public void FromBuffer(string buf)
  35.         {
  36.             buf = buf.Replace("\r", "").Trim();
  37.             string[] xlines = buf.Split(new char[] { '\n' }, StringSplitOptions.RemoveEmptyEntries);
  38.             Nodes = new NodeInfo[xlines.Length, xlines[0].Trim().Length];
  39.             Row = Nodes.GetLength(0);
  40.             Column = Nodes.GetLength(1);
  41.             int y = 0;
  42.             foreach (string xu in xlines)
  43.             {
  44.                 for (int x = 0; x < xu.Trim().Length; x++)
  45.                 {
  46.                     NodeInfo node = new NodeInfo(y, x, Int32.Parse(xu.Trim().Substring(x, 1)) > 0);
  47.                     Nodes[y, x] = node;
  48.                 }
  49.                 y++;
  50.             }
  51.         }
  52.  
  53.         /// <summary>
  54.         /// 从文件创建地图
  55.         /// </summary>
  56.         /// <param name="filename"></param>
  57.         /// <exception cref="Exception"></exception>
  58.         public void FromFile(string filename)
  59.         {
  60.             try
  61.             {
  62.                 string buf = File.ReadAllText(filename);
  63.                 FromBuffer(buf);
  64.             }
  65.             catch (Exception ex)
  66.             {
  67.                 throw new Exception("");
  68.             }
  69.         }
  70.         /// <summary>
  71.         /// this重载
  72.         /// </summary>
  73.         /// <param name="y"></param>
  74.         /// <param name="x"></param>
  75.         /// <returns></returns>
  76.         public NodeInfo this[int y, int x]
  77.         {
  78.             set { Nodes[y, x] = value; }
  79.             get { return Nodes[y, x]; }
  80.         }
  81.         /// <summary>
  82.         /// 第一个节点
  83.         /// </summary>
  84.         public NodeInfo First
  85.         {
  86.             get { return Nodes[0, 0]; }
  87.         }
  88.         /// <summary>
  89.         /// 最后一个节点
  90.         /// </summary>
  91.         public NodeInfo Last
  92.         {
  93.             get { return Nodes[Row - 1, Column - 1]; }
  94.         }
  95.  
  96.         /// <summary>
  97.         /// 清除路径
  98.         /// </summary>
  99.         public void ClearPath()
  100.         {
  101.             for (int i = 0; i < Row; i++)
  102.             {
  103.                 for (int j = 0; j < Column; j++)
  104.                 {
  105.                     this[i, j].Visited = false;
  106.                 }
  107.             }
  108.         }
  109.     }
  110. }

3、核心代码 AStar Kernal

  1. #define ANIMATE
  2.  
  3. using System;
  4. using System.IO;
  5. using System.Text;
  6. using System.Linq;
  7. using System.Collections.Generic;
  8. using System.Text.RegularExpressions;
  9.  
  10. namespace Legalsoft.Truffer.Algorithm
  11. {
  12.     public class AStar
  13.     {
  14.         /// <summary>
  15.         /// 开放列表
  16.         /// </summary>
  17.         private List<NodeInfo> openList { get; set; } = new List<NodeInfo>();
  18.         /// <summary>
  19.         /// 当前访问的节点
  20.         /// </summary>
  21.         private NodeInfo curNode { get; set; } = null;
  22.         /// <summary>
  23.         /// 起始节点
  24.         /// </summary>
  25.         private NodeInfo startNode { get; set; } = null;
  26.         /// <summary>
  27.         /// 终止节点
  28.         /// </summary>
  29.         private NodeInfo endNode { get; set; } = null;
  30.         /// <summary>
  31.         /// 地图
  32.         /// </summary>
  33.         public MapInfo map { get; set; } = new MapInfo();
  34.  
  35.         /// <summary>
  36.         /// 构造函数
  37.         /// </summary>
  38.         public AStar()
  39.         {
  40.         }
  41.  
  42.         /// <summary>
  43.         /// 节点加入列表，并计算相关权值
  44.         /// </summary>
  45.         /// <param name="node"></param>
  46.         private void AddToList(NodeInfo node)
  47.         {
  48.             if (node.IsWall) return;
  49.             if (node.InClose) return;
  50.             if (node.InOpen) return;
  51.             openList.Add(node);
  52.             node.InOpen = true;
  53.             node.ManhattanWeight = curNode.ManhattanWeight + WeightOf(node, curNode);
  54.             node.RemoteWeight = WeightOf(node, endNode);
  55.             node.TotalWeight = node.ManhattanWeight + node.RemoteWeight;
  56.             node.Last = curNode;
  57.         }
  58.  
  59.         /// <summary>
  60.         /// 路径规划A*算法(最短路径A*算法)
  61.         /// 默认左上角(0,0)为起点；右下角为终点；
  62.         /// </summary>
  63.         /// <param name="start">起点</param>
  64.         /// <param name="end">终点</param>
  65.         public void Tracing(NodeInfo start = null, NodeInfo end = null)
  66.         {
  67.             List<int[]> steps = new List<int[]>() {
  68.                 new int[2] {  1, 1 },
  69.                 new int[2] {  0, 1 },
  70.                 new int[2] {  1, 0 },
  71.                 new int[2] {  0,-1 },
  72.                 new int[2] { -1, 0 },
  73.                 new int[2] {  1,-1 },
  74.                 new int[2] { -1, 1 },
  75.                 new int[2] { -1,-1 },
  76.             };
  77.  
  78.             startNode = (start == null) ? map.First : start;
  79.             endNode = (end == null) ? map.Last : end;
  80.  
  81.             // 将起点加入到开放列表中
  82.             openList.Add(startNode);
  83.             while (true)
  84.             {
  85.                 openList.Sort(SortByCost);
  86.  
  87.                 curNode = openList[0];
  88.                 openList.Remove(curNode);
  89.                 curNode.InOpen = false;
  90.                 curNode.InClose = true;
  91.  
  92.                 // 终点?
  93.                 if (curNode == endNode)
  94.                 {
  95.                     return;
  96.                 }
  97.  
  98.                 MarkPath();
  99.                 map.ClearPath();
  100.             }
  101.         }
  102.  
  103.         /// <summary>
  104.         /// 节点1到节点2的Weight
  105.         /// </summary>
  106.         /// <param name="a">节点1</param>
  107.         /// <param name="b">节点2</param>
  108.         /// <returns></returns>
  109.         private int WeightOf(NodeInfo a, NodeInfo b)
  110.         {
  111.             // 直行步长
  112.             double straightDistance = 1.0;
  113.             // 斜行步长
  114.             double diagonalDistance = 1.414;
  115.  
  116.             int deltaX = Math.Abs(a.Column - b.Column);
  117.             int deltaY = Math.Abs(a.Row - b.Row);
  118.             if (deltaX == deltaY)
  119.             {
  120.                 return (int)(deltaX * diagonalDistance);
  121.             }
  122.             else if (deltaX < deltaY)
  123.             {
  124.                 return (int)(deltaX * diagonalDistance + (deltaY - deltaX) * straightDistance);
  125.             }
  126.             else
  127.             {
  128.                 return (int)(deltaY * diagonalDistance + (deltaX - deltaY) * straightDistance);
  129.             }
  130.         }
  131.  
  132.         /// <summary>
  133.         /// 排序的委托函数
  134.         /// </summary>
  135.         /// <param name="a">节点1</param>
  136.         /// <param name="b">节点2</param>
  137.         /// <returns></returns>
  138.         private int SortByCost(NodeInfo a, NodeInfo b)
  139.         {
  140.             if (a.TotalWeight.CompareTo(b.TotalWeight) != 0)
  141.             {
  142.                 return (a.TotalWeight.CompareTo(b.TotalWeight));
  143.             }
  144.             else if (a.RemoteWeight.CompareTo(b.RemoteWeight) != 0)
  145.             {
  146.                 return (a.RemoteWeight.CompareTo(b.RemoteWeight));
  147.             }
  148.             else
  149.             {
  150.                 return 1;
  151.             }
  152.         }
  153.  
  154.         /// <summary>
  155.         /// 标记当前获取的路径
  156.         /// </summary>
  157.         public void MarkPath()
  158.         {
  159.             NodeInfo cd = curNode;
  160.             while (cd != null)
  161.             {
  162.                 if (cd == startNode)
  163.                 {
  164.                     break;
  165.                 }
  166.                 cd.Visited = true;
  167.                 cd = cd.Last;
  168.             }
  169.         }
  170.     }
  171. }

--------------------------------------------------------------------------------

POWER BY TRUFFER.CN

声明：本文内容由网友自发贡献，不代表【wpsshop博客】立场，版权归原作者所有，本站不承担相应法律责任。如您发现有侵权的内容，请联系我们。转载请注明出处：https://www.wpsshop.cn/w/笔触狂放9/article/detail/218652
推荐阅读
相关标签

Copyright © 2003-2013 www.wpsshop.cn 版权所有，并保留所有权利。

  

闽ICP备14008679号