当前位置：首页 > news >正文

C#进阶11：C#局部路径规划算法_DWA

news 2025/11/4 12:19:28

本节目标：

1）了解局部路径规划算法DWA的基本原理；

2）使用C#实现DWA算法；

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

下面是DWA算法的Winform实现结果，运行程序后，任意点击一个位置，该位置作为开始点，再点击一个位置作为结束点，然后程序会自动移动到结束点，中间的障碍物会自动进行避让；

运行环境：

VS2013（.net framework 4.5）

1.1 算法原理

动态窗口法（Dynamic Window Approach, DWA）是一种局部路径规划算法，主要用于动态环境中的实时避障和运动规划。它通过在机器人的速度空间中采样，预测机器人在不同速度下的轨迹，并评估这些轨迹的优劣来选择最佳速度。

1.1.1 动态空间窗口

DWA算法最终输出是机器人运行的速度和角速度，所以根据运动的一些限定条件可以约束出速度和角速度的范围，为后面的确定最优速度组合提供基础。

假设机器人t时刻的速度为(vt ，wt), vrmin最小速度，vrmax最大速度，wrmin最小角速度，wrmax最大角速度，amax是最大加速度，αmax是最大角加速度。

$\begin{aligned} &v_{min}=max(v_{rmin},v_t-a_{max}*\Delta t)\\ &v_{max}=min(v_{rmax},v_t+a_{max}*\Delta t)\\ &w_{min}=max(w_{rmin},w_t-\alpha_{max}*\Delta t)\\ &w_{max}=min(w_{rmax},v_t+\alpha_{max}*\Delta t) \end{aligned}$

1.1.2 速度采样

根据自己的需求，将速度和角速度进行离散处理，其中vstep是速度间隔，wstep是角速度间隔。

（vmin ，wmin )	（vmin ，wmin+wstep )	...	（vmin ，wmax )
（vmin +vstep，wmin )	（vmin +vstep，wmin +wstep)
...		...
（vmax ，wmin )			（vmax ，wmax )

1.1.3 轨迹计算

离散化后的计算公式如下所示。

$\begin{aligned} &x_{t+1}=x_{t}+v*cos(\theta)*\Delta t\\ &y_{t+1}=y_{t}+v*sin(\theta)*\Delta t\\ &\theta_{t+1}=\theta_t+w*\Delta t\\ \end{aligned}$

每一个模拟速度和角速度根据上面的公式可以模拟出下一个Δt时间后的位置，如果需要模拟20个Δt时间，上面的公式就需要重复计算20次，得到一个轨迹。

1.1.4 评价函数

每一个速度组合的评价函数如下，分数越低越好，最终你选择分数最小的那个速度组合。

$\begin{aligned} &Score(j)=k_{h}*\frac{H_{j}}{\sum_{j=0}^n H_{j}} + k_{d}*\frac{D_{j}}{\sum_{j=0}^n D_{j}} + k_{v}*\frac{V_{j}}{\sum_{j=0}^n V_{j}}\\ \end{aligned}$

其中，kh是距离目标距离的权重系数，kd是与最近障碍物距离倒数的权重系数，kv是机器人速度的权重系数。

1.2 实现

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

项目程序结构，其中NavDWA.cs是用于编写DWA算法。

1.2.1 变量

Form1中的变量

bool bStartPointFlg = false;
bool bGoalPointFlg = false;
int upvalue = 0;
int nx = 0;
int ObstacleRadius = 50;
Point[] dwaObstacleC = new Point[10];
Point[] dwaObstacle = new Point[10];
Point Startpoint, Goalpoint;
NavDWA navdwa;
const double pi = 3.141592654;

1.2.2 初始化

障碍物位置初始化

private void Init()
{          dwaObstacleC[0].X = 350;dwaObstacleC[0].Y = 250;dwaObstacleC[1].X = 500;dwaObstacleC[1].Y = 250;dwaObstacleC[2].X = 650;dwaObstacleC[2].Y = 250;dwaObstacleC[3].X = 650;dwaObstacleC[3].Y = 100;dwaObstacleC[4].X = 650;dwaObstacleC[4].Y = 400;dwaObstacleC[5].X = 200;dwaObstacleC[5].Y = 400;dwaObstacleC[6].X = 200;dwaObstacleC[6].Y = 100;dwaObstacleC[7].X = 200;dwaObstacleC[7].Y = 250;Startpoint.X = 10;Startpoint.Y = 10;Goalpoint.X = 100;Goalpoint.Y = 100;navdwa = new NavDWA();navdwa.OnProgress += this.RefreshUI;navdwa.OnDWAend += this.DWAend;
}
private void RefreshUI(int v)
{// 判断一下是否需要跨线程if (this.InvokeRequired)this.BeginInvoke((MethodInvoker)(() => this.Invalidate()));elsethis.Invalidate();
}
private void DWAend()
{MessageBox.Show("DWA End!!");
}

