MPC模型预测控制器

参考如下链接：

一个模型预测控制（MPC）的简单实现

对于以下控制模型：

x ( k + 1 ) = A x ( k ) + B u ( k ) x(k+1)=A x(k)+B u(k) x(k+1)=Ax(k)+Bu(k)

注：需要注意对应的输入矩阵和状态矩阵每个矩阵的维度

x矩阵：n*1

A矩阵：n*n

B矩阵：n*p

u矩阵：p*1

写成矩阵格式：

[ x 1 ( k + 1 ) x 2 ( k + 1 ) ] = [ 1 0.1 0 2 ] [ x 1 ( k ) x 2 ( k ) ] + [ 0 0.5 ] u ( k ) \left[\begin{array}{l}x_{1}(k+1) \\x_{2}(k+1)\end{array}\right]=\left[\begin{array}{cc}1 & 0.1 \\0 & 2\end{array}\right]\left[\begin{array}{l}x_{1}(k) \\x_{2}(k)\end{array}\right]+\left[\begin{array}{c}0 \\0.5\end{array}\right] u(k) [x1​(k+1)x2​(k+1)​]=[10​0.12​][x1​(k)x2​(k)​]+[00.5​]u(k)

此处的 x 1 x_{1} x1​和 x 2 x_{2} x2​可以理解为误差项

X ( k ) = [ x ( k ∣ k ) x ( k + 1 ∣ k ) ⋮ x ( k + j ∣ k ) ⋮ x ( k + N ∣ k ) ] X(k)=\left[\begin{array}{c}x(k \mid k) \\x(k+1 \mid k) \\\vdots\\ x(k+j \mid k)\\\vdots \\x(k+N \mid k)\end{array}\right] X(k)=⎣⎢⎢⎢⎢⎢⎢⎢⎢⎡​x(k∣k)x(k+1∣k)⋮x(k+j∣k)⋮x(k+N∣k)​⎦⎥⎥⎥⎥⎥⎥⎥⎥⎤​ U ( k ) = [ u ( k ∣ k ) u ( k + 1 ∣ k ) ⋮ u ( k + i ∣ k ) ⋮ u ( k + N − 1 ∣ k ) ] U(k)=\left[\begin{array}{c}u(k \mid k) \\u(k+1 \mid k) \\\vdots\\ u(k+i \mid k)\\\vdots \\u(k+N-1 \mid k)\end{array}\right] U(k)=⎣⎢⎢⎢⎢⎢⎢⎢⎢⎡​u(k∣k)u(k+1∣k)⋮u(k+i∣k)⋮u(k+N−1∣k)​⎦⎥⎥⎥⎥⎥⎥⎥⎥⎤​

矩阵的维度

U(k) → NP1

X(k) → (N+1)n*1

对于MPC矩阵：

x k = M x k + C u k x_{k}=M x_{k}+C u_{k} xk​=Mxk​+Cuk​

M = [ I n × n A n × n A n 2 ⋮ A N ] M=\left[\begin{array}{c}I_{n \times n} \\A_{n \times n} \\A_{n}^{2} \\\vdots \\A^{N}\end{array}\right] M=⎣⎢⎢⎢⎢⎢⎡​In×n​An×n​An2​⋮AN​⎦⎥⎥⎥⎥⎥⎤​ C = [ 0 0 … 0 ⋮ … ⋮ ⋮ 0 0 0 0 … 0 B n × p 0 … 0 A B n × p B ⋮ 0 ⋮ ⋮ B ] C=\left[\begin{array}{cccc} 0 & 0 & \ldots & 0 \\ & \vdots & \ldots & \vdots \\ \vdots & 0 & & 0 \\ 0 & & 0 & \ldots & 0 \\ B_{n \times p} & 0 & \ldots & 0 \\ A B_{n \times p} & B & \vdots & 0 \\ \vdots & \vdots & & B \end{array}\right] C=⎣⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎡​0⋮0Bn×p​ABn×p​⋮​0⋮00B⋮​……0…⋮​0⋮0…00B​0​⎦⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎤​

矩阵的维度

M → (N+1)nn

X(k) → (N+1)nN*P

