C#进阶13：C#全局路径规划算法_A*

葛小白12025-11-18 17:27

本节目标：

1）了解全局路径规划算法A*的基本原理；

2）使用C#实现A*算法；

3）使用Winform对A*算法进行仿真，更直观理解Astar算法；

下面是A*算法的Winform实现结果，运行程序后，任意点击一个位置，该位置作为开始点，再点击一个位置作为结束点，然后程序会自动从开始点向外搜索，直到搜索到结束点后停止，然后取其中距离最短的路径作为最优路径；

运行环境：

VS2013（.net framework 4.5）

1.1 算法原理

A*算法和Dijkstra算法类似，只是在其基础上叠加一个"到终点的估算距离"，于是搜索方向被强行拉向目标点，速度通常快得多。本节演示的是非网络的搜索示例，搜索的边长为2，斜对角线为3，如下图所示：

估价函数计算方式：

G(n) 为从起始节点到当前节点的路径代价;

*H(n)*为启发函数，是当前点到目标点距离的预估值，这里使用曼哈顿距离；

算法的基本流程：

1）新建两个点集合CloseList和OpenList，CloseList用于存放搜索过的点，OpenList用于存放待向目标点搜索的点；

2）初始化两个集合，将开始点和估价值F放入CloseList，将开始点周边未有障碍物的点和估价值F放入OpenList；

3）先判断OpenList是否为空，为空结束未找到路径，不为空，执行第4步；

4）取OpenList中第一个估价值F最小的点，将其放入CloseList中，判断其是不是目标点，如果是结束，将其路径还原出来，绘制在界面上，如果不是执行第5步；

5）依次周边的8个点搜索，判断是不是障碍物以及在没在CloseList中，如果都不是将其放入OpenList中，返回执行第3步；

流程图如下：

1.2 实现

根据上面对Astar算法的分析，进行C#程序设计，程序流程图如下；

1.2.1 变量

Form1.cs

cs 复制代码 
int paintpix;
Point[] obstacleStart = new Point[100], obstacleEnd = new Point[100];
bool bStartPointFlg;
bool bGoalPointFlg;
Point Startpoint;
Point Goalpoint;
NavAstar NavDiAs = new NavAstar();
int upvalue;

1.2.2 障碍物初始化

cs 复制代码 
void Init()
{
    //Dijkstra,Astar
    paintpix = 20;
    obstacleStart[0].X = (1 * paintpix);
    obstacleStart[0].Y = (2 * paintpix);
    obstacleEnd[0].X = (2 * paintpix);
    obstacleEnd[0].Y = (23 * paintpix);
    obstacleStart[1].X = (38 * paintpix);
    obstacleStart[1].Y = (2 * paintpix);
    obstacleEnd[1].X = (39 * paintpix);
    obstacleEnd[1].Y = (23 * paintpix);
    obstacleStart[2].X = (2 * paintpix);
    obstacleStart[2].Y = (2 * paintpix);
    obstacleEnd[2].X = (38 * paintpix);
    obstacleEnd[2].Y = (3 * paintpix);
    obstacleStart[3].X = (2 * paintpix);
    obstacleStart[3].Y = (22 * paintpix);
    obstacleEnd[3].X = (38 * paintpix);
    obstacleEnd[3].Y = (23 * paintpix);
    obstacleStart[4].X = (24 * paintpix);
    obstacleStart[4].Y = (2 * paintpix);
    obstacleEnd[4].X = (25 * paintpix);
    obstacleEnd[4].Y = (16 * paintpix);
    obstacleStart[5].X = (12 * paintpix);
    obstacleStart[5].Y = (10 * paintpix);
    obstacleEnd[5].X = (13 * paintpix);
    obstacleEnd[5].Y = (23 * paintpix);
    NavDiAs.OnSignal += this.reponsefunc;
    upvalue = 0;
}

1.2.3 重绘