1.2.3 主界面重绘

界面上的图案是通过重绘显示出来的，下面是重绘方法中的内容。

protected override void OnPaint(PaintEventArgs e)
{base.OnPaint(e);Graphics g = e.Graphics;//开始点if (bStartPointFlg)ellipse(Startpoint, 10, Color.Red, g);//结束点if (bGoalPointFlg)ellipse(Goalpoint, 10, Color.Green, g);//障碍物for (int k = 0; k<2; k++){dwaObstacle[k].X=(dwaObstacleC[k].X);dwaObstacle[k].Y=(dwaObstacleC[k].Y + (int)(100 * Math.Sin((k + 1)*nx*pi / 720)));navdwa.dwaObstacle[k].x = (double)dwaObstacle[k].X / 100;navdwa.dwaObstacle[k].y = (double)dwaObstacle[k].Y / 100;ellipse(dwaObstacle[k], ObstacleRadius, Color.Black,g);nx++;}for (int k = 2; k<8; k++){dwaObstacle[k].X=(dwaObstacleC[k].X);dwaObstacle[k].Y=(dwaObstacleC[k].Y);navdwa.dwaObstacle[k].x = (double)dwaObstacle[k].X / 100;navdwa.dwaObstacle[k].y = (double)dwaObstacle[k].Y / 100;ellipse(dwaObstacle[k], ObstacleRadius, Color.Black, g);}//小车if (navdwa.bCarShowFlg){Point pCarCurrentPoint = new Point();pCarCurrentPoint.X = (int)(navdwa.pCarCurrentPoint.x * 100);pCarCurrentPoint.Y = (int)(navdwa.pCarCurrentPoint.y * 100);ellipse(pCarCurrentPoint, 10, Color.Red,g);}//路径显示if (navdwa.bCarPathShowFlg){Point pCarCurrentPathX = new Point(), pCarCurrentPathY = new Point();for (int k = 0; k<navdwa.iCarPathNum; k++){pCarCurrentPathX.X=(navdwa.pCarPathStartPoint[k].x);pCarCurrentPathX.Y=(navdwa.pCarPathStartPoint[k].y);pCarCurrentPathY.X=(navdwa.pCarPathEndPoint[k].x);pCarCurrentPathY.Y=(navdwa.pCarPathEndPoint[k].y);line(pCarCurrentPathX, pCarCurrentPathY, Color.Blue,g);}}
}//画椭圆
void ellipse(Point pt, int radius, Color color,Graphics g)
{SolidBrush brush = new SolidBrush(color);g.FillEllipse(brush,pt.X - radius, pt.Y - radius, 2 * radius, 2 * radius);
}
//画直线
void line(Point Beg, Point End, Color color,Graphics g)
{SolidBrush brush = new SolidBrush(color);Pen pen = new Pen(brush, 1);g.DrawLine(pen,Beg,End);
}

1.2.4 鼠标左键事件

鼠标第一此点击确定初始位置，第二次点击确定结束位置，并且开始DWA算法模拟，第三此点击结束运行。

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;this.Invalidate();navdwa.dwaStartPoint.x = (double)Startpoint.X / 100;navdwa.dwaStartPoint.y = (double)Startpoint.Y / 100;navdwa.dwaEndPoint.x = (double)Goalpoint.X / 100;navdwa.dwaEndPoint.y = (double)Goalpoint.Y / 100;navdwa.dwaStartTheta = 0;for (int k = 0; k<8; k++) {navdwa.dwaObstacle[k].x = (double)dwaObstacle[k].X / 100;navdwa.dwaObstacle[k].y = (double)dwaObstacle[k].Y / 100;}StartDWA();}else if (e.Button == MouseButtons.Left && upvalue == 2){bStartPointFlg = false;bGoalPointFlg = false;navdwa.bCarShowFlg = false;navdwa.bCarPathShowFlg = false;upvalue = 0;this.Invalidate();}
}void StartDWA()
{Task.Run(() =>{navdwa.DWA();});            
}

