无人车系统11：轨迹跟踪的MPC原理与Python实现 - 只需40行代码！

前面介绍的PID，pure pursuit方法，Stanley方法都只是利用当前的系统误差来设计控制器。人们对这些控制器的设计过程中都利用了构建模型对无人车未来状态的估计（或者说利用模型估计未来的运动趋势）。每个控制周期只选择一个目标路点作为跟踪对象，因此，我们也可以说以上控制器只利用了模型进行向前一步预测。那么如果在更远的未来，参考轨迹变化不是那么平缓，并且有很多弯度小（急）的部分，那么只利用一步预测很难对整条轨迹进行有效的跟踪。为了让无人车的控制器更有前瞻性（远见），设计控制器必须得利用模型对未来状态进行多步预测。本文介绍的模型预测控制(MPC)以及后一篇的线性二次调节(LQR)设计的控制器都基于模型多步预测结果。

基础链接：
无人车系统（一）：运动学模型及其线性化
无人车系统（七）:Udacity 's无人驾驶仿真环境（社区介绍）
无人车系统（八）：Udacity 's无人驾驶仿真环境（python与c++数据接口）
无人车系统（十）：c++与python非线性规(优)划（化）工具
相关链接：
无人车系统（四）：轨迹跟踪PID控制
无人车系统（五）：轨迹跟踪Pure Pursuit方法
无人车系统（六）：轨迹跟踪Stanley方法

同时也希望大家多多支持本人第一个付费专栏《自动驾驶规划入门（已完结）》，满足我一点点的虚荣心=v=，里面有更多将pyomo用于路径规划与轨迹规划的案例。

1. 模型预测控制原理

模型预测控制（MPC）的最核心思想就是利用三维的空间模型加上时间构成四维时空模型，然后在这个时空模型的基础上，求解最优控制器。MPC控制器基于一段时间的时空模型，因此得到的控制输出也是系统在未来有限时间步的控制序列。 由于，理论构建的模型与系统真实模型都有误差；从而，更远未来的控制输出对系统控制的价值很低，MPC仅执行输出序列的中第一个控制输出。

假设向前预测步长为$T$，那么T步的时空模型要比原始的空间模型要大很多。MPC在每个控制周期都需要重新利用未来T步的模型计算得到当前执行的控制指令。MPC利用了一个比原始空间模型大很多的模型（更高的计算成本）仅仅只得到当前一步的最优控制器（跟PID，pure pursuit, stanley做了同样的事）。

3. MPC控制器设计

3.1 线性模型

当模型是线性的形式（或者利用无人车系统（一）：运动学模型及其线性化相同的方法对非线性模型进行线性化），MPC控制器的求解问可以转化为一个二次规划问题，

假定线性模型为以下形式：
$$x_{k+1}=A{x}_k+B{u}_k+C\text{\hspace{1em}(1)}$$
假定未来$T$步的控制输入已知，为$u_k,u_{k+1},u_{k+2},...,u_{k+T}$，根据以上模型与输入，我们可以计算未来$T$步的状态：
$x_{k+1}=A{x}_k+B{u}_k+C$
$x_{k+2}=A{x}_{k+1}+B{u}_{k+1}+C=A(A{x}_k+B{u}_k+C)+B{u}_{k+1}+C=A^2{x}_{k+1}+AB{u}_k+B{u}_{k+1}+AC+C$
$x_{k+3}=A^3{x}_k+A^{2}B{u}_k+A{B}_{k+1}+B{u}_{k+2}+A^{2}C+AC+C$
$...$
$x_{k+T}=A^T{x}_k+A^{T-1}B{u}_k+A^{T-2}B{u}_{k+1}+...+A^{T-i}B{u}_{k+i-1}+...+B{u}_{k+T-1}+A^{T-1}C+A^{T-2}C+...+C$

将上面$T$规划成矩阵向量形式：
$$\mathcal{X}=\mathcal{A}{x}_k+\mathcal{B}\mathbf{u}+\mathcal{C}\text{\hspace{1em}(2)}$$

其中，
$\mathcal{X}={\left[\begin{array}{ccccc}{x}_{k+1}& x_{k+2}& x_{k+3}& ...& x_{k+T}\end{array}\right]}^T$