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

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

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

1. 最优控制问题

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

$$\min {\int }_0^t{e}^{2}dt$$

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

$$\min {\int }_0^{t}q\times e^{2}dt+r\times u^{2}dt$$

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

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

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

$$\begin{align*} \frac{dX}{dt} & = AX + BU \\ Y & = CX \end{align*}$$ dtdX​Y​=AX+BU=CX​

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

$$J={\int }_0^t{E}^{T}QE+U^{T}RUdt$$

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

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 ]

$$\begin{align*} \frac{d}{dt} \begin{bmatrix} x_1 \\ x_2 \end{bmatrix} & = A \begin{bmatrix} x_1 \\ x_2 \end{bmatrix} + B \begin{bmatrix} u_1 \\ u_2 \end{bmatrix} \\ \begin{bmatrix} y_1 \\ y_2 \end{bmatrix} & = \begin{bmatrix} x_1 \\ x_2 \end{bmatrix} \end{align*}$$ dtd​[x1​x2​​][y1​y2​​]​=A[x1​x2​​]+B[u1​u2​​]=[x1​x2​​]​

设置的参考值$R$为:

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

$$\begin{bmatrix} r_1 \\ r_2 \end{bmatrix}$$ = $$\begin{bmatrix} 0 \\ 0 \end{bmatrix}$$ R=[r1​r2​​]=[00​]

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

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

$$\begin{bmatrix} e_1 \\ e_2 \end{bmatrix}$$ = $$\begin{bmatrix} y_1 - r_1 \\ y_2 - r_2 \end{bmatrix}$$ = $$\begin{bmatrix} x_1 \\ x_2 \end{bmatrix}$$ 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

$$\begin{align*} E^TQE & = \begin{bmatrix} x_1 \\ x_2 \end{bmatrix}^T \begin{bmatrix} q_1 & 0 \\ 0 & q_2 \end{bmatrix}\begin{bmatrix} x_1 \\ x_2 \end{bmatrix} = q_1 x_1^2 + q_2 x_2^2 \\ U^TRU & = \begin{bmatrix} u_1 \\ u_2 \end{bmatrix}^T \begin{bmatrix} r_1 & 0 \\ 0 & r_2 \end{bmatrix}\begin{bmatrix} u_1 \\ u_2 \end{bmatrix} = r_1 u_1^2 +r_2 u_2^2 \\ J & = \int^t_0 q_1 x_1^2 + q_2 x_2^2 + r_1 u_1^2 + r_2 u_2^2 \end{align*}$$ 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：基于${\mathbf{u}}_k,{\mathbf{u}}_{k+1},\cdots ,{\mathbf{u}}_{k+N}$来进行最优化
  $$J=\sum _k^{N-1}{E}_k^{T}Q{E}_k+U_k^{T}R{U}_k+\underset{\text{Terminal Cost}}{\underbrace{E_N^{T}F{E}_N}}$$
  其中Terminal Cost代表了模型滑动窗口的末端的控制误差。
• step3：只取${\mathbf{u}}_k$，进行滚动优化控制。

3. 二次规划Quadratic Programming

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

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

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

$$\begin{align*} & \min_\bold{x} \bold{x}^TH\bold{x} + f^T\bold{x} \\ & \text{subject to} \quad... \end{align*}$$ ​xmin​xTHx+fTxsubject to...​

其中$H$是正定的对称矩阵，$f$，$\mathbf{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} =

$$\begin{bmatrix} x_1 \\ x_2 \\ \vdots \\ x_n \end{bmatrix}$$ , \bold{u}_{p\times1} = $$\begin{bmatrix} u_1 \\ u_2 \\ \vdots \\ u_p \end{bmatrix}$$ x(k+1)=An×n​x(k)+Bn×p​u(k)xn×1​= ​x1​x2​⋮xn​​ ​,up×1​= ​u1​u2​⋮up​​ ​

假设滚动的窗口大小（预测区间，Predictive Horizon）为$N$，在$k$时刻，预测$k$时刻的输入为$\mathbf{u}(k|k)$，预测$k+1$时刻的输入为$\mathbf{u}(k+1|k)$，以此类推预测，预测区间的最后一个时刻的输入为$\mathbf{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 =

$$\begin{bmatrix} \bold{u}(k|k) \\ \bold{u}(k+1|k) \\ \vdots \\ \bold{u}(k+i|k) \\ \bold{u}(k+N-1|k) \end{bmatrix}$$ Uk​= ​u(k∣k)u(k+1∣k)⋮u(k+i∣k)u(k+N−1∣k)​ ​

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

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

$$\begin{bmatrix} \bold{x}(k|k) \\ \bold{x}(k+1|k) \\ \vdots \\ \bold{x}(k+i|k) \\ \bold{x}(k+N|k) \end{bmatrix}$$ Xk​= ​x(k∣k)x(k+1∣k)⋮x(k+i∣k)x(k+N∣k)​ ​

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

$$\underset{\mathbf{u}}{\min }J=\sum _{i=0}^{N-1}(\mathbf{x}(k+i|k{)}^{T}Q\mathbf{x}(k+i|k)+\mathbf{u}(k+i|k{)}^{T}R\mathbf{u}(k+i|k))+\underset{\text{Terminal Cost}}{\underbrace{\mathbf{x}(k+N{)}^{T}F\mathbf{x}(k+N)}}$$

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

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

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 )