1.2.5 NavDWA类变量

DWA参数以及其初始化

double dwaObstacleRadius = 0.5;
bool bStopFlg = false;
const double pi = 3.141592654;
int NumMax = 8000;
double dwaStartVel = 0;//开始速度
double dwaStartAngular = 0;//开始角速度double velocitymin = 0;			//最小速度
double velocitymax = 0.4;		//最大速度
double Vacc = 0.6;				//加速度
double velocitypix = 0.01;		//速度分辨率
double angularmin = -50 * pi / 180;	//最小角速度
double angularmax = 50 * pi / 180;	//最大角速度
double Wacc = 45.0 * pi / 180.0;	//角加速度
double angularpix = 1 * pi / 180;			//角速度分辨率
double Robotradius = 0.1;             //机器人模型半径
int samplingtime = 200;		//采样时间 (ms)
int Predict_Time = 2000; //模拟轨迹的持续时间 (ms)
double dKHead = 1.5;//方位角，权重
double dKDist = 0.15;//小车与最近障碍物之间的距离，权重
double dKVel = 1;//轨迹对应的速度大小，权重
double dSafyDistance = 0.05;//安全距离
double dwaEndTheta = 0;public bool bCarShowFlg = false;
public bool bCarPathShowFlg = false;
public dPoint pCarCurrentPoint;//小车当前位置
public int iCarPathNum;
public dPoint dwaStartPoint;
public dPoint dwaEndPoint;
public point[] pCarPathStartPoint=new point[8000];
public point[] pCarPathEndPoint = new point[8000];
public dPoint[] dwaObstacle = new dPoint[10];//圆形障碍物
public double dwaStartTheta;
public event Action<int> OnProgress;   // UI刷新事件
public event Action OnDWAend;//DWA运行结束事件public struct dPoint//双精度位置
{public double x;public double y;
};
public struct point//整形位置
{public int x;public int y;
};

1.2.6 DWA算法

下面是DWA算法的实现流程图：

DWA实现的核心代码如下：

