自動駕駛中的模型預測控制（Model Predictive Control）

一、什麼是模型預測控制（MPC）

MPC主要用于車道線的追蹤，保持車輛軌迹相對平穩。

MPC将車道追蹤任務重構成一個尋找最優解的問題，優化問題的最優解就是最優的軌迹。我們每走一步都會按照目前的狀态求解一個最優化的軌迹，然後按照軌迹走一步，緊接着繼續按照傳感器獲得的新值繼續求解最優軌迹，保證軌迹跟 我們要追蹤的車道線的最大拟合。這個過程中，因為我們每動一步，就是一個時間片段，因為各種誤差的存在，導緻我們的車輛并不能完全符合預測出來的軌迹，是以每一次都得重新計算調整。

二、車輛的模型

想要對車輛進行模型預測控制，首先我們得對車輛進行模組化。這裡我們對車輛的動力學模型進行一個簡化，當然越複雜的模型預測起來就會更加準确，但是簡單的模型更加友善計算和了解。

運動學模型

運動學模型忽略了輪胎力，重力以及品質，這種模型可以說是經過了極大的的簡化，是以精确度低，但是因為經過簡化，是以很好計算，而且在低中速的運動中還有這相當不錯的精度

動力學模型

動力學模型是盡可能的展現出實際上車輛的動态。它計算到了輪胎的摩擦力，橫向和縱向的加速度，慣性，中立，空氣阻力，品質以及車輛的實體形狀，是以不同的車的動力學模型很可能是不一樣的，而且考慮的因素越多也就相對越精确。有的複雜的動力學模型甚至會考慮到底盤懸挂如何響應。

2.1 靜态模型

首先我們來描述一輛車的靜态的狀态，那麼就有坐标x，y，然後車頭會有一個跟參考方向的夾角ψ。

2.2 動态模型

如果汽車動起來，那麼就會增加一個參數v，即速度：

是以我們汽車的狀态向量就是

X = [x，y，ψ，v]

2.3 狀态控制向量

我們想要控制汽車的動作，需要通過方向盤和油門踏闆來實作，這裡我們簡化一下，把這兩個都看作是單獨的執行器。

δ代表方向的轉動角度（注意不是方向盤的轉角，而是車輪的偏向角）；

a代表油門踏闆的動作，正數為加速，負數為減速。

是以狀态控制向量為：

[δ，a]

2.4 車道線的拟合

一般來說對于大多數道路，我們如果使用多項式拟合的話，那麼三次多項式就足夠在一定距離内比較好的拟合車道了。

2.5 各種公式

假設我們計算的時間間隔為Δt。

首先，針對位置資訊x和y：

然後，對于偏向角ψ：

我們用到了轉角加速度δ，然後Lf表示汽車的半軸長，與轉彎半徑相關，這個值越大，轉彎半徑越大。然後呢，當去讀越快的時候，轉彎速度也是最快的，是以速度也包含在内。

接下來就是速度：

其中a為油門踏闆了，取值為-1——1。

是以我們就有了一個模型：

狀态計算

VectorXd globalKinematic(const VectorXd &state, 
                         const VectorXd &actuators, double dt) {
  // Create a new vector for the next state.
  VectorXd next_state(state.size());
  // state is [x, y, psi, v] and actuators is [delta, a]

  double x_t = state(0);
  double y_t = state(1);
  double psi_t = state(2);
  double v_t = state(3);

  double delta_t = actuators(0);
  double a_t = actuators(1);

  double x = x_t + v_t * cos(psi_t) * dt;
  double y = y_t + v_t * sin(psi_t) * dt;
  double psi = psi_t + v_t / Lf * delta_t * dt;
  double v = v_t + a_t * dt;

  next_state << x,y,psi,v;

  return next_state;
}
           

拟合三次曲線

#include <iostream>
#include "Eigen-3.3/Eigen/Core"
#include "Eigen-3.3/Eigen/QR"

using Eigen::VectorXd;

// Evaluate a polynomial.
double polyeval(const VectorXd &coeffs, double x);
// Fit a polynomial.
VectorXd polyfit(const VectorXd &xvals, const VectorXd &yvals, int order);

