模型预测控制（MPC）的核心思想就是以优化方法求解最优控制器，其中优化方法大多时候采用二次规划（Quadratic Programming）。

MPC控制器优化得到的控制输出也是系统在未来有限时间步的控制序列。 当然，由于理论构建的模型与系统真实模型都有误差，所以，实际上更远未来的控制输出对系统控制的价值很低，故MPC仅执行输出序列中的第一个控制输出。

模型(Model)

分为机理模型和基于数据的模型（例如用神经网络训练的一个model）使用基于数据的模型的MPC可以结合model based RL使用。

预测(Predict)

模型就是用来预测的，预测的目的是为了更好的决策

控制(Control)

控制即决策，根据预测来作出决策。

MPC利用一个已有的模型、系统当前的状态和未来的控制量，来预测系统未来的输出，然后与我们期望的系统输出做比较，得到一个损失函数（代价函数），即：

损失函数 = ( 未来输出 ( 模型，当前状态，未来控制量 ) − 期望输出 ) 2 损失函数 = (未来输出(模型，当前状态，未来控制量)-期望输出)^2 损失函数=(未来输出(模型，当前状态，未来控制量)−期望输出)2

由于上式中模型、当前状态、期望输出都是已知的，因此只有未来控制量一个自变量。采用二次规划的方法求解出某个未来控制量，使得损失函数最小，前面提到，这个未来控制量的第一个元素就是当前控制周期的控制量。

最优控制（optimal control）指的是在一定的约束情况下达到最优状态的系统表现，其中约束情况通常是实际环境所带来的限制，例如汽车中的油门不能无限大等。

最优控制强调的是“最优”，一般最优控制需要在整个时间域上进行求优化(从0时刻到正无穷时刻的积分)，这样才能保证最优性，这是一种很贪婪的行为，需要消耗大量算力。同时，系统如果是一个时变系统，或者面临扰动的话，前一时刻得到的最优并不一定是下一时刻的最优值。

J = ∫ 0 ∞ E T Q E + U T R U d t J=\int_{0}^{\infty} E^{T} Q E+U^{T} R U d t J=∫0∞​ETQE+UTRUdt

最优控制常用解法有： 变分法，极大值原理，动态规划，LQR（LQR可以参考博客）。

MPC仅考虑未来几个时间步，一定程度上牺牲了最优性。

MPC善于处理多输入多输出系统（MIMO）；

MPC可以处理约束，如安全性约束，上下阈值；

MPC是一种向前考虑未来时间步的有限时域优化方法（一定的预测能力）

最优控制要求在整个时间优化

实际上MPC采用了一个折中的策略，既不是像最优控制那样考虑整个时域，也不是仅仅考虑当前，而是考虑未来的有限时间域。

对于一般的离散化系统（因为实际计算机实现的控制系统都是离散的系统，连续系统可以进行离散化操作），在k时刻，我们可以测量出系统的当前状态 y ( k ) y(k) y(k)，再通过计算得到的 u ( k ) , u ( k + 1 ) , u ( k + 2 ) . . . u ( k + j ) u(k),u(k+1),u(k+2)...u(k+j) u(k),u(k+1),u(k+2)...u(k+j)得到系统未来状态的估计值 y ( k + 1 ) , y ( k + 2 ) . . . y ( k + j ) y(k+1),y(k+2)...y(k+j) y(k+1),y(k+2)...y(k+j)；

将预测状态估计的部分称为预测区间（Predictive Horizon）,指的是一次优化后预测未来输出的时间步的个数。

将控制估计的部分称为控制区间（Control Horizon），在得到最优输入之后，我们只施加当前时刻的输入u(k)，即控制区间的第一位控制输入。

如下图 [ k , k + m ] [k, k+m] [k,k+m]范围为控制区间，之后的红色部分称为 held constant，其中控制区间是要通过优化器来进行优化的参数。

过小的控制区间，可能无法做到较好的控制，而较大的控制区间，比如与预测区间相等，则会导致只有前一部分的控制范围才会有较好的效果，而后一部分的控制范围则收效甚微，而且将带来大量的计算开销。

对于约束，一般分为Hard约束和Soft约束，Hard约束是不可违背必须遵守的，在控制系统中，输入输出都可能会有约束限制，但是在设计时不建议将输入输出都给予Hard约束，因为这两部的约束有可能是有重叠的，导致优化器会产生不可行解。

Hard约束不能违反，Soft约束可以；比如Hard约束是刹车踩的幅度；Soft约束是速度

