ROS局部规划器中的轨迹模拟策略-DWA使用与否的差别

#本文记录学习过程中的个人理解。欢迎指正，共同进步。

---

Navigation堆栈中的TrajectoryPlanner（在BaseLocalPlanner里）和DWAPlanner都可以作为局部规划器，前者为默认设置。二者在生成轨迹时都会判断是否开启dwa标志位，进而执行不同的轨迹模拟策略。

这里通过以下三个方面比较轨迹模拟过程中开启dwa与否的差异：

• 速度采样范围
• 轨迹生成方式
• 控制命令

速度包括线速度和角速度。

---

【TrajectoryPlanner】

1. 速度采样范围

使用dwa时，计算的是sim_period_时间段内机器人由当前速度能够达到的速度；

不使用dwa时，计算的是sim_time_时间段内机器人由当前速度能够达到的速度。

sim_time_是仿真时间，它表示模拟出的该段轨迹对应的运动时间，默认1秒。所以它生成的速度范围也是指接下来一段时间内机器人可达的运动速度。

sim_period_是仿真周期，它指算法的实际控制周期，默认为0.1秒。故它生成的速度范围是指 “下一步” 机器人可达的速度。

故开启dwa后，模拟的是“瞬时”能达到的速度，而不开启则模拟一段时间内能达到的速度。

    if (dwa_) {
      max_vel_x = max(min(max_vel_x, vx + acc_x * sim_period_), min_vel_x_);
      min_vel_x = max(min_vel_x_, vx - acc_x * sim_period_);

      max_vel_theta = min(max_vel_th_, vtheta + acc_theta * sim_period_);
      min_vel_theta = max(min_vel_th_, vtheta - acc_theta * sim_period_);
    } else {
      max_vel_x = max(min(max_vel_x, vx + acc_x * sim_time_), min_vel_x_);
      min_vel_x = max(min_vel_x_, vx - acc_x * sim_time_);

      max_vel_theta = min(max_vel_th_, vtheta + acc_theta * sim_time_);
      min_vel_theta = max(min_vel_th_, vtheta - acc_theta * sim_time_);
    }

2. 轨迹生成方式与控制命令

这里不论使用dwa与否，都把采样速度作为控制命令。

    traj.xv_ = vx_samp;
    traj.yv_ = vy_samp;
    traj.thetav_ = vtheta_samp;
    traj.cost_ = -1.0;

在生成轨迹时，都让现在的速度以一定加速度趋于并稳定于采样速度，以此得到的路径点作为轨迹。

      traj.addPoint(x_i, y_i, theta_i);
      vx_i = computeNewVelocity(vx_samp, vx_i, acc_x, dt);
      vy_i = computeNewVelocity(vy_samp, vy_i, acc_y, dt);
      vtheta_i = computeNewVelocity(vtheta_samp, vtheta_i, acc_theta, dt);

      //通过计算出的速度计算下一个位置、姿态
      x_i = computeNewXPosition(x_i, vx_i, vy_i, theta_i, dt);
      y_i = computeNewYPosition(y_i, vx_i, vy_i, theta_i, dt);
      theta_i = computeNewThetaPosition(theta_i, vtheta_i, dt);

SimpleTrajectoryGenerator类

1. 速度采样范围