int main() {
  VectorXd xvals(6);
  VectorXd yvals(6);
  // x waypoint coordinates
  xvals << 9.261977, -2.06803, -19.6663, -36.868, -51.6263, -66.3482;
  // y waypoint coordinates
  yvals << 5.17, -2.25, -15.306, -29.46, -42.85, -57.6116;


  auto coeffs = polyfit(xvals, yvals, 3);

  for (double x = 0; x <= 20; ++x) {

    std::cout << polyeval(coeffs,x) << std::endl; 
  }

}

double polyeval(const VectorXd &coeffs, double x) {
  double result = 0.0;
  for (int i = 0; i < coeffs.size(); ++i) {
    result += coeffs[i] * pow(x, i);
  }
  return result;
}

// Adapted from:
// https://github.com/JuliaMath/Polynomials.jl/blob/master/src/Polynomials.jl#L676-L716
VectorXd polyfit(const VectorXd &xvals, const VectorXd &yvals, int order) {
  assert(xvals.size() == yvals.size());
  assert(order >= 1 && order <= xvals.size() - 1);

  Eigen::MatrixXd A(xvals.size(), order + 1);

  for (int i = 0; i < xvals.size(); ++i) {
    A(i, 0) = 1.0;
  }

  for (int j = 0; j < xvals.size(); ++j) {
    for (int i = 0; i < order; ++i) {
      A(j, i + 1) = A(j, i) * xvals(j);
    }
  }

  auto Q = A.householderQr();
  auto result = Q.solve(yvals);

  return result;
}
           

2.6 誤差計算

我們通過将誤差最小化作為調優的目标。

我們新的狀态向量是 [x,y,ψ,v,cte,eψ].

2.6.1 航向偏差

航向偏差表示汽車的行進路線與道路中心線的偏差，

ctet+1=ctet+v​t∗sin(eψ​t)∗dt

ctet可以表示為車輛目前位置與yt（道路中心線）之間的內插補點，是以有：

ctet=f(xt)−yt

然後帶入上式可得：

ctet+1=f(xt)−yt+v​t∗sin(eψ​t)∗dt

其中的誤差可以分為兩部分：

• f(xt)−yt為目前的航迹偏差
• v​t∗sin(eψ​t)∗dt為車輛運動引起的偏差

2.6.2 方向偏差

接下來就是方向偏差：

計算方法跟ψ類似。

eψt的計算方法是使用eψt減去目标角度。

eψt = ψt - ψdest

ψt是已知的，但是ψdest是未知的，我們隻知道路徑的多項式，我們可以使用目前點的正切角來計算這個值，arctan(f′​​ (xt))，f′​​是軌迹的導數。

• eψt是目前的方向偏差
• 後部分是速度造成的偏差

三、MPC的實作

3.1 工具

這裡我們用到兩個工具，輔助我們的計算

3.1.1 Ipopt

ipopt是一個解決非線性規劃最優化問題的工具集，當然，它也能夠用于解決線性規劃問題的求解。

ipopt可以根據我們的限制條件來求解局部最優，是以很适用與非線性問題。但是這個工具要求我們提供限制條件的Jacobian矩陣和目标函數的Hessian矩陣，這個其實還是比較費勁的，是以我們有了以下的工具：CppAD

3.1.2 CppAD

CppAD是一個自動計算導數的工具，

為了使用其計算導數，我們所有的計算函數以及資料類型都需要使用這個包裡自帶的。例如：

CppAD::pow(x, 2);
// instead of 
pow(x, 2);
           

3.2 初始化

首先設定各種初始化的值

/**
 *  設定時間間隔和預測的步數
 */
size_t N = 10;
double dt = 0.1;


//設定半軸長
const double Lf = 2.67;

//設定目标車速
double ref_v = 40.0;

// 定義每一個state的起始點，因為函數的需要，隻能傳入一個數組，是以将誤差以及各種狀态的點都放在一個數組中展開
size_t x_start = 0;
size_t y_start = x_start + N;
size_t psi_start = y_start + N;
size_t v_start = psi_start + N;
size_t cte_start = v_start + N;
size_t epsi_start = cte_start + N;
size_t delta_start = epsi_start + N;
size_t a_start = delta_start + N - 1;

           

3.3 設定限制

