自动驾驶-MPC控制器

上节介绍了使用PID控制器控制车辆，PID控制器的优点是实现简单，处理速度快，但是缺点是不能处理有延迟的系统。本章介绍的MPC（modle predictive control）控制器能够很好的解决延迟的问题。

MPC控制器的和PID控制器一样，控制器输入包括车辆下一步的运行轨迹，车辆的当前状态，输出是航向角和加速度，用来控制方向盘，油门和刹车。不同之处在于，PID控制器是实时处理当前车辆与目标轨迹的差距来调整输出，使车辆接近目标轨迹，而MPC控制器将未来一个时间段$t$分成$N$个节点，预测每个节点的车辆状态，再调整控制器的输出使车辆尽可能接近参考轨迹。

MPC控制器的一般流程如下：

1. 通过定位和地图模块得到当前路线，将这条路线离散化成一串坐标对，再拟合为三阶多项式；
2. 根据车辆的当前位置和第一步生成的多项式系数预测未来一段时间车辆的行驶状态；
3. 根据当前状态和预测状态的差异，调整航向角和加速度，使车辆状态接近预测状态；
4. 重复以上三部。
   其中第三步是MPC控制器的核心。

需要注意的是，调整航向角和加速度时，还要考虑乘车人的感受，所以要使加速度和航向角变化尽量平滑，下面详细介绍每一步的原理并给出代码实现。

拟合多项式

拟合多项式很好理解，就是把公路路线拟合称为一个三阶多项式，返回函数每一项系数。

// Fit a polynomial.
// Adapted from
// https://github.com/JuliaMath/Polynomials.jl/blob/master/src/Polynomials.jl#L676-L716
Eigen::VectorXd polyfit(Eigen::VectorXd xvals, Eigen::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;
}

预测未来时间的行驶状态

有了上一步的多项式系数，就可以规划出未来一段时间内车辆的坐标位置，注意，地图给出的路线使用的是全剧坐标系，但是控制车辆是应该以车辆为中心，所以要进行坐标转换。

// Evaluate a polynomial.
double polyeval(Eigen::VectorXd coeffs, double x) {
  double result = 0.0;
  for (int i = 0; i < coeffs.size(); i++) {
    result += coeffs[i] * pow(x, i);
  }
  return result;
}

Eigen::VectorXd roadside_x(ptsx.si