1 """ 2 version1.1 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
动态窗口法(DWA)python
最新推荐文章于 2024-07-05 18:50:31 发布
本文详细介绍了如何使用Python实现动态窗口法(DWA),一种用于路径规划的算法。通过阅读,读者可以理解DWA的基本原理,并学习到具体的Python代码实现。
摘要由CSDN通过智能技术生成