局部路径规划算法——实现DWA（dynamic window approach）控制空间采样_局部路径规划算法盘点

DWA算法是局部路径规划算法，在全局路径规划算法完成后，DWA算法能够根据当前小车（机器人）位置、障碍物、终点的位置进行控制空间（速度、角速度）的采用，从而完成局部路径规划。

DWA算法流程：

初始化一一小车最大最小速度、加速度，评价函数权重等循环
{
判断是否到达目的地
计算当前采样的速度范围(动态窗口)
遍历所有速度v&w，根据模型模拟一段时间的路径根据评价函数打分(包括评价函数、归一化、权重)选取最优解——v&w，下发给运动底盘
小车继续移动
}

下面是DWA算法的代码实现，主要步骤给了注释：
变量定义：

#include <iostream>
#include <vector>

using namespace std;

#define M_PI 3.1415927
#define MAX_VELOCITY 1.0						//弧形轨迹：最大速度
#define MIN_VELOCITY 0							//弧形轨迹：最小速度
#define MAX_OMEGA 20.0 / 180.0 * M_PI			//弧形轨迹：最大角速度
#define MIN_OMEGA 0								//弧形轨迹：最小角速度
#define MAX_ACCELERATE 0.2						//动态窗口：最大加速度
#define MAX_ACCOMEGA 50.0 / 180.0 * M_PI		//动态窗口：最大角加速度
#define SAMPLING_VELOCITY 0.01					//速度采样间隔
#define SAMPLING_OMEGA 1 / 180.0 * M_PI			//角速度采样间隔
#define DT 0.1									//采样时间间隔
#define PREDICT_TIME 3.0						//预测时间
#define WEIGHT_HEADING 0.05						//HEADING权重——小车与终点的角度
#define WEIGHT_CLEARANCE 0.2					//CLEARANCE权重——小车与最近障碍物的距离
#define WEIGHT_VELOCITY 0.1						//VELOCITY权重——小车速度
#define GOAL_X 10								//目标横坐标
#define GOAL_Y 10								//目标纵坐标
#define ROBOT_RADIUS 0.5						//机器人半径

struct RobotState
{
	// x坐标，y坐标，机器朝向，速度，角速度
	float xPosition, yPosition, orientation, velocity, omega;
};

// 障碍物
int obstacle[18][2] = { { 0, 2 },
							{ 4, 2 },
							{ 4, 4 },
							{ 5, 4 },
							{ 5, 5 },
							{ 5, 6 },
							{ 5, 9 },
							{ 8, 8 },
							{ 8, 9 },
							{ 7, 9 },
							{ 6, 5 },
							{ 6, 3 },
							{ 6, 8 },
							{ 6, 7 },
							{ 7, 4 },
							{ 9, 8 },
							{ 9, 11 },
							{ 9, 6 } };

//评估指标与速度采样空间
struct EvaluationPara
{
	float heading, clearance, velocity, v, w;
};
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54

具体实现：

#include <iostream>
#include "Constant.h"
#include <numeric>
#include <fstream>

using namespace std;

vector<float> DynamicWindowApproach(RobotState rState, int obs[][2], int target[]);
RobotState Motion(RobotState curState, float velocity, float omega);
vector<float> CreateDW(RobotState state);
vector<RobotState> GenerateTraj(RobotState initState, float vel, float ome);
float CalcHeading(RobotState rState, int goal[]);
float CalcClearance(RobotState rState, int obs[][2]);
float CalcBreakingDist(float velo);


