路徑規劃與避障算法(六)---DWA算法流程之二---自行車模型與路徑採樣空間

版權聲明：本文爲博主原創文章，原創不易, 轉載請聯繫博主。
本篇博客主要介紹如何生成速度採樣空間以及利用車輛運動學模型生成對應的軌跡空間

1.運動學模型

車輛運動學模型與動力學模型的建立是出於車輛運動的規劃與控制考慮的。自動駕駛場景下，車輛大多按照規劃軌跡行駛，控制模塊的作用就是控制車輛儘可能精準的按照規劃軌跡行駛。這就要求規劃軌跡儘可能貼近實際情況，也就是說，軌跡規劃過程中應儘可能考慮車輛運動學及動力學約束，使得運動跟蹤控制的性能更好。

在使用自行車模型之前也需要考慮以下三點假設 [1]:

• 車輛在垂直方向的運動被忽略掉了，也就是說我們描述的車輛是一個二維平面上的運動物體（可以等價與我們是站在天空中的俯視視角）
• 我們假設車輛的結構就像自行車一樣，也就是說車輛的前面兩個輪胎擁有一直的角度和轉速等，同樣後面的兩個輪胎也是如此，那麼前後的輪胎就可以各用一個輪胎來描述
• 我們假設車輛運動也和自行車一樣，這意味着是前面的輪胎控制這車輛的轉角

如上圖所示，自行車運動學模型將前後輪胎分別用一個輪胎來描述並且將輪胎置於前後中心線上。假定車輪沒有橫向漂移且只有前向車輪是可以轉向的。

公式推導

限制該模型在平面上運動，前後輪的非完整約束方程爲：
$\dot{x}_f\sin(\theta+\delta)-\dot{y}\cos({\theta+\delta}) = 0\tag{1}$

$\dot{x}\sin(\theta)-\dot{y}\cos(\theta) = 0\tag{2}$
其中$(x,y)$ 是後輪的全局座標，$(x_f,y_f )$是前輪的全局座標，$θ$是車輛在$yaw$方向的偏轉角度，$δ$是車輛的轉向角度。由於車輪距離（前後輪胎之間的距離）爲$L$，因此$(x_f,y_f )$可以表示爲：
$x_f = x+L\cos(\theta)\tag{3}$
$y_f = y +L\sin(\theta) \tag{4}$
將公式3以及公式4帶入公式1中消除$（x_f,y_f）:$
$0=\dot{x}\sin(\theta+\delta)-\dot{y}\cos(\theta+\delta)-\dot{\theta}L\cos(\delta)\tag{5}$
$(\dot{x},\dot{y})$可以通過縱向速度$v$來表示：
$\dot{x} = v\cos(\theta)\tag{6}$
$\dot{y}=v\sin{(\theta)}\tag{7}$
將公式6,7的限制條件應用於公式5可以得到$\dot{\theta}$
$\dot{\theta}= v\tan(\delta)/L\tag{8}$
車輛的瞬時曲率半徑$R$是由$v$以及$\dot\theta$來決定的：
$R=v/\dot\theta\tag{9}$
結合公式8可得：
$\tan(\delta)=L/R\tag{10}$
最終以上運動學模型可以通過矩陣形式表達出來：
$\left[ \begin{matrix} \dot{x} \\ \dot{y} \\ \dot{\theta}\\ v\\ \dot{\delta} \end{matrix} \right] = \left[ \begin{matrix} \cos{\theta} \\ \sin{\theta} \\ (\tan(\delta)/L)\\ 1\\ 0 \end{matrix} \right]v+ \left[ \begin{matrix} 0 \\ 0 \\ 0 \\ 0\\ 1 \end{matrix} \right] \dot{\delta}\tag{11}$
其中，$v$和$\dot{\delta}$分別代表車輛的縱向速度以及轉向輪的角速度。
[1]: https://blog.csdn.net/adamshan/article/details/78696874

樣例代碼（c++）

Eigen::Vector3f computeNewPositions( const Eigen::Vector3f &pos,
                    				 const Eigen::Vector3f &vel, 
                    				 double dt)
{
  Eigen::Vector3f new_pos = Eigen::Vector3f::Zero();

  new_pos[0] = pos[0] + (vel[0] * cos(pos[2])) * dt;
  new_pos[1] = pos[1] + (vel[0] * sin(pos[2])) * dt;
  new_pos[2] = pos[2] + vel[2] * dt; 
  return new_pos;
}

2.速度採樣

