一、DWA概念
DWA(动态窗口法)属于局部路径规划方法，为ROS中主要采用的方法。其原理主要是在速度空间（v,w）中采样多组速度，并模拟这些速度在一定时间内的运动轨迹，再通过一个评价函数对这些轨迹打分，最优的轨迹对应的速度被选择出来发送给机器人。
动态窗口：依据移动机器人的加减速性能限定速度采用空间在一个可行的动态范围内。
二、DWA的算法步骤
1.建立机器人运动模型
2.产生轨迹（利用动态窗口缩小需要搜索的动作空间）
3.评价轨迹选出最优轨迹和速度，控制机器人运动
4.代码实现
三、具体实现
1.机器人运动模型
在动态窗口算法中，要模拟机器人的轨迹，需要知道机器人的运动模型。它采用的是假设两轮移动机器人的轨迹是一段一段的圆弧或者直线(旋转速度为0时)，一对(Vt,Wt)就代表一个圆弧轨迹。具体推导如下：
模型一
a.非全向移动机器人（只能前进和旋转）
计算机器人的移动轨迹时，假设相邻时刻机器人的轨迹为直线，该时刻机器人自身坐标系与世界坐标系的夹角为Θ，即机器人在该时刻的位移可以简单换算成在世界坐标系的位移。
计算如下：
某一段时间内的轨迹累加求和：
b.全向移动机器人（有y轴速度）
原理相同，假设相邻时刻机器人运动轨迹为直线，与世界坐标系夹角为Θ，则将该时刻内机器人的x轴为y轴方向位移依次分解到世界坐标系再累加即可。
计算如下：
模型二
模型一假设机器人在相邻时间段内的位移近似为直线，这是不准确的，所以模型二采用圆弧来代替。
a.非全向移动机器人
b.全向移动机器人
2.产生轨迹
动态窗口的大小受电机的线加速度、角加速度、障碍物等因素影响。
(1)计算动态窗口
a.移动机器人受自身最大速度最小速度的限制
b. 移动机器人受电机性能的影响:由于电机力矩有限,存在最大的加減速限制,因此移动机器人軌迹前向模拟的周期sim_period内,
存在一个动态窗口,在该窗口内的速度是机器人能够实际达到的速度：
c.基于移动机器人安全的考虑:为了能够在碰到障碍物前停下来, 因此在最大减速度条件下, 速度有一个范围:
(2)预测轨迹
在满足约束条件的情况下，进行速度采样（v,w），可以得到相应的轨迹：
3.评价函数（选出最优轨迹和速度）
在速度空间（v,w）中采样，根据运动学模型推测对应的轨迹，接下来引入评价函数，对轨迹进行打分，选取最优的轨迹。
一般来说，评价函数如下：
其中，
- heading(v,w)为方位角评价函数：评价机器人在当前的设定的速度下，轨迹末端朝向与目标点之间的角度差距；
- dist(v,w) 主要的意义为机器人处于预测轨迹末端点位置时与地图上最近障碍物的距离，对于靠近障碍物的采样点进行惩罚，
确保机器人的避障能力，降低机器人与障碍物发生碰撞的概率；
- velocity(v,w)为当前机器人的线速度，为了促进机器人快速到达目标；δ、α、β、γ 为权重。当然，可以对评价函数进行优化，添加更多的评价函数指标。
heading(v,w)
- 平滑处理
4.代码实现
1 """
2 version1.1
3 Mobile robot motion planning sample with Dynamic Window Approach
4 结合https://blog.csdn.net/heyijia0327/article/details/44983551来看，里面含中文注释
5 符号参考《煤矿救援机器人地图构建与路径规划研究》矿大硕士论文
6 """
7
8 import math
9 import numpy as np
10 import matplotlib.pyplot as plt
11
12 show_animation = True # 动画
13
14
15 class Config(object):
16 """
17 用来仿真的参数，
18 """
19
20 def __init__(self):
21 # robot parameter
22 self.max_speed = 1.4 # [m/s] # 最大速度
23 # self.min_speed = -0.5 # [m/s] # 最小速度，设置为可以倒车
24 self.min_speed = 0 # [m/s] # 最小速度，设置为不倒车
25 self.max_yawrate = 40.0 * math.pi / 180.0 # [rad/s] # 最大角速度
26 self.max_accel = 0.2 # [m/ss] # 最大加速度
27 self.max_dyawrate = 40.0 * math.pi / 180.0 # [rad/ss] # 最大角加速度
28 self.v_reso = 0.01 # [m/s]，速度分辨率
29 self.yawrate_reso = 0.1 * math.pi / 180.0 # [rad/s]，角速度分辨率
30 self.dt = 0.1 # [s] # 采样周期
31 self.predict_time = 3.0 # [s] # 向前预估三秒
32 self.to_goal_cost_gain = 1.0 # 目标代价增益
33 self.speed_cost_gain = 1.0 # 速度代价增益
34 self.robot_radius = 1.0 # [m] # 机器人半径
35
36 #机器人运动方程
37 #在dt的时间间隔内，基于当前状态x、动作指令u=[v,w]，转移到下一个状态x，
38 #两个状态更新机器人的位置(x,y)，以及其超向orientation。
39 def motion(x, u, dt):
40 """
41 :param x: 位置参数，在此叫做位置空间
42 :param u: 速度和加速度，在此叫做速度空间
43 :param dt: 采样时间
44 :return:
45 """
46 #假设近似，机器人在dt的时间间隔内，默认为速度与转向速度（v,w）是常数
47 # 速度更新公式比较简单，在极短时间内，车辆位移也变化较大
48 # 采用圆弧求解如何？
49 x[0] += u[0] * math.cos(x[2]) * dt # x方向位移
50 x[1] += u[0] * math.sin(x[2]) * dt # y方向位移
51 x[2] += u[1] * dt # 航向角
52 x[3] = u[0] # 速度v
53 x[4] = u[1] # 角速度w
54 # print(x)
55
56 return x
57
58
59 def calc_dynamic_window(x, config):
60 """
61 位置空间集合
62 :param x:当前位置空间,符号参考硕士论文
63 :param config:
64 :return:目前是两个速度的交集，还差一个
65 """
66
67 # 车辆能够达到的最大最小速度
68 vs = [config.min_speed, config.max_speed,
69 -config.max_yawrate, config.max_yawrate]
70
71 # 一个采样周期能够变化的最大最小速度
72 vd = [x[3] - config.max_accel * config.dt,
73 x[3] + config.max_accel * config.dt,
74 x[4] - config.max_dyawrate * config.dt,
75 x[4] + config.max_dyawrate * config.dt]
76 # print(Vs, Vd)
77
78 # 求出两个速度集合的交集
79 vr = [max(vs[0], vd[0]), min(vs[1], vd[1]),
80 max(vs[2], vd[2]), min(vs[3], vd[3])]
81
82 return vr
83
84 #产生运动轨迹
85 def calc_trajectory(x_init, v, w, config):
86 """
87 预测3秒内的轨迹
88 :param x_init:位置空间
89 :param v:速度
90 :param w:角速度
91 :param config:
92 :return: 每一次采样更新的轨迹，位置空间垂直堆叠
93 """
94 x = np.array(x_init)
95 trajectory = np.array(x)
96 time = 0
97 while time <= config.predict_time:
98 x = motion(x, [v, w], config.dt)
99 trajectory = np.vstack((trajectory, x)) # 垂直堆叠，vertical
100 time += config.dt
101
102 # print(trajectory)
103 return trajectory
104
105
106 def calc_to_goal_cost(trajectory, goal, config):
107 """
108 计算轨迹到目标点的代价
109 :param trajectory:轨迹搜索空间
110 :param goal:
111 :param config:
112 :return: 轨迹到目标点欧式距离
113 """
114 # calc to goal cost. It is 2D norm.
115
116 dx = goal[0] - trajectory[-1, 0]
117 dy = goal[1] - trajectory[-1, 1]
118 goal_dis = math.sqrt(dx ** 2 + dy ** 2)
119 cost = config.to_goal_cost_gain * goal_dis
120
121 return cost
122
123
124 def calc_obstacle_cost(traj, ob, config):
125 """
126 计算预测轨迹和障碍物的最小距离，dist(v,w)
127 :param traj:
128 :param ob:
129 :param config:
130 :return:
131 """
132 # calc obstacle cost inf: collision, 0:free
133
134 min_r = float("inf") # 距离初始化为无穷大
135
136 for ii in range(0, len(traj[:, 1])):
137 for i in range(len(ob[:, 0])):
138 ox = ob[i, 0]
139 oy = ob[i, 1]
140 dx = traj[ii, 0] - ox
141 dy = traj[ii, 1] - oy
142
143 r = math.sqrt(dx ** 2 + dy ** 2)
144 if r <= config.robot_radius:
145 return float("Inf") # collision
146
147 if min_r >= r:
148 min_r = r
149
150 return 1.0 / min_r # 越小越好 与障碍物距离越近，该值会越大；
151
152
153 def calc_final_input(x, u, vr, config, goal, ob):
154 """
155 计算采样空间的评价函数，选择最合适的那一个作为最终输入
156 :param x:位置空间
157 :param u:速度空间
158 :param vr:速度空间交集
159 :param config:
160 :param goal:目标位置
161 :param ob:障碍物
162 :return:
163 """
164 x_init = x[:]
165 min_cost = 10000.0
166 min_u = u
167
168 best_trajectory = np.array([x])
169
170 # evaluate all trajectory with sampled input in dynamic window
171 # v,生成一系列速度，w，生成一系列角速度
172 for v in np.arange(vr[0], vr[1], config.v_reso): #对搜索空间dw中的v遍历
173 for w in np.arange(vr[2], vr[3], config.yawrate_reso): #对搜索空间dw中的w遍历
174 # 计算出每一个可能的动作在未来一段时间内所产生的轨迹trajectory(v,w)
175 trajectory = calc_trajectory(x_init, v, w, config)
176
177 # calc cost
178 to_goal_cost = calc_to_goal_cost(trajectory, goal, config) # trajectory与目标的欧式距离
179 speed_cost = config.speed_cost_gain * (config.max_speed - trajectory[-1, 3]) # v越大，该项越小，则效果越好；
180 ob_cost = calc_obstacle_cost(trajectory, ob, config) # 返回的是距离的倒数，所以该值约小，距离越大，越好
181 # print(ob_cost)
182
183 # 评价函数多种多样，看自己选择
184 # 本文构造的是越小越好
185 final_cost = to_goal_cost + speed_cost + ob_cost # 总计代价，越小轨迹约好
186
187 # search minimum trajectory
188 # 在所有动态窗口划出的动作空间(v,w)里，找到一个最好的动作，在这个动作下，未来预测的轨迹评价最好
189 if min_cost >= final_cost:
190 min_cost = final_cost
191 min_u = [v, w] #代价最小的时候的运动空间
192 best_trajectory = trajectory #记此时预测轨迹为最优轨迹
193
194 # print(min_u)
195 # input()
196
197 return min_u, best_trajectory
198
199
200 def dwa_control(x, u, config, goal, ob):
201 """
202 调用前面的几个函数，生成最合适的速度空间和轨迹搜索空间
203 :param x: 起始位置
204 :param u:初始速度空间（v,w）
205 :param config:
206 :param goal:目标位置
207 :param ob:障碍物
208 :return:
209 """
210 # Dynamic Window control
211
212 vr = calc_dynamic_window(x, config) #计算动态窗口，即机器人的电机实际物理参数范围
213
214 u, trajectory = calc_final_input(x, u, vr, config, goal, ob) #选出最优轨迹搜索空间和速度空间
215
216 return u, trajectory
217
218
219 def plot_arrow(x, y, yaw, length=0.5, width=0.1):
220 """
221 arrow函数绘制箭头
222 :param x:
223 :param y:
224 :param yaw:航向角
225 :param length:
226 :param width:参数值为浮点数，代表箭头尾部的宽度，默认值为0.001
227 :return:
228 length_includes_head：代表箭头整体长度是否包含箭头头部的长度，默认值为False
229 head_width：代表箭头头部的宽度，默认值为3*width，即尾部宽度的3倍
230 head_length：代表箭头头部的长度度，默认值为1.5*head_width，即头部宽度的1.5倍
231 shape：参数值为'full'、'left'、'right'，表示箭头的形状，默认值为'full'
232 overhang：代表箭头头部三角形底边与箭头尾部直接的夹角关系，通过该参数可改变箭头的形状。
233 默认值为0，即头部为三角形，当该值小于0时，头部为菱形，当值大于0时，头部为鱼尾状
234 """
235 plt.arrow(x, y, length * math.cos(yaw), length * math.sin(yaw),
236 head_length=1.5 * width, head_width=width)
237 plt.plot(x, y)
238
239
240 def main():
241 """
242 主函数
243 :return:
244 """
245 # print(__file__ + " start!!")
246 # 初始化位置空间
247 x = np.array([0.0, 0.0, math.pi / 2.0, 0.2, 0.0])
248
249 goal = np.array([10, 10])
250
251 #matrix二维矩阵 障碍物
252 ob = np.matrix([[-1, -1],
253 [0, 2],
254 [4.0, 2.0],
255 [5.0, 4.0],
256 [5.0, 5.0],
257 [5.0, 6.0],
258 [5.0, 9.0],
259 [8.0, 9.0],
260 [7.0, 9.0],
261 [12.0, 12.0]
262 ])
263 #ob = np.matrix([[0, 2]])
264 #ob = np.matrix([[2, 2]])
265
266 u = np.array([0.2, 0.0]) #初始速度空间(v,w)
267 config = Config() #初始化物理运动参数
268 trajectory = np.array(x) #初始轨迹空间
269
270 for i in range(1000):
271
272 u, best_trajectory = dwa_control(x, u, config, goal, ob) #获取最优速度空间和轨迹参数
273
274 x = motion(x, u, config.dt) #机器人运动方程 x为机器人状态空间
275 print(x)
276
277 trajectory = np.vstack((trajectory, x)) # store state history
278
279 if show_animation:
280 draw_dynamic_search(best_trajectory, x, goal, ob)
281
282 # check goal
283 if math.sqrt((x[0] - goal[0]) ** 2 + (x[1] - goal[1]) ** 2) <= config.robot_radius:
284 print("Goal!!")
285
286 break
287
288 print("Done")
289
290 draw_path(trajectory, goal, ob, x)
291
292
293 def draw_dynamic_search(best_trajectory, x, goal, ob):
294 """
295 画出动态搜索过程图
296 :return:
297 """
298 plt.cla() # 清除上次绘制图像
299 plt.plot(best_trajectory[:, 0], best_trajectory[:, 1], "-g")
300 plt.plot(x[0], x[1], "xr")
301 plt.plot(0, 0, "og")
302 plt.plot(goal[0], goal[1], "ro")
303 plt.plot(ob[:, 0], ob[:, 1], "bs")
304 plot_arrow(x[0], x[1], x[2])
305 plt.axis("equal")
306 plt.grid(True)
307 plt.pause(0.0001)
308
309
310 def draw_path(trajectory, goal, ob, x):
311 """
312 画图函数
313 :return:
314 """
315 plt.cla() # 清除上次绘制图像
316
317 plt.plot(x[0], x[1], "xr")
318 plt.plot(0, 0, "og")
319 plt.plot(goal[0], goal[1], "ro")
320 plt.plot(ob[:, 0], ob[:, 1], "bs")
321 plot_arrow(x[0], x[1], x[2])
322 plt.axis("equal")
323 plt.grid(True)
324 plt.plot(trajectory[:, 0], trajectory[:, 1], 'r')
325 plt.show()
326
327
328 if __name__ == '__main__':
329 main()参考文章：
DWA算法总结 - 知乎 (zhihu.com)
(82条消息) 机器人局部避障的动态窗口法(dynamic window approach)_dwa算法_白巧克力亦唯心的博客-CSDN博客
(82条消息) DWA算法分析_MARK&的博客-CSDN博客
