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速度空间:

    DWA(dynamic window approach)算法学习_第1张图片

  • 滑动窗口 Dynamic window
    考虑到机器人存在加速度限制,搜索空间被限定动态窗口Vd中(在下一个规划间隔可达到的速度),具体如下:

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

    DWA(dynamic window approach)算法学习_第2张图片

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

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

  • 方位角 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
    

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

    DWA(dynamic window approach)算法学习_第3张图片


总结

机器人轨迹规划的基本算法,之后做实验跑一跑!!!之后还要研究下欧几里得距离转换算法(Euclidean Distance Transform, EDT),建立EDT梯度图衡量障碍物代价以优化障碍物判断优化。

你可能感兴趣的:(算法与数据结构,机器人导航,算法,学习,python)