DWA算法学习

一、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博客