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

2023-12-20 11:04

本文主要是介绍动态窗口法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)

这篇关于动态窗口法Dynamic Window Approach在动态环境中避障的文章就介绍到这儿,希望我们推荐的文章对编程师们有所帮助!



http://www.chinasem.cn/article/515808

相关文章

Java汇编源码如何查看环境搭建

《Java汇编源码如何查看环境搭建》:本文主要介绍如何在IntelliJIDEA开发环境中搭建字节码和汇编环境,以便更好地进行代码调优和JVM学习,首先,介绍了如何配置IntelliJIDEA以方... 目录一、简介二、在IDEA开发环境中搭建汇编环境2.1 在IDEA中搭建字节码查看环境2.1.1 搭建步

在 VSCode 中配置 C++ 开发环境的详细教程

《在VSCode中配置C++开发环境的详细教程》本文详细介绍了如何在VisualStudioCode(VSCode)中配置C++开发环境,包括安装必要的工具、配置编译器、设置调试环境等步骤,通... 目录如何在 VSCode 中配置 C++ 开发环境:详细教程1. 什么是 VSCode?2. 安装 VSCo

如何用Python绘制简易动态圣诞树

《如何用Python绘制简易动态圣诞树》这篇文章主要给大家介绍了关于如何用Python绘制简易动态圣诞树,文中讲解了如何通过编写代码来实现特定的效果,包括代码的编写技巧和效果的展示,需要的朋友可以参考... 目录代码:效果:总结 代码:import randomimport timefrom math

bat脚本启动git bash窗口,并执行命令方式

《bat脚本启动gitbash窗口,并执行命令方式》本文介绍了如何在Windows服务器上使用cmd启动jar包时出现乱码的问题,并提供了解决方法——使用GitBash窗口启动并设置编码,通过编写s... 目录一、简介二、使用说明2.1 start.BAT脚本2.2 参数说明2.3 效果总结一、简介某些情

C# dynamic类型使用详解

《C#dynamic类型使用详解》C#中的dynamic类型允许在运行时确定对象的类型和成员,跳过编译时类型检查,适用于处理未知类型的对象或与动态语言互操作,dynamic支持动态成员解析、添加和删... 目录简介dynamic 的定义dynamic 的使用动态类型赋值访问成员动态方法调用dynamic 的

基于Redis有序集合实现滑动窗口限流的步骤

《基于Redis有序集合实现滑动窗口限流的步骤》滑动窗口算法是一种基于时间窗口的限流算法,通过动态地滑动窗口,可以动态调整限流的速率,Redis有序集合可以用来实现滑动窗口限流,本文介绍基于Redis... 滑动窗口算法是一种基于时间窗口的限流算法,它将时间划分为若干个固定大小的窗口,每个窗口内记录了该时间

Java中JSON字符串反序列化(动态泛型)

《Java中JSON字符串反序列化(动态泛型)》文章讨论了在定时任务中使用反射调用目标对象时处理动态参数的问题,通过将方法参数存储为JSON字符串并进行反序列化,可以实现动态调用,然而,这种方式容易导... 需求:定时任务扫描,反射调用目标对象,但是,方法的传参不是固定的。方案一:将方法参数存成jsON字

鸿蒙开发搭建flutter适配的开发环境

《鸿蒙开发搭建flutter适配的开发环境》文章详细介绍了在Windows系统上如何创建和运行鸿蒙Flutter项目,包括使用flutterdoctor检测环境、创建项目、编译HAP包以及在真机上运... 目录环境搭建创建运行项目打包项目总结环境搭建1.安装 DevEco Studio NEXT IDE

.NET利用C#字节流动态操作Excel文件

《.NET利用C#字节流动态操作Excel文件》在.NET开发中,通过字节流动态操作Excel文件提供了一种高效且灵活的方式处理数据,本文将演示如何在.NET平台使用C#通过字节流创建,读取,编辑及保... 目录用C#创建并保存Excel工作簿为字节流用C#通过字节流直接读取Excel文件数据用C#通过字节

第10章 中断和动态时钟显示

第10章 中断和动态时钟显示 从本章开始,按照书籍的划分,第10章开始就进入保护模式(Protected Mode)部分了,感觉从这里开始难度突然就增加了。 书中介绍了为什么有中断(Interrupt)的设计,中断的几种方式:外部硬件中断、内部中断和软中断。通过中断做了一个会走的时钟和屏幕上输入字符的程序。 我自己理解中断的一些作用: 为了更好的利用处理器的性能。协同快速和慢速设备一起工作