MPC预测算法介绍

MPC（Model Predictive Control，模型预测控制）是一种先进的控制策略，广泛应用于工业过程控制、汽车动力系统、能源管理等领域。

下面是对MPC预测算法的简要介绍：

基本原理

1. 模型基础：MPC算法基于一个动态过程模型，该模型描述了系统的输入、输出和内部状态之间的关系。这个模型通常是线性的或非线性的，离散时间或连续时间的。

2. 预测未来：MPC算法通过模型预测未来一段时间内系统的行为。它考虑了未来的输入变化、系统约束和目标函数，以优化未来的控制动作。

3. 滚动优化：在每一个控制周期，MPC算法都会重新求解一个优化问题，这个优化问题考虑了从当前时刻到未来有限时间范围内的系统行为。通过优化目标函数（通常涉及跟踪参考轨迹、最小化控制输入的变化等），算法计算出最优的控制输入序列。

4. 执行当前控制：尽管MPC算法计算出的是未来一段时间内的最优控制输入序列，但实际执行时，只应用序列中的第一个控制输入。在下一个控制周期，算法会根据新的系统状态重新进行优化。

关键组成部分

• 预测模型：用于预测系统未来行为的数学模型。
• 目标函数：用于评估控制策略的性能，通常包括跟踪误差、控制输入的惩罚项等。
• 约束处理：包括输入、输出、状态和动态约束，确保控制策略的可行性。

优势与应用

• 优势：MPC算法能够处理多变量控制问题，同时考虑硬约束和软约束，提供了一种灵活的控制策略。
• 应用：广泛应用于化工、电力、汽车、航空航天等行业，特别是在那些需要精确控制、操作变量之间存在强耦合、系统动态变化复杂的场合。

挑战

• 计算复杂性：MPC算法通常涉及求解一个优化问题，这可能会带来较高的计算负担。
• 模型准确性：算法的性能很大程度上依赖于预测模型的准确性，模型不准确可能导致控制性能下降。

MPC预测算法是一种强大的控制策略，它通过不断优化和调整控制输入，使得系统输出能够跟踪期望的轨迹，同时满足各种约束条件。随着计算能力的提升和算法的改进，MPC在工业和学术领域中的应用将会更加广泛。

========================================

 source：

source： ；  模型预测控制MPC详解（附带案例实现）-CSDN博客

模型预测控制MPC详解（附带案例实现）

文章目录

• 模型预测控制MPC详解（附带案例实现）
• 1. 最优控制问题
• 2. 什么是MPC
• 3. 二次规划Quadratic Programming
• 4. MPC为什么可以转换成QP问题（推导过程）
• 5. MPC总结
• 5.1 MPC的优势劣势
• 5.2 MPC的衍生算法
• 6. 示例实现
• Reference

 

写在前面本文是记录学习B站博主Dr.can的学习笔记，如有侵权请联系笔者删除此文。

1. 最优控制问题

最优控制问题就是研究在约束条件下达到最优的系统表现，通常系统的表现是综合分析的结果。比如考虑一个单输入单输出的系统（SISO），状态变量x xx，输出为y yy，要求其输出能跟踪预设的参考值r rr，误差可以表示为e = y − r e=y-re=y−r，那么最优控制的目标是

min ⁡ ∫ 0 t e 2 d t \min \int_0^t e^2 dtmin∫0t​e2dt

如果同时希望输入量u uu也能越小越好（一般的目的是减少能耗），那最优控制的目标可以是

min ⁡ ∫ 0 t q × e 2 d t + r × u 2 d t \min \int_0^t q\times e^2 dt + r\times u^2 dtmin∫0t​q×e2dt+r×u2dt

其中q , r q,rq,r分别是权重参数，用于调节两个目标的重要性。

考虑一个多输入多输出的系统（MIMO），系统的模型为：

d X d t = A X + B U Y = C X

dXdtY=AX+BU=CXdXdt=AX+BUY=CX

dtdX​Y​=AX+BU=CX​

那么可以将上述的最优化目标改写为：

J = ∫ 0 t E T Q E + U T R U d t J = \int^t_0 E^T Q E + U^T R U dtJ=∫0t​ETQE+UTRUdt

举一个实际的例子，系统的模型如下所示：

d d t [ x 1 x 2 ] = A [ x 1 x 2 ] + B [ u 1 u 2 ] [ y 1 y 2 ] = [ x 1 x 2 ]

ddt[x1x2][y1y2]=A[x1x2]+B[u1u2]=[x1x2]ddt[x1x2]=A[x1x2]+B[u1u2][y1y2]=[x1x2]

dtd​[x1​x2​​][y1​y2​​]​=A[x1​x2​​]+B[u1​u2​​]=[x1​x2​​]​

设置的参考值R RR为:

R = [ r 1 r 2 ] = [ 0 0 ] R =

[r1r2][r1r2]

=

[00][00]

R=[r1​r2​​]=[00​]

那么可以推导出误差E EE为：

E = [ e 1 e 2 ] = [ y 1 − r 1 y 2 − r 2 ] = [ x 1 x 2 ] E =

[e1e2][e1e2]

=

[y1−r1y2−r2][y1−r1y2−r2]

=

[x1x2][x1x2]

E=[e1​e2​​]=[y1​−r1​y2​−r2​​]=[x1​x2​​]

那么上述最优控制的目标函数可以写成：

E T Q E = [ x 1 x 2 ] T [ q 1 0 0 q 2 ] [ x 1 x 2 ] = q 1 x 1 2 + q 2 x 2 2 U T R U = [ u 1 u 2 ] T [ r 1 0 0 r 2 ] [ u 1 u 2 ] = r 1 u 1 2 + r 2 u 2 2 J = ∫ 0 t q 1 x 1 2 + q 2 x 2 2 + r 1 u 1 2 + r 2 u 2 2

ETQEUTRUJ=[x1x2]T[q100q2][x1x2]=q1x21+q2x22=[u1u2]T[r100r2][u1u2]=r1u21+r2u22=∫t0q1x21+q2x22+r1u21+r2u22ETQE=[x1x2]T[q100q2][x1x2]=q1x12+q2x22UTRU=[u1u2]T[r100r2][u1u2]=r1u12+r2u22J=∫0tq1x12+q2x22+r1u12+r2u22

ETQEUTRUJ​=[x1​x2​​]T[q1​0​0q2​​][x1​x2​​]=q1​x12​+q2​x22​=[u1​u2​​]T[r1​0​0r2​​][u1​u2​​]=r1​u12​+r2​u22​=∫0t​q1​x12​+q2​x22​+r1​u12​+r2​u22​​

2. 什么是MPC

MPC（Model Predictive Control，模型预测控制）的基本思想是通过建立一个系统的动态模型，并在每一个控制时刻使用这个模型来预测系统未来的行为。基于这些预测，它可以生成一个优化控制序列，然后通过执行第一个控制动作来调整系统状态，接着在下一个时刻重新计算和执行。这个过程反复进行，以使系统能够在未来的一段时间内优化一个特定的性能指标。

通常来说，MPC包括以下四个基本步骤：

• 系统模型化：建立描述系统动态行为的数学模型，通常是差分方程或微分方程。

• 预测：在当前时刻基于系统状态和控制输入，使用模型预测未来一段时间内的系统响应。

• 优化：基于预测的系统响应，通过求解一个优化问题来计算最优的控制输入序列，以最大化或最小化一个性能指标（如系统响应时间、能耗等）。

• 执行：根据优化得到的控制输入序列中的第一个值，执行这个控制动作，并将实际的系统状态反馈到下一个控制周期中

在具体实施的过程中MPC主要分为下列的三个步骤：

在k时刻：

• step1：估计/测量/读取当前的系统状态；
• step2：基于u k , u k + 1 , ⋯   , u k + N \bold{u}_k, \bold{u}_{k+1}, \cdots, \bold{u}_{k+N}uk​,uk+1​,⋯,uk+N​来进行最优化
  J = ∑ k N − 1 E k T Q E k + U k T R U k + E N T F E N ⏟ Terminal Cost J = \sum_k^{N-1} E_k^TQE_k + U_k^T R U_k + \underbrace{E_N^TFE_N}_{\text{Terminal Cost}}J=k∑N−1​EkT​QEk​+UkT​RUk​+Terminal CostENT​FEN​​​
  其中Terminal Cost代表了模型滑动窗口的末端的控制误差。
• step3：只取u k \bold{u}_kuk​，进行滚动优化控制。

3. 二次规划Quadratic Programming

为了能求解MPC问题，我们需要将其转换成二次规划（Quadratic Programming）的形式，对于二次规划已经有很多成熟的求解器了，我们只需要使用这些求解器就能顺利求解。