車輛的運動學模型有了，根據速度就可以推算出車輛的運動軌跡。因此只需要採樣很多速度，推算軌跡，然後根據代價函數評估所得的軌跡選出最優軌跡。
在DWA算法中，速度如何採樣是一個極其重要的核心：在速度$(v,\dot{\delta})$的二維空間中，存在無窮多組速度，因此也存在無窮多組運動軌跡。但是根據車輛本身的運動學限制和環境限制可以將採樣速度控制在一定的範圍內：

• 車輛受自身最大速度以及最小速度的限制：

$V_1=\{(v,\dot\delta)|v\in[v_{min},v_{max}],\dot{\delta}\in[\dot\delta_{min},\delta_{max}]\}\tag{12}$

• 車輛受發動機以及變速箱等性能影響：
  由於發動機力矩有限制以及行駛道路上摩擦係數等外界環境影響，存在最大加速度限制，因此車輛前向模擬的週期內，存在一個動態窗口，在該窗口內的速度是車輛當前能夠實際達到的速度：

$V_2 = \{(v,\dot\delta)|v\in[v-\dot{v_a}\Delta t,v+\dot{v_b}\Delta t] \cap [\dot\delta-\ddot{\delta_a}\Delta t,v+\ddot{\delta_b}\Delta t] \} \tag{13}$
其中，$\dot{v_a},\dot{v_b}$分別表示最大減速度與最大加速度；$\ddot{\delta_a},\ddot{\delta_b}$分別表示角速度的最大減速度與最大加速度。
針對以上兩條準則生成的速度$V_1,V_2$進行交集處理$（V_3=V_1\cap V_2)$並將處理後的速度集合$V_3$作爲採樣的有效範圍區間。爲了簡化每組速度對應軌跡的計算，DWA算法假設車輛在往前模擬的這段時間內速度不變，直到下一時刻採樣給定新的速度命令。

樣例代碼(c++)

void GenerateVelocity(
    const Eigen::Vector3f &pos,
    const Eigen::Vector3f &vel,
    const Eigen::Vector3f &goal,
    LocalPlannerLimits *limits,
    const Eigen::Vector3f &vsamples,
    bool discretize_by_time)
{
  /*
   * We actually generate all velocity sample vectors here, from which to generate trajectories later on
   */

  double max_vel_th = limits->max_rot_vel;
  double min_vel_th = limits->min_rot_vel;
  discretize_by_time_ = discretize_by_time;
  Eigen::Vector3f acc_lim = limits->getAccLimits();
  pos_ = pos;
  vel_ = vel;
  limits_ = limits;
  next_sample_index_ = 0;
  sample_params_.clear();

  double min_vel_x = limits->min_vel_x;
  double max_vel_x = limits->max_vel_x;
  double min_vel_y = limits->min_vel_y;
  double max_vel_y = limits->max_vel_y;

  // if sampling number is zero in any dimension, we don't generate samples generically
  if (vsamples[0] * vsamples[1] * vsamples[2] > 0)
  {
    //compute the feasible velocity space based on the rate at which we run
    Eigen::Vector3f max_vel = Eigen::Vector3f::Zero();
    Eigen::Vector3f min_vel = Eigen::Vector3f::Zero();

    max_vel[0] = std::min(max_vel_x, vel[0] + acc_lim[0] * sim_period_);
    max_vel[1] = 0.0;
    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] = 0.0;
    min_vel[2] = std::max(min_vel_th, vel[2] - acc_lim[2] * sim_period_);
  

    Eigen::Vector3f vel_samp = Eigen::Vector3f::Zero();
    VelocityIterator x_it(min_vel[0], max_vel[0], vsamples[0]);
    VelocityIterator y_it(min_vel[1], max_vel[1], vsamples[1]);
    VelocityIterator th_it(min_vel[2], max_vel[2], vsamples[2]);
    for (; !x_it.isFinished(); x_it++)
    {
      vel_samp[0] = x_it.getVelocity();
      //cout << "vel_x: " << vel_samp[0] << endl;
      for (; !y_it.isFinished(); y_it++)
      {
        vel_samp[1] = y_it.getVelocity();
        //cout << "vel_y: " << vel_samp[1] << endl;
        for (; !th_it.isFinished(); th_it++)
        {
          vel_samp[2] = th_it.getVelocity();
          //cout << "vel_th: " << vel_samp[2] << endl;
          sample_params_.push_back(vel_samp);
        }
        th_it.reset();
      }
      y_it.reset();
    }
  }
}

---

下部分將會介紹DWA具體流程中:具體的路徑評價函數