class FG_eval {
 public:
  // Fitted polynomial coefficients
  VectorXd coeffs;
  FG_eval(VectorXd coeffs) { this->coeffs = coeffs; }

  typedef CPPAD_TESTVECTOR(AD<double>) ADvector;
  void operator()(ADvector& fg, const ADvector& vars) {
// The cost is stored is the first element of `fg`.
    // Any additions to the cost should be added to `fg[0]`.
    fg[0] = 0;

    // Reference State Cost

    // 損失函數,目的是将損失函數降到最小，求最優化解
    //基于參考狀态的損失，求狀态的最優化，最小的距離，最小的夾角，最小的速度
    for (unsigned int i = 0; i < N; i++)
    {
      fg[0] += 500 * CppAD::pow(vars[cte_start + i], 2);
      fg[0] += 500 * CppAD::pow(vars[epsi_start + i],2);
      fg[0] += CppAD::pow(vars[v_start + i] - ref_v,2);
    }

    //執行器的損失，求執行器的最優化值
    for (unsigned int i = 0; i < N - 1; i++)
    {
      fg[0] += 50 * CppAD::pow(vars[delta_start + i], 2);
      fg[0] += 50 * CppAD::pow(vars[a_start + i],2);
      // try adding penalty for speed + steer
      // fg[0] += 700*CppAD::pow(vars[delta_start + i] * vars[v_start+i], 2);
    
    }

    // 求兩次動作的最優化值
    for (int i = 0; i < N - 2; i++) {
      fg[0] += 200*CppAD::pow(vars[delta_start + i + 1] - vars[delta_start + i], 2);
      fg[0] += 10*CppAD::pow(vars[a_start + i + 1] - vars[a_start + i], 2);
    }
    //
    // Setup Constraints
    // 設定限制，在這裡設定限制條件
    //

    // Initial constraints
    //
    // We add 1 to each of the starting indices due to cost being located at
    // index 0 of `fg`.
    // This bumps up the position of all the other values.
    //因為fg【0】是損失，其餘的狀态從fg【1】開始
    fg[1 + x_start] = vars[x_start];
    fg[1 + y_start] = vars[y_start];
    fg[1 + psi_start] = vars[psi_start];
    fg[1 + v_start] = vars[v_start];
    fg[1 + cte_start] = vars[cte_start];
    fg[1 + epsi_start] = vars[epsi_start];

    // The rest of the constraints
    for (int t = 1; t < N; ++t) {

      //t時刻的狀态值
      AD<double> x1 = vars[x_start + t];
      AD<double> y1 = vars[y_start + t];
      AD<double> psi1 = vars[psi_start + t];
      AD<double> v1 = vars[v_start + t];
      AD<double> cte1 = vars[cte_start + t];
      AD<double> epsi1 = vars[epsi_start + t];

      //t-1時刻的狀态
      AD<double> x0 = vars[x_start + t - 1];
      AD<double> y0 = vars[y_start + t - 1];
      AD<double> psi0 = vars[psi_start + t - 1];
      AD<double> v0 = vars[v_start + t - 1];
      AD<double> cte0 = vars[cte_start + t - 1];
      AD<double> epsi0 = vars[epsi_start + t - 1];

      //隻計算t-1時刻
      AD<double> delta0 = vars[delta_start + t - 1];
      AD<double> a0 = vars[a_start + t - 1];
      if (t > 1) {   // use previous actuations (to account for latency)
        a0 = vars[a_start + t - 2];
        delta0 = vars[delta_start + t - 2];
      }

      AD<double> f0 = coeffs[0] + coeffs[1] * x0  + coeffs[2] * CppAD::pow(x0, 2) + coeffs[3] * CppAD::pow(x0, 3);
      AD<double> psides0 = CppAD::atan(coeffs[1] + 2 * coeffs[2] * x0 + 3 * coeffs[3] * CppAD::pow(x0, 2));

      // The idea here is to constraint this value to be 0.
      //
      // NOTE: The use of `AD<double>` and use of `CppAD`!
      // CppAD can compute derivatives and pass these to the solver.

      //限制條件，目的是讓該值等于0 ，是以前一時刻的狀态經過變換應該等于下一時刻的狀态
      // x_[t+1] = x[t] + v[t] * cos(psi[t]) * dt
      // y_[t+1] = y[t] + v[t] * sin(psi[t]) * dt
      // psi_[t+1] = psi[t] + v[t] / Lf * delta[t] * dt
      // v_[t+1] = v[t] + a[t] * dt
      // cte[t+1] = f(x[t]) - y[t] + v[t] * sin(epsi[t]) * dt
      // epsi[t+1] = psi[t] - psides[t] + v[t] * delta[t] / Lf * dt
      
      fg[1 + x_start + t] = x1 - (x0 + v0 * CppAD::cos(psi0) * dt);
      fg[1 + y_start + t] = y1 - (y0 + v0 * CppAD::sin(psi0) * dt);
      fg[1 + psi_start + t] = psi1 - (psi0 - v0 * delta0 / Lf * dt);
      fg[1 + v_start + t] = v1 - (v0 + a0 * dt);
      fg[1 + cte_start + t] = cte1 - ((f0 - y0) + (v0 * CppAD::sin(epsi0) * dt));
      fg[1 + epsi_start + t] = epsi1 - ((psi0 - psides0) + v0 * delta0 / Lf * dt);


    }


  }
};
           