二次规划一般具有如下的形式：

min ⁡ x x T H x + f T x subject to . . .

min\boldx\boldxTH\boldx+fT\boldxsubject to...min\boldx\boldxTH\boldx+fT\boldxsubject to...

​xmin​xTHx+fTxsubject to...​

其中H HH是正定的对称矩阵，f ff，x \bold{x}x是向量。

4. MPC为什么可以转换成QP问题（推导过程）

考虑一个离散的线性系统

x ( k + 1 ) = A n × n x ( k ) + B n × p u ( k ) x n × 1 = [ x 1 x 2 ⋮ x n ] , u p × 1 = [ u 1 u 2 ⋮ u p ] \bold{x}(k+1) = A_{n\times n}\bold{x}(k) + B_{n\times p}\bold{u}(k) \\ \bold{x}_{n\times 1} =

⎡⎣⎢⎢⎢⎢x1x2⋮xn⎤⎦⎥⎥⎥⎥[x1x2⋮xn]

, \bold{u}_{p\times1} =

⎡⎣⎢⎢⎢⎢u1u2⋮up⎤⎦⎥⎥⎥⎥[u1u2⋮up]

x(k+1)=An×n​x(k)+Bn×p​u(k)xn×1​= ​x1​x2​⋮xn​​​,up×1​= ​u1​u2​⋮up​​​

假设滚动的窗口大小（预测区间，Predictive Horizon）为N NN，在k kk时刻，预测k kk时刻的输入为u ( k ∣ k ) \bold{u}(k|k)u(k∣k)，预测k + 1 k+1k+1时刻的输入为u ( k + 1 ∣ k ) \bold{u}(k+1|k)u(k+1∣k)，以此类推预测，预测区间的最后一个时刻的输入为u ( k + N − 1 ∣ k ) \bold{u}(k+N-1|k)u(k+N−1∣k)，将这些预测输入整合成一个向量

U k = [ u ( k ∣ k ) u ( k + 1 ∣ k ) ⋮ u ( k + i ∣ k ) u ( k + N − 1 ∣ k ) ] \bold{U}_k =

⎡⎣⎢⎢⎢⎢⎢⎢⎢\boldu(k|k)\boldu(k+1|k)⋮\boldu(k+i|k)\boldu(k+N−1|k)⎤⎦⎥⎥⎥⎥⎥⎥⎥[\boldu(k|k)\boldu(k+1|k)⋮\boldu(k+i|k)\boldu(k+N−1|k)]

Uk​= ​u(k∣k)u(k+1∣k)⋮u(k+i∣k)u(k+N−1∣k)​​

在k kk时刻，系统的状态为x ( k ∣ k ) \bold{x}(k|k)x(k∣k)，k + 1 k+1k+1系统的状态为x ( k + 1 ∣ k ) \bold{x}(k+1|k)x(k+1∣k)，以此类推预测，预测区间的最后一个时刻的系统的状态为x ( k + N − 1 ∣ k ) \bold{x}(k+N-1|k)x(k+N−1∣k)，然后再加上区间结束后的第一个状态x ( k + N ∣ k ) \bold{x}(k+N|k)x(k+N∣k)，将这些系统的状态整合成一个向量

X k = [ x ( k ∣ k ) x ( k + 1 ∣ k ) ⋮ x ( k + i ∣ k ) x ( k + N ∣ k ) ] \bold{X}_k =

⎡⎣⎢⎢⎢⎢⎢⎢⎢\boldx(k|k)\boldx(k+1|k)⋮\boldx(k+i|k)\boldx(k+N|k)⎤⎦⎥⎥⎥⎥⎥⎥⎥[\boldx(k|k)\boldx(k+1|k)⋮\boldx(k+i|k)\boldx(k+N|k)]

Xk​= ​x(k∣k)x(k+1∣k)⋮x(k+i∣k)x(k+N∣k)​​

假设系统的输出y = x \bold{y}=\bold{x}y=x，设定的参考值r = 0 \bold{r}=0r=0，那么系统的误差为e = y − r = x − 0 = x \bold{e}=\bold{y}-\bold{r}=\bold{x}-0 = \bold{x}e=y−r=x−0=x，为了最优化误差和最优化输入，我们可以这样表示代价函数（Cost Function)

min ⁡ u J = ∑ i = 0 N − 1 ( x ( k + i ∣ k ) T Q x ( k + i ∣ k ) + u ( k + i ∣ k ) T R u ( k + i ∣ k ) ) + x ( k + N ) T F x ( k + N ) ⏟ Terminal Cost \min_\bold{u} J = \sum^{N-1}_{i=0} \Big(\bold{x}(k+i|k)^TQ \bold{x}(k+i|k) + \bold{u}(k+i|k)^TR \bold{u}(k+i|k) \Big) + \underbrace{\bold{x}(k+N)^T F \bold{x}(k+N)}_{\text{Terminal Cost}}umin​J=i=0∑N−1​(x(k+i∣k)TQx(k+i∣k)+u(k+i∣k)TRu(k+i∣k))+Terminal Costx(k+N)TFx(k+N)​​

但是乍一看这并不是二次规划的形式，我们可以通过化简将其转化为标准的二次规划形式。

在k kk时刻，我们的系统状态可以表示为

x ( k ∣ k ) = x k x ( k + 1 ∣ k ) = A x ( k ∣ k ) + B u ( k ∣ k ) = A x k + B u ( k ∣ k ) x ( k + 2 ∣ k ) = A x ( k + 1 ∣ k ) + B u ( k ∣ k ) = A 2 x k + A B u ( k ∣ k ) + B u ( k + 1 ∣ k ) ⋮ x ( k + N ∣ k ) = A N x k + A N − 1 B u ( k ∣ k ) + ⋯ + B u ( k + N − 1 ∣ k )

\boldx(k|k)\boldx(k+1|k)\boldx(k+2|k)⋮\boldx(k+N|k)=\boldxk=A\boldx(k|k)+B\boldu(k|k)=A\boldxk+B\boldu(k|k)=A\boldx(k+1|k)+B\boldu(k|k)=A2\boldxk+AB\boldu(k|k)+B\boldu(k+1|k)=AN\boldxk+AN−1B\boldu(k|k)+⋯+B\boldu(k+N−1|k)\boldx(k|k)=\boldxk\boldx(k+1|k)=A\boldx(k|k)+B\boldu(k|k)=A\boldxk+B\boldu(k|k)\boldx(k+2|k)=A\boldx(k+1|k)+B\boldu(k|k)=A2\boldxk+AB\boldu(k|k)+B\boldu(k+1|k)⋮\boldx(k+N|k)=AN\boldxk+AN−1B\boldu(k|k)+⋯+B\boldu(k+N−1|k)

x(k∣k)x(k+1∣k)x(k+2∣k)⋮x(k+N∣k)​=xk​=Ax(k∣k)+Bu(k∣k)=Axk​+Bu(k∣k)=Ax(k+1∣k)+Bu(k∣k)=A2xk​+ABu(k∣k)+Bu(k+1∣k)=ANxk​+AN−1Bu(k∣k)+⋯+Bu(k+N−1∣k)​

左边即为X k \bold{X}_kXk​，然后将右边写成矩阵的形式

X k = [ I A A 2 ⋮ A N ] ⏟ M x k + [ 0 0 ⋯ 0 ⋮ ⋮ ⋱ ⋮ 0 0 ⋯ 0 B 0 ⋯ 0 A B B ⋯ 0 ⋮ ⋮ ⋱ ⋮ A N − 1 B A N − 2 B ⋯ B ] ⏟ C U k X k = M x k + C U k

\boldXk\boldXk=⎡⎣⎢⎢⎢⎢⎢⎢⎢IAA2⋮AN⎤⎦⎥⎥⎥⎥⎥⎥⎥M\boldxk+⎡⎣⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢0⋮0BAB⋮AN−1B0⋮00B⋮AN−2B⋯⋱⋯⋯⋯⋱⋯0⋮000⋮B⎤⎦⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥C\boldUk=M\boldxk+C\boldUk\boldXk=[IAA2⋮AN]⏟M\boldxk+[00⋯0⋮⋮⋱⋮00⋯0B0⋯0ABB⋯0⋮⋮⋱⋮AN−1BAN−2B⋯B]⏟C\boldUk\boldXk=M\boldxk+C\boldUk

Xk​Xk​​=M    ​IAA2⋮AN​​​​xk​+C    ​0⋮0BAB⋮AN−1B​0⋮00B⋮AN−2B​⋯⋱⋯⋯⋯⋱⋯​0⋮000⋮B​​​​Uk​=Mxk​+CUk​​

其中C CC的前n nn行都是0 00。接下来，我们来将代价函数化成QP的形式