void main()
{
	// 初始化机器人当前的参数
	RobotState currentState = { 0, 0, M_PI / 2, 0, 0 };
	int goal[2] = { GOAL_X, GOAL_Y };
	vector<RobotState> path;
	cout << "Begin!" << endl;

	int i = 0;

	while (1)
	{
		// 到达目标点退出循环
		if (currentState.xPosition < (goal[0] + 0.1) && currentState.xPosition > (goal[0] - 0.1) && currentState.yPosition < (goal[1] + 0.1) && currentState.yPosition > (goal[1] - 0.1))
		{
			cout << "Reach the Goal!" << endl;
			break;
		}

		//dwa算法选择速度和角速度
		vector<float> selectedVelocity = DynamicWindowApproach(currentState, obstacle, goal);

		// 机器人移动
		currentState = Motion(currentState, selectedVelocity[0], selectedVelocity[1]);

		path.push_back(currentState);

		cout << selectedVelocity[0] << " " << selectedVelocity[1] << " ";

		cout << currentState.xPosition << " " << currentState.yPosition <<  endl;
	}

	//输出坐标到txt
	ofstream file("map.txt");
	for (vector<RobotState>::iterator i = path.begin(); i < path.end(); i++)
	{
		file << i->xPosition << " " << i->yPosition << endl;
	}
	file.close();

	system("pause");

}

//dwa算法实现
vector<float> DynamicWindowApproach(RobotState rState, int obs[][2], int target[])
{
	// 0:minVelocity, 1:maxVelocity, 2:minOmega, 3:maxOmega
	vector<float> velocityAndOmegaRange = CreateDW(rState); //计算小车的速度和角速度变化范围
	vector<EvaluationPara>evalParas; 
	float sumHeading = 0;
	float sumClearance = 0;
	float sumVelocity = 0;

	//遍历速度和角速度区间范围内的量
	for (double v = velocityAndOmegaRange[0]; v < velocityAndOmegaRange[1]; v += SAMPLING_VELOCITY)
	{
		for (double w = velocityAndOmegaRange[2]; w < velocityAndOmegaRange[3]; w += SAMPLING_OMEGA)
		{
			vector<RobotState> trajectories = GenerateTraj(rState, v, w);

			//计算评估参数
			EvaluationPara tempEvalPara;
			//计算距小车最近的障碍物和小车在当前速度行驶的最短距离
			float tempClearance = CalcClearance(trajectories.back(), obstacle);	
			float stopDist = CalcBreakingDist(v);
			//如果在最近的障碍物前能停下来则此速度和角速度可行
			if (tempClearance > stopDist)
			{
				tempEvalPara.heading = CalcHeading(trajectories.back(), target); //小车与指点角度
				tempEvalPara.clearance = tempClearance;
				tempEvalPara.velocity = abs(v);
				tempEvalPara.v = v;
				tempEvalPara.w = w;

				sumHeading = sumHeading + tempEvalPara.heading;
				sumClearance = sumHeading + tempEvalPara.clearance;
				sumVelocity = sumVelocity + tempEvalPara.velocity;

				evalParas.push_back(tempEvalPara); //存入可行状态空间
			}
		}
	}

	//平滑评价参数并选择最优速度
	float selectedVelocity = 0;
	float selectedOmega = 0;
	float G = 0;
	for (vector<EvaluationPara>::iterator i = evalParas.begin(); i < evalParas.end(); i++)
	{
		//归一化
		float smoothHeading = i->heading / sumHeading;
		float smoothClearance = i->clearance / sumClearance;
		float smoothVelocity = i->velocity / sumVelocity;
		
		//计算评估函数并比较选最优
		float tempG = WEIGHT_HEADING*smoothHeading + WEIGHT_CLEARANCE*smoothClearance + WEIGHT_VELOCITY*smoothVelocity;

		if (tempG > G)
		{
			G = tempG;
			selectedVelocity = i->v;
			selectedOmega = i->w;
		}
	}

	vector<float> selVelocity(2);
	selVelocity[0] = selectedVelocity;
	selVelocity[1] = selectedOmega;

	return selVelocity;
}