和TrajectoryPlanner一样，dwa开启后使用sim_period_，否则使用sim_time_

    if ( ! use_dwa_) {
      double dist = hypot(goal[0] - pos[0], goal[1] - pos[1]);
      max_vel_x = std::max(std::min(max_vel_x, dist / sim_time_), min_vel_x);
      max_vel_y = std::max(std::min(max_vel_y, dist / sim_time_), min_vel_y);

      max_vel[0] = std::min(max_vel_x, vel[0] + acc_lim[0] * sim_time_);
      max_vel[1] = std::min(max_vel_y, vel[1] + acc_lim[1] * sim_time_);
      max_vel[2] = std::min(max_vel_th, vel[2] + acc_lim[2] * sim_time_);

      min_vel[0] = std::max(min_vel_x, vel[0] - acc_lim[0] * sim_time_);
      min_vel[1] = std::max(min_vel_y, vel[1] - acc_lim[1] * sim_time_);
      min_vel[2] = std::max(min_vel_th, vel[2] - acc_lim[2] * sim_time_);
    } else {
      max_vel[0] = std::min(max_vel_x, vel[0] + acc_lim[0] * sim_period_);
      max_vel[1] = std::min(max_vel_y, vel[1] + acc_lim[1] * sim_period_);
      max_vel[2] = std::min(max_vel_th, vel[2] + acc_lim[2] * sim_period_);

      min_vel[0] = std::max(min_vel_x, vel[0] - acc_lim[0] * sim_period_);
      min_vel[1] = std::max(min_vel_y, vel[1] - acc_lim[1] * sim_period_);
      min_vel[2] = std::max(min_vel_th, vel[2] - acc_lim[2] * sim_period_);
    }

2. 轨迹生成方式与控制命令

continued_acceleration_值为!use_dwa_，开启dwa_后continued_acceleration_被关闭。

故使用dwa后，速度控制命令为采样速度；

不使用dwa时，速度控制命令为通过一次加/减速度后得到的速度。

  if (continued_acceleration_) {
    loop_vel = computeNewVelocities(sample_target_vel, vel, limits_->getAccLimits(), dt);
    traj.xv_     = loop_vel[0];
    traj.yv_     = loop_vel[1];
    traj.thetav_ = loop_vel[2];
  } else {//dwa
    loop_vel = sample_target_vel;
    traj.xv_     = sample_target_vel[0];
    traj.yv_     = sample_target_vel[1];
    traj.thetav_ = sample_target_vel[2];
  }

使用dwa后，用恒定采样速度生成轨迹；

不使用dwa时，产生一系列趋于并稳定于采样速度的速度，并以此产生路径点作为轨迹。

  for (int i = 0; i < num_steps; ++i) {
    traj.addPoint(pos[0], pos[1], pos[2]);

    if (continued_acceleration_) {
      //calculate velocities
      loop_vel = computeNewVelocities(sample_target_vel, loop_vel, limits_->getAccLimits(), dt);
      //ROS_WARN_NAMED("Generator", "Flag: %d, Loop_Vel %f, %f, %f", continued_acceleration_, loop_vel[0], loop_vel[1], loop_vel[2]);
    }

    //update the position of the robot using the velocities passed in
    pos = computeNewPositions(pos, loop_vel, dt);

  } // end for simulation steps

---

【总结】

所以，在TrajectoryPlanner中，不论使用dwa与否，生成的轨迹都在sim_time_时间内趋于并稳定于采样速度，而dwa计算速度范围时使用的时间段更短，速度采样的区间更小，这样它在sim_time_内的轨迹实际上是迅速趋于采样速度后近乎匀速地外推得到的。而不使用dwa时，速度采样的区间大得多，得到的轨迹对应更大的速度变化范围。不过相应地，当采样数一定时，区间宽则间隔大，故不使用dwa时生成的路径比较稀疏，不细致。

而给机器人发送速度指令时，dwa发送的速度是机器人可以近乎瞬时达到的，变速过程比较短，仿真轨迹的拟合度比较高；而不使用dwa时，当发送的采样速度和当前机器人速度相差比较大时，机器人实际的变速时间长，过程可能比较复杂，仿真轨迹可能和真实轨迹略有差异。

在DWAPlanner中，速度采样和轨迹生成方面与TrajectoryPlanner差不多，虽然使用dwa时是直接用采样速度生成轨迹，不过因为采样区间小，和上面的变速过程其实没什么区别。最大的差异是，不使用dwa时，不是直接发送采样速度（即变速结果），而是发送dt（sim_time_/步数）时间段内达到的速度，这样缩短了变速的时间，提高了小范围内仿真轨迹的拟合水平。

总而言之，使用DWA能够获得小范围内比较细致的仿真轨迹，并且比较接近真实轨迹。但速度采样区间较小，故无法涵盖到速度变化稍大的轨迹。