J = ∑ i = 0 N − 1 ( x ( k + i ∣ k ) T Q x ( k + i ∣ k ) + u ( k + i ∣ k ) T R u ( k + i ∣ k ) ) + x ( k + N ) T F x ( k + N ) = x ( k ∣ k ) T Q x ( k ∣ k ) + x ( k + 1 ∣ k ) T Q x ( k + 1 ∣ k ) + ⋯ + x ( k + N − 1 ∣ k ) T Q x ( k + N − 1 ∣ k ) + x ( k + N ∣ k ) T Q x ( k + N ∣ k ) + ∑ i = 0 N − 1 u ( k + i ∣ k ) T R u ( k + i ∣ k ) = [ x ( k ∣ k ) x ( k + 1 ∣ k ) ⋮ x ( k + i ∣ k ) x ( k + N ∣ k ) ] T [ Q Q Q ⋱ F ] ⏟ Q ˉ [ x ( k ∣ k ) x ( k + 1 ∣ k ) ⋮ x ( k + i ∣ k ) x ( k + N ∣ k ) ] + [ u ( k ∣ k ) u ( k + 1 ∣ k ) ⋮ u ( k + i ∣ k ) u ( k + N − 1 ∣ k ) ] T [ R R R ⋱ R ] ⏟ R ˉ [ u ( k ∣ k ) u ( k + 1 ∣ k ) ⋮ u ( k + i ∣ k ) u ( k + N − 1 ∣ k ) ] = X k T Q ˉ X k + U k R ˉ U k

J=∑i=0N−1((\boldx(k+i|k)TQ\boldx(k+i|k)+\boldu(k+i|k)TR\boldu(k+i|k)))+\boldx(k+N)TF\boldx(k+N)=\boldx(k|k)TQ\boldx(k|k)+\boldx(k+1|k)TQ\boldx(k+1|k)+⋯+\boldx(k+N−1|k)TQ\boldx(k+N−1|k)+\boldx(k+N|k)TQ\boldx(k+N|k)+∑i=0N−1\boldu(k+i|k)TR\boldu(k+i|k)=⎡⎣⎢⎢⎢⎢⎢⎢⎢\boldx(k|k)\boldx(k+1|k)⋮\boldx(k+i|k)\boldx(k+N|k)⎤⎦⎥⎥⎥⎥⎥⎥⎥T⎡⎣⎢⎢⎢⎢⎢⎢⎢QQQ⋱F⎤⎦⎥⎥⎥⎥⎥⎥⎥Q¯⎡⎣⎢⎢⎢⎢⎢⎢⎢\boldx(k|k)\boldx(k+1|k)⋮\boldx(k+i|k)\boldx(k+N|k)⎤⎦⎥⎥⎥⎥⎥⎥⎥+⎡⎣⎢⎢⎢⎢⎢⎢⎢\boldu(k|k)\boldu(k+1|k)⋮\boldu(k+i|k)\boldu(k+N−1|k)⎤⎦⎥⎥⎥⎥⎥⎥⎥T⎡⎣⎢⎢⎢⎢⎢⎢⎢RRR⋱R⎤⎦⎥⎥⎥⎥⎥⎥⎥R¯⎡⎣⎢⎢⎢⎢⎢⎢⎢\boldu(k|k)\boldu(k+1|k)⋮\boldu(k+i|k)\boldu(k+N−1|k)⎤⎦⎥⎥⎥⎥⎥⎥⎥=\boldXTkQ¯\boldXk+\boldUkR¯\boldUkJ=∑i=0N−1(\boldx(k+i|k)TQ\boldx(k+i|k)+\boldu(k+i|k)TR\boldu(k+i|k))+\boldx(k+N)TF\boldx(k+N)=\boldx(k|k)TQ\boldx(k|k)+\boldx(k+1|k)TQ\boldx(k+1|k)+⋯+\boldx(k+N−1|k)TQ\boldx(k+N−1|k)+\boldx(k+N|k)TQ\boldx(k+N|k)+∑i=0N−1\boldu(k+i|k)TR\boldu(k+i|k)=[\boldx(k|k)\boldx(k+1|k)⋮\boldx(k+i|k)\boldx(k+N|k)]T[QQQ⋱F]⏟Q¯[\boldx(k|k)\boldx(k+1|k)⋮\boldx(k+i|k)\boldx(k+N|k)]+[\boldu(k|k)\boldu(k+1|k)⋮\boldu(k+i|k)\boldu(k+N−1|k)]T[RRR⋱R]⏟R¯[\boldu(k|k)\boldu(k+1|k)⋮\boldu(k+i|k)\boldu(k+N−1|k)]=\boldXkTQ¯\boldXk+\boldUkR¯\boldUk

J​=i=0∑N−1​(x(k+i∣k)TQx(k+i∣k)+u(k+i∣k)TRu(k+i∣k))+x(k+N)TFx(k+N)=x(k∣k)TQx(k∣k)+x(k+1∣k)TQx(k+1∣k)+⋯+x(k+N−1∣k)TQx(k+N−1∣k)+x(k+N∣k)TQx(k+N∣k)+i=0∑N−1​u(k+i∣k)TRu(k+i∣k)= ​x(k∣k)x(k+1∣k)⋮x(k+i∣k)x(k+N∣k)​​TQˉ​   ​Q​Q​Q​⋱​F​​​​​x(k∣k)x(k+1∣k)⋮x(k+i∣k)x(k+N∣k)​​+ ​u(k∣k)u(k+1∣k)⋮u(k+i∣k)u(k+N−1∣k)​​TRˉ    ​R​R​R​⋱​R​​​​​u(k∣k)u(k+1∣k)⋮u(k+i∣k)u(k+N−1∣k)​​=XkT​Qˉ​Xk​+Uk​RˉUk​​

然后再将前面推导的条件带入

J = X k T Q ˉ X k + U k R ˉ U k = ( M x k + C U k ) T Q ˉ ( M x k + C U k ) + U k R ˉ U k = x k T M T Q ˉ M x k + U k T C T Q ˉ M x k + x k T M T Q ˉ C ⏟ 2 x T C T Q ˉ M U k U k + U k T C T Q ˉ C U k + U k R ˉ U k = x k T M T Q ˉ M x k + 2 x T C T Q ˉ M U k U k + U k T C T Q ˉ C U k + U k R ˉ U k

J=\boldXTkQ¯\boldXk+\boldUkR¯\boldUk=(M\boldxk+C\boldUk)TQ¯(M\boldxk+C\boldUk)+\boldUkR¯\boldUk=\boldxTkMTQ¯M\boldxk+\boldUTkCTQ¯M\boldxk+\boldxTkMTQ¯C2\boldxTCTQ¯M\boldUk\boldUk+\boldUTkCTQ¯C\boldUk+\boldUkR¯\boldUk=\boldxTkMTQ¯M\boldxk+2\boldxTCTQ¯M\boldUk\boldUk+\boldUTkCTQ¯C\boldUk+\boldUkR¯\boldUkJ=\boldXkTQ¯\boldXk+\boldUkR¯\boldUk=(M\boldxk+C\boldUk)TQ¯(M\boldxk+C\boldUk)+\boldUkR¯\boldUk=\boldxkTMTQ¯M\boldxk+\boldUkTCTQ¯M\boldxk+\boldxkTMTQ¯C⏟2\boldxTCTQ¯M\boldUk\boldUk+\boldUkTCTQ¯C\boldUk+\boldUkR¯\boldUk=\boldxkTMTQ¯M\boldxk+2\boldxTCTQ¯M\boldUk\boldUk+\boldUkTCTQ¯C\boldUk+\boldUkR¯\boldUk

J​=XkT​Qˉ​Xk​+Uk​RˉUk​=(Mxk​+CUk​)TQˉ​(Mxk​+CUk​)+Uk​RˉUk​=xkT​MTQˉ​Mxk​+2xTCTQˉ​MUk​  UkT​CTQˉ​Mxk​+xkT​MTQˉ​C​​Uk​+UkT​CTQˉ​CUk​+Uk​RˉUk​=xkT​MTQˉ​Mxk​+2xTCTQˉ​MUk​Uk​+UkT​CTQˉ​CUk​+Uk​RˉUk​​

其中G = M T Q ˉ M G=M^T\bar{Q}MG=MTQˉ​M，E = C T Q ˉ M E=C^T\bar{Q}ME=CTQˉ​M，H = C T R ˉ C + R ˉ H=C^T\bar{R}C+\bar{R}H=CTRˉC+Rˉ，那么J JJ可以化简为：

