1 """
2 version1.3
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 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] * math.cos(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
53 return 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
79 return 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
94 while time <= config.predict_time:
95 x = motion(x, [v, w], config.dt)
96 trajectory = np.vstack((trajectory, x)) # 垂直堆叠，vertical
97 time += config.dt
98
99 # print(trajectory)
100 return 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 = config.to_goal_cost_gain * goal_dis
117
118 return 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
133 for ii in range(0, len(traj[:, 1])):
134 for 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)
141 if r <= config.robot_radius:
142 return float("Inf") # collision
143
144 if min_r >= r:
145 min_r = r
146
147 return 1.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 trajectory_space = np.array([x]) # 记录搜索所有采样的轨迹，用来画图
168
169 # evaluate all trajectory with sampled input in dynamic window
170 # v,生成一系列速度，w，生成一系列角速度
171 for v in np.arange(vr[0], vr[1], config.v_reso):
172 for w in np.arange(vr[2], vr[3], config.yawrate_reso):
173
174 trajectory = calc_trajectory(x_init, v, w, config)
175
176 trajectory_space = np.vstack((trajectory_space, trajectory))
177
178 # calc cost
179 to_goal_cost = calc_to_goal_cost(trajectory, goal, config)
180 speed_cost = config.speed_cost_gain * (config.max_speed - trajectory[-1, 3])
181 ob_cost = calc_obstacle_cost(trajectory, ob, config)
182 # print(ob_cost)
183
184 # 评价函数多种多样，看自己选择
185 # 本文构造的是越小越好
186 final_cost = to_goal_cost + speed_cost + ob_cost
187
188 # search minimum trajectory
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, trajectory_space
198
199
200 def dwa_control(x, u, config, goal, ob):
201 """
202 调用前面的几个函数，生成最合适的速度空间和轨迹搜索空间
203 :param x:
204 :param u:
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, trajectory_space = calc_final_input(x, u, vr, config, goal, ob)
215
216 return u, trajectory, trajectory_space
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 u = np.array([0.2, 0.0])
265 config = Config()
266 trajectory = np.array(x)
267
268 for i in range(1000):
269
270 u, best_trajectory, trajectory_space = dwa_control(x, u, config, goal, ob)
271
272 x = motion(x, u, config.dt)
273 # print(x)
274
275 trajectory = np.vstack((trajectory, x)) # store state history
276
277 if show_animation:
278 draw_dynamic_search(best_trajectory, x, goal, ob, trajectory_space)
279
280 # check goal
281 if math.sqrt((x[0] - goal[0]) ** 2 + (x[1] - goal[1]) ** 2) <= config.robot_radius:
282 print("Goal!!")
283
284 break
285
286 print("Done")
287
288 # draw_path(trajectory, goal, ob, x)
289
290
291 def draw_dynamic_search(best_trajectory, x, goal, ob, trajectory_space):
292 """
293 画出动态搜索过程图
294 :return:
295 """
296 plt.cla() # 清除上次绘制图像
297 plt.plot(best_trajectory[:, 0], best_trajectory[:, 1], "-g")
298
299 plt.plot(trajectory_space[:, 0], trajectory_space[:, 1], '-g')
300
301 plt.plot(x[0], x[1], "xr")
302 plt.plot(0, 0, "og")
303 plt.plot(goal[0], goal[1], "ro")
304 plt.plot(ob[:, 0], ob[:, 1], "bs")
305 plot_arrow(x[0], x[1], x[2])
306 plt.axis("equal")
307 plt.grid(True)
308 plt.pause(0.0001)
309
310
311 def draw_path(trajectory, goal, ob, x):
312 """
313 画图函数
314 :return:
315 """
316 plt.cla() # 清除上次绘制图像
317
318 plt.plot(x[0], x[1], "xr")
319 plt.plot(0, 0, "og")
320 plt.plot(goal[0], goal[1], "ro")
321 plt.plot(ob[:, 0], ob[:, 1], "bs")
322 plot_arrow(x[0], x[1], x[2])
323 plt.axis("equal")
324 plt.grid(True)
325 plt.plot(trajectory[:, 0], trajectory[:, 1], 'r')
326 plt.show()
327
328
329 if __name__ == '__main__':
330 main()
