MPC-模型预测控制
MPC推导过程梳理,在此记录。
DR_CAN老师原视频链接:【MPC模型预测控制器】。
状态方程
离散系统状态方程
x
(
k
+
1
)
=
A
x
(
k
)
+
B
u
(
k
)
(1)
\mathbf{x}(k+1)=\mathbf{A}\mathbf{x}(k)+\mathbf{B}\mathbf{u}(k) \tag {1}
x(k+1)=Ax(k)+Bu(k)(1)
其中
x
m
×
1
=
(
x
1
x
2
⋯
x
m
)
T
u
n
×
1
=
(
u
1
u
2
⋯
u
n
)
T
A
m
×
m
B
m
×
n
(2)
\mathop{\mathbf{x}}\limits_{m\times 1} = \begin{pmatrix} x_1 & x_2 & \cdots & x_m \end{pmatrix}^T\\ \mathop{\mathbf{u}}\limits_{n\times 1} = \begin{pmatrix} u_1 & u_2 & \cdots & u_n \end{pmatrix}^T\\ \mathop{\mathbf{A}}\limits_{m \times m}\\ \mathop{\mathbf{B}}\limits_{m\times n} \tag{2}
m×1x=(x1x2⋯xm)Tn×1u=(u1u2⋯un)Tm×mAm×nB(2)
在第
k
k
k时刻,对未来
N
N
N步进行预测,采样时间间隔为
Δ
t
\Delta t
Δt
U
k
n
N
×
1
=
(
u
(
k
∣
k
)
u
(
k
+
1
∣
k
)
⋮
u
(
k
+
i
∣
k
)
⋮
u
(
k
+
N
−
1
∣
k
)
)
X
k
m
(
N
+
1
)
×
1
=
(
x
(
k
∣
k
)
x
(
k
+
1
∣
k
)
⋮
x
(
k
+
i
∣
k
)
⋮
x
(
k
+
N
∣
k
)
)
(3)
\mathop{\mathbb{U}_k}\limits_{nN\times 1}=\begin{pmatrix} \mathbf{u}(k|k)\\ \mathbf{u}(k+1|k)\\ \vdots\\ \mathbf{u}(k+i|k)\\ \vdots\\ \mathbf{u}(k+N-1|k) \end{pmatrix} \mathop{\mathbb{X}_k}\limits_{m(N+1)\times 1}= \begin{pmatrix}\ \mathbf{x}(k|k)\\ \mathbf{x}(k+1|k)\\ \vdots\\ \mathbf{x}(k+i|k)\\ \vdots\\ \mathbf{x}(k+N|k)\\ \end{pmatrix}\tag{3}
nN×1Uk=⎝
⎛u(k∣k)u(k+1∣k)⋮u(k+i∣k)⋮u(k+N−1∣k)⎠
⎞m(N+1)×1Xk=⎝
⎛ x(k∣k)x(k+1∣k)⋮x(k+i∣k)⋮x(k+N∣k)⎠
⎞(3)
为简化问题,输出量等于状态量
y
=
x
\mathbf{y}=\mathbf{x}
y=x,参考量
R
=
0
\mathbf{R}=\mathbf{0}
R=0,误差可以表达为:
E
=
y
−
R
=
x
\mathbf{E} = \mathbf{y} - \mathbf{R} = \mathbf{x}
E=y−R=x
代价函数由误差代价加权和,输入代价加权和,终端误差加权和组成:
J
=
∑
i
=
0
N
−
1
[
x
(
k
+
i
∣
k
)
T
Q
i
x
(
k
+
i
∣
k
)
+
u
(
k
+
i
∣
k
)
T
R
i
u
(
k
+
i
∣
k
)
]
+
x
(
k
+
N
∣
k
)
T
Q
N
x
(
k
+
N
∣
k
)
(4)
J = \sum_{i=0}^{N-1}\left[\mathbf{x}(k+i|k)^T \mathbf{Q}_i \mathbf{x}(k+i|k) + \mathbf{u}(k+i|k)^T \mathbf{R}_i \mathbf{u}(k+i|k)\right] + \mathbf{x}(k+N|k)^T\mathbf{Q}_N\mathbf{x}(k+N|k) \tag{4}
J=i=0∑N−1[x(k+i∣k)TQix(k+i∣k)+u(k+i∣k)TRiu(k+i∣k)]+x(k+N∣k)TQNx(k+N∣k)(4)
问题描述
二次规划的一般形式
min
x
f
(
x
)
=
1
2
x
T
H
x
+
c
T
x
,
x
∈
R
n
s
.
t
.
A
x
≤
b
C
x
=
d
(5)
\begin{align} &&\min_{\mathbf{x}} f(\mathbf{x}) = \frac{1}{2}\mathbf{x}^T \mathbf{H} \mathbf{x} + \mathbf{c}^T\mathbf{x}, & \mathbf{x} \in \mathbb{R}^n\\ &s.t. & \mathbf{A}\mathbf{x} \le \mathbf{b}\\ & & \mathbf{C}\mathbf{x} = \mathbf{d}\\ \end{align}\tag {5}
s.t.xminf(x)=21xTHx+cTx,Ax≤bCx=dx∈Rn(5)
在下一部分内容中,将MPC问题标准化为二次规划的一般形式。
标准化
对(3)进行化简
X
k
=
(
x
(
k
∣
k
)
x
(
k
+
1
∣
k
)
x
(
k
+
2
∣
k
)
⋮
x
(
k
+
N
∣
k
)
)
=
(
x
k
A
x
(
k
∣
k
)
+
B
u
(
k
∣
k
)
=
A
x
k
+
B
u
(
k
∣
k
)
A
x
(
k
+
1
∣
k
)
+
B
u
(
k
+
1
∣
k
)
=
A
2
x
k
+
A
B
u
(
k
∣
k
)
+
B
u
(
k
+
1
∣
k
)
⋮
A
N
x
k
+
A
N
−
1
B
u
(
k
∣
k
)
+
⋯
+
B
u
(
k
+
N
−
1
∣
k
)
)
(6)
\begin{align} \mathop{\mathbb{X}_k}=\begin{pmatrix} \mathbf{x}(k|k)\\ \mathbf{x}(k+1|k)\\ \mathbf{x}(k+2|k)\\ \vdots\\ \mathbf{x}(k+N|k) \end{pmatrix}=\begin{pmatrix}\ \mathbf{x}_k\\ \mathbf{A}\mathbf{x}(k|k) + \mathbf{B}\mathbf{u}(k|k) = \mathbf{A}\mathbf{x}_k+\mathbf{B}\mathbf{u}(k|k)\\ \mathbf{A}\mathbf{x}(k+1|k) + \mathbf{B}\mathbf{u}(k+1|k) = \mathbf{A}^2\mathbf{x}_k+\mathbf{AB}\mathbf{u}(k|k) + \mathbf{B}\mathbf{u}(k+1|k)\\ \vdots\\ \mathbf{A}^N\mathbf{x}_k+\mathbf{A}^{N-1}\mathbf{B}\mathbf{u}(k|k) + \cdots + \mathbf{B}\mathbf{u}(k+N-1|k)\\ \end{pmatrix} \end{align} \tag {6}
Xk=⎝
⎛x(k∣k)x(k+1∣k)x(k+2∣k)⋮x(k+N∣k)⎠
⎞=⎝
⎛ xkAx(k∣k)+Bu(k∣k)=Axk+Bu(k∣k)Ax(k+1∣k)+Bu(k+1∣k)=A2xk+ABu(k∣k)+Bu(k+1∣k)⋮ANxk+AN−1Bu(k∣k)+⋯+Bu(k+N−1∣k)⎠
⎞(6)
即
X
k
n
N
×
1
=
(
I
A
A
2
⋮
A
N
)
m
(
N
+
1
)
×
m
x
k
m
×
1
+
(
0
0
0
⋯
0
B
0
0
⋯
0
A
B
B
0
⋯
0
A
2
B
A
B
B
⋯
0
⋮
⋮
⋮
⋱
⋮
A
N
−
1
B
A
N
−
2
B
A
N
−
3
B
⋯
0
)
m
(
N
+
1
)
×
n
N
(
u
(
k
∣
k
)
u
(
k
+
1
∣
k
)
u
(
k
+
2
∣
k
)
⋮
u
(
k
+
N
−
1
∣
k
)
)
n
N
×
1
(7)
\begin{align}\mathop{\mathbb{X}_k}_{nN\times 1}= \mathop{\begin{pmatrix}\ \mathbf{I}\\ \mathbf{A}\\ \mathbf{A}^2\\ \vdots\\ \mathbf{A}^N \end{pmatrix}}\limits_{m(N+1) \times m} \mathop{\mathbf{x}_k}\limits_{m\times 1}+\mathop{\begin{pmatrix}\ \mathbf{0} & \mathbf{0} & \mathbf{0} & \cdots & \mathbf{0} &\\ \mathbf{B} & \mathbf{0} & \mathbf{0} & \cdots & \mathbf{0} &\\ \mathbf{AB} & \mathbf{B} & \mathbf{0} & \cdots & \mathbf{0} &\\ \mathbf{A^2B} & \mathbf{AB} & \mathbf{B} & \cdots & \mathbf{0} &\\ \vdots & \vdots & \vdots & \ddots & \vdots &\\ \mathbf{A^{N-1}B} & \mathbf{A^{N-2}B} & \mathbf{A^{N-3}B} & \cdots & \mathbf{0} \end{pmatrix}}\limits_{m(N+1) \times nN} \mathop{ \begin{pmatrix}\ \mathbf{u}(k|k)\\ \mathbf{u}(k+1|k)\\ \mathbf{u}(k+2|k)\\ \vdots\\ \mathbf{u}(k+N-1|k)\\ \end{pmatrix} }\limits_{nN\times 1} \end{align}\tag {7}
XknN×1=m(N+1)×m⎝
⎛ IAA2⋮AN⎠
⎞m×1xk+m(N+1)×nN⎝
⎛ 0BABA2B⋮AN−1B00BAB⋮AN−2B000B⋮AN−3B⋯⋯⋯⋯⋱⋯0000⋮0⎠
⎞nN×1⎝
⎛ u(k∣k)u(k+1∣k)u(k+2∣k)⋮u(k+N−1∣k)⎠
⎞(7)
表示为
X
k
=
M
x
k
+
C
U
k
(8)
\mathbb{X}_k= \mathbf{M} \mathbf{x}_k + \mathbf{C} \mathbb{U}_k \tag {8}
Xk=Mxk+CUk(8)
向量化
J
=
∑
i
=
0
N
−
1
(
x
(
k
+
i
∣
k
)
T
Q
i
x
(
k
+
i
∣
k
)
+
u
(
k
+
i
∣
k
)
T
R
u
(
k
+
i
∣
k
)
)
+
x
(
k
+
N
∣
k
)
T
Q
N
x
(
k
+
N
∣
k
)
=
(
x
(
k
∣
k
)
x
(
k
+
1
∣
k
)
⋮
x
(
k
+
N
∣
k
)
)
T
(
Q
1
Q
2
⋱
Q
N
)
(
x
(
k
∣
k
)
x
(
k
+
1
∣
k
)
⋮
x
(
k
+
N
∣
k
)
)
+
(
u
(
k
∣
k
)
u
(
k
+
1
∣
k
)
⋮
u
(
k
+
N
∣
k
)
)
T
(
R
1
R
2
⋱
R
N
)
(
u
(
k
∣
k
)
u
(
k
+
1
∣
k
)
⋮
u
(
k
+
N
∣
k
)
)
=
X
k
T
Q
X
k
+
U
k
T
R
U
k
(9)
\begin{align} J & = \sum_{i=0}^{N-1}\left(\mathbf{x}(k+i|k)^T\mathbf{Q}_i\mathbf{x}(k+i|k) + \mathbf{u}(k+i|k)^T\mathbf{R}\mathbf{u}(k+i|k)\right) + \mathbf{x}(k+N|k)^T\mathbf{Q}_N\mathbf{x}(k+N|k) \\ & = \begin{pmatrix} \mathbf{x}(k|k)\\ \mathbf{x}(k+1|k)\\ \vdots\\ \mathbf{x}(k+N|k)\\ \end{pmatrix}^T \begin{pmatrix}\ \mathbf{Q}_1 & & &\\ & \mathbf{Q}_2 & & &\\ & & \ddots & &\\ & & & \mathbf{Q}_N &\\ \end{pmatrix} \begin{pmatrix}\ \mathbf{x}(k|k)\\ \mathbf{x}(k+1|k)\\ \vdots\\ \mathbf{x}(k+N|k)\\ \end{pmatrix}\\ &+ \begin{pmatrix} \mathbf{u}(k|k)\\ \mathbf{u}(k+1|k)\\ \vdots\\ \mathbf{u}(k+N|k)\\ \end{pmatrix}^T \begin{pmatrix}\ \mathbf{R}_1 & & &\\ & \mathbf{R}_2 & & &\\ & & \ddots & &\\ & & & \mathbf{R}_N &\\ \end{pmatrix} \begin{pmatrix}\ \mathbf{u}(k|k)\\ \mathbf{u}(k+1|k)\\ \vdots\\ \mathbf{u}(k+N|k)\\ \end{pmatrix} \\ & = \mathbb{X}_k^T \mathbb{Q} \mathbb{X}_k + \mathbb{U}_k^T \mathbb{R} \mathbb{U}_k \end{align} \tag {9}
J=i=0∑N−1(x(k+i∣k)TQix(k+i∣k)+u(k+i∣k)TRu(k+i∣k))+x(k+N∣k)TQNx(k+N∣k)=⎝
⎛x(k∣k)x(k+1∣k)⋮x(k+N∣k)⎠
⎞T⎝
⎛ Q1Q2⋱QN⎠
⎞⎝
⎛ x(k∣k)x(k+1∣k)⋮x(k+N∣k)⎠
⎞+⎝
⎛u(k∣k)u(k+1∣k)⋮u(k+N∣k)⎠
⎞T⎝
⎛ R1R2⋱RN⎠
⎞⎝
⎛ u(k∣k)u(k+1∣k)⋮u(k+N∣k)⎠
⎞=XkTQXk+UkTRUk(9)
将代价函数表示为
U
\mathbb{U}
U的函数形式
J
(
U
k
)
=
X
k
T
Q
X
k
+
U
k
T
R
U
k
=
(
M
x
k
+
C
U
k
)
T
Q
(
M
x
k
+
C
U
k
)
+
U
k
T
R
U
k
=
(
x
k
T
M
T
+
U
k
T
C
T
)
Q
(
M
x
k
+
C
U
k
)
+
U
k
T
R
U
k
=
x
k
T
M
T
Q
M
x
k
+
2
x
k
T
M
T
Q
C
U
k
+
U
k
T
C
T
Q
C
U
k
+
U
k
T
R
U
k
=
U
k
T
(
C
T
Q
C
+
R
)
U
k
+
2
x
k
T
M
T
Q
C
U
k
+
x
k
T
M
T
Q
M
x
k
=
U
k
T
H
U
k
+
2
x
k
T
E
U
k
+
x
k
T
G
x
k
(10)
\begin{align} J(\mathbb{U}_k) =& \mathbb{X}_k^T \mathbb{Q} \mathbb{X}_k + \mathbb{U}_k^T \mathbb{R} \mathbb{U}_k\\ =& (\mathbf{M} \mathbf{x}_k + \mathbf{C} \mathbb{U}_k)^T \mathbb{Q} (\mathbf{M} \mathbf{x}_k + \mathbf{C} \mathbb{U}_k) + \mathbb{U}_k^T \mathbb{R} \mathbb{U}_k\\ =& (\mathbf{x}_k^T\mathbf{M}^T + \mathbb{U}_k^T\mathbf{C}^T) \mathbb{Q} (\mathbf{M} \mathbf{x}_k + \mathbf{C} \mathbb{U}_k) + \mathbb{U}_k^T \mathbb{R} \mathbb{U}_k\\ =& \mathbf{x}_k^T\mathbf{M}^T\mathbb{Q} \mathbf{M} \mathbf{x}_k + 2\mathbf{x}_k^T\mathbf{M}^T \mathbb{Q} \mathbf{C} \mathbb{U}_k + \mathbb{U}_k^T\mathbf{C}^T \mathbb{Q} \mathbf{C} \mathbb{U}_k + \mathbb{U}_k^T \mathbb{R} \mathbb{U}_k\\ =& \mathbb{U}_k^T (\mathbf{C}^T \mathbb{Q} \mathbf{C} + \mathbb{R}) \mathbb{U}_k + 2\mathbf{x}_k^T\mathbf{M}^T \mathbb{Q} \mathbf{C} \mathbb{U}_k + \mathbf{x}_k^T\mathbf{M}^T\mathbb{Q} \mathbf{M} \mathbf{x}_k\\ =& \mathbb{U}_k^T \mathbb{H} \mathbb{U}_k + 2\mathbf{x}_k^T \mathbb{E} \mathbb{U}_k + \mathbf{x}_k^T \mathbb{G} \mathbf{x}_k \end{align} \tag {10}
J(Uk)======XkTQXk+UkTRUk(Mxk+CUk)TQ(Mxk+CUk)+UkTRUk(xkTMT+UkTCT)Q(Mxk+CUk)+UkTRUkxkTMTQMxk+2xkTMTQCUk+UkTCTQCUk+UkTRUkUkT(CTQC+R)Uk+2xkTMTQCUk+xkTMTQMxkUkTHUk+2xkTEUk+xkTGxk(10)
由标准形式(5)
f
(
U
k
)
=
U
k
T
H
U
k
+
2
x
k
T
E
U
k
(11)
\begin{align} f(\mathbb{U}_k) =& \mathbb{U}_k^T \mathbb{H} \mathbb{U}_k + 2\mathbf{x}_k^T \mathbb{E} \mathbb{U}_k \\ \end{align} \tag {11}
f(Uk)=UkTHUk+2xkTEUk(11)
对比(5)
min
x
f
(
x
)
=
1
2
x
T
H
x
+
c
T
x
x
=
U
k
s
.
t
.
H
=
2
H
c
T
=
2
x
k
T
E
(12)
\begin{align} &&\min_{\mathbf{x}} f(\mathbf{x}) &= \frac{1}{2}\mathbf{x}^T \mathbf{H} \mathbf{x} + \mathbf{c}^T\mathbf{x} \\ &&\mathbf{x} &= \mathbb{U}_k\\ s.t. &&\mathbf{H} &= 2\mathbb{H}\\ &&\mathbf{c}^T &= 2\mathbf{x}_k^T \mathbb{E} \end{align}\tag {12}
s.t.xminf(x)xHcT=21xTHx+cTx=Uk=2H=2xkTE(12)