J = x k T G x k + 2 U k T E x k + U k T H U k J = \bold{x}_k^T G \bold{x}_k + 2\bold{U}_k^T E \bold{x}_k + \bold{U}_k^T H \bold{U}_kJ=xkT​Gxk​+2UkT​Exk​+UkT​HUk​

其中x k T G x k \bold{x}_k^T G \bold{x}_kxkT​Gxk​是初始状态，是一个常量，在优化的过程中可以忽略。最终的代价函数可以表示为

min ⁡ U J = U k T E x k + 0.5 × U k T H U k G = M T Q ˉ M E = C T Q ˉ M H = C T R ˉ C + R ˉ Q ˉ = [ Q ⋯ ⋮ Q ⋮ ⋯ F ] , R ˉ = [ R ⋯ ⋮ ⋱ ⋮ ⋯ R ] \min_\bold{U} J = \bold{U}_k^T E \bold{x}_k + 0.5 \times\bold{U}_k^T H \bold{U}_k \\ G=M^T\bar{Q}M \\ E=C^T\bar{Q}M \\ H=C^T\bar{R}C+\bar{R} \\ \bar{Q} =

⎡⎣⎢⎢Q⋮⋯Q⋯⋮F⎤⎦⎥⎥[Q⋯⋮Q⋮⋯F]

, \bar{R} =

⎡⎣⎢⎢R⋮⋯⋱⋯⋮R⎤⎦⎥⎥[R⋯⋮⋱⋮⋯R]

Umin​J=UkT​Exk​+0.5×UkT​HUk​G=MTQˉ​ME=CTQˉ​MH=CTRˉC+RˉQˉ​= ​Q⋮​⋯Q⋯​⋮F​​,Rˉ= ​R⋮​⋯⋱⋯​⋮R​​

这就是一个标准的QP问题。

5. MPC总结

5.1 MPC的优势劣势

😄MPC的优势在于：

• 可以处理多输入多数出的系统（MIMO），PID控制只能在一个PID环内控制一个系统状态，当系统状态相互影响的时候PID控制往往难以设计，MPC就体现出了其优势
• MPC的另一个优势在于可以处理约束条件，约束很重要，因为违反它们会导致不良后果。

😄MPC的不足在于：

• MPC是在线滚动优化的，所以需要比较强的算力。

5.2 MPC的衍生算法

😄如何选择合适的MPC？

自适应MPC（Adaptive MPC）

在自适应MPC 中，线性模型是随着工作条件的变化而动态计算的，并且在每个时间步长，您都可以使用此线性模型更新。MPC控制器使用的内部被控对象模型，请注意，在自适应MPC 中，优化问题的结构在不同的工作点上保持不变。这意味着在预测范围内，状态数量和约束数量不会因不同的操作条件而改变。

增益调度MPC（Gain-scheduled MPC）

如果它们确实发生了变化，则应使用增益调度MPC。在增益调度MPC 中，您可以在感兴趣的工作点进行离线线性化，并为每个工作点设计一个线性MPC控制器。每个控制器彼此独立，因此可能具有不同数量的状态和不同数量的约束。

小结：

如果被控对象是非线性的，但可以通过线性模型逼近，则可以使用自适应MPC 控制器和如果被控对象是非线性的且状态的维度和约束的数量会发生变化，那么应该使用增益调度的MPC控制器。如果优化问题的结构在不同的工作条件下没有变化，则应使用自适应 MPC；但是，如果该结构有变化，则使用增益调度 MPC；如果因您有一个无法通过线性化进行良好逼近的高度非线性系统，从而导致以上这些方法都不起作用，则可以使用非线性MPC。具体可以如下图所示：

6. 示例实现

这个示例系统的状态方程为

[ x 1 ( k + 1 ) x 2 ( k + 1 ) ] = [ 1 0.1 − 1 2 ] [ x 1 ( k ) x 2 ( k ) ] + [ 0.2 1 0.5 2 ] [ u 1 ( k ) u 2 ( k ) ]

[x1(k+1)x2(k+1)][x1(k+1)x2(k+1)]

=

[1−10.12][10.1−12]

[x1(k)x2(k)][x1(k)x2(k)]

+

[0.20.512][0.210.52]

[u1(k)u2(k)][u1(k)u2(k)]

[x1​(k+1)x2​(k+1)​]=[1−1​0.12​][x1​(k)x2​(k)​]+[0.20.5​12​][u1​(k)u2​(k)​]

[ y 1 ( k ) y 2 ( k ) ] = [ x 1 ( k ) x 2 ( k ) ] , [ r 1 ( k ) r 2 ( k ) ] = [ 0 0 ]

[y1(k)y2(k)][y1(k)y2(k)]

=

[x1(k)x2(k)][x1(k)x2(k)]

,

[r1(k)r2(k)][r1(k)r2(k)]

=

[00][00]

[y1​(k)y2​(k)​]=[x1​(k)x2​(k)​],[r1​(k)r2​(k)​]=[00​]

Dr_can提供的matlab/octave示例代码

MPC_Test.m

<span style="color:#000000"><span style="background-color:#282c34"><code class="language-matlab"><span style="color:#5c6370">%% 清屏</span>
clear<span style="color:#999999">;</span>
close all<span style="color:#999999">;</span>
clc<span style="color:#999999">;</span>
<span style="color:#5c6370">%% 加载 optim package,若使用matlab，则注释掉此行</span>
pkg load optim<span style="color:#999999">;</span>

<span style="color:#5c6370">%%%%%%%%%%%%%%%%%%%%%%%%%%</span>
<span style="color:#5c6370">%% 第一步，定义状态空间矩阵</span>
<span style="color:#5c6370">%% 定义状态矩阵 A, n x n 矩阵</span>
A<span style="color:#669900">=</span><span style="color:#999999">[</span><span style="color:#98c379">1</span> <span style="color:#98c379">0.1</span><span style="color:#999999">;</span> <span style="color:#669900">-</span><span style="color:#98c379">1</span> <span style="color:#98c379">2</span><span style="color:#999999">]</span><span style="color:#999999">;</span>
n<span style="color:#669900">=</span><span style="color:#61aeee">size</span><span style="color:#999999">(</span>A<span style="color:#999999">,</span><span style="color:#98c379">1</span><span style="color:#999999">)</span><span style="color:#999999">;</span>
<span style="color:#5c6370">%% 定义输入矩阵 B, n x p 矩阵</span>
B<span style="color:#669900">=</span><span style="color:#999999">[</span> <span style="color:#98c379">0.2</span> <span style="color:#98c379">1</span><span style="color:#999999">;</span> <span style="color:#98c379">0.5</span> <span style="color:#98c379">2</span><span style="color:#999999">]</span><span style="color:#999999">;</span>
p<span style="color:#669900">=</span><span style="color:#61aeee">size</span><span style="color:#999999">(</span>B<span style="color:#999999">,</span><span style="color:#98c379">2</span><span style="color:#999999">)</span><span style="color:#999999">;</span>
<span style="color:#5c6370">%% 定义Q矩阵，n x n 矩阵</span>
Q<span style="color:#669900">=</span><span style="color:#999999">[</span><span style="color:#98c379">100</span> <span style="color:#98c379">0</span><span style="color:#999999">;</span><span style="color:#98c379">0</span> <span style="color:#98c379">1</span><span style="color:#999999">]</span><span style="color:#999999">;</span>
<span style="color:#5c6370">%% 定义F矩阵，n x n 矩阵</span>
F<span style="color:#669900">=</span><span style="color:#999999">[</span><span style="color:#98c379">100</span> <span style="color:#98c379">0</span><span style="color:#999999">;</span><span style="color:#98c379">0</span> <span style="color:#98c379">1</span><span style="color:#999999">]</span><span style="color:#999999">;</span>
<span style="color:#5c6370">%% 定义R矩阵，p x p 矩阵</span>
R<span style="color:#669900">=</span><span style="color:#999999">[</span><span style="color:#98c379">1</span> <span style="color:#98c379">0</span> <span style="color:#999999">;</span><span style="color:#98c379">0</span> <span style="color:#98c379">.1</span><span style="color:#999999">]</span><span style="color:#999999">;</span>
<span style="color:#5c6370">%% 定义step数量k</span>
k_steps<span style="color:#669900">=</span><span style="color:#98c379">100</span><span style="color:#999999">;</span>
<span style="color:#5c6370">%% 定义矩阵 X_K， n x k 矩 阵</span>
X_K <span style="color:#669900">=</span> <span style="color:#61aeee">zeros</span><span style="color:#999999">(</span>n<span style="color:#999999">,</span>k_steps<span style="color:#999999">)</span><span style="color:#999999">;</span>
<span style="color:#5c6370">%% 初始状态变量值， n x 1 向量</span>
<span style="color:#61aeee">X_K</span><span style="color:#999999">(</span><span style="color:#669900">:</span><span style="color:#999999">,</span><span style="color:#98c379">1</span><span style="color:#999999">)</span> <span style="color:#669900">=</span><span style="color:#999999">[</span><span style="color:#98c379">20</span><span style="color:#999999">;</span><span style="color:#669900">-</span><span style="color:#98c379">20</span><span style="color:#999999">]</span><span style="color:#999999">;</span>
<span style="color:#5c6370">%% 定义输入矩阵 U_K， p x k 矩阵</span>
U_K<span style="color:#669900">=</span><span style="color:#61aeee">zeros</span><span style="color:#999999">(</span>p<span style="color:#999999">,</span>k_steps<span style="color:#999999">)</span><span style="color:#999999">;</span>
<span style="color:#5c6370">%% 定义预测区间K</span>
N<span style="color:#669900">=</span><span style="color:#98c379">5</span><span style="color:#999999">;</span>
<span style="color:#5c6370">%% Call MPC_Matrices 函数 求得 E,H矩阵 </span>
<span style="color:#999999">[</span>E<span style="color:#999999">,</span>H<span style="color:#999999">]</span><span style="color:#669900">=</span><span style="color:#61aeee">MPC_Matrices</span><span style="color:#999999">(</span>A<span style="color:#999999">,</span>B<span style="color:#999999">,</span>Q<span style="color:#999999">,</span>R<span style="color:#999999">,</span>F<span style="color:#999999">,</span>N<span style="color:#999999">)</span><span style="color:#999999">;</span>

