基于A*算法的单AGV路径规划

  • 基于A*算法的单AGV路径规划已关闭评论
  • 115 views
  • A+
所属分类:AGV图纸程序
摘要

基于A*算法的单AGV路径规划

基于A*算法的单AGV路径规划

基于曼哈顿距离权重的Astar算法的单AGV路径规划(C#版),并对Astar算法的运行时间进行可视化显示。
基于A*算法的单AGV路径规划
主界面.cs:

  1. using System;  
  2. using System.Collections.Generic;  
  3. using System.ComponentModel;  
  4. using System.Data;  
  5. using System.Drawing;  
  6. using System.Linq;  
  7. using System.Text;  
  8. using System.Threading.Tasks;  
  9. using System.Windows.Forms;  
  10. using System.Threading;  
  11.   
  12.   
  13.   
  14. namespace AGVS  
  15. {  
  16.    
  17.     public partial class 主界面 : Form  
  18.     {  
  19.         public class mybutton : Button  
  20.         {  
  21.             int x;  
  22.             int y;  
  23.             public int X  
  24.             {  
  25.                 set { x = value; }  
  26.                 get { return x; }  
  27.             }  
  28.   
  29.             public int Y  
  30.             {  
  31.                 set { y = value; }  
  32.                 get { return y; }  
  33.             }  
  34.   
  35.             private void InitializeComponent()  
  36.             {  
  37.                 this.SuspendLayout();  
  38.                 this.ResumeLayout(false);  
  39.   
  40.             }  
  41.         }  
  42.         enum mycc   //定义枚举变量  
  43.         {  
  44.             wall,  
  45.             start,  
  46.             des  
  47.         }  
  48.         mycc mychoose = mycc.wall;  
  49.         byte[,] R = new byte[10, 10] {     
  50.           { 1, 1, 1, 1, 1, 1, 1, 1, 1, 1 },    
  51.           { 1, 1, 1, 1, 1, 1, 1, 1, 1, 1 },    
  52.           { 1, 1, 1, 1, 1, 1, 1, 1, 1, 1 },    
  53.           { 1, 1, 1, 1, 1, 1, 1, 1, 1, 1 },    
  54.           { 1, 1, 1, 1, 1, 1, 1, 1, 1, 1 },    
  55.           { 1, 1, 1, 1, 1, 1, 1, 1, 1, 1 },    
  56.           { 1, 1, 1, 1, 1, 1, 1, 1, 1, 1 },    
  57.           { 1, 1, 1, 1, 1, 1, 1, 1, 1, 1 },    
  58.           { 1, 1, 1, 1, 1, 1, 1, 1, 1, 1 },    
  59.           { 1, 1, 1, 1, 1, 1, 1, 1, 1, 1 }    
  60.               
  61.           };  
  62.   
  63.         mybutton[,] mybut = new mybutton[21, 11];  
  64.   
  65.         byte[,] myR = new byte[21, 11];/////////////建立我的地图  
  66.   
  67.   
  68.         MyPoint pa = new MyPoint();  
  69.         MyPoint pb = new MyPoint();  
  70.   
  71.    
  72.         public 主界面()  
  73.         {  
  74.             InitializeComponent();  
  75.         }  
  76.   
  77.         private void 退出ToolStripMenuItem_Click(object sender, EventArgs e)  
  78.         {  
  79.   
  80.         }  
  81.   
  82.         private void menuStrip1_ItemClicked(object sender, ToolStripItemClickedEventArgs e)  
  83.         {  
  84.   
  85.         }  
  86.   
  87.         private void 曲线显示ToolStripMenuItem_Click(object sender, EventArgs e)  
  88.         {  
  89.             Form ff = new 参数曲线();  
  90.             ff.ShowDialog();  
  91.         }  
  92.   
  93.         //private void 退出ToolStripMenuItem1_Click(object sender, EventArgs e)  
  94.         //{  
  95.         //    this.Close();  
  96.         //}  
  97.   
  98.         private void label2_Click(object sender, EventArgs e)  
  99.         {  
  100.   
  101.         }  
  102.   
  103.         private void chart1_Click(object sender, EventArgs e)  
  104.         {  
  105.   
  106.         }  
  107.   
  108.         private void 主界面_Load(object sender, EventArgs e)  
  109.         {  
  110. //////////////////////////////场景地图建模  
  111.             for (int i = 0; i < 21; i++)  
  112.             {  
  113.                 for (int j = 0; j < 11; j++)  
  114.                 {  
  115.                     mybut[i, j] = new mybutton();  
  116.                     mybut[i, j].X = i;  
  117.                     mybut[i, j].Y = j;  
  118.                     mybut[i, j].Size = new Size(40, 40);  
  119.                     mybut[i, j].Location = new Point(100+i * 40, 100+j * 40); ////////定位起点450 100  
  120.                     this.Controls.Add(mybut[i, j]);  
  121.                     myR[i, j] = 1;                                          //////表示当前单元格空闲  
  122.                     mybut[i, j].MouseDown += 主界面_MouseDown;  
  123.   
  124.                 }  
  125.             }   
  126.         }  
  127.         void 主界面_MouseDown(object sender, MouseEventArgs e)  
  128.         {  
  129.             mybutton myb = (mybutton)sender;  
  130.             int x = myb.X;  
  131.             int y = myb.Y;  
  132.             if (mychoose == mycc.wall)  
  133.             {  
  134.                 mybut[x, y].BackColor = Color.Black;  
  135.                 myR[y, x] = 0;  
  136.             }  
  137.   
  138.             if (mychoose == mycc.start)  
  139.             {  
  140.                 mybut[x, y].BackColor = Color.Green;  
  141.                 //  myR[x, y] = 1;    
  142.                 pa.x = x;  
  143.                 pa.y = y;  
  144.             }  
  145.   
  146.             if (mychoose == mycc.des)  
  147.             {  
  148.                 mybut[x, y].BackColor = Color.Red;  
  149.                 //  myR[x, y] = 1;    
  150.                 pb.x = x;  
  151.                 pb.y = y;  
  152.             }  
  153.         }  
  154.         private void 任务下达ToolStripMenuItem_Click(object sender, EventArgs e)  
  155.         {  
  156.             mychoose = mycc.start;  
  157.         }  
  158.   
  159.         private void 设置终点ToolStripMenuItem_Click(object sender, EventArgs e)  
  160.         {  
  161.             mychoose = mycc.des;  
  162.         }  
  163.   
  164.         private void 生成地图ToolStripMenuItem_Click(object sender, EventArgs e)  
  165.         {  
  166.             System.Diagnostics.Stopwatch myWatch = new System.Diagnostics.Stopwatch();  
  167.             myWatch.Start();                       
  168.             AxingTest ax = new AxingTest(myR);  
  169.             List<MyPoint> myp = new List<MyPoint>();  
  170.             myp = ax.FindeWay(pa, pb);  
  171.             myWatch.Stop();  
  172.             long myUseTime = myWatch.ElapsedMilliseconds;  
  173.             搜路时间.Text = myUseTime.ToString() + " ms";  
  174.                 foreach (MyPoint p in myp)  
  175.                 {  
  176.                     mybut[p.x, p.y].BackColor = Color.Yellow;  
  177.                 }  
  178.                 mybut[pb.x, pb.y].BackColor = Color.Red;  
  179.         }  
  180.   
  181.         private void 障碍点ToolStripMenuItem_Click(object sender, EventArgs e)  
  182.         {  
  183.             mychoose = mycc.wall;  
  184.         }  
  185.   
  186.         private void 任务管理ToolStripMenuItem_Click(object sender, EventArgs e)  
  187.         {  
  188.   
  189.         }  
  190.   
  191.   
  192.     }  
  193. }  
  194. class AxingTest  
  195. {  
  196.     List<MyPoint> Open_List = new List<MyPoint>();  
  197.     List<MyPoint> Close_List = new List<MyPoint>();  
  198.     byte[,] R;  
  199.   
  200.     public AxingTest(byte[,] R)  
  201.     {  
  202.         this.R = R;  
  203.     }  
  204.   
  205.     //从开启列表查找F值最小的节点    
  206.      
  207.   
  208.     //判断一个点是否为障碍物    
  209.     private bool IsBar(MyPoint p, byte[,] map)  
  210.     {  
  211.         if (map[p.y, p.x] == 0) return true;  
  212.         return false;  
  213.     }  
  214.   
  215.     //判断关闭列表是否包含某个坐标的点    
  216.     private bool IsInCloseList(int x, int y)  
  217.     {  
  218.         foreach (MyPoint p in Close_List)  
  219.             if (p.x == x && p.y == y)  
  220.                 return true;  
  221.         return false;  
  222.     }  
  223.   
  224.     //从关闭列表返回对应坐标的点    
  225.     private MyPoint GetPointFromCloseList(int x, int y)  
  226.     {  
  227.         foreach (MyPoint p in Close_List)  
  228.             if (p.x == x && p.y == y)  
  229.                 return p;  
  230.         return null;  
  231.     }  
  232.     //判断开始列表是否包含某个坐标的点  
  233.     private bool IsInOpenList(int x, int y)  
  234.     {  
  235.         foreach (MyPoint p in Open_List)  
  236.             if (p.x == x && p.y == y)  
  237.                 return true;  
  238.         return false;  
  239.     }  
  240.     //从开始列表返回对应坐标的点   
  241.     private MyPoint GetPointFromeOpenList(int x, int y)  
  242.     {  
  243.         foreach (MyPoint p in Open_List)  
  244.             if (p.x == x && p.y == y)  
  245.                 return p;  
  246.         return null;  
  247.     }  
  248.     //计算g  
  249.     private int GetG(MyPoint p)  
  250.     {  
  251.         if (p.father == nullreturn 0;  
  252.         if (p.x == p.father.x || p.y == p.father.y)  
  253.             return p.father.G + 10;  
  254.         else  
  255.             return 0;  
  256.     }  
  257.     //计算h  
  258.     private int GetH(MyPoint p, MyPoint pb)  
  259.     {  
  260.         return Math.Abs(p.x - pb.x) * 10 + Math.Abs(p.y - pb.y) * 10;  
  261.     }  
  262.     //更新开启列表  
  263.     private MyPoint CheckP4(MyPoint p0, byte[,] map, MyPoint pa, ref MyPoint pb)  
  264.     {  
  265.         MyPoint Hmin = new MyPoint();  
  266.         Hmin =null;  
  267.         for (int xt = p0.x - 1; xt <= p0.x + 1; xt++)  
  268.         {  
  269.             for (int yt = p0.y - 1; yt <= p0.y + 1; yt++)  
  270.             {  
  271.                 //取上下左右邻点  
  272.                 if (xt == p0.x || yt == p0.y)  
  273.                 {  
  274.                     //排除超过边界和关闭自身的点    
  275.                     if ((xt >= 0 && xt < R.GetLength(0) && yt >= 0 && yt < R.GetLength(1)) && !(xt == p0.x && yt == p0.y) )  
  276.                     {  
  277.                         //排除障碍点和关闭列表中的点    
  278.                         if (map[xt, yt] != 0 && !IsInCloseList(xt, yt))  
  279.                         {  
  280.                             //如果在上一次开启列表里  
  281.                             if (IsInOpenList(xt, yt))  
  282.                             {  
  283.                                 MyPoint pt = GetPointFromeOpenList(xt, yt);  
  284.                                     pt.G = p0.G + 10;  
  285.                                     pt.father = p0;  
  286.                                     pt.H = GetH(pt, pb);  
  287.                                     if (Hmin == null ||  pt.H < p0.H)  
  288.                                         Hmin = pt;   
  289.                            }  
  290.                             else  
  291.                             {  
  292.                                 //不在开启列表中    
  293.                                 MyPoint pt = new MyPoint();  
  294.                                 pt.x = xt;  
  295.                                 pt.y = yt;  
  296.                                 pt.father = p0;  
  297.                                 pt.G = GetG(pt);  
  298.                                 pt.H = GetH(pt, pb);  
  299.                                 Open_List.Add(pt);  
  300.                                 if (Hmin==null || pt.H < Hmin.H) Hmin = pt;  
  301.                             }  
  302.                         }  
  303.                     }  
  304.                 }  
  305.             }  
  306.         }  
  307.         return Hmin;  
  308.   
  309.     }  
  310.     ///智能寻路  
  311.     public List<MyPoint> FindeWay(MyPoint pa, MyPoint pb)  
  312.     {  
  313.         List<MyPoint> myp = new List<MyPoint>();  
  314.         Open_List.Add(pa);  
  315.         MyPoint p0 = pa;  
  316.         while (!(IsInOpenList(pb.x, pb.y) || Open_List.Count == 0))  
  317.         {                 
  318.              
  319.             if (p0 != pa)  
  320.             {  
  321.             Open_List.Remove(p0);  
  322.             Close_List.Add(p0);   
  323.             }  
  324.               
  325.             p0 = CheckP4(p0, R, pa, ref pb);  
  326.         }  
  327.   
  328.         MyPoint p1 = GetPointFromeOpenList(pb.x, pb.y);  
  329.         while (p1.father != null && p1!= pa)  
  330.         {  
  331.             myp.Add(p1);  
  332.             p1 = p1.father;  
  333.             // R[p.y, p.x] = 3;    
  334.         }  
  335.         return myp;  
  336.     }  
  337. }  
  338. class MyPoint  
  339. {  
  340.     public int x;  
  341.     public int y;  
  342.     public int G;  
  343.     public int H;  
  344.     public MyPoint father;  
  345.     public MyPoint()  
  346.     {  
  347.     }  
  348.     public MyPoint(int x0, int y0, int G0, int H0, MyPoint F)  
  349.     {  
  350.         x = x0;  
  351.         y = y0;  
  352.         G = G0;  
  353.         H = H0;  
  354.         father = F;  
  355.     }  
  356.   
  357.     public override string ToString()  
  358.     {  
  359.         return "{" + x.ToString() + "," + y.ToString() + "}";  
  360.     }  
  361. }  
抱歉,此资源仅限赞助会员下载,请先
注意:本站资源多为网络收集,如涉及版权问题请及时与站长联系QQ:2766242327,我们会在第一时间内与您协商解决。如非特殊说明,本站所有资源解压密码均为:agvba.com。
weinxin
微信公众号
agvba是一个分享AGV知识和agv案例视频的网站。