public void DWA()
{int i,j;dwaEndTheta = 90*pi/180;double[] dHeading = new double[NumMax];//距离目标点评价函数double[] dDisting = new double[NumMax];//小车与最近障碍物之间的距离double[] dVelocitying = new double[NumMax];// 轨迹对应的速度大小pCarCurrentPoint = dwaStartPoint;//当前位置bCarShowFlg = true;bCarPathShowFlg = true;double dCarCurrentVel = dwaStartVel;//当前速度double dCarCurrentAngular = dwaStartAngular;//当前速度double dCarCurrentTheta = dwaStartTheta;//当前角度while ((Math.Abs(pCarCurrentPoint.x-dwaEndPoint.x)>0.02||Math.Abs(pCarCurrentPoint.y-dwaEndPoint.y)>0.02) && !bStopFlg){i=0;double velocity1;double[] dCarSampleTheta = new double[NumMax];double[] dCarSampleVel = new double[NumMax];double[] dCarSampleAngular = new double[NumMax];//计算速度采样范围double dCarSampleVelMin;//本次采样速度最小值double dCarSampleVelMax;//本次采样速度最大值if (velocitymin>= (dCarCurrentVel- (double)(Vacc*samplingtime)/1000))dCarSampleVelMin = velocitymin;elsedCarSampleVelMin = dCarCurrentVel-(double)(Vacc*samplingtime)/1000;if (velocitymax<= (dCarCurrentVel+ (double)(Vacc*samplingtime)/1000))dCarSampleVelMax = velocitymax;elsedCarSampleVelMax = dCarCurrentVel+(double)(Vacc*samplingtime)/1000;//计算角速度采样范围double dCarSampleAngularMin;//本次采样角速度最小值double dCarSampleAngularMax;//本次采样角速度最大值if (angularmin >= (dCarCurrentAngular-(double)(Wacc*samplingtime)/1000 ))dCarSampleAngularMin = angularmin;elsedCarSampleAngularMin =dCarCurrentAngular - (double)(Wacc*samplingtime)/1000;if (angularmax<= (dCarCurrentAngular+ (double)(Wacc*samplingtime)/1000))dCarSampleAngularMax = angularmax;elsedCarSampleAngularMax = dCarCurrentAngular+ (double)(Wacc*samplingtime)/1000;//遍历所有速度v & w， 根据模型模拟一段时间的路径velocity1 = dCarSampleVelMin;double dSampleTime = (double)samplingtime/1000;iCarPathNum = 0;while (velocity1<=dCarSampleVelMax){double angular1 = dCarSampleAngularMin;while (angular1<=dCarSampleAngularMax){int time1 = 0;dPoint pCarSampleTimePoint = pCarCurrentPoint;//选定速度下的位置点point paintpointstart;paintpointstart.x = (int)(pCarSampleTimePoint.x*100);paintpointstart.y = (int)(pCarSampleTimePoint.y*100);double dCarSampleTimeTheta = dCarCurrentTheta;j=0;double dObstacleDistanceAll=0;bool isSafy = true;while(time1 < Predict_Time){pCarSampleTimePoint.x = pCarSampleTimePoint.x + velocity1* dSampleTime*Math.Cos(dCarSampleTimeTheta);pCarSampleTimePoint.y = pCarSampleTimePoint.y + velocity1* dSampleTime*Math.Sin(dCarSampleTimeTheta);				dCarSampleTimeTheta = dCarSampleTimeTheta + angular1*dSampleTime;double qwe = obstacle(pCarSampleTimePoint);if(qwe<=0){isSafy = false;break;}point paintpointend;paintpointend.x = (int)(pCarSampleTimePoint.x*100);paintpointend.y = (int)(pCarSampleTimePoint.y*100);dObstacleDistanceAll = dObstacleDistanceAll + 1/qwe;pCarPathStartPoint[iCarPathNum] = paintpointstart;pCarPathEndPoint[iCarPathNum] = paintpointend;paintpointstart = paintpointend;j++;iCarPathNum++;time1 = time1 + samplingtime;}if(!isSafy){angular1 = angular1 + angularpix;continue;}dHeading[i] = Euclid1(pCarSampleTimePoint,dwaEndPoint);//目标位置评价值dDisting[i] = dObstacleDistanceAll/j;//距离障碍物评价值dVelocitying[i]= velocitymax - velocity1;//速度评价值	dCarSampleVel[i] = velocity1;dCarSampleAngular[i] = angular1;dCarSampleTheta[i] = dCarCurrentTheta + angular1*dSampleTime;angular1 = angular1 + angularpix;i++;}velocity1 = velocity1 + velocitypix;}double dHeadingAll=0;double dDistingAll=0;double dVelocityingAll=0;for(j=0;j<i;j++){dHeadingAll = dHeadingAll + dHeading[j];dDistingAll = dDistingAll + dDisting[j];dVelocityingAll = dVelocityingAll + dVelocitying[j];}double[] dScore = new double[NumMax];//评价函数,分数越小越好double dScoreMin=100000;int iScoreMinNum = 0;for(j=0;j<i;j++){dScore[j] = 1*(dKHead*dHeading[j]/dHeadingAll + dKDist*dDisting[j]/dDistingAll + dKVel*dVelocitying[j]/dVelocityingAll);if (dScore[j]<=dScoreMin){dScoreMin = dScore[j];iScoreMinNum = j;}}OnProgress.Invoke(1);//界面刷新pCarCurrentPoint.x = pCarCurrentPoint.x + dCarSampleVel[iScoreMinNum]*dSampleTime*Math.Cos(dCarSampleTheta[iScoreMinNum]);pCarCurrentPoint.y = pCarCurrentPoint.y + dCarSampleVel[iScoreMinNum]*dSampleTime*Math.Sin(dCarSampleTheta[iScoreMinNum]);dCarCurrentVel = dCarSampleVel[iScoreMinNum];//当前速度dCarCurrentAngular = dCarSampleAngular[iScoreMinNum];//当前角速度dCarCurrentTheta = dCarSampleTheta[iScoreMinNum];//当前角度				System.Threading.Thread.Sleep(samplingtime);}OnDWAend.Invoke();
}double obstacle(dPoint dp)
{double[] distance = new double[10];double re;for(int k=0;k<8;k++){distance[k]=Euclid1(dp,dwaObstacle[k]) - dwaObstacleRadius;}re = 100000;for(int k=0;k<8;k++){if(re>=distance[k])re = distance[k];}return re - Robotradius - dSafyDistance;
}double Euclid1(dPoint p1,dPoint p2)
{double m;m = Math.Sqrt(Math.Pow((p1.x - p2.x), 2) + Math.Pow((p1.y - p2.y), 2));return m;
}