<span style="color:#5c6370">%% 计算每一步的状态变量的值</span>
<span style="color:#c678dd">for</span> k <span style="color:#669900">=</span> <span style="color:#98c379">1</span> <span style="color:#669900">:</span> k_steps
<span style="color:#5c6370">%% 求得U_K(:,k)</span>
<span style="color:#61aeee">U_K</span><span style="color:#999999">(</span><span style="color:#669900">:</span><span style="color:#999999">,</span>k<span style="color:#999999">)</span> <span style="color:#669900">=</span> <span style="color:#61aeee">Prediction</span><span style="color:#999999">(</span><span style="color:#61aeee">X_K</span><span style="color:#999999">(</span><span style="color:#669900">:</span><span style="color:#999999">,</span>k<span style="color:#999999">)</span><span style="color:#999999">,</span>E<span style="color:#999999">,</span>H<span style="color:#999999">,</span>N<span style="color:#999999">,</span>p<span style="color:#999999">)</span><span style="color:#999999">;</span>
<span style="color:#5c6370">%% 计算第k+1步时状态变量的值</span>
<span style="color:#61aeee">X_K</span><span style="color:#999999">(</span><span style="color:#669900">:</span><span style="color:#999999">,</span>k<span style="color:#669900">+</span><span style="color:#98c379">1</span><span style="color:#999999">)</span><span style="color:#669900">=</span><span style="color:#999999">(</span>A<span style="color:#669900">*</span><span style="color:#61aeee">X_K</span><span style="color:#999999">(</span><span style="color:#669900">:</span><span style="color:#999999">,</span>k<span style="color:#999999">)</span><span style="color:#669900">+</span>B<span style="color:#669900">*</span><span style="color:#61aeee">U_K</span><span style="color:#999999">(</span><span style="color:#669900">:</span><span style="color:#999999">,</span>k<span style="color:#999999">)</span><span style="color:#999999">)</span><span style="color:#999999">;</span>
<span style="color:#c678dd">end</span>

<span style="color:#5c6370">%% 绘制状态变量和输入的变化</span>
<span style="color:#61aeee">subplot</span><span style="color:#999999">(</span><span style="color:#98c379">2</span><span style="color:#999999">,</span> <span style="color:#98c379">1</span><span style="color:#999999">,</span> <span style="color:#98c379">1</span><span style="color:#999999">)</span><span style="color:#999999">;</span>
hold<span style="color:#999999">;</span>
<span style="color:#c678dd">for</span> <span style="color:#98c379">i</span> <span style="color:#669900">=</span><span style="color:#98c379">1</span> <span style="color:#669900">:</span><span style="color:#61aeee">size</span> <span style="color:#999999">(</span>X_K<span style="color:#999999">,</span><span style="color:#98c379">1</span><span style="color:#999999">)</span>
<span style="color:#61aeee">plot</span> <span style="color:#999999">(</span><span style="color:#61aeee">X_K</span><span style="color:#999999">(</span><span style="color:#98c379">i</span><span style="color:#999999">,</span><span style="color:#669900">:</span><span style="color:#999999">)</span><span style="color:#999999">)</span><span style="color:#999999">;</span>
<span style="color:#c678dd">end</span>

<span style="color:#61aeee">legend</span><span style="color:#999999">(</span>"x1"<span style="color:#999999">,</span>"x2"<span style="color:#999999">)</span>
hold off<span style="color:#999999">;</span>
<span style="color:#61aeee">subplot</span><span style="color:#999999">(</span><span style="color:#98c379">2</span><span style="color:#999999">,</span> <span style="color:#98c379">1</span><span style="color:#999999">,</span> <span style="color:#98c379">2</span><span style="color:#999999">)</span><span style="color:#999999">;</span>
hold<span style="color:#999999">;</span>

<span style="color:#c678dd">for</span> <span style="color:#98c379">i</span> <span style="color:#669900">=</span><span style="color:#98c379">1</span> <span style="color:#669900">:</span> <span style="color:#61aeee">size</span> <span style="color:#999999">(</span>U_K<span style="color:#999999">,</span><span style="color:#98c379">1</span><span style="color:#999999">)</span>
<span style="color:#61aeee">plot</span> <span style="color:#999999">(</span><span style="color:#61aeee">U_K</span><span style="color:#999999">(</span><span style="color:#98c379">i</span><span style="color:#999999">,</span><span style="color:#669900">:</span><span style="color:#999999">)</span><span style="color:#999999">)</span><span style="color:#999999">;</span>
<span style="color:#c678dd">end</span>

<span style="color:#61aeee">legend</span><span style="color:#999999">(</span>"u1"<span style="color:#999999">,</span>"u2"<span style="color:#999999">)</span> 
</code></span></span>

MPC_Matrices.m

<span style="color:#000000"><span style="background-color:#282c34"><code class="language-matlab"><span style="color:#c678dd">function</span> <span style="color:#999999">[</span>E <span style="color:#999999">,</span> H<span style="color:#999999">]</span><span style="color:#669900">=</span><span style="color:#61aeee">MPC_Matrices</span><span style="color:#999999">(</span>A<span style="color:#999999">,</span>B<span style="color:#999999">,</span>Q<span style="color:#999999">,</span>R<span style="color:#999999">,</span>F<span style="color:#999999">,</span>N<span style="color:#999999">)</span>
n<span style="color:#669900">=</span><span style="color:#61aeee">size</span><span style="color:#999999">(</span>A<span style="color:#999999">,</span><span style="color:#98c379">1</span><span style="color:#999999">)</span><span style="color:#999999">;</span><span style="color:#5c6370">% A 是 n x n 矩阵, 得到 n</span>
p<span style="color:#669900">=</span><span style="color:#61aeee">size</span><span style="color:#999999">(</span>B<span style="color:#999999">,</span><span style="color:#98c379">2</span><span style="color:#999999">)</span><span style="color:#999999">;</span><span style="color:#5c6370">% B 是 n x p 矩阵, 得到 p</span>
<span style="color:#5c6370">%%%%%%%%%%%%</span>
M<span style="color:#669900">=</span><span style="color:#999999">[</span><span style="color:#61aeee">eye</span><span style="color:#999999">(</span>n<span style="color:#999999">)</span><span style="color:#999999">;</span><span style="color:#61aeee">zeros</span><span style="color:#999999">(</span>N<span style="color:#669900">*</span>n<span style="color:#999999">,</span>n<span style="color:#999999">)</span><span style="color:#999999">]</span><span style="color:#999999">;</span><span style="color:#5c6370">% 初始化 M 矩阵. M 矩阵是 (N+1)n x n的，它上面是 n x n 个 "I", 这一步先把下半部分写成 0 </span>
C<span style="color:#669900">=</span><span style="color:#61aeee">zeros</span><span style="color:#999999">(</span><span style="color:#999999">(</span>N<span style="color:#669900">+</span><span style="color:#98c379">1</span><span style="color:#999999">)</span><span style="color:#669900">*</span>n<span style="color:#999999">,</span>N<span style="color:#669900">*</span>p<span style="color:#999999">)</span><span style="color:#999999">;</span> <span style="color:#5c6370">% 初始化 C 矩阵, 这一步令它有 (N+1)n x NP 个 0</span>
<span style="color:#5c6370">% 定义M 和 C </span>
tmp<span style="color:#669900">=</span><span style="color:#61aeee">eye</span><span style="color:#999999">(</span>n<span style="color:#999999">)</span><span style="color:#999999">;</span><span style="color:#5c6370">%定义一个n x n 的 I 矩阵</span>
<span style="color:#5c6370">%　更新Ｍ和C</span>
<span style="color:#c678dd">for</span> <span style="color:#98c379">i</span><span style="color:#669900">=</span><span style="color:#98c379">1</span><span style="color:#669900">:</span>N <span style="color:#5c6370">% 循环，i 从 1到 N</span>
<span style="color:#5c6370">%定义当前行数，从i x n开始，共n行</span>
rows <span style="color:#669900">=</span><span style="color:#98c379">i</span><span style="color:#669900">*</span>n<span style="color:#669900">+</span><span style="color:#999999">(</span><span style="color:#98c379">1</span><span style="color:#669900">:</span>n<span style="color:#999999">)</span><span style="color:#999999">;</span>
<span style="color:#61aeee">C</span><span style="color:#999999">(</span>rows<span style="color:#999999">,</span><span style="color:#669900">:</span><span style="color:#999999">)</span><span style="color:#669900">=</span><span style="color:#999999">[</span>tmp<span style="color:#669900">*</span>B<span style="color:#999999">,</span><span style="color:#61aeee">C</span><span style="color:#999999">(</span>rows<span style="color:#669900">-</span>n<span style="color:#999999">,</span> <span style="color:#98c379">1</span><span style="color:#669900">:</span><span style="color:#c678dd">end</span><span style="color:#669900">-</span>p<span style="color:#999999">)</span><span style="color:#999999">]</span><span style="color:#999999">;</span> <span style="color:#5c6370">%将c矩阵填满</span>
tmp<span style="color:#669900">=</span> A<span style="color:#669900">*</span>tmp<span style="color:#999999">;</span><span style="color:#5c6370">%每一次将tmp左乘一次A</span>
<span style="color:#61aeee">M</span><span style="color:#999999">(</span>rows<span style="color:#999999">,</span><span style="color:#669900">:</span><span style="color:#999999">)</span><span style="color:#669900">=</span>tmp<span style="color:#999999">;</span><span style="color:#5c6370">%将M矩阵写满</span>
<span style="color:#c678dd">end</span>

