php p2p网站建设,西部建设网站,知乎营销软件,打开网站乱码怎么做以这个博主的代码为基础#xff0c;加了一个碰撞检测#xff0c;但是这个碰撞检测目前还不完善#xff0c;思路应该是这个思路#xff0c;以后有时间再完善吧。
动态窗口法#xff1a;【路径规划】局部路径规划算法——DWA算法#xff08;动态窗口法#xff09;|#…以这个博主的代码为基础加了一个碰撞检测但是这个碰撞检测目前还不完善思路应该是这个思路以后有时间再完善吧。
动态窗口法【路径规划】局部路径规划算法——DWA算法动态窗口法|含python实现 | c实现-CSDN博客
DWA的大致思路就是在一个线速度和角度的可行二维空间中进行采样计算每个可行速度在未来一定时间内的轨迹假定匀速运动 对这些轨迹进行评价取最优轨迹以最有轨迹对应的当前下一时刻的线速度和角速度最为速度控制器的v_target。缺点是容易陷入局部最优。
import randomimport numpy as np
import matplotlib.pyplot as plt
import math
import timeclass Config:simulation parameter classdef __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_radius0.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_: 速度边界限制后的速度空间Vmreturn [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_:考虑加速度时的速度空间Vdv_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, axis0)distances np.linalg.norm(trajectory - traj_dyob, axis2)min_values np.min(distances, axis1)min_value np.min(min_values, axis0)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, length0.5, width0.1): # pragma: no coverplt.arrow(x, y, length * math.cos(yaw), length * math.sin(yaw),head_lengthwidth, head_widthwidth)plt.plot(x, y)def plot_robot(x, y, yaw, config): # pragma: no covercircle plt.Circle((x, y), config.robot_radius, colorb)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_x0.0, current_y0.0, goal_x10.0, goal_y10.0, obnp.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)# figplt.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, obob)plt.scatter(ob[:, 0], ob[:, 1], ck)plt.plot(trajectory[:, 0], trajectory[:, 1], -r)plt.show()print(trajectory)