$$\begin{align*} \bold{x}(k|k) & = \bold{x}_k \\ \bold{x}(k+1|k) & = A \bold{x}(k|k) + B\bold{u}(k|k) = A\bold{x}_k + B\bold{u}(k|k) \\ \bold{x}(k+2|k) & = A \bold{x}(k+1|k) + B\bold{u}(k|k) = A^2\bold{x}_k + AB\bold{u}(k|k) + B\bold{u}(k+1|k) \\ \vdots \\ \bold{x}(k+N|k) & = A^N \bold{x}_k + A^{N-1}B\bold{u}(k|k) + \cdots + B \bold{u}(k+N-1|k) \end{align*}$$ 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)​

左边即为${\mathbf{X}}_k$，然后将右边写成矩阵的形式

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

$$\begin{align*} \bold{X}_k & = \underbrace{\begin{bmatrix} I \\ A \\ A^2 \\ \vdots \\ A^N \end{bmatrix}}_{M} \bold{x}_k + \underbrace{\begin{bmatrix} 0 & 0 & \cdots & 0 \\ \vdots & \vdots & \ddots & \vdots \\ 0 & 0 & \cdots & 0 \\ B & 0 & \cdots & 0 \\ AB & B & \cdots & 0 \\ \vdots & \vdots & \ddots & \vdots \\ A^{N-1}B & A^{N-2}B & \cdots & B \end{bmatrix}}_{C} \bold{U}_k \\ \bold{X}_k & = M \bold{x}_k + C \bold{U}_k \end{align*}$$ Xk​Xk​​=M ​IAA2⋮AN​ ​​​xk​+C ​0⋮0BAB⋮AN−1B​0⋮00B⋮AN−2B​⋯⋱⋯⋯⋯⋱⋯​0⋮000⋮B​ ​​​Uk​=Mxk​+CUk​​

其中$M$的前$n$行都是$0$。接下来，我们来将代价函数化成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

$$\begin{align*} 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) + \bold{x}(k+N)^T F \bold{x}(k+N) \\ & = \bold{x}(k|k)^TQ\bold{x}(k|k) + \bold{x}(k+1|k)^TQ\bold{x}(k+1|k) + \cdots + \bold{x}(k+N-1|k)^TQ\bold{x}(k+N-1|k) + \bold{x}(k+N|k)^TQ\bold{x}(k+N|k) \\ & + \sum^{N-1}_{i=0}\bold{u}(k+i|k)^TR\bold{u}(k+i|k) \\ & = \begin{bmatrix} \bold{x}(k|k) \\ \bold{x}(k+1|k) \\ \vdots \\ \bold{x}(k+i|k) \\ \bold{x}(k+N|k) \end{bmatrix}^T \underbrace{\begin{bmatrix} Q & & & &\\ & Q & & &\\ & & Q & &\\ & & & \ddots & \\ & & & & F \end{bmatrix}}_{\bar{Q}}\begin{bmatrix} \bold{x}(k|k) \\ \bold{x}(k+1|k) \\ \vdots \\ \bold{x}(k+i|k) \\ \bold{x}(k+N|k) \end{bmatrix} + \begin{bmatrix} \bold{u}(k|k) \\ \bold{u}(k+1|k) \\ \vdots \\ \bold{u}(k+i|k) \\ \bold{u}(k+N-1|k) \end{bmatrix}^T \underbrace{\begin{bmatrix} R & & & &\\ & R & & &\\ & & R & &\\ & & & \ddots & \\ & & & & R \end{bmatrix}}_{\bar{R}}\begin{bmatrix} \bold{u}(k|k) \\ \bold{u}(k+1|k) \\ \vdots \\ \bold{u}(k+i|k) \\ \bold{u}(k+N-1|k) \end{bmatrix} \\ & = \bold{X}_k^T \bar{Q} \bold{X}_k + \bold{U}_k \bar{R} \bold{U}_k \end{align*}$$ 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

$$\begin{align*} J & = \bold{X}_k^T \bar{Q} \bold{X}_k + \bold{U}_k \bar{R} \bold{U}_k \\ & = (M \bold{x}_k + C \bold{U}_k)^T \bar{Q}(M \bold{x}_k + C \bold{U}_k) + \bold{U}_k \bar{R} \bold{U}_k \\ & = \bold{x}_k^TM^T\bar{Q}M\bold{x}_k + \underbrace{\bold{U}_k^TC^T\bar{Q}M\bold{x}_k + \bold{x}_k^TM^T\bar{Q}C}_{2\bold{x}^TC^T\bar{Q}M\bold{U}_k} \bold{U}_k + \bold{U}_k^TC^T\bar{Q}C \bold{U}_k + \bold{U}_k \bar{R} \bold{U}_k \\ & = \bold{x}_k^TM^T\bar{Q}M\bold{x}_k + 2\bold{x}^TC^T\bar{Q}M\bold{U}_k \bold{U}_k + \bold{U}_k^TC^T\bar{Q}C \bold{U}_k + \bold{U}_k \bar{R} \bold{U}_k \end{align*}$$ 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\bar{Q}M$，$E=C^T\bar{Q}M$，$H=C^T\bar{R}C+\bar{R}$，那么$J$可以化简为：

$$J={\mathbf{x}}_k^{T}G{\mathbf{x}}_k+2{\mathbf{U}}_k^{T}E{\mathbf{x}}_k+{\mathbf{U}}_k^{T}H{\mathbf{U}}_k$$

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

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} =

$$\begin{bmatrix} Q & \cdots & \\ \vdots & Q & \vdots \\ & \cdots & F \end{bmatrix}$$ , \bar{R} = $$\begin{bmatrix} R & \cdots & \\ \vdots & \ddots & \vdots \\ & \cdots & R \end{bmatrix}$$ 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的优势劣势

本文内容由网友自发贡献，转载请注明出处：【wpsshop博客】