<span style="color:#5c6370">% 定义Q_bar和R_bar</span>
Q_bar <span style="color:#669900">=</span> <span style="color:#61aeee">kron</span><span style="color:#999999">(</span><span style="color:#61aeee">eye</span><span style="color:#999999">(</span>N<span style="color:#999999">)</span><span style="color:#999999">,</span>Q<span style="color:#999999">)</span><span style="color:#999999">;</span>
Q_bar <span style="color:#669900">=</span> <span style="color:#61aeee">blkdiag</span><span style="color:#999999">(</span>Q_bar<span style="color:#999999">,</span>F<span style="color:#999999">)</span><span style="color:#999999">;</span>
R_bar <span style="color:#669900">=</span> <span style="color:#61aeee">kron</span><span style="color:#999999">(</span><span style="color:#61aeee">eye</span><span style="color:#999999">(</span>N<span style="color:#999999">)</span><span style="color:#999999">,</span>R<span style="color:#999999">)</span><span style="color:#999999">;</span>

<span style="color:#5c6370">% 计算G, E, H</span>
G<span style="color:#669900">=</span>M<span style="color:#669900">'</span><span style="color:#669900">*</span>Q_bar<span style="color:#669900">*</span>M<span style="color:#999999">;</span><span style="color:#5c6370">% G: n x n</span>
E<span style="color:#669900">=</span>C<span style="color:#669900">'</span><span style="color:#669900">*</span>Q_bar<span style="color:#669900">*</span>M<span style="color:#999999">;</span><span style="color:#5c6370">% E: NP x n</span>
H<span style="color:#669900">=</span>C<span style="color:#669900">'</span><span style="color:#669900">*</span>Q_bar<span style="color:#669900">*</span>C<span style="color:#669900">+</span>R_bar<span style="color:#999999">;</span><span style="color:#5c6370">% NP x NP </span>
<span style="color:#c678dd">end</span>
</code></span></span>

Prediction.m

<span style="color:#000000"><span style="background-color:#282c34"><code class="language-matlab"><span style="color:#c678dd">function</span> u_k<span style="color:#669900">=</span> <span style="color:#61aeee">Prediction</span><span style="color:#999999">(</span>x_k<span style="color:#999999">,</span>E<span style="color:#999999">,</span>H<span style="color:#999999">,</span>N<span style="color:#999999">,</span>p<span style="color:#999999">)</span>
U_k <span style="color:#669900">=</span> <span style="color:#61aeee">zeros</span><span style="color:#999999">(</span>N<span style="color:#669900">*</span>p<span style="color:#999999">,</span><span style="color:#98c379">1</span><span style="color:#999999">)</span><span style="color:#999999">;</span> <span style="color:#5c6370">% NP x 1</span>
U_k <span style="color:#669900">=</span> <span style="color:#61aeee">quadprog</span><span style="color:#999999">(</span>H<span style="color:#999999">,</span>E<span style="color:#669900">*</span>x_k<span style="color:#999999">)</span><span style="color:#999999">;</span>
u_k <span style="color:#669900">=</span> <span style="color:#61aeee">U_k</span><span style="color:#999999">(</span><span style="color:#98c379">1</span><span style="color:#669900">:</span>p<span style="color:#999999">,</span><span style="color:#98c379">1</span><span style="color:#999999">)</span><span style="color:#999999">;</span> <span style="color:#5c6370">% 取第一个结果</span>
<span style="color:#c678dd">end</span>
</code></span></span>

仿真的结果如下

使用CasaDi Python API重新实现了Dr_can视频里提到的示例代码

<span style="color:#000000"><span style="background-color:#282c34"><code class="language-python"><span style="color:#c678dd">import</span> numpy <span style="color:#c678dd">as</span> np
<span style="color:#c678dd">import</span> matplotlib<span style="color:#999999">.</span>pyplot <span style="color:#c678dd">as</span> plt
<span style="color:#c678dd">import</span> casadi <span style="color:#c678dd">as</span> ca

<span style="color:#5c6370"># 系统参数</span>
N <span style="color:#669900">=</span> <span style="color:#98c379">5</span>  <span style="color:#5c6370"># 预测区间</span>
k_steps <span style="color:#669900">=</span> <span style="color:#98c379">100</span>

<span style="color:#5c6370"># 状态矩阵A和输入矩阵B</span>
A <span style="color:#669900">=</span> np<span style="color:#999999">.</span>array<span style="color:#999999">(</span><span style="color:#999999">[</span><span style="color:#999999">[</span><span style="color:#98c379">1</span><span style="color:#999999">,</span> <span style="color:#98c379">0.1</span><span style="color:#999999">]</span><span style="color:#999999">,</span> <span style="color:#999999">[</span><span style="color:#669900">-</span><span style="color:#98c379">1</span><span style="color:#999999">,</span> <span style="color:#98c379">2</span><span style="color:#999999">]</span><span style="color:#999999">]</span><span style="color:#999999">)</span>
B <span style="color:#669900">=</span> np<span style="color:#999999">.</span>array<span style="color:#999999">(</span><span style="color:#999999">[</span><span style="color:#999999">[</span><span style="color:#98c379">0.2</span><span style="color:#999999">,</span> <span style="color:#98c379">1</span><span style="color:#999999">]</span><span style="color:#999999">,</span> <span style="color:#999999">[</span><span style="color:#98c379">0.5</span><span style="color:#999999">,</span> <span style="color:#98c379">2</span><span style="color:#999999">]</span><span style="color:#999999">]</span><span style="color:#999999">)</span>
n <span style="color:#669900">=</span> A<span style="color:#999999">.</span>shape<span style="color:#999999">[</span><span style="color:#98c379">0</span><span style="color:#999999">]</span>
p <span style="color:#669900">=</span> B<span style="color:#999999">.</span>shape<span style="color:#999999">[</span><span style="color:#98c379">1</span><span style="color:#999999">]</span>

