动态窗口法（DWA）python

  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