3.4 根據限制求解最優解

std::vector<double> MPC::Solve(const VectorXd &state, const VectorXd &coeffs) {
  bool ok = true;
  typedef CPPAD_TESTVECTOR(double) Dvector;

  double x = state[0];
  double y = state[1];
  double psi = state[2];
  double v = state[3];
  double cte = state[4];
  double epsi = state[5];
  /**
   * Set the number of model variables (includes both states and inputs).
   * For example: If the state is a 4 element vector, the actuators is a 2
   *   element vector and there are 10 timesteps. The number of variables is:
   *   4 * 10 + 2 * 9
   */
   // 獨立變量的個數
  // N timesteps == N - 1 actuations
  size_t n_vars = state.size() * 10 + (N - 1) * 2;
  /**
   * 設定變量個數
   */
  size_t n_constraints = N * 6;

  // 初始化所有獨立變量的值
  // SHOULD BE 0 besides initial state.
  Dvector vars(n_vars);
  for (int i = 0; i < n_vars; ++i) {
    vars[i] = 0.0;
  }

  // Set the initial variable values
  //設定初始狀态
  vars[x_start] = x;
  vars[y_start] = y;
  vars[psi_start] = psi;
  vars[v_start] = v;
  vars[cte_start] = cte;
  vars[epsi_start] = epsi;


  /**
   * TODO: Set lower and upper limits for variables.
   */
  // Lower and upper limits for x
  //所有x的極限值
  Dvector vars_lowerbound(n_vars);
  Dvector vars_upperbound(n_vars);

  // Set all non-actuators upper and lowerlimits
  // to the max negative and positive values.
  //将所有非執行器的狀态量的最大最小值分别都拉到最大
  for (int i = 0; i < delta_start; ++i) {
    vars_lowerbound[i] = -1.0e19;
    vars_upperbound[i] = 1.0e19;
  }

  // The upper and lower limits of delta are set to -25 and 25
  // degrees (values in radians).
  //将所有的delta的限制設定為±25°

  for (int i = delta_start; i < a_start; ++i) {
    vars_lowerbound[i] = -0.436332;
    vars_upperbound[i] = 0.436332;
  }

  // Acceleration/decceleration upper and lower limits.
  // 加速/減速上限和下限。
  for (int i = a_start; i < n_vars; ++i) {
    vars_lowerbound[i] = -1.0;
    vars_upperbound[i] = 1.0;
  }

  // Lower and upper limits for the constraints
  // 限制的下限和上限
  Dvector constraints_lowerbound(n_constraints);
  Dvector constraints_upperbound(n_constraints);
  for (int i = 0; i < n_constraints; ++i) {
    constraints_lowerbound[i] = 0;
    constraints_upperbound[i] = 0;
  }

  constraints_lowerbound[x_start] = x;
  constraints_lowerbound[y_start] = y;
  constraints_lowerbound[psi_start] = psi;
  constraints_lowerbound[v_start] = v;
  constraints_lowerbound[cte_start] = cte;
  constraints_lowerbound[epsi_start] = epsi;

  constraints_upperbound[x_start] = x;
  constraints_upperbound[y_start] = y;
  constraints_upperbound[psi_start] = psi;
  constraints_upperbound[v_start] = v;
  constraints_upperbound[cte_start] = cte;
  constraints_upperbound[epsi_start] = epsi;

  // object that computes objective and constraints
  FG_eval fg_eval(coeffs);

  // NOTE: You don't have to worry about these options
  // options for IPOPT solver
  std::string options;
  // Uncomment this if you'd like more print information
  options += "Integer print_level  0\n";
  // NOTE: Setting sparse to true allows the solver to take advantage
  //   of sparse routines, this makes the computation MUCH FASTER. If you can
  //   uncomment 1 of these and see if it makes a difference or not but if you
  //   uncomment both the computation time should go up in orders of magnitude.
  options += "Sparse  true        forward\n";
  options += "Sparse  true        reverse\n";
  // NOTE: Currently the solver has a maximum time limit of 0.5 seconds.
  // Change this as you see fit.
  options += "Numeric max_cpu_time          0.5\n";

  // place to return solution
  CppAD::ipopt::solve_result<Dvector> solution;

  // solve the problem
  CppAD::ipopt::solve<Dvector, FG_eval>(
      options, vars, vars_lowerbound, vars_upperbound, constraints_lowerbound,
      constraints_upperbound, fg_eval, solution);

  // Check some of the solution values
  ok &= solution.status == CppAD::ipopt::solve_result<Dvector>::success;

  // Cost
  auto cost = solution.obj_value;
  std::cout << "Cost " << cost << std::endl;

  /**
   * TODO: Return the first actuator values. The variables can be accessed with
   *   `solution.x[i]`.
   *
   * {...} is shorthand for creating a vector, so auto x1 = {1.0,2.0}
   *   creates a 2 element double vector.
   */

  std::vector<double> res;
  res.push_back(solution.x[delta_start]);
  res.push_back(solution.x[a_start]);
  for (size_t i = 0; i < N - 2; i++)
  {
    res.push_back(solution.x[x_start + i + 1 ]);
    res.push_back(solution.x[y_start + i + 1 ]);
  }
  
  return res;
}
           