<span style="color:#5c6370"># Q、F、R矩阵</span>
Q <span style="color:#669900">=</span> np<span style="color:#999999">.</span>diag<span style="color:#999999">(</span><span style="color:#999999">[</span><span style="color:#98c379">100</span><span style="color:#999999">,</span> <span style="color:#98c379">1</span><span style="color:#999999">]</span><span style="color:#999999">)</span>
F <span style="color:#669900">=</span> np<span style="color:#999999">.</span>diag<span style="color:#999999">(</span><span style="color:#999999">[</span><span style="color:#98c379">100</span><span style="color:#999999">,</span> <span style="color:#98c379">1</span><span style="color:#999999">]</span><span style="color:#999999">)</span>
R <span style="color:#669900">=</span> np<span style="color:#999999">.</span>diag<span style="color:#999999">(</span><span style="color:#999999">[</span><span style="color:#98c379">1</span><span style="color:#999999">,</span> <span style="color:#98c379">0.1</span><span style="color:#999999">]</span><span style="color:#999999">)</span>

<span style="color:#5c6370"># 定义step的数量</span>
k_steps <span style="color:#669900">=</span> <span style="color:#98c379">100</span>

<span style="color:#5c6370"># 开辟所有状态x的存储空间并初始状态</span>
X_k <span style="color:#669900">=</span> np<span style="color:#999999">.</span>zeros<span style="color:#999999">(</span><span style="color:#999999">(</span>n<span style="color:#999999">,</span> k_steps<span style="color:#669900">+</span><span style="color:#98c379">1</span><span style="color:#999999">)</span><span style="color:#999999">)</span>
X_k<span style="color:#999999">[</span><span style="color:#999999">:</span><span style="color:#999999">,</span><span style="color:#98c379">0</span><span style="color:#999999">]</span> <span style="color:#669900">=</span> np<span style="color:#999999">.</span>array<span style="color:#999999">(</span><span style="color:#999999">[</span><span style="color:#98c379">20</span><span style="color:#999999">,</span><span style="color:#669900">-</span><span style="color:#98c379">20</span><span style="color:#999999">]</span><span style="color:#999999">)</span>
<span style="color:#5c6370"># 开辟所有控制输入u的存储空间</span>
U_k <span style="color:#669900">=</span> np<span style="color:#999999">.</span>zeros<span style="color:#999999">(</span><span style="color:#999999">(</span>p<span style="color:#999999">,</span> k_steps<span style="color:#999999">)</span><span style="color:#999999">)</span>

<span style="color:#5c6370"># 计算QP中代价函数相关的矩阵</span>
<span style="color:#c678dd">def</span> <span style="color:#61aeee">get_QPMatrix</span><span style="color:#999999">(</span>A<span style="color:#999999">,</span> B<span style="color:#999999">,</span> Q<span style="color:#999999">,</span> R<span style="color:#999999">,</span> F<span style="color:#999999">,</span> N<span style="color:#999999">)</span><span style="color:#999999">:</span>
    M <span style="color:#669900">=</span> np<span style="color:#999999">.</span>vstack<span style="color:#999999">(</span><span style="color:#999999">[</span>np<span style="color:#999999">.</span>eye<span style="color:#999999">(</span>n<span style="color:#999999">)</span><span style="color:#999999">,</span> np<span style="color:#999999">.</span>zeros<span style="color:#999999">(</span><span style="color:#999999">(</span>N<span style="color:#669900">*</span>n<span style="color:#999999">,</span> n<span style="color:#999999">)</span><span style="color:#999999">)</span><span style="color:#999999">]</span><span style="color:#999999">)</span>
    C <span style="color:#669900">=</span> np<span style="color:#999999">.</span>zeros<span style="color:#999999">(</span><span style="color:#999999">(</span><span style="color:#999999">(</span>N<span style="color:#669900">+</span><span style="color:#98c379">1</span><span style="color:#999999">)</span><span style="color:#669900">*</span>n<span style="color:#999999">,</span> N<span style="color:#669900">*</span>p<span style="color:#999999">)</span><span style="color:#999999">)</span>
    temp <span style="color:#669900">=</span> np<span style="color:#999999">.</span>eye<span style="color:#999999">(</span>n<span style="color:#999999">)</span>
    <span style="color:#c678dd">for</span> i <span style="color:#c678dd">in</span> <span style="color:#669900">range</span><span style="color:#999999">(</span><span style="color:#98c379">1</span><span style="color:#999999">,</span>N<span style="color:#669900">+</span><span style="color:#98c379">1</span><span style="color:#999999">)</span><span style="color:#999999">:</span>
        rows <span style="color:#669900">=</span> i <span style="color:#669900">*</span> n <span style="color:#669900">+</span> np<span style="color:#999999">.</span>arange<span style="color:#999999">(</span>n<span style="color:#999999">)</span>
        C<span style="color:#999999">[</span>rows<span style="color:#999999">,</span><span style="color:#999999">:</span><span style="color:#999999">]</span> <span style="color:#669900">=</span> np<span style="color:#999999">.</span>hstack<span style="color:#999999">(</span><span style="color:#999999">[</span>temp @ B<span style="color:#999999">,</span> C<span style="color:#999999">[</span>rows<span style="color:#669900">-</span>n<span style="color:#999999">,</span> <span style="color:#999999">:</span><span style="color:#669900">-</span>p<span style="color:#999999">]</span><span style="color:#999999">]</span><span style="color:#999999">)</span>
        temp <span style="color:#669900">=</span> A @ temp
        M<span style="color:#999999">[</span>rows<span style="color:#999999">,</span><span style="color:#999999">:</span><span style="color:#999999">]</span> <span style="color:#669900">=</span> temp

    Q_ <span style="color:#669900">=</span> np<span style="color:#999999">.</span>kron<span style="color:#999999">(</span>np<span style="color:#999999">.</span>eye<span style="color:#999999">(</span>N<span style="color:#999999">)</span><span style="color:#999999">,</span> Q<span style="color:#999999">)</span>
    rows_Q<span style="color:#999999">,</span> cols_Q <span style="color:#669900">=</span> Q_<span style="color:#999999">.</span>shape
    rows_F<span style="color:#999999">,</span> cols_F <span style="color:#669900">=</span> F<span style="color:#999999">.</span>shape
    Q_bar <span style="color:#669900">=</span> np<span style="color:#999999">.</span>zeros<span style="color:#999999">(</span><span style="color:#999999">(</span>rows_Q<span style="color:#669900">+</span>rows_F<span style="color:#999999">,</span> cols_Q<span style="color:#669900">+</span>cols_F<span style="color:#999999">)</span><span style="color:#999999">)</span>
    Q_bar<span style="color:#999999">[</span><span style="color:#999999">:</span>rows_Q<span style="color:#999999">,</span> <span style="color:#999999">:</span>cols_Q<span style="color:#999999">]</span> <span style="color:#669900">=</span> Q_
    Q_bar<span style="color:#999999">[</span>rows_Q<span style="color:#999999">:</span><span style="color:#999999">,</span> cols_Q<span style="color:#999999">:</span><span style="color:#999999">]</span> <span style="color:#669900">=</span> F
    R_bar <span style="color:#669900">=</span> np<span style="color:#999999">.</span>kron<span style="color:#999999">(</span>np<span style="color:#999999">.</span>eye<span style="color:#999999">(</span>N<span style="color:#999999">)</span><span style="color:#999999">,</span> R<span style="color:#999999">)</span>

    <span style="color:#5c6370"># G = M.T @ Q_bar @ M</span>
    E <span style="color:#669900">=</span> C<span style="color:#999999">.</span>T @ Q_bar @ M
    H <span style="color:#669900">=</span> C<span style="color:#999999">.</span>T @ Q_bar @ C <span style="color:#669900">+</span> R_bar
    <span style="color:#c678dd">return</span> E<span style="color:#999999">,</span> H


