DWA（dynamic window approach）算法学习

系列文章目录

A*算法学习-CSDN博客

弗洛伊德算法（Floyd）和路径平滑弗洛伊德算法（Smooth Floyd）学习-CSDN博客

D*算法学习-CSDN博客

系列文章目录

前言

搜索空间 —减小速度搜索空间

优化过程 —最大化目标函数

算法实现

总结

前言

在机器人的路径规划中少不了DWA算法，学习！！！

DWA动态窗口法的原理及应用:The Dynamic Window Approach to Collision Avoidance - 知乎 (zhihu.com)

自动驾驶决策规划算法—DWA 动态窗口法 - 知乎 (zhihu.com)

动态窗口法（Dynamic Window Approach， DWA）是一种避障规划方法，DWA算法通过对速度空间施加约束以确保动力学模型和避障的要求，在速度空间中搜索机器人最优控制速度，最终实现快速安全地到达目的地。

DWA算法的实现主要由两部分组成： 1.减小速度空间2.定义目标函数，并最大化目标函数

搜索空间 —减小速度搜索空间

通过施加弧线轨迹、允许速度、滑动窗口约束以减小搜索空间。

弧线轨迹 Circular trajectories
在原论文中，作者对简化了机器人的运动学模型，并得出了机器人运动轨迹可由一系列弧线和直线组成。所以，作者将速度空间约束在由机器人的平移速度和旋转速度(v,w)组成的二维速度搜索空间。
允许速度 Admissible velocities
允许速度（Admissible velocities）确保机器人可以在障碍前停下，最大的允许速度取决于当前轨迹距最近障碍的距离dist(v,w)。允许速度集被定义为：

下图中展示了Vs,Va速度空间：
滑动窗口 Dynamic window
考虑到机器人存在加速度限制，搜索空间被限定动态窗口Vd中（在下一个规划间隔可达到的速度），具体如下：

其中，v˙,w˙表示机器人的加速度。
最终的搜索空间：Vr=Vs∩Va∩Vd,如下图所示：

优化过程 —最大化目标函数

目标函数考虑了方位角、安全距离和速度：

方位角 Target heading
方位角项heading(v,w)可确保机器人在运动过程中快速对准目标点。
安全距离 Clearance
dist(v,w)确保机器人不发生任何碰撞
速度 Velocity
速度项Velocity(v,w)可确保机器人尽快到达目标点

目标函数被定义为：

其中， $\alpha$ , $\beta$ , $\gamma$ 可以根据需求调整。这三个指标是目标函数的重要组成部分，缺一不可。仅使clearance和velocity最大化，机器人始终在无障碍空间运动，但不会有向目标位置移动的趋势。单独最大化heading，机器人很快就会被阻碍其前进的第一个障碍所阻挡，无法在其周围移动。通过组合三个指标，机器人在上述限制条件下能够快速地绕过碰撞，同时朝着目标方向运动。

算法实现

具体的DWA算法参考：https://github.com/AtsushiSakai/PythonRoboticsGifsplanning

函数讲解

动态窗口
创建动态窗口

def calc_dynamic_window(x, config):
  """
  calculation dynamic window based on current state x
  """

  # Dynamic window from robot specification
  Vs = [config.min_speed, config.max_speed,
        -config.max_yaw_rate, config.max_yaw_rate]

  # Dynamic window from motion model
  Vd = [x[3] - config.max_accel * config.dt,
        x[3] + config.max_accel * config.dt,
        x[4] - config.max_delta_yaw_rate * config.dt,
        x[4] + config.max_delta_yaw_rate * config.dt]

  #  [v_min, v_max, yaw_rate_min, yaw_rate_max]
  dw = [max(Vs[0], Vd[0]), min(Vs[1], Vd[1]),
        max(Vs[2], Vd[2]), min(Vs[3], Vd[3])]

  return dw

计算动态窗口内最优速度和最优轨迹

在动态窗口dw中采样并计算可能的轨迹，并求解各条轨迹的目标函数
选取令目标函数最小化（最小化指标，即对每个指标取倒数）的轨迹作为最优轨迹

def calc_control_and_trajectory(x, dw, config, goal, ob):
  """
  calculation final input with dynamic window
  """

  x_init = x[:]
  min_cost = float("inf")
  best_u = [0.0, 0.0]
  best_trajectory = np.array([x])

  # evaluate all trajectory with sampled input in dynamic window
  for v in np.arange(dw[0], dw[1], config.v_resolution):
      # v_resolution: speed interval
      for y in np.arange(dw[2], dw[3], config.yaw_rate_resolution):
          # yaw_rate_resolution: yaw_rate interval
          trajectory = predict_trajectory(x_init, v, y, config)
          # calc cost
          to_goal_cost = config.to_goal_cost_gain * calc_to_goal_cost(trajectory, goal)
          speed_cost = config.speed_cost_gain * (config.max_speed - trajectory[-1, 3])
          ob_cost = config.obstacle_cost_gain * calc_obstacle_cost(trajectory, ob, config)

          final_cost = to_goal_cost + speed_cost + ob_cost

          # search minimum trajectory
          if min_cost >= final_cost:
              min_cost = final_cost
              best_u = [v, y]
              best_trajectory = trajectory
              if abs(best_u[0]) < config.robot_stuck_flag_cons \
                      and abs(x[3]) < config.robot_stuck_flag_cons:
                  # to ensure the robot do not get stuck in
                  # best v=0 m/s (in front of an obstacle) and
                  # best omega=0 rad/s (heading to the goal with
                  # angle difference of 0)
                  best_u[1] = -config.max_delta_yaw_rate
  return best_u, best_trajectory

目标函数

方位角目标函数

def calc_to_goal_cost(trajectory, goal):
  """
      calc to goal cost with angle difference
  """

  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 = abs(math.atan2(math.sin(cost_angle), math.cos(cost_angle)))

  return cost

障碍目标函数
计算机器人与最近障碍的距离

def calc_obstacle_cost(trajectory, ob, config):
  """
  calc obstacle cost inf: collision
  """
  ox = ob[:, 0]
  oy = ob[:, 1]
  dx = trajectory[:, 0] - ox[:, None]
  dy = trajectory[:, 1] - oy[:, None]
  # r = sqrt(dx^2 + dy^2)
  r = np.hypot(dx, dy)

  if config.robot_type == RobotType.rectangle:
      yaw = trajectory[:, 2]
      rot = np.array([[np.cos(yaw), -np.sin(yaw)], [np.sin(yaw), np.cos(yaw)]])
      rot = np.transpose(rot, [2, 0, 1])
      local_ob = ob[:, None] - trajectory[:, 0:2]
      local_ob = local_ob.reshape(-1, local_ob.shape[-1])
      local_ob = np.array([local_ob @ x for x in rot])
      local_ob = local_ob.reshape(-1, local_ob.shape[-1])
      upper_check = local_ob[:, 0] <= config.robot_length / 2
      right_check = local_ob[:, 1] <= config.robot_width / 2
      bottom_check = local_ob[:, 0] >= -config.robot_length / 2
      left_check = local_ob[:, 1] >= -config.robot_width / 2
      if (np.logical_and(np.logical_and(upper_check, right_check),
                         np.logical_and(bottom_check, left_check))).any():
          return float("Inf")
  elif config.robot_type == RobotType.circle:
      if np.array(r <= config.robot_radius).any():
          return float("Inf")

  min_r = np.min(r)
  return 1.0 / min_r  # OK

最终机器人规划避障轨迹：