自动驾驶中的模型预测控制（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：

然后，对于偏向角ψ：

我们用到了转角加速度δ，然后L_f表示汽车的半轴长，与转弯半径相关，这个值越大，转弯半径越大。然后呢，当去读越快的时候，转弯速度也是最快的，所以速度也包含在内。
接下来就是速度：

其中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