动态窗⼝法（DWA）python 1"""
2 version1.1
3 Mobile robot motion planning sample with Dynamic Window Approach
4结合blog.csdn/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
15class Config(object):
16"""
17⽤来仿真的参数，
18"""
19
20 def __init__(lf):
21 # robot parameter
22 lf.max_speed = 1.4 # [m/s] # 最⼤速度
bb霜和隔离霜的区别
23 # lf.min_speed = -0.5 # [m/s] # 最⼩速度，设置为可以倒车
24 lf.min_speed = 0 # [m/s] # 最⼩速度，设置为不倒车
25 lf.max_yawrate = 40.0 * math.pi / 180.0 # [rad/s] # 最⼤⾓速度
26 lf.max_accel = 0.2 # [m/ss] # 最⼤加速度
27 lf.max_dyawrate = 40.0 * math.pi / 180.0 # [rad/ss] # 最⼤⾓加速度
28 lf.v_reso = 0.01 # [m/s]，速度分辨率
29 lf.yawrate_reso = 0.1 * math.pi / 180.0 # [rad/s]，⾓速度分辨率
30 lf.dt = 0.1 # [s] # 采样周期
31 lf.predict_time = 3.0 # [s] # 向前预估三秒
32 lf.to_goal_cost_gain = 1.0 # ⽬标代价增益
33 lf.speed_cost_gain = 1.0 # 速度代价增益
韩版风衣
34 lf.robot_radius = 1.0 # [m] # 机器⼈半径
35
36
37 def motion(x, u, dt):
38"""
39 :param x: 位置参数，在此叫做位置空间
40 :param u: 速度和加速度，在此叫做速度空间
41 :param dt: 采样时间
42 :return:
43"""
44 # 速度更新公式⽐较简单，在极短时间内，车辆位移也变化较⼤
45 # 采⽤圆弧求解如何？
46 x[0] += u[0] * s(x[2]) * dt # x⽅向位移
47 x[1] += u[0] * math.sin(x[2]) * dt # y
48 x[2] += u[1] * dt # 航向⾓
49 x[3] = u[0] # 速度v
50 x[4] = u[1] # ⾓速度w
51 # print(x)
52
53return x
54
55
56 def calc_dynamic_window(x, config):
57"""
58位置空间集合
59 :param x：当前位置空间，符号参考硕⼠论⽂
60 :param config:
61 :return:⽬前是两个速度的交集，还差⼀个
62"""
63
64 # 车辆能够达到的最⼤最⼩速度
65 vs = [config.min_speed, config.max_speed,
66 -config.max_yawrate, config.max_yawrate]
67
68 # ⼀个采样周期能够变化的最⼤最⼩速度
69 vd = [x[3] - config.max_accel * config.dt,
70 x[3] + config.max_accel * config.dt,
71 x[4] - config.max_dyawrate * config.dt,
72 x[4] + config.max_dyawrate * config.dt]
73 # print(Vs, Vd)
美味妹妹74
75 # 求出两个速度集合的交集
76 vr = [max(vs[0], vd[0]), min(vs[1], vd[1]),
77 max(vs[2], vd[2]), min(vs[3], vd[3])]
78
79return vr
80
81
82 def calc_trajectory(x_init, v, w, config):
83"""
84预测3秒内的轨迹
85 :param x_init：位置空间
86 :param v：速度
87 :param w:⾓速度
88 :param config:
89 :return: 每⼀次采样更新的轨迹，位置空间垂直堆叠
90"""
91 x = np.array(x_init)
92 trajectory = np.array(x)
93 time = 0
94while time <= config.predict_time:
95 x = motion(x, [v, w], config.dt)
96 trajectory = np.vstack((trajectory, x)) # 垂直堆叠，vertical
97 time += config.dt
98
春节作文100
99 # print(trajectory)
100return trajectory
101
102
103 def calc_to_goal_cost(trajectory, goal, config):
104"""
105计算轨迹到⽬标点的代价
106 :param trajectory：轨迹搜索空间
107 :param goal:
108 :param config:
109 :return: 轨迹到⽬标点欧式距离
110"""
111 # calc to goal cost. It is 2D norm.
112
113 dx = goal[0] - trajectory[-1, 0]
114 dy = goal[1] - trajectory[-1, 1]
115 goal_dis = math.sqrt(dx ** 2 + dy ** 2)
116 cost = _goal_cost_gain * goal_dis
117
118return cost
119
120
121 def calc_obstacle_cost(traj, ob, config):
122"""
123计算预测轨迹和障碍物的最⼩距离，dist(v,w)
124 :param traj:
125 :param ob:
126 :param config:
127 :return:
128"""
129 # calc obstacle cost inf: collision, 0:free
130
131 min_r = float("inf") # 距离初始化为⽆穷⼤
132
133for ii in range(0, len(traj[:, 1])):
134for i in range(len(ob[:, 0])):
135 ox = ob[i, 0]
136 oy = ob[i, 1]
137 dx = traj[ii, 0] - ox
138 dy = traj[ii, 1] - oy
139
140 r = math.sqrt(dx ** 2 + dy ** 2)
141if r <= bot_radius:
142return float("Inf") # collision
143
144if min_r >= r:
145 min_r = r
146
147return1.0 / min_r # 越⼩越好
148
149
150 def calc_final_input(x, u, vr, config, goal, ob):
151"""
152计算采样空间的评价函数，选择最合适的那⼀个作为最终输⼊153 :param x：位置空间
154 :param u：速度空间
155 :param vr：速度空间交集
156 :param config:
157 :param goal:⽬标位置
158 :param ob：障碍物
159 :return:
160"""
161 x_init = x[:]
162 min_cost = 10000.0
163 min_u = u
164
165 best_trajectory = np.array([x])
166
167 # evaluate all trajectory with sampled input in dynamic window
168 # v,⽣成⼀系列速度，w，⽣成⼀系列⾓速度
169for v in np.arange(vr[0], vr[1], config.v_reso):
170for w in np.arange(vr[2], vr[3], config.yawrate_reso):
171
172 trajectory = calc_trajectory(x_init, v, w, config)
173
174 # calc cost
175 to_goal_cost = calc_to_goal_cost(trajectory, goal, config)
176 speed_cost = config.speed_cost_gain * (config.max_speed - trajectory[-1, 3])
177 ob_cost = calc_obstacle_cost(trajectory, ob, config)
178 # print(ob_cost)
179
180 # 评价函数多种多样，看⾃⼰选择
181 # 本⽂构造的是越⼩越好
182 final_cost = to_goal_cost + speed_cost + ob_cost
183
184 # arch minimum trajectory
185if min_cost >= final_cost:
186 min_cost = final_cost
187 min_u = [v, w]
188 best_trajectory = trajectory
189
190 # print(min_u)
191 # input()
192
193return min_u, best_trajectory
194
195
196 def dwa_control(x, u, config, goal, ob):
避孕套怎么使用197"""
塔布羊198调⽤前⾯的⼏个函数，⽣成最合适的速度空间和轨迹搜索空间
199 :param x:
200 :param u:
201 :param config:
202 :param goal:
203 :param ob:
204 :return:
205"""
206 # Dynamic Window control
207
208 vr = calc_dynamic_window(x, config)
209
210 u, trajectory = calc_final_input(x, u, vr, config, goal, ob)
211
212return u, trajectory
213
214
215 def plot_arrow(x, y, yaw, length=0.5, width=0.1):
216"""
217 arrow函数绘制箭头
218 :param x:
219 :param y:
220 :param yaw：航向⾓
221 :param length:
222 :param width：参数值为浮点数，代表箭头尾部的宽度，默认值为0.001
223 :return:
224 length_includes_head：代表箭头整体长度是否包含箭头头部的长度，默认值为Fal
225 head_width：代表箭头头部的宽度，默认值为3*width，即尾部宽度的3倍
错误769226 head_length：代表箭头头部的长度度，默认值为1.5*head_width，即头部宽度的1.5倍
227 shape：参数值为'full'、'left'、'right'，表⽰箭头的形状，默认值为'full'
228 overhang：代表箭头头部三⾓形底边与箭头尾部直接的夹⾓关系，通过该参数可改变箭头的形状。229默认值为0，即头部为三⾓形，当该值⼩于0时，头部为菱形，当值⼤于0时，头部为鱼尾状230"""
231 plt.arrow(x, y, length * s(yaw), length * math.sin(yaw),
232 head_length=1.5 * width, head_width=width)
233 plt.plot(x, y)
234
235
236 def main():
237"""
238主函数
239 :return:
240"""
241 # print(__file__ + " start!!")
242 # 初始化位置空间
243 x = np.array([0.0, 0.0, math.pi / 2.0, 0.2, 0.0])
244
245 goal = np.array([10, 10])
246
247 # matrix⼆维矩阵
248 # ob = np.matrix([[-1, -1],
249 # [0, 2],
250 # [4.0, 2.0],
251 # [5.0, 4.0],
252 # [5.0, 5.0],
253 # [5.0, 6.0],
254 # [5.0, 9.0],
255 # [8.0, 9.0],
256 # [7.0, 9.0],
257 # [12.0, 12.0]
258 # ])
259 ob = np.matrix([[0, 2]])
260 u = np.array([0.2, 0.0])
261 config = Config()
262 trajectory = np.array(x)
263硬盘的作用
264for i in range(1000):
265
266 u, best_trajectory = dwa_control(x, u, config, goal, ob)
267
268 x = motion(x, u, config.dt)
269 print(x)
270
271 trajectory = np.vstack((trajectory, x)) # store state history
272
273if show_animation:
274 draw_dynamic_arch(best_trajectory, x, goal, ob)
275
276 # check goal
277if math.sqrt((x[0] - goal[0]) ** 2 + (x[1] - goal[1]) ** 2) <= bot_radius: 278 print("Goal!!")
279
280break
281
282 print("Done")
283
284 draw_path(trajectory, goal, ob, x)
285
286
287 def draw_dynamic_arch(best_trajectory, x, goal, ob):
288"""
289画出动态搜索过程图
290 :return:
291"""
292 plt.cla() # 清除上次绘制图像
293 plt.plot(best_trajectory[:, 0], best_trajectory[:, 1], "-g")
294 plt.plot(x[0], x[1], "xr")
295 plt.plot(0, 0, "og")
296 plt.plot(goal[0], goal[1], "ro")
297 plt.plot(ob[:, 0], ob[:, 1], "bs")
298 plot_arrow(x[0], x[1], x[2])
299 plt.axis("equal")
300 id(True)
301 plt.pau(0.0001)
302
303
304 def draw_path(trajectory, goal, ob, x):
305"""
306画图函数
307 :return:
308"""
309 plt.cla() # 清除上次绘制图像
310
311 plt.plot(x[0], x[1], "xr")
312 plt.plot(0, 0, "og")
313 plt.plot(goal[0], goal[1], "ro")
314 plt.plot(ob[:, 0], ob[:, 1], "bs")
315 plot_arrow(x[0], x[1], x[2])
316 plt.axis("equal")
317 id(True)
318 plt.plot(trajectory[:, 0], trajectory[:, 1], 'r')
319 plt.show()
320
321
322if __name__ == '__main__':
323 main()