建议输出采用Soft约束，而输入的话建议输入和输入参数变化率二者之间不要同时为Hard约束，可以一个Hard一个Soft。

模型预测控制在k时刻共需三步；

第一步：获取系统的当前状态；

第二步：基于 u ( k ) , u ( k + 1 ) , u ( k + 2 ) . . . u ( k + m ) u(k),u(k+1),u(k+2)...u(k+m) u(k),u(k+1),u(k+2)...u(k+m)进行最优化处理；

离散系统的代价函数可以参考

J = ∑ k m − 1 E k T Q E k + u k T R u k + E N T F E N J=\sum_{k}^{m-1}E_k^TQE_k+u_k^TRu_k+E_N^TFE_N J=k∑m−1​EkT​QEk​+ukT​Ruk​+ENT​FEN​

其中 E N E_N EN​表示误差的终值，也是衡量优劣的一种标准。

第三步：只取 u ( k ) u(k) u(k)作为控制输入施加在系统上。

在下一时刻重复以上三步，在下一步进行预测时使用的就是下一步的状态值，我们将这样的方案称为滚动优化控制（Receding Horizon Control）。

预测控制的优化不是一次离线进行，而是随着采样时刻的前进反复地在线进行，故称为滚动优化。这种滚动优化虽然得不到理想的全局最优解，但是反复对每一采样时刻的偏差进行优化计算，将可及时地校正控制过程中出现的各种复杂情况。

从以下几个方面进行阐述：

具体可参考博客

当模型是线性的时候（非线性系统可以线性化），MPC的设计求解一般使用二次规划方法。

设线性模型为以下形式： x k + 1 = A x k + B u k + C (1) x_{k+1}=Ax_k+Bu_k+C \tag{1} xk+1​=Axk​+Buk​+C(1)

假定未来 m m m步的控制输入已知，为 u k , u k + 1 , u k + 2 , . . . , u k + m u_k, u_{k+1}, u_{k+2}, ..., u_{k+m} uk​,uk+1​,uk+2​,...,uk+m​​，根据以上模型与输入，我们可以计算未来 m m m步的状态：

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 + A B u k + B u k + 1 + A C + C x k + 3 = A 3 x k + A 2 B u k + A B k + 1 + B u k + 2 + A 2 C + A C + C . . . x k + m = A m x k + A m − 1 B u k + A m − 2 B u k + 1 + . . . + A m − i B u k + i − 1 + . . . + B u k + m − 1 + A m − 1 C + A m − 2 C + . . . + C \begin{aligned} x_{k+1}&=Ax_k+Bu_k+C \\ x_{k+2}&=Ax_{k+1}+Bu_{k+1}+C=A(Ax_k+Bu_k+C)+Bu_{k+1}+C=A^2x_{k}+ABu_k+Bu_{k+1}+AC+C \\ x_{k+3}&=A^3x_k+A^2Bu_{k}+AB_{k+1}+Bu_{k+2}+A^2C+AC+C\\ ...\\ x_{k+m}&=A^{m}x_{k}+A^{m-1}Bu_k+A^{m-2}Bu_{k+1}+...+A^{m-i}Bu_{k+i-1}+...+Bu_{k+m-1}+A^{m-1}C+A^{m-2}C+...+C \end{aligned} xk+1​xk+2​xk+3​...xk+m​​=Axk​+Buk​+C=Axk+1​+Buk+1​+C=A(Axk​+Buk​+C)+Buk+1​+C=A2xk​+ABuk​+Buk+1​+AC+C=A3xk​+A2Buk​+ABk+1​+Buk+2​+A2C+AC+C=Amxk​+Am−1Buk​+Am−2Buk+1​+...+Am−iBuk+i−1​+...+Buk+m−1​+Am−1C+Am−2C+...+C​

将上面 m m m步写成矩阵向量形式：

X = A x k + B u + C (2) \mathcal{X}=\mathcal{A}x_k+\mathcal{B}\mathbf{u}+\mathcal{C} \tag{2} X=Axk​+Bu+C(2)

其中， X = [ x k + 1 , x k + 2 , x k + 3 , . . . x k + m ] T \mathcal{X}=\left[x_{k+1}, x_{k+2}, x_{k+3},...x_{k+m}\right]^T X=[xk+1​,xk+2​,xk+3​,...xk+m​]T u = [ u k , u k + 1 , u k + 2 , . . . , u k + m − 1 ] T \mathbf{u}=\left[u_k,u_{k+1},u_{k+2},...,u_{k+m-1}\right]^T u=[uk​,uk+1​,uk+2​,...,uk+m−1​]T A = [ A , A 2 , A 3 , . . . , A m ] T \mathcal{A}=\left[A, A^2 ,A^3 ,... ,A^m\right]^T A=[A,A2,A3,...,Am]T