cs 复制代码 
protected override void OnPaint(PaintEventArgs e)
{
    base.OnPaint(e);
    Graphics g = e.Graphics;
    Point xx = new Point(), yy = new Point();
    //开始点
    if (bStartPointFlg)
    {
        //Point xx = new Point(), yy = new Point();
        xx.X = ((Startpoint.X / paintpix) * paintpix);
        xx.Y = ((Startpoint.Y / paintpix) * paintpix);
        yy.X = ((Startpoint.X / paintpix) * paintpix + paintpix);
        yy.Y = ((Startpoint.Y / paintpix) * paintpix + paintpix);
        rect(xx, yy, Color.Red, g);
    }

    //障碍物
    for (int m = 0; m <= 5; m++)
        rect(obstacleStart[m], obstacleEnd[m], Color.Black, g);
    //路过点显示
    if (NavDiAs.bPassPointShowFlg)
    {
        for (int k = 0; k < NavDiAs.iPassNum; k++)
        {
            //Point xx = new Point(), yy = new Point();
            xx.X = (NavDiAs.pPassPoint[k].X * paintpix);
            xx.Y = (NavDiAs.pPassPoint[k].Y * paintpix);
            yy.X = (NavDiAs.pPassPoint[k].X * paintpix + paintpix);
            yy.Y = (NavDiAs.pPassPoint[k].Y * paintpix + paintpix);
            rect(xx, yy, Color.YellowGreen, g);
        }
    }
    if (NavDiAs.bFinalPassShowFlg)
    {
        for (int k = 0; k < NavDiAs.iFinalPassNum; k++)
        {

            xx.X = (NavDiAs.pFinalPassPoint[k].X * paintpix);
            xx.Y = (NavDiAs.pFinalPassPoint[k].Y * paintpix);
            yy.X = (NavDiAs.pFinalPassPoint[k].X * paintpix + paintpix);
            yy.Y = (NavDiAs.pFinalPassPoint[k].Y * paintpix + paintpix);
            rect(xx, yy, Color.Blue, g);
        }
    }
    //结束点
    if (bGoalPointFlg)
    {
        //Point xx = new Point(), yy = new Point();
        xx.X = ((Goalpoint.X / paintpix) * paintpix);
        xx.Y = ((Goalpoint.Y / paintpix) * paintpix);
        yy.X = ((Goalpoint.X / paintpix) * paintpix + paintpix);
        yy.Y = ((Goalpoint.Y / paintpix) * paintpix + paintpix);
        rect(xx, yy, Color.Green, g);
    }
}

//画矩形
void rect(Point startp, Point endp, Color color, Graphics g)
{
    SolidBrush brush = new SolidBrush(color);
    g.FillRectangle(brush, startp.X, startp.Y, Math.Abs(endp.X - startp.X), Math.Abs(endp.Y - startp.Y));
}

void reponsefunc() //槽函数
{
    this.Invalidate();
    //update();
}

1.2.4 鼠标事件

cs 复制代码 
private void Form1_MouseClick(object sender, MouseEventArgs e)
{
    if (e.Button == MouseButtons.Left && upvalue == 0)
    {
        Startpoint.X = (e.X);
        Startpoint.Y = (e.Y);
        bStartPointFlg = true;
        upvalue = 1;
        this.Invalidate();
    }
    else if (e.Button == MouseButtons.Left && upvalue == 1)
    {
        Goalpoint.X = (e.X);
        Goalpoint.Y = (e.Y);
        bGoalPointFlg = true;
        upvalue = 2;

        NavDiAs.startpoint.X = Startpoint.X / paintpix;
        NavDiAs.startpoint.Y = Startpoint.Y / paintpix;
        NavDiAs.goalpoint.X = Goalpoint.X / paintpix;
        NavDiAs.goalpoint.Y = Goalpoint.Y / paintpix;
        for (int m = 0; m <= 5; m++)
        {
            NavDiAs.obstacleStart[m].X = obstacleStart[m].X / paintpix;
            NavDiAs.obstacleStart[m].Y = obstacleStart[m].Y / paintpix;
            NavDiAs.obstacleEnd[m].X = obstacleEnd[m].X / paintpix;
            NavDiAs.obstacleEnd[m].Y = obstacleEnd[m].Y / paintpix;
        }
        StartAstar();
    }
    else if (e.Button == MouseButtons.Left && upvalue == 2)
    {
        bStartPointFlg = false;
        bGoalPointFlg = false;
        NavDiAs.bPassPointShowFlg = false;
        NavDiAs.bFinalPassShowFlg = false;
        upvalue = 0;
        this.Invalidate();
    }
}