X ( k ) = [ x ( k ∣ k ) x ( k + 1 ∣ k ) x ( k + 2 ∣ k ) x ( k + 3 ∣ k ) ] = [ x 1 ( k ∣ k ) x 2 ( k ∣ k ) x 1 ( k + 1 ∣ k ) x 2 ( k + 1 ∣ k ) x 1 ( k + 2 ∣ k ) x 2 ( k + 2 ∣ k ) x 1 ( k + 3 ∣ k ) x 2 ( k + 3 ∣ k ) ] X(k)=\left[\begin{array}{c}x(k \mid k) \\x(k+1 \mid k) \\x(k+2 \mid k) \\x(k+3 \mid k)\end{array}\right]=\left[\begin{array}{c}x_{1}(k \mid k) \\x_{2}(k \mid k) \\x_{1}(k+1 \mid k) \\x_{2}(k+1 \mid k) \\x_{1}(k+2 \mid k) \\x_{2}(k+2 \mid k) \\x_{1}(k+3 \mid k) \\x_{2}(k+3 \mid k)\end{array}\right] X(k)=⎣⎢⎢⎡​x(k∣k)x(k+1∣k)x(k+2∣k)x(k+3∣k)​⎦⎥⎥⎤​=⎣⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎡​x1​(k∣k)x2​(k∣k)x1​(k+1∣k)x2​(k+1∣k)x1​(k+2∣k)x2​(k+2∣k)x1​(k+3∣k)x2​(k+3∣k)​⎦⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎤​

U ( k ) = [ u ( k ∣ k ) u ( k + 1 ∣ k ) u ( k + 2 ∣ k ) ] = [ u ( k ∣ k ) u ( k + 1 ∣ k ) u ( k + 2 ∣ k ) ] U(k)=\left[\begin{array}{c}u(k \mid k) \\u(k+1 \mid k) \\u(k+2 \mid k)\end{array}\right]=\left[\begin{array}{c}\frac{u(k \mid k)}{u(k+1 \mid k)} \\u(k+2 \mid k)\end{array}\right] U(k)=⎣⎡​u(k∣k)u(k+1∣k)u(k+2∣k)​⎦⎤​=[u(k+1∣k)u(k∣k)​u(k+2∣k)​]

计算出M矩阵和C矩阵（此处省略计算过程）

之后构建二次规划模式（cost function）

J = ∑ k = 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 ∣ k ) T F x ( k + i ∣ k ) J=\sum_{k=0}^{N-1} x(k+i \mid k)^{T} Q x(k+i \mid k)+u(k+i \mid k)^{T} R u(k+i \mid k)+x(k+N \mid k)^{T} F x(k+i \mid k) J=k=0∑N−1​x(k+i∣k)TQx(k+i∣k)+u(k+i∣k)TRu(k+i∣k)+x(k+N∣k)TFx(k+i∣k)

Q、R、F表示对应输入输出的权重系数矩阵

矩阵的维度

Q → n*n

R → 1*1

F → n*n

Q矩阵影响x变量的相关性，R矩阵跟输入相关，F矩阵跟终值相关

x ( k + N ∣ k ) T F x ( k + i ∣ k ) x(k+N \mid k)^{T} F x(k+i \mid k) x(k+N∣k)TFx(k+i∣k)与系统对应的终值状态相关

简化上面的 cost function 矩阵

J = x ( k ) T G x ( k ) + U ( k ) T H U ( k ) + 2 x ( k ) T E U ( k ) J=\boldsymbol{x}(k)^{T} \boldsymbol{G} \boldsymbol{x}(k)+\boldsymbol{U}(k)^{T} \boldsymbol{H} \boldsymbol{U}(k)+2 \boldsymbol{x}(k)^{T} \boldsymbol{E} \boldsymbol{U}(k) J=x(k)TGx(k)+U(k)THU(k)+2x(k)TEU(k)

该矩阵只包含了输入项和已知的初始输入项x(k)

G = M T Q ˉ M E = C T Q ˉ M H = C T Q ˉ C + R ˉ \begin{aligned}&G=M^{T} \bar{Q} M \\&E=C^{T} \bar{Q} M \\&H=C^{T} \bar{Q} C+\bar{R}\end{aligned} ​G=MTQˉ​ME=CTQˉ​MH=CTQˉ​C+Rˉ​

Q ˉ \bar{Q} Qˉ​和 R ˉ \bar{R} Rˉ表示上面式子中的Q和R矩阵的增广形式

G、H、E矩阵跟上面的M和C矩阵相关

Q ‾ = [ Q ⋯ ⋮ Q ⋮ ⋯ F ] R ‾ = [ R ⋯ ⋮ ⋱ ⋮ ⋯ R ] \overline{\boldsymbol{Q}}=\left[\begin{array}{ccc}\boldsymbol{Q} & \cdots & \\\vdots & \boldsymbol{Q} & \vdots \\& \cdots & \boldsymbol{F}\end{array}\right] \quad \overline{\boldsymbol{R}}=\left[\begin{array}{ccc}\boldsymbol{R} & \cdots & \\\vdots & \ddots & \vdots \\& \cdots & R\end{array}\right] Q​=⎣⎢⎡​Q⋮​⋯Q⋯​⋮F​⎦⎥⎤​R=⎣⎢⎡​R⋮​⋯⋱⋯​⋮R​⎦⎥⎤​

main.m

MPC_Zero_Ref.m