动态窗口法Dynamic Window Approach在动态环境中避障

以这个博主的代码为基础，加了一个碰撞检测，但是这个碰撞检测目前还不完善，思路应该是这个思路，以后有时间再完善吧。

动态窗口法：【路径规划】局部路径规划算法——DWA算法（动态窗口法）|（含python实现 | c++实现）-CSDN博客

DWA的大致思路就是，在一个线速度和角度的可行二维空间中，进行采样，计算每个可行速度在未来一定时间内的轨迹（假定匀速运动）， 对这些轨迹进行评价，取最优轨迹，以最有轨迹对应的当前下一时刻的线速度和角速度，最为速度控制器的v_target。缺点是容易陷入局部最优。

import randomimport numpy as np
import matplotlib.pyplot as plt
import math
import timeclass Config:"""simulation parameter class"""def __init__(self):# robot parameter# 线速度边界self.v_max = 1.0  # [m/s]self.v_min = -0.5  # [m/s]# 角速度边界self.w_max = 40.0 * math.pi / 180.0  # [rad/s]self.w_min = -40.0 * math.pi / 180.0  # [rad/s]# 线加速度和角加速度最大值self.a_vmax = 0.2  # [m/ss]self.a_wmax = 40.0 * math.pi / 180.0  # [rad/ss]# 采样分辨率self.v_sample = 0.01  # [m/s]self.w_sample = 0.1 * math.pi / 180.0  # [rad/s]# 离散时间self.dt = 0.1  # [s] Time tick for motion prediction# 轨迹推算时间长度self.predict_time = 3.0  # [s]# 轨迹评价函数系数self.alpha = 0.15self.beta = 1.0self.gamma = 1.0# Also used to check if goal is reached in both typesself.robot_radius = 1.0  # [m] for collision checkself.judge_distance = 10  # 若与障碍物的最小距离大于阈值（例如这里设置的阈值为robot_radius+0.2）,则设为一个较大的常值class DWA:def __init__(self, config) -> None:"""初始化Args:config (_type_): 参数类"""self.dt = config.dtself.v_min = config.v_minself.w_min = config.w_minself.v_max = config.v_maxself.w_max = config.w_maxself.predict_time = config.predict_timeself.a_vmax = config.a_vmaxself.a_wmax = config.a_wmaxself.v_sample = config.v_sample  # 线速度采样分辨率self.w_sample = config.w_sample  # 角速度采样分辨率self.alpha = config.alphaself.beta = config.betaself.gamma = config.gammaself.radius = config.robot_radiusself.judge_distance = config.judge_distancedef dwa_control(self, state, goal, obstacle, ob_dyna, dyna_ob_v):"""滚动窗口算法入口Args:state (_type_): 机器人当前状态--[x,y,yaw,v,w]goal (_type_): 目标点位置，[x,y]obstacle (_type_): 障碍物位置，dim:[num_ob,2]Returns:_type_: 控制量、轨迹（便于绘画）"""control, trajectory = self.trajectory_evaluation(state, goal, obstacle, ob_dyna, dyna_ob_v)return control, trajectorydef cal_dynamic_window_vel(self, v, w, state, obstacle):"""速度采样,得到速度空间窗口Args:v (_type_): 当前时刻线速度w (_type_): 当前时刻角速度state (_type_): 当前机器人状态obstacle (_type_): 障碍物位置Returns:[v_low,v_high,w_low,w_high]: 最终采样后的速度空间"""Vm = self.__cal_vel_limit()Vd = self.__cal_accel_limit(v, w)Va = self.__cal_obstacle_limit(state, obstacle)a = max([Vm[0], Vd[0], Va[0]])b = min([Vm[1], Vd[1], Va[1]])c = max([Vm[2], Vd[2], Va[2]])d = min([Vm[3], Vd[3], Va[3]])return [a, b, c, d]def __cal_vel_limit(self):"""计算速度边界限制VmReturns:_type_: 速度边界限制后的速度空间Vm"""return [self.v_min, self.v_max, self.w_min, self.w_max]def __cal_accel_limit(self, v, w):"""计算加速度限制VdArgs:v (_type_): 当前时刻线速度w (_type_): 当前时刻角速度Returns:_type_:考虑加速度时的速度空间Vd"""v_low = v - self.a_vmax * self.dtv_high = v + self.a_vmax * self.dtw_low = w - self.a_wmax * self.dtw_high = w + self.a_wmax * self.dtreturn [v_low, v_high, w_low, w_high]def __cal_obstacle_limit(self, state, obstacle):"""环境障碍物限制VaArgs:state (_type_): 当前机器人状态obstacle (_type_): 障碍物位置Returns:_type_: 某一时刻移动机器人不与周围障碍物发生碰撞的速度空间Va"""# 不与静态障碍物碰撞v_low = self.v_minv_high = np.sqrt(2 * self._dist(state, obstacle[:, :]) * self.a_vmax)w_low = self.w_minw_high = np.sqrt(2 * self._dist(state, obstacle[:, :]) * self.a_wmax)return [v_low, v_high, w_low, w_high]def trajectory_predict(self, state_init, v, w):"""轨迹推算Args:state_init (_type_): 当前状态---x,y,yaw,v,wv (_type_): 当前时刻线速度w (_type_): 当前时刻线速度Returns:_type_: _description_"""state = np.array(state_init)trajectory = state_time = 0# 在预测时间段内while _time <= self.predict_time:x = KinematicModel(state, [v, w], self.dt)  # 运动学模型trajectory = np.vstack((trajectory, x))_time += self.dtreturn trajectorydef predict_dyna_ob_traj(self, ob_dyna, dyna_ob_v):trajectory = np.zeros((ob_dyna.shape[0], int(self.predict_time / self.dt) + 1, 2))trajectory[0, 0, 0] = ob_dyna[0, 0]trajectory[0, 0, 1] = ob_dyna[0, 1]trajectory[1, 0, 0] = ob_dyna[1, 0]trajectory[1, 0, 1] = ob_dyna[1, 1]time = 0idx = 1while time <= self.predict_time:# 维度：个体数 时刻 坐标trajectory[0, idx, 0] = trajectory[0, idx - 1, 0] + dyna_ob_v[0, 0] * self.dttrajectory[0, idx, 1] = trajectory[0, idx - 1, 1] + dyna_ob_v[0, 1] * self.dttrajectory[1, idx, 0] = trajectory[1, idx - 1, 0] + dyna_ob_v[1, 0] * self.dttrajectory[1, idx, 1] = trajectory[1, idx - 1, 1] + dyna_ob_v[1, 1] * self.dttime += self.dtidx += 1return trajectorydef collision_detection(self, trajectory, traj_dyob):trajectory = np.repeat(trajectory[np.newaxis, :, :], 2, axis=0)distances = np.linalg.norm(trajectory - traj_dyob, axis=2)min_values = np.min(distances, axis=1)min_value = np.min(min_values, axis=0)if min_value < 0.3:return Trueelse:return Falsedef trajectory_evaluation(self, state, goal, obstacle, ob_dyna, ob_dyna_v):"""轨迹评价函数,评价越高，轨迹越优Args:state (_type_): 当前状态---x,y,yaw,v,wdynamic_window_vel (_type_): 采样的速度空间窗口---[v_low,v_high,w_low,w_high]goal (_type_): 目标点位置，[x,y]obstacle (_type_): 障碍物位置，dim:[num_ob,2]Returns:_type_: 最优控制量、最优轨迹"""G_max = -float('inf')  # 最优评价trajectory_opt = state  # 最优轨迹control_opt = [0., 0.]  # 最优控制dynamic_window_vel = self.cal_dynamic_window_vel(state[3], state[4], state, obstacle)  # 第1步--计算速度空间# 在速度空间中按照预先设定的分辨率采样sum_heading, sum_dist, sum_vel = 1, 1, 1  # 不进行归一化for v in np.arange(dynamic_window_vel[0], dynamic_window_vel[1], self.v_sample):for w in np.arange(dynamic_window_vel[2], dynamic_window_vel[3], self.w_sample):trajectory = self.trajectory_predict(state, v, w)  # 第2步--轨迹推算heading_eval = self.alpha * self.__heading(trajectory, goal) / sum_headingdist_eval = self.beta * self.__dist(trajectory, obstacle) / sum_distvel_eval = self.gamma * self.__velocity(trajectory) / sum_velG = heading_eval + dist_eval + vel_eval  # 第3步--轨迹评价if G_max <= G:G_max = Gtrajectory_opt = trajectorycontrol_opt = [v, w]traj_dyob = self.predict_dyna_ob_traj(ob_dyna, ob_dyna_v)collision = self.collision_detection(trajectory_opt[:, :2], traj_dyob)if collision:control_opt = [0, 0]return control_opt, trajectory_optdef _dist(self, state, obstacle):"""计算当前移动机器人距离障碍物最近的几何距离Args:state (_type_): 当前机器人状态obstacle (_type_): 障碍物位置Returns:_type_: 移动机器人距离障碍物最近的几何距离"""ox = obstacle[:, 0]oy = obstacle[:, 1]dx = state[0, None] - ox[:, None]dy = state[1, None] - oy[:, None]r = np.hypot(dx, dy)return np.min(r)def __dist(self, trajectory, obstacle):"""距离评价函数表示当前速度下对应模拟轨迹与障碍物之间的最近距离；如果没有障碍物或者最近距离大于设定的阈值，那么就将其值设为一个较大的常数值。Args:trajectory (_type_): 轨迹，dim:[n,5]obstacle (_type_): 障碍物位置，dim:[num_ob,2]Returns:_type_: _description_"""ox = obstacle[:, 0]oy = obstacle[:, 1]dx = trajectory[:, 0] - ox[:, None]dy = trajectory[:, 1] - oy[:, None]r = np.hypot(dx, dy)return np.min(r) if np.array(r < self.radius + 0.2).any() else self.judge_distancedef __heading(self, trajectory, goal):"""方位角评价函数评估在当前采样速度下产生的轨迹终点位置方向与目标点连线的夹角的误差Args:trajectory (_type_): 轨迹，dim:[n,5]goal (_type_): 目标点位置[x,y]Returns:_type_: 方位角评价数值"""dx = goal[0] - trajectory[-1, 0]dy = goal[1] - trajectory[-1, 1]error_angle = math.atan2(dy, dx)cost_angle = error_angle - trajectory[-1, 2]cost = math.pi - abs(cost_angle)return costdef __velocity(self, trajectory):"""速度评价函数， 表示当前的速度大小，可以用模拟轨迹末端位置的线速度的大小来表示Args:trajectory (_type_): 轨迹，dim:[n,5]Returns:_type_: 速度评价"""return trajectory[-1, 3]def KinematicModel(state, control, dt):"""机器人运动学模型Args:state (_type_): 状态量---x,y,yaw,v,wcontrol (_type_): 控制量---v,w,线速度和角速度dt (_type_): 离散时间Returns:_type_: 下一步的状态"""state[0] += control[0] * math.cos(state[2]) * dtstate[1] += control[0] * math.sin(state[2]) * dtstate[2] += control[1] * dtstate[3] = control[0]state[4] = control[1]return statedef plot_arrow(x, y, yaw, length=0.5, width=0.1):  # pragma: no coverplt.arrow(x, y, length * math.cos(yaw), length * math.sin(yaw),head_length=width, head_width=width)plt.plot(x, y)def plot_robot(x, y, yaw, config):  # pragma: no covercircle = plt.Circle((x, y), config.robot_radius, color="b")plt.gcf().gca().add_artist(circle)out_x, out_y = (np.array([x, y]) +np.array([np.cos(yaw), np.sin(yaw)]) * config.robot_radius)plt.plot([x, out_x], [y, out_y], "-k")def dynamic_ob(t, dyna_ob_v):y1 = (dyna_ob_v[0, 1] * t) % 14x2 = (dyna_ob_v[1, 0] * t) % 9return np.array([[9, y1],[x2, 9]])def do_dwa(current_x=0.0, current_y=0.0, goal_x=10.0, goal_y=10.0, ob=np.array([[-1, -1]])):# initial state [x(m), y(m), yaw(rad), v(m/s), omega(rad/s)]x = np.array([current_x, current_y, math.pi / 8.0, 0.0, 0.0])# goal position [x(m), y(m)]goal = np.array([goal_x, goal_y])config = Config()trajectory = np.array(x)dwa = DWA(config)# fig=plt.figure(1)st_i = 0while True:ob_speed = 0.2dyna_ob_v = np.array([[0, ob_speed],[ob_speed, 0]])ob_dyna = dynamic_ob(st_i, dyna_ob_v)u, predicted_trajectory = dwa.dwa_control(x, goal, ob, ob_dyna, dyna_ob_v)x = KinematicModel(x, u, config.dt)  # simulate robottrajectory = np.vstack((trajectory, x))  # store state historyplt.cla()# for stopping simulation with the esc key.plt.gcf().canvas.mpl_connect('key_release_event',lambda event: [exit(0) if event.key == 'escape' else None])try:plt.plot(predicted_trajectory[:, 0], predicted_trajectory[:, 1], "-g")except:passplt.plot(x[0], x[1], "xr")plt.plot(goal[0], goal[1], "xb")plt.plot(ob[:, 0], ob[:, 1], "ok")plt.plot(ob_dyna[:, 0], ob_dyna[:, 1], "ok")plot_robot(x[0], x[1], x[2], config)plot_arrow(x[0], x[1], x[2])plt.axis("equal")plt.grid(True)plt.pause(0.001)st_i += 1# check reaching goaldist_to_goal = math.hypot(x[0] - goal[0], x[1] - goal[1])if dist_to_goal <= config.robot_radius:print("Goal!!")breakprint("Done")# plt.plot(trajectory[:, 0], trajectory[:, 1], "-r")# plt.pause(0.001)# plt.show()return trajectoryif __name__ == "__main__":num_points = 10points = [(random.uniform(0, 16), random.uniform(0, 16)) for _ in range(num_points)]ob = np.array(points)current_x = 0.0current_y = 0.0goal_x = 10.0goal_y = 10.0current_x = random.randint(-5, 2)current_y = random.randint(2, 7)trajectory = do_dwa(current_x, current_y, goal_x, goal_y, ob=ob)plt.scatter(ob[:, 0], ob[:, 1], c="k")plt.plot(trajectory[:, 0], trajectory[:, 1], "-r")plt.show()print(trajectory)