void StartAstar()
{
    Task.Run(() =>
    {
        NavDiAs.Nva_init();
        NavDiAs.AStar();
    });
}

1.2.5 算法

NavAstar.cs

cs 复制代码 
using System;
using System.Collections.Generic;
using System.Drawing;
using System.Linq;
using System.Text;
using System.Threading.Tasks;

namespace Winform_Astar
{
    class NavAstar
    {
        public bool bPassPointShowFlg = false;//
        public Point[] pFinalPassPoint = new Point[200];
        public int iPassNum = 0;
        public Point[] pPassPoint = new Point[2000];
        public bool bFinalPassShowFlg = false;//Dijkstra&Astar最终路径
        public int iFinalPassNum = 0;
        public event Action OnSignal;//触发更新

        public Point[] obstacleStart = new Point[10], obstacleEnd = new Point[10];
        public Point goalpoint, startpoint;
        int openlistnum = 0;
        int closelistnum = 0;
        liststruct openlist = new liststruct(1000);
        liststruct closelist = new liststruct(1000);
        static int delaytime = 10; //延迟时间

        public struct liststruct
        {
            //public const int num = 1000;
            public int[] f;
            public Point[] parrant;
            public Point[] curent;
            public int[] value;//路径值
            public string[] path;//记录路径
            public liststruct(int size)
            {
                f = new int[size];
                parrant = new Point[size];
                curent = new Point[size];
                value = new int[size];
                path = new string[size];
            }
        }
        public void Nva_init()
        {
            //	point aroundpoint;//四周目标点
            openlistnum = 0;
            closelistnum = 0;
            bPassPointShowFlg = true;
            iPassNum = 0;
            iFinalPassNum = 0;
            ABCDEFGH_Init(startpoint, "B");
            ABCDEFGH_Init(startpoint, "D");
            ABCDEFGH_Init(startpoint, "F");
            ABCDEFGH_Init(startpoint, "G");
            ABCDEFGH_Init(startpoint, "A");
            ABCDEFGH_Init(startpoint, "C");
            ABCDEFGH_Init(startpoint, "E");
            ABCDEFGH_Init(startpoint, "H");

            closelist.curent[closelistnum].X = startpoint.X;
            closelist.curent[closelistnum].Y = startpoint.Y;
            closelist.value[closelistnum] = 0;
            closelistnum++;

        }
        void ABCDEFGH_Init(Point centerpoint, string str)
        {
            Point targetpoint = new Point();
            int ab = 0;
            if (str == "B")
            {
                targetpoint.X = centerpoint.X;
                targetpoint.Y = centerpoint.Y - 1;
                ab = 2;
            }
            if (str == "D")
            {
                targetpoint.X = centerpoint.X + 1;
                targetpoint.Y = centerpoint.Y;
                ab = 2;
            }
            if (str == "F")
            {
                targetpoint.X = centerpoint.X;
                targetpoint.Y = centerpoint.Y + 1;
                ab = 2;
            }
            if (str == "H")
            {
                targetpoint.X = centerpoint.X - 1;
                targetpoint.Y = centerpoint.Y;
                ab = 2;
            }
            if (str == "A")
            {
                targetpoint.X = centerpoint.X - 1;
                targetpoint.Y = centerpoint.Y - 1;
                ab = 3;
            }
            if (str == "C")
            {
                targetpoint.X = centerpoint.X + 1;
                targetpoint.Y = centerpoint.Y - 1;
                ab = 3;
            }
            if (str == "E")
            {
                targetpoint.X = centerpoint.X + 1;
                targetpoint.Y = centerpoint.Y + 1;
                ab = 3;
            }
            if (str == "G")
            {
                targetpoint.X = centerpoint.X - 1;
                targetpoint.Y = centerpoint.Y + 1;
                ab = 3;
            }
            //碰撞检测
            //bool ishit = obstacleTest(targetpoint);
            if (targetpoint.X >= 0 && targetpoint.Y >= 0 && !obstacleTest(targetpoint))
            {
                openlist.f[openlistnum] = ab + Manhattan(targetpoint, goalpoint);
                openlist.parrant[openlistnum].X = startpoint.X;
                openlist.parrant[openlistnum].Y = startpoint.Y;
                openlist.curent[openlistnum].X = targetpoint.X;
                openlist.curent[openlistnum].Y = targetpoint.Y;
                openlist.value[openlistnum] = ab;
                openlist.path[openlistnum] = str;
                //rect(HWnd,hDC,NewBrush,ps,targetpoint,"yellow");
                pPassPoint[iPassNum] = targetpoint;
                iPassNum++;
                OnSignal.Invoke();
                //emit ASignal();
                openlistnum++;
            }
        }        
        public void AStar()
        {	
	        Point curentpoint = new Point();//中心点
	        int curentvalue,curentf;//中心点路程值
	        string curentpath;//中心点路程
	        int minnum,minf;
	        while (openlistnum!=0)
	        {
		        //取openlist中F值最小的点为中心点
		        minf = openlist.f[0];
		        minnum = 0;
		        for(int i=1;i<openlistnum;i++)
		        {
			        if (minf>openlist.f[i])
			        {
				        minf=openlist.f[i];
				        minnum = i;
			        }
		        }
		        //将最小路径的点放入closelist中
		        closelist.curent[closelistnum]=openlist.curent[minnum];
		        closelist.f[closelistnum]=openlist.f[minnum];
		        closelist.parrant[closelistnum]=openlist.parrant[minnum];
		        closelist.value[closelistnum]=openlist.value[minnum];
		        closelist.path[closelistnum]=openlist.path[minnum];
		        closelistnum++;

		        curentpoint.X = openlist.curent[minnum].X;
		        curentpoint.Y = openlist.curent[minnum].Y;
		        curentvalue = openlist.value[minnum];
		        curentpath = openlist.path[minnum];
		        curentf = openlist.f[minnum];
		        //删掉openlist中的最小路径的点
		        for(int i=minnum;i<openlistnum;i++)
		        {
			        openlist.curent[i]=openlist.curent[i+1];
			        openlist.f[i]=openlist.f[i+1];
			        openlist.parrant[i]=openlist.parrant[i+1];
			        openlist.value[i] = openlist.value[i+1];
			        openlist.path[i] = openlist.path[i+1];
		        }
		        openlistnum--;
		        //如果中心点是目标点,结束
		        if (curentpoint.X==goalpoint.X&&curentpoint.Y==goalpoint.Y)
		        {
			        //cout<<"结束"<<endl;
			        //cout<<curentvalue<<endl;
			        //cout<<curentpath<<endl;
			        //cout<<"("<<startpoint.x<<","<<startpoint.y<<")"<<endl;
			        bFinalPassShowFlg = true;
			        string aa;
			        Point pp;
			        pp = startpoint;
			        int length = curentpath.Length;
			        for(int j=0;j<length;j++)
			        {
				        aa = curentpath.Substring(j, 1);
				        if(aa=="A")
				        {
					        pp.X = pp.X-1;
					        pp.Y = pp.Y-1;
				        }
				        if(aa=="B")
				        {
					        pp.X = pp.X;
					        pp.Y = pp.Y-1;
				        }
				        if(aa=="C")
				        {
					        pp.X = pp.X+1;
					        pp.Y = pp.Y-1;
				        }
				        if(aa=="D")
				        {
					        pp.X = pp.X+1;
					        pp.Y = pp.Y;
				        }
				        if(aa=="E")
				        {
					        pp.X = pp.X+1;
					        pp.Y = pp.Y+1;
				        }
				        if(aa=="F")
				        {
					        pp.X = pp.X;
					        pp.Y = pp.Y+1;
				        }
				        if(aa=="G")
				        {
					        pp.X = pp.X-1;
					        pp.Y = pp.Y+1;
				        }
				        if(aa=="H")
				        {
					        pp.X = pp.X-1;
					        pp.Y = pp.Y;
				        }
				        //rect(HWnd,hDC,NewBrush,ps,pp,"blue");
				        //cout<<"("<<pp.x<<","<<pp.y<<")"<<endl;
				        pFinalPassPoint[iFinalPassNum] = pp;
				        iFinalPassNum++;
				        //emit ASignal();
                        OnSignal.Invoke();
			        }
			        //rect(HWnd,hDC,NewBrush,ps,goalpoint,"green");
			        break;
		        }
		        else
		        {
			        AStarABCDEFGH(curentpoint,curentvalue,"B",curentpath);
			        AStarABCDEFGH(curentpoint,curentvalue,"D",curentpath);
			        AStarABCDEFGH(curentpoint,curentvalue,"F",curentpath);
			        AStarABCDEFGH(curentpoint,curentvalue,"H",curentpath);
			        AStarABCDEFGH(curentpoint,curentvalue,"A",curentpath);
			        AStarABCDEFGH(curentpoint,curentvalue,"C",curentpath);
			        AStarABCDEFGH(curentpoint,curentvalue,"E",curentpath);
			        AStarABCDEFGH(curentpoint,curentvalue,"G",curentpath);		
		        }
                System.Threading.Thread.Sleep(delaytime);	
	        }
        }
        void AStarABCDEFGH(Point centerpoint,int curentvalue,string str,string centerpath)
        {
	        bool isclose = false;//在closelist中
	        bool isopen = false;//在openlist中
	        Point targetpoint = new Point();
	        int centervalue = 0,targetf;
	        string targetpath;
	        /*
	        A B C
	        H S D
	        G F E
	        */
	        targetpath = centerpath + str;
	        if (str=="B")
	        {
		        targetpoint.X=centerpoint.X;
		        targetpoint.Y=centerpoint.Y-1;
		        centervalue = curentvalue+2;
	        }
	        if (str=="D")
	        {
		        targetpoint.X=centerpoint.X+1;
		        targetpoint.Y=centerpoint.Y;
		        centervalue = curentvalue+2;
	        }
	        if (str=="F")
	        {
		        targetpoint.X=centerpoint.X;
		        targetpoint.Y=centerpoint.Y+1;
		        centervalue = curentvalue+2;
	        }
	        if (str=="H")
	        {
		        targetpoint.X=centerpoint.X-1;
		        targetpoint.Y=centerpoint.Y;
		        centervalue = curentvalue+2;
	        }
	        if (str=="A")
	        {
		        targetpoint.X=centerpoint.X-1;
		        targetpoint.Y=centerpoint.Y-1;
		        centervalue = curentvalue+3;
	        }
	        if (str=="C")
	        {
		        targetpoint.X=centerpoint.X+1;
		        targetpoint.Y=centerpoint.Y-1;
		        centervalue = curentvalue+3;
	        }
	        if (str=="E")
	        {
		        targetpoint.X=centerpoint.X+1;
		        targetpoint.Y=centerpoint.Y+1;
		        centervalue = curentvalue+3;
	        }
	        if (str=="G")
	        {
		        targetpoint.X=centerpoint.X-1;
		        targetpoint.Y=centerpoint.Y+1;
		        centervalue = curentvalue+3;		
	        }
	        targetf = centervalue + Manhattan(targetpoint,goalpoint);
	        //是否在closelist中
	        for(int i=0;i<closelistnum;i++)
	        {
		        if((targetpoint.X==closelist.curent[i].X)&&(targetpoint.Y==closelist.curent[i].Y))
			        isclose = true;
	        }
	        //碰撞检测
	        //bool ishit=false;

	        if(!isclose&&targetpoint.X>=0&&targetpoint.Y>=0&&!obstacleTest(targetpoint))
	        {
		        int isopennum = 0;
		        isopen = false;
		        //是否在openlist中
		        for(int i=0;i<openlistnum;i++)
		        {
			        if((targetpoint.X==openlist.curent[i].X)&&(targetpoint.Y==openlist.curent[i].Y))
			        {
				        isopen = true;
				        isopennum = i;
			        }
		        }
		        if(!isopen)
		        {
			        openlist.f[openlistnum]=targetf;
			        openlist.parrant[openlistnum].X = centerpoint.X;
			        openlist.parrant[openlistnum].Y = centerpoint.Y;
			        openlist.curent[openlistnum].X = targetpoint.X;
			        openlist.curent[openlistnum].Y = targetpoint.Y;
			        openlist.value[openlistnum] = centervalue;
			        openlist.path[openlistnum] = targetpath;
			        Point paintstart = new Point();
			        paintstart.X = targetpoint.X;
			        paintstart.Y = targetpoint.Y;
			        //rect(HWnd,hDC,NewBrush,ps,paintstart,"yellow");
			        pPassPoint[iPassNum] = targetpoint;
			        iPassNum ++;
			        //emit ASignal();
                    OnSignal.Invoke();
			        openlistnum ++;
		        }
		        else
		        {
			        if(centervalue<openlist.value[isopennum])
			        {
				        openlist.f[isopennum]=targetf;
				        openlist.parrant[isopennum].X = centerpoint.X;
				        openlist.parrant[isopennum].Y = centerpoint.Y;
				        openlist.curent[isopennum].X = targetpoint.X;
				        openlist.curent[isopennum].Y = targetpoint.Y;
				        openlist.value[isopennum] = centervalue;
				        openlist.path[isopennum] = targetpath;
			        }
		        }
	        }
        }
        bool obstacleTest(Point targetpoint) //碰撞检测
        {
            bool ishit = false;
            for (int m = 0; m <= 5; m++)
            {
                if (targetpoint.X >= obstacleStart[m].X && targetpoint.X < obstacleEnd[m].X
                    && targetpoint.Y >= obstacleStart[m].Y && targetpoint.Y < obstacleEnd[m].Y)
                    ishit = true;
            }
            return ishit;
        }
        int Manhattan(Point p1, Point p2)
        {
            int m;
            m = Math.Abs(p1.X - p2.X) + Math.Abs(p1.Y - p2.Y);
            return m * 2;
        }
    }
}