<span style="color:#5c6370"># 定义MPC优化问题</span>
<span style="color:#c678dd">def</span> <span style="color:#61aeee">mpc_prediction</span><span style="color:#999999">(</span>x_k<span style="color:#999999">,</span> E<span style="color:#999999">,</span> H<span style="color:#999999">,</span> N<span style="color:#999999">,</span> p<span style="color:#999999">)</span><span style="color:#999999">:</span>
    <span style="color:#5c6370"># 定义优化变量</span>
    U <span style="color:#669900">=</span> ca<span style="color:#999999">.</span>SX<span style="color:#999999">.</span>sym<span style="color:#999999">(</span><span style="color:#669900">'U'</span><span style="color:#999999">,</span> N <span style="color:#669900">*</span> p<span style="color:#999999">)</span>
    <span style="color:#5c6370"># 定义目标函数</span>
    objective <span style="color:#669900">=</span> <span style="color:#98c379">0.5</span> <span style="color:#669900">*</span> ca<span style="color:#999999">.</span>mtimes<span style="color:#999999">(</span><span style="color:#999999">[</span>U<span style="color:#999999">.</span>T<span style="color:#999999">,</span> H<span style="color:#999999">,</span> U<span style="color:#999999">]</span><span style="color:#999999">)</span> <span style="color:#669900">+</span> ca<span style="color:#999999">.</span>mtimes<span style="color:#999999">(</span><span style="color:#999999">[</span>U<span style="color:#999999">.</span>T<span style="color:#999999">,</span> E<span style="color:#999999">,</span> x_k<span style="color:#999999">]</span><span style="color:#999999">)</span>
    qp <span style="color:#669900">=</span> <span style="color:#999999">{</span><span style="color:#669900">'x'</span><span style="color:#999999">:</span> U<span style="color:#999999">,</span> <span style="color:#669900">'f'</span><span style="color:#999999">:</span> objective<span style="color:#999999">}</span>
    opts <span style="color:#669900">=</span> <span style="color:#999999">{</span><span style="color:#669900">'print_time'</span><span style="color:#999999">:</span> <span style="color:#56b6c2">False</span><span style="color:#999999">,</span> <span style="color:#669900">'ipopt'</span><span style="color:#999999">:</span> <span style="color:#999999">{</span><span style="color:#669900">'print_level'</span><span style="color:#999999">:</span> <span style="color:#98c379">0</span><span style="color:#999999">}</span><span style="color:#999999">}</span>
    solver <span style="color:#669900">=</span> ca<span style="color:#999999">.</span>nlpsol<span style="color:#999999">(</span><span style="color:#669900">'solver'</span><span style="color:#999999">,</span> <span style="color:#669900">'ipopt'</span><span style="color:#999999">,</span> qp<span style="color:#999999">,</span> opts<span style="color:#999999">)</span>

    <span style="color:#5c6370"># 求解问题</span>
    sol <span style="color:#669900">=</span> solver<span style="color:#999999">(</span><span style="color:#999999">)</span>
    <span style="color:#5c6370"># 提取最优解</span>
    U_k <span style="color:#669900">=</span> sol<span style="color:#999999">[</span><span style="color:#669900">'x'</span><span style="color:#999999">]</span><span style="color:#999999">.</span>full<span style="color:#999999">(</span><span style="color:#999999">)</span><span style="color:#999999">.</span>flatten<span style="color:#999999">(</span><span style="color:#999999">)</span>
    u_k <span style="color:#669900">=</span> U_k<span style="color:#999999">[</span><span style="color:#999999">:</span>p<span style="color:#999999">]</span>  <span style="color:#5c6370"># 取第一个结果</span>

    <span style="color:#c678dd">return</span> u_k

<span style="color:#c678dd">if</span> __name__ <span style="color:#669900">==</span> <span style="color:#669900">"__main__"</span><span style="color:#999999">:</span>
    <span style="color:#5c6370"># Get QP Matrix</span>
    E<span style="color:#999999">,</span> H <span style="color:#669900">=</span> get_QPMatrix<span style="color:#999999">(</span>A<span style="color:#999999">,</span> B<span style="color:#999999">,</span> Q<span style="color:#999999">,</span> R<span style="color:#999999">,</span> F<span style="color:#999999">,</span> N<span style="color:#999999">)</span>
    <span style="color:#5c6370"># Simulation</span>
    <span style="color:#c678dd">for</span> i <span style="color:#c678dd">in</span> <span style="color:#669900">range</span><span style="color:#999999">(</span>k_steps<span style="color:#999999">)</span><span style="color:#999999">:</span>
        x_k <span style="color:#669900">=</span> X_k<span style="color:#999999">[</span><span style="color:#999999">:</span><span style="color:#999999">,</span>i<span style="color:#999999">]</span>
        u_k <span style="color:#669900">=</span> mpc_prediction<span style="color:#999999">(</span>x_k<span style="color:#999999">,</span> E<span style="color:#999999">,</span> H<span style="color:#999999">,</span> N<span style="color:#999999">,</span> p<span style="color:#999999">)</span>
        x_k <span style="color:#669900">=</span> A @ x_k <span style="color:#669900">+</span> B @ u_k
        X_k<span style="color:#999999">[</span><span style="color:#999999">:</span><span style="color:#999999">,</span>i<span style="color:#669900">+</span><span style="color:#98c379">1</span><span style="color:#999999">]</span> <span style="color:#669900">=</span> x_k
        U_k<span style="color:#999999">[</span><span style="color:#999999">:</span><span style="color:#999999">,</span>i<span style="color:#999999">]</span> <span style="color:#669900">=</span> u_k

    <span style="color:#5c6370"># 绘制结果</span>
    plt<span style="color:#999999">.</span>subplot<span style="color:#999999">(</span><span style="color:#98c379">2</span><span style="color:#999999">,</span> <span style="color:#98c379">1</span><span style="color:#999999">,</span> <span style="color:#98c379">1</span><span style="color:#999999">)</span>
    <span style="color:#c678dd">for</span> i <span style="color:#c678dd">in</span> <span style="color:#669900">range</span><span style="color:#999999">(</span>X_k<span style="color:#999999">.</span>shape<span style="color:#999999">[</span><span style="color:#98c379">0</span><span style="color:#999999">]</span><span style="color:#999999">)</span><span style="color:#999999">:</span>
        plt<span style="color:#999999">.</span>plot<span style="color:#999999">(</span>X_k<span style="color:#999999">[</span>i<span style="color:#999999">,</span> <span style="color:#999999">:</span><span style="color:#999999">]</span><span style="color:#999999">,</span> label<span style="color:#669900">=</span><span style="color:#669900">f"x</span><span style="color:#999999">{</span>i<span style="color:#669900">+</span><span style="color:#98c379">1</span><span style="color:#999999">}</span><span style="color:#669900">"</span><span style="color:#999999">)</span>
    plt<span style="color:#999999">.</span>legend<span style="color:#999999">(</span><span style="color:#999999">)</span>
    plt<span style="color:#999999">.</span>title<span style="color:#999999">(</span><span style="color:#669900">"State Variables"</span><span style="color:#999999">)</span>
    plt<span style="color:#999999">.</span>xlabel<span style="color:#999999">(</span><span style="color:#669900">"Time Step"</span><span style="color:#999999">)</span>
    plt<span style="color:#999999">.</span>ylabel<span style="color:#999999">(</span><span style="color:#669900">"State Value"</span><span style="color:#999999">)</span>
    
    <span style="color:#5c6370"># 第二个子图: 控制输入</span>
    plt<span style="color:#999999">.</span>subplot<span style="color:#999999">(</span><span style="color:#98c379">2</span><span style="color:#999999">,</span> <span style="color:#98c379">1</span><span style="color:#999999">,</span> <span style="color:#98c379">2</span><span style="color:#999999">)</span>
    <span style="color:#c678dd">for</span> i <span style="color:#c678dd">in</span> <span style="color:#669900">range</span><span style="color:#999999">(</span>U_k<span style="color:#999999">.</span>shape<span style="color:#999999">[</span><span style="color:#98c379">0</span><span style="color:#999999">]</span><span style="color:#999999">)</span><span style="color:#999999">:</span>
        plt<span style="color:#999999">.</span>plot<span style="color:#999999">(</span>U_k<span style="color:#999999">[</span>i<span style="color:#999999">,</span> <span style="color:#999999">:</span><span style="color:#999999">]</span><span style="color:#999999">,</span> label<span style="color:#669900">=</span><span style="color:#669900">f"u</span><span style="color:#999999">{</span>i<span style="color:#669900">+</span><span style="color:#98c379">1</span><span style="color:#999999">}</span><span style="color:#669900">"</span><span style="color:#999999">)</span>
    plt<span style="color:#999999">.</span>legend<span style="color:#999999">(</span><span style="color:#999999">)</span>
    plt<span style="color:#999999">.</span>title<span style="color:#999999">(</span><span style="color:#669900">"Control Inputs"</span><span style="color:#999999">)</span>
    plt<span style="color:#999999">.</span>xlabel<span style="color:#999999">(</span><span style="color:#669900">"Time Step"</span><span style="color:#999999">)</span>
    plt<span style="color:#999999">.</span>ylabel<span style="color:#999999">(</span><span style="color:#669900">"Control Value"</span><span style="color:#999999">)</span>
    
    <span style="color:#5c6370"># 调整布局并显示</span>
    plt<span style="color:#999999">.</span>tight_layout<span style="color:#999999">(</span><span style="color:#999999">)</span>
    plt<span style="color:#999999">.</span>show<span style="color:#999999">(</span><span style="color:#999999">)</span>
</code></span></span>

结果如下

Reference

[1]MPC模型预测控制器
[2]Understanding Model Predictive Control
[3]CasADi_MPC_MHE_Python
[4]MPC-and-MHE-implementation-in-MATLAB-using-Casadi
[5]MPC and MHE implementation in Matlab using Casadi