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 参数值为浮点数，代表箭头尾部的宽度，默认值为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()