3.5 轉換坐标系

将世界坐标系轉換為車輛坐标系

vector<double> ptsx = j[1]["ptsx"];
          vector<double> ptsy = j[1]["ptsy"];
          double px = j[1]["x"];
          double py = j[1]["y"];
          double psi = j[1]["psi"];
          double v = j[1]["speed"];

          assert(ptsx.size() == ptsy.size());

          // 轉化世界坐标系航點為車輛坐标系航點
          vector<double> waypoints_x;
          vector<double> waypoints_y;
          for (int i = 0; i < ptsx.size(); i++)
          {
            double dx = ptsx[i] - px;
            double dy = ptsy[i] - py;
            waypoints_x.push_back(dx * cos(-psi) - dy * sin(-psi));
            waypoints_y.push_back(dx * sin(-psi) + dy * cos(-psi));
          }
          //将指針指派位址
          double* ptrx = &waypoints_x[0];
          double* ptry = &waypoints_y[0];
          //使用指針來建構矩陣或向量，map是一個引用
          Eigen::Map<Eigen::VectorXd> waypoints_mx(ptrx, 6);
          Eigen::Map<Eigen::VectorXd> waypoints_my(ptry, 6);
           

四、PID和MPC

在真是的汽車中，我們的控制動作不會立即作用于系統中，往往有個遲滞，這個對于PID來說是個挑戰，但是對于MPC，我們可以把這個遲滞添加到模型中。

4.1 PID

PID控制是計算目前狀态的偏差，無法對未來進行預測，是以未來的狀态對于汽車來說完全是未知的，汽車無法預測自己的動作對于未來的影響。

如果PID計算通過預測未來的偏差來計算控制量，系統也因為沒有模型而無法準确控制。

MPC

MPC是動态的，考慮到了影響到了汽車動作的各種因素，是以可以很容易的将類似于遲滞這樣的因素加入到模型中，更有利于我們精細的控制汽車。

五、完整代碼

完整代碼見我的github