B = ( 0 0 . . . 0 B 0 . . . 0 A B B . . . 0 . . . . . . . . . . . . A m − 1 B A m − 2 B . . . B ) \mathcal{B}=\begin{pmatrix}0&0&...&0\\ B&0&...&0\\ AB&B&...&0\\ ...&...&...&...\\ A^{m-1}B&A^{m-2}B&...&B\end{pmatrix} B=⎝ ⎛​0BAB...Am−1B​00B...Am−2B​...............​000...B​⎠ ⎞​

C = [ C A C + C A 2 C + A C + C … A k + m − 1 C + … + C ] \mathcal{C}=\left[\begin{array}{c} C \\ A C+C \\ A^{2} C+A C+C \\ \ldots \\ A^{k+m-1} C+\ldots+C \end{array}\right] C=⎣ ⎡​CAC+CA2C+AC+C…Ak+m−1C+…+C​⎦ ⎤​

上式 B \mathcal{B} B中的下三角形式，直接反映了系统在时间上的因果关系，即 k + 1 k+1 k+1时刻的输入对 k k k 时刻的输出没有影响， k + 2 k+2 k+2 时刻的输入对 k k k 和 k + 1 k+1 k+1 时刻没有影响。

假定参考轨迹为 X ‾ = [ x ˉ k + 1 x ˉ k + 2 x ˉ k + 3 … x ˉ k + m ] T \overline{\mathcal{X}}=\left[\begin{array}{lllll}\bar{x}_{k+1} & \bar{x}_{k+2} & \bar{x}_{k+3} & \ldots & \bar{x}_{k+m}\end{array}\right]^{T} X=[xˉk+1​​xˉk+2​​xˉk+3​​…​xˉk+m​​]T,则MPC的一个简单的目标代价函数如下： min ⁡ J = E T Q E + u T R u s.t.  u m i n ≤ u ≤ u m a x (3) \min \mathcal{J}=\mathcal{E}^T Q \mathcal{E}+\mathbf{u}^T R \mathbf{u} \\ \text{s.t. } u_{min}\leq \mathbf{u}\leq u_{max} \tag{3} minJ=ETQE+uTRus.t. umin​≤u≤umax​(3)

其中， E = X − X ‾ = [ x k + 1 − x ˉ k + 1 x k + 2 − x ˉ k + 2 … x k + m − x ˉ k + m ] T \mathcal{E}=\mathcal{X}-\overline{\mathcal{X}}=\left[\begin{array}{llll}x_{k+1}-\bar{x}_{k+1} & x_{k+2}-\bar{x}_{k+2} & \ldots & x_{k+m}-\bar{x}_{k+m}\end{array}\right]^{T} E=X−X=[xk+1​−xˉk+1​​xk+2​−xˉk+2​​…​xk+m​−xˉk+m​​]T

u T R u \mathbf{u}^T R \mathbf{u} uTRu这一项是为了让控制输入不会太大，因此代价函数中添加了一项对控制量的约束。

将式(2)代入式(3)，则优化变量仅剩 u \mathbf{u} u。以上最优化问题可用二次规划方法求解，得到满足目标代价函数的最优控制序列 u = { u k , ​​ u k + 1 , ​​ u k + 2 ​​ . . . ​ u k + m − 1 } \mathbf{u}=\left\{u_k,​​u_{k+1},​​u_{k+2}​​...​u_{k+m−1}\right\} u={uk​,​​uk+1​,​​uk+2​​​...​uk+m−1​}。

当转换成式（3）后，可以利用凸优化库进行二次规划求解，例如python的cvxopt，OSQP: An Operator Splitting Solver for Quadratic Programs，Casdi等。