//计算小车的速度和角速度变化范围
vector<float> CreateDW(RobotState curState)
{
	vector<float> dw(4);
	float tmpMinVelocity = curState.velocity - MAX_ACCELERATE*DT;
	float tmpMaxVelocity = curState.velocity + MAX_ACCELERATE*DT;
	float tmpMinOmega = curState.omega - MAX_ACCOMEGA*DT;
	float tmpMaxOmega = curState.omega + MAX_ACCOMEGA*DT;

	dw[0] = tmpMinVelocity > MIN_VELOCITY ? tmpMinVelocity : MIN_VELOCITY;
	dw[1] = tmpMaxVelocity < MAX_VELOCITY ? tmpMaxVelocity : MAX_VELOCITY;
	dw[2] = tmpMinOmega;
	dw[3] = tmpMaxOmega < MAX_OMEGA ? tmpMaxOmega : MAX_OMEGA;

	return dw;
}

//小车运动
RobotState Motion(RobotState curState, float velocity, float omega)
{
	RobotState afterMoveState;

	//if (omega != 0)
	//{
	//	afterMoveState.xPosition = curState.xPosition + velocity / omega*sin(curState.orientation)
	//		- velocity / omega*sin(curState.orientation + omega*DT);

	//	afterMoveState.yPosition = curState.yPosition - velocity / omega*cos(curState.orientation)
	//		- velocity / omega*cos(curState.orientation + omega*DT);
	//}
	//else
	//{
	//	afterMoveState.xPosition = curState.xPosition + velocity*cos(curState.orientation)*DT;

	//	afterMoveState.yPosition = curState.yPosition + velocity*sin(curState.orientation)*DT;
	//}

	afterMoveState.xPosition = curState.xPosition + velocity*DT*cos(curState.orientation);
	afterMoveState.yPosition = curState.yPosition + velocity*DT*sin(curState.orientation);

	afterMoveState.orientation = curState.orientation + omega * DT;
	afterMoveState.velocity = velocity;
	afterMoveState.omega = omega;	

	return afterMoveState;
}

//生成路径
vector<RobotState> GenerateTraj(RobotState initState, float vel, float ome)
{
	RobotState tempState = initState;
	vector<RobotState> trajectories;
	float time = 0;
	trajectories.push_back(initState);
	while (time < PREDICT_TIME)
	{
		tempState = Motion(tempState, vel, ome);
		trajectories.push_back(tempState);
		time += DT;
	}

	return trajectories;
}

//计算小车与终点的角度
float CalcHeading(RobotState rState, int goal[])
{
	float heading;

	float dy = goal[1] - rState.yPosition;
	float dx = goal[0] - rState.xPosition;

	float goalTheta = atan2(dy, dx);
	float targetTheta;
	if (goalTheta > rState.orientation)
	{
		targetTheta = goalTheta - rState.orientation;
	}
	else
	{
		targetTheta = rState.orientation - goalTheta;
	}

	heading = 180 - targetTheta / M_PI * 180;

	return heading;
}

//计算小车到最近的障碍物的距离
float CalcClearance(RobotState rState, int obs[][2])
{
	float dist = 100;
	float distTemp;
	for (int i = 0; i < 18; i++)
	{
		float dx = rState.xPosition - obs[i][0];
		float dy = rState.yPosition - obs[i][1];
		distTemp = sqrt(pow(dx,2) + pow(dy,2)) - ROBOT_RADIUS;

		if (dist > distTemp)
		{
			dist = distTemp;
		}
	}

	if (dist >= 2 * ROBOT_RADIUS)
	{
		dist = 2 * ROBOT_RADIUS;
	}

	return dist;
}

//计算小车当前状态最少能行驶多少
float CalcBreakingDist(float velo)
{
	float stopDist = 0;
	while (velo > 0)
	{
		stopDist = stopDist + velo*DT;
		velo = velo - MAX_ACCELERATE*DT;
	}

	return stopDist;
}
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
181
182
183
184
185
186
187
188
189
190
191
192
193
194
195
196
197
198
199
200
201
202
203
204
205
206
207
208
209
210
211
212
213
214
215
216
217
218
219
220
221
222
223
224
225
226
227
228
229
230
231
232
233
234
235
236
237
238
239
240
241
242
243
244
245
246
247
248
249
250
251
252
253
254

ROS 2D导航原理系列（六）|局部路径规划-DWA算法
DWA算法总结