1.3 运行

下面是Astar算法的Winform实现结果，运行程序后，任意点击一个位置，该位置作为开始点，再点击一个位置作为结束点，然后程序会自动从开始点向外搜索，直到搜索到结束点后停止，然后取其中距离最短的路径作为最优路径。

该算法明显比Dijkstra算法的搜索效率高。

  • 权值全为非负，且能轻松估算剩余距离 → A*（绝大多数游戏、地图、机器人）；
  • 权值复杂/无法估算/剩余距离=0 → Dijkstra（或 Bellman-Ford 负权）；
  • 负权边 → Bellman-Ford / SPFA；
相关推荐
时光追逐者5 小时前
C#/.NET/.NET Core技术前沿周刊 | 第 61 期（2025年11.10-11.16）
c#·.net·.netcore
世洋Blog6 小时前
Unity面经-List底层原理、如何基于数组、如何扩容、List存储泛型、List有关在内存中的结构
unity·面试·c#·list
PfCoder8 小时前
C# async / await 用法以及和Task的关系
c#·多线程·winform·async、await
唐青枫11 小时前
.NET Web 应用 Linux 部署全指南：从环境搭建到生产上线
c#·.net
Charles_go21 小时前
41、C#什么是单例设计模式
java·设计模式·c#
夏霞21 小时前
c# ASP.NET Core SignalR 客户端与服务端自动重连配置指南
开发语言·c#·asp.net
Scout-leaf1 天前
九成九新自用C#入门文档
c#
烛阴1 天前
隐式vs显式：解密C#类型转换的底层逻辑
前端·c#
梦里不知身是客111 天前
kafka作为Sink
c#·linq
热门推荐
01GitHub 镜像站点02UV安装并设置国内源03BongoCat - 跨平台键盘猫动画工具04Linux下V2Ray安装配置指南05安娜的档案(Anna’s Archive) 镜像网站/国内最新可访问入口（持续更新）06Valdi：Snapchat 开源的新一代跨平台 UI 框架07Labelme从安装到标注：零基础完整指南08Visual Studio Code设置个性化背景教程092025 最新教程：注册并切换到美区 Apple ID10综合整理：pdf预览显示：你尝试预览的文件可能对你的计算机有害。如果你信任此文件以及其来源，请打开此文件以看其内容，如何解决以正常预览文件