设车辆的状态量偏差和控制量偏差如式 ( 4 ) 所示： x ~ = [ x ˙ − x ˙ r y ˙ − y ˙ r φ ˙ − φ ˙ r ] , u ~ = [ v − v r δ − δ r ] (4) \tag{4} \tilde{\boldsymbol{x}}=\left[\begin{array}{c} \dot{x}-\dot{x}_{r} \\ \dot{y}-\dot{y}_{r} \\ \dot{\varphi}-\dot{\varphi}_{r} \end{array}\right], \tilde{\boldsymbol{u}}=\left[\begin{array}{c} v-v_{r} \\ \delta-\delta_{r} \end{array}\right] x~=⎣ ⎡​x˙−x˙r​y˙​−y˙​r​φ˙​−φ˙​r​​⎦ ⎤​,u~=[v−vr​δ−δr​​](4) 基于先前的运动学模型的离散状态空间方程如下， x ~ ( k + 1 ) = [ 1 0 − T v r sin ⁡ φ r 0 1 T v r cos ⁡ φ r 0 0 1 ] x ~ ( k ) + [ T cos ⁡ φ r 0 T sin ⁡ φ r 0 T tan ⁡ δ r l T v r l cos ⁡ 2 δ r ] u ~ ( k ) = A x ~ ( k ) + B u ~ ( k ) (5) \tag{5} \tilde{\boldsymbol{x}}(k+1)=\left[\begin{array}{ccc} 1 & 0 & -T v_{r} \sin \varphi_{r} \\ 0 & 1 & T v_{r} \cos \varphi_{r} \\ 0 & 0 & 1 \end{array}\right] \tilde{\boldsymbol{x}}(k)+\left[\begin{array}{cc} T \cos \varphi_{r} & 0 \\ T \sin \varphi_{r} & 0 \\ T \frac{\tan \delta_{r}}{l} & T \frac{v_{r}}{l \cos ^{2} \delta_{r}} \end{array}\right] \tilde{\boldsymbol{u}}(k)=\boldsymbol{A} \tilde{\boldsymbol{x}}(k)+\boldsymbol{B} \tilde{\boldsymbol{u}}(k) x~(k+1)=⎣ ⎡​100​010​−Tvr​sinφr​Tvr​cosφr​1​⎦ ⎤​x~(k)+⎣ ⎡​Tcosφr​Tsinφr​Tltanδr​​​00Tlcos2δr​vr​​​⎦ ⎤​u~(k)=Ax~(k)+Bu~(k)(5)

为了表示控制系统达到稳定控制所付出的代价，MPC控制的代价函数定义如下： min ⁡ J ( U ) = ∑ k = 0 N − 1 ( x ~ ( k ) T Q x ~ ( k ) + u ~ ( k ) T R u ~ ( k ) ) + x ~ ( N ) T Q f x ~ ( N ) (6) \tag{6} \min J(\boldsymbol{U})=\sum_{k=0}^{N-1}\left(\tilde{\boldsymbol{x}}(k)^{T} Q \tilde{\boldsymbol{x}}(k)+\tilde{\boldsymbol{u}}(k)^{T} R \tilde{\boldsymbol{u}}(k)\right)+\tilde{\boldsymbol{x}}(N)^{T} Q_{f} \tilde{\boldsymbol{x}}(N) minJ(U)=k=0∑N−1​(x~(k)TQx~(k)+u~(k)TRu~(k))+x~(N)TQf​x~(N)(6) 其中函数参数 U = ( u 0 , u 1 , … , u N ) U=\left(u_{0}, u_{1}, \ldots, u_{N}\right) U=(u0​,u1​,…,uN​) ，并且矩阵 Q , Q f , R Q, Q_{f}, R Q,Qf​,R 为正定矩阵，即 Q = Q T ≥ 0 , Q f = Q f T ≥ 0 , R = R T > 0 Q=Q^{T} \geq 0, \quad Q_{f}=Q_{f}^{T} \geq 0, \quad R=R^{T}>0 Q=QT≥0,Qf​=QfT​≥0,R=RT>0

对于公式(6)，它需要服从的约束条件包括 { 运动学模型约束—— x ~ ( k + 1 ) = A x ~ ( k ) + B u ~ ( k ) 控制量约束—— ∣ u ~ ( k ) ∣ ≤ u ~ m a x 初始状态—— x ~ ( 0 ) = x ~ 0 (7) \tag{7} \left\{ \begin{aligned} &\text{运动学模型约束——}&\tilde{\boldsymbol{x}}(k+1)=\boldsymbol{A} \tilde{\boldsymbol{x}}(k)+\boldsymbol{B} \tilde{\boldsymbol{u}}(k)\\ &\text{控制量约束——}&\left|\tilde{\boldsymbol{u}}(k)\right| \leq \tilde{\boldsymbol{u}}_{max}\\ &\text{初始状态——}&\tilde{\boldsymbol{x}}(0)=\tilde{\boldsymbol{x}}_0 \end{aligned} \right. ⎩ ⎨ ⎧​​运动学模型约束——控制量约束——初始状态——​x~(k+1)=Ax~(k)+Bu~(k)∣u~(k)∣≤u~max​x~(0)=x~0​​(7)

完整程序见GitHub仓库