模型预测控制系列(MPC)系列：2.求解MPC问题
作者：互联网
求解MPC: 在滚动时间窗内建立并求解QP问题
该小节的目标是根据上一节得到的离散误差动力学模型,在MPCD的滚动时间窗内建立并求解QP问题.
那么,我们要回答以下两个问题:
1.
什
么
是
Q
P
问
题
?
其
标
准
形
式
的
什
么
样
的
?
2.
怎
么
把
M
P
C
中
的
优
化
问
题
转
化
为
求
解
Q
P
问
题
?
\begin{aligned} &1.什么是QP问题?其标准形式的什么样的? \\ &2.怎么把MPC中的优化问题转化为求解QP问题? \end{aligned}
1.什么是QP问题?其标准形式的什么样的?2.怎么把MPC中的优化问题转化为求解QP问题?
首先简要回答第一个问题:QP(Quadratic programming),即二次规划.旨在优化(最小化或最大化)一个带线性约束的多元二次型代价函数.其标准形式为
m
i
n
x
J
=
m
i
n
x
1
2
x
T
Q
x
+
x
T
g
s
.
t
.
A
x
≤
b
\begin{aligned} &\mathop{min}\limits_{x}\quad J \quad=\quad \mathop{min}\limits_{x} &&\frac{1}{2}x^TQx+x^Tg\\ &s.t. && Ax \leq b\\ \end{aligned}
xminJ=xmins.t.21xTQx+xTgAx≤b
其中
Q
Q
Q 需为半正定矩阵,因为当且仅当
Q
⪰
0
Q \succeq 0
Q⪰0时,QP问题才是一个凸优化问题
目前众多开源的QP问题求解器如 ( O S Q P / q p O A S E S ) (OSQP/qpOASES) (OSQP/qpOASES) 以及 M a t l a b Matlab Matlab 中的 q u a d p r o g quadprog quadprog 都为以上QP问题提供了求解的API.
那么第二个问题,如何将MPC转化为求解QP问题?
通常的,MPC控制算法包含以下四个步骤:
1.根据已知的系统模型和控制序列,预测时间窗内各状态变量
2.计算在满足约束的条件下最小化损失函数
J
的控制序列
3.将第二步中优化得到的控制序列的第一个控制量输入到系统中
4.在下一次采样时间,将时间窗推动一步,重复步骤1-3
\begin{aligned} &\text{1.根据已知的系统模型和控制序列,预测时间窗内各状态变量}\\ &\text{2.计算在满足约束的条件下最小化损失函数 $J$ 的控制序列}\\ &\text{3.将第二步中优化得到的控制序列的第一个控制量输入到系统中}\\ &\text{4.在下一次采样时间,将时间窗推动一步,重复步骤1-3} \end{aligned}
1.根据已知的系统模型和控制序列,预测时间窗内各状态变量2.计算在满足约束的条件下最小化损失函数 J 的控制序列3.将第二步中优化得到的控制序列的第一个控制量输入到系统中4.在下一次采样时间,将时间窗推动一步,重复步骤1-3
步骤一中,预测状态量依赖于系统模型,因此在建立损失函数之前,需要通过差分迭代的方法得到系统的预测方程.
步骤二中,我们可以把控制目标构造为一个二次型损失函数,并把特定问题的约束条件以标准形式写出来.至此,我们就已构造了一个QP问题,接下来就是通过合适的优化方法求解.
步骤三中,只输入控制序列中的第一项可以说是MPC策略的精髓了.这么做的原因是因为前两步在特定时间窗内进行预测以及优化的过程都是开环的,在系统不确定度的影响下,这种预测是粗略的.这一点和Kalman滤波中的预测过程较为相似,如果要想解的收敛,则需要不停地将最新的观测纳入系统,对相关量进行修正.在Kalman滤波的算法中,就是通过观测量对协方差矩阵进行修正.而MPC的策略是,只将优化得到的控制序列的第一项作为系统的真实输入,在进入下一次采样时间后,利用观测量作为初始条件,重新对状态进行更新,预测,优化,即滚动时域优化.考虑到MPC的优化求解是在一个有限时间的窗口下进行的,因此是无法保障时域的全局最优.事实上,不带约束线性MPC是等价于有限时域LQR的.这种策略,是以损失了全局最优性为代价换取更大灵活性.MPC算法通过滚动时域优化,保障了最终控制收敛性,虽然每一步不一定是最优的.因此相较于如LQR/LQGD等最优控制器,MPC对于复杂非线性约束的对应更加自如,以及模型准确度要求也相对较低.
所以MPC并不强调最优,而强调预测.
综上,回答第二个问题:通过滚动时域优化的方法,将MPC算法转化为在每一个时间窗下求解一个QP问题.
预测方程
我们将离散误差动力学模型改写为以控制量增量
Δ
δ
\Delta\delta
Δδ 的输入的增量式模型.当然了你也可以不用增量式模型,直接用原有模型也是可以的.这并不影响预测方程的形式,只不过我选择了增量式模型作为例子,MPC的模型和损失函数的构造都是很灵活的.回到主题,增量式模型如下:
x
(
k
+
1
)
=
A
d
x
(
k
)
+
B
d
δ
(
k
)
+
B
c
d
ψ
˙
d
e
s
(
k
)
=
A
d
x
(
k
)
+
B
d
[
δ
(
k
−
1
)
+
Δ
δ
(
k
)
]
+
B
c
d
ψ
˙
d
e
s
(
k
)
\begin{aligned} x(k+1) &= A_dx(k)+B_d\delta(k)+B_{cd}\dot{\psi}_{des}(k) \\ &= A_dx(k)+B_d[\delta(k-1)+\Delta\delta(k)]+B_{cd}\dot{\psi}_{des}(k) \end{aligned}
x(k+1)=Adx(k)+Bdδ(k)+Bcdψ˙des(k)=Adx(k)+Bd[δ(k−1)+Δδ(k)]+Bcdψ˙des(k)
进一步的, 将以上模型改写为矩阵形式,可得
[
x
(
k
+
1
)
δ
(
k
)
]
=
[
A
d
B
d
0
I
]
[
x
(
k
)
δ
(
k
−
1
)
]
+
[
B
d
I
]
Δ
δ
(
k
)
+
[
B
c
d
0
]
ψ
˙
d
e
s
(
k
)
y
(
k
)
=
[
C
d
0
]
[
x
(
k
)
δ
(
k
−
1
)
]
\begin{aligned} &\begin{bmatrix} x(k+1) \\ \delta(k) \end{bmatrix} = \begin{bmatrix} A_d & B_d\\ 0 & I \end{bmatrix} \begin{bmatrix} x(k) \\ \delta(k-1) \end{bmatrix} + \begin{bmatrix} B_d \\ I \end{bmatrix}\Delta\delta(k)+ \begin{bmatrix} B_{cd} \\ 0 \end{bmatrix}\dot{\psi}_{des}(k)\\ \\ &y(k) = \begin{bmatrix} C_d & 0 \end{bmatrix} \begin{bmatrix} x(k) \\ \delta(k-1) \end{bmatrix} \end{aligned}
[x(k+1)δ(k)]=[Ad0BdI][x(k)δ(k−1)]+[BdI]Δδ(k)+[Bcd0]ψ˙des(k)y(k)=[Cd0][x(k)δ(k−1)]
定义
ξ
(
k
∣
t
)
=
[
x
(
k
)
δ
(
k
−
1
)
]
A
~
d
=
[
A
d
B
d
0
I
]
B
~
d
=
[
B
d
I
]
B
~
c
d
=
[
B
c
d
0
]
C
~
d
=
[
C
d
0
]
\begin{aligned} &\xi(k|t) = \begin{bmatrix} x(k) \\ \delta(k-1) \end{bmatrix} \quad \widetilde{A}_d = \begin{bmatrix} A_d & B_d\\ 0 & I \end{bmatrix} \\ &\widetilde{B}_d = \begin{bmatrix} B_d \\ I \end{bmatrix} \quad \widetilde{B}_{cd} = \begin{bmatrix} B_{cd} \\ 0 \end{bmatrix} \quad \widetilde{C}_d = \begin{bmatrix} C_d & 0 \end{bmatrix} \end{aligned}
ξ(k∣t)=[x(k)δ(k−1)]A
d=[Ad0BdI]B
d=[BdI]B
cd=[Bcd0]C
d=[Cd0]
其中, ξ ( k ∣ t ) \xi(k|t) ξ(k∣t) 代表在 t t t时刻,预测 k k k个周期得到的状态.为了简便, 在后续的使用中 ξ ( k ) \xi(k) ξ(k) 与 ξ ( k ∣ t ) \xi(k|t) ξ(k∣t) 通用.
综上,增量式离散误差动力学模型为
ξ
(
k
+
1
)
=
A
~
d
ξ
(
k
)
+
B
~
d
Δ
δ
(
k
)
+
B
~
c
d
ψ
˙
d
e
s
(
k
)
y
(
k
)
=
C
~
d
ξ
(
k
)
\begin{aligned} &\xi(k+1) = \widetilde{A}_d\xi(k)+\widetilde{B}_d\Delta\delta(k)+\widetilde{B}_{cd}\dot{\psi}_{des}(k)\\ &y(k) = \widetilde{C}_d\xi(k) \end{aligned}
ξ(k+1)=A
dξ(k)+B
dΔδ(k)+B
cdψ˙des(k)y(k)=C
dξ(k)
假设预测步数为
k
k
k,即MPC的预测时域为
k
k
k个周期,则预测方程有
ξ
(
1
)
=
A
~
d
ξ
(
0
)
+
B
~
d
Δ
δ
(
0
)
+
B
~
c
d
ψ
˙
d
e
s
(
0
)
ξ
(
2
)
=
A
~
d
ξ
(
1
)
+
B
~
d
Δ
δ
(
1
)
+
B
~
c
d
ψ
˙
d
e
s
(
1
)
=
A
~
d
[
A
~
d
ξ
(
0
)
+
B
~
d
Δ
δ
(
0
)
+
B
~
c
d
ψ
˙
d
e
s
(
0
)
]
+
B
~
d
Δ
δ
(
1
)
+
B
~
c
d
ψ
˙
d
e
s
(
1
)
=
A
~
d
2
ξ
(
0
)
+
A
~
d
B
~
d
Δ
δ
(
0
)
+
B
~
d
Δ
δ
(
1
)
+
A
~
d
B
~
c
d
ψ
˙
d
e
s
(
0
)
+
B
~
c
d
ψ
˙
d
e
s
(
1
)
ξ
(
3
)
=
A
~
d
ξ
(
2
)
+
B
~
d
Δ
δ
(
2
)
+
B
~
c
d
ψ
˙
d
e
s
(
2
)
=
A
~
d
[
A
~
d
2
ξ
(
0
)
+
A
~
d
B
~
d
Δ
δ
(
0
)
+
B
~
d
Δ
δ
(
1
)
+
A
~
d
B
~
c
d
ψ
˙
d
e
s
(
0
)
+
B
~
c
d
ψ
˙
d
e
s
(
1
)
]
+
B
~
d
Δ
δ
(
2
)
+
B
~
c
d
ψ
˙
d
e
s
(
2
)
=
A
~
d
3
ξ
(
0
)
+
A
~
d
2
B
~
d
Δ
δ
(
0
)
+
A
~
d
B
~
d
Δ
δ
(
1
)
+
B
~
d
Δ
δ
(
2
)
+
A
~
d
2
B
~
c
d
ψ
˙
d
e
s
(
0
)
+
A
~
d
B
~
c
d
ψ
˙
d
e
s
(
1
)
+
B
~
c
d
ψ
˙
d
e
s
(
2
)
.
.
.
ξ
(
k
)
=
A
~
d
(
k
)
ξ
(
0
)
+
∑
i
=
0
k
−
1
A
~
d
(
i
)
B
~
d
Δ
δ
(
k
−
i
−
1
)
+
∑
j
=
0
k
−
1
A
~
d
(
j
)
B
~
c
d
ψ
˙
d
e
s
(
k
−
j
−
1
)
y
(
k
)
=
C
~
d
ξ
(
k
)
=
C
~
d
A
~
d
(
k
)
ξ
(
0
)
+
C
~
d
∑
i
=
0
k
−
1
A
~
d
(
i
)
B
~
d
Δ
δ
(
k
−
i
−
1
)
+
C
~
d
∑
j
=
0
k
−
1
A
~
d
(
j
)
B
~
c
d
ψ
˙
d
e
s
(
k
−
j
−
1
)
\begin{aligned} &\xi(1) &&=\widetilde{A}_d\xi(0)+\widetilde{B}_d\Delta\delta(0)+\widetilde{B}_{cd}\dot{\psi}_{des}(0) \\ \\ &\xi(2) &&=\widetilde{A}_d\xi(1)+\widetilde{B}_d\Delta\delta(1)+\widetilde{B}_{cd}\dot{\psi}_{des}(1) \\ & &&=\widetilde{A}_d[\widetilde{A}_d\xi(0)+\widetilde{B}_d\Delta\delta(0)+\widetilde{B}_{cd}\dot{\psi}_{des}(0)] +\widetilde{B}_d\Delta\delta(1)+\widetilde{B}_{cd}\dot{\psi}_{des}(1) \\ & &&=\widetilde{A}^2_d\xi(0)+\widetilde{A}_d\widetilde{B}_d\Delta\delta(0)+\widetilde{B}_d\Delta\delta(1)+ \widetilde{A}_d\widetilde{B}_{cd}\dot{\psi}_{des}(0)+\widetilde{B}_{cd}\dot{\psi}_{des}(1) \\ \\ &\xi(3) &&=\widetilde{A}_d\xi(2)+\widetilde{B}_d\Delta\delta(2)+\widetilde{B}_{cd}\dot{\psi}_{des}(2) \\ & &&=\widetilde{A}_d[\widetilde{A}^2_d\xi(0)+\widetilde{A}_d\widetilde{B}_d\Delta\delta(0)+\widetilde{B}_d\Delta\delta(1)+ \widetilde{A}_d\widetilde{B}_{cd}\dot{\psi}_{des}(0)+\widetilde{B}_{cd}\dot{\psi}_{des}(1)]+\widetilde{B}_d\Delta\delta(2)+\widetilde{B}_{cd}\dot{\psi}_{des}(2)\\ & &&=\widetilde{A}^3_d\xi(0)+\widetilde{A}^2_d\widetilde{B}_d\Delta\delta(0)+\widetilde{A}_d\widetilde{B}_d\Delta\delta(1)+\widetilde{B}_d\Delta\delta(2)+ \widetilde{A}^2_d\widetilde{B}_{cd}\dot{\psi}_{des}(0)+\widetilde{A}_d\widetilde{B}_{cd}\dot{\psi}_{des}(1)+\widetilde{B}_{cd}\dot{\psi}_{des}(2) \\ \\ & \ ... \\ \\ &\xi(k) &&= \widetilde{A}^{(k)}_d\xi(0)+\sum_{i=0}^{k-1}\widetilde{A}_d^{(i)}\widetilde{B}_d\Delta\delta(k-i-1)+ \sum_{j=0}^{k-1}\widetilde{A}_d^{(j)}\widetilde{B}_{cd}\dot{\psi}_{des}(k-j-1) \\ &y(k) &&=\widetilde{C}_d\xi(k)=\widetilde{C}_d\widetilde{A}^{(k)}_d\xi(0)+\widetilde{C}_d\sum_{i=0}^{k-1}\widetilde{A}_d^{(i)}\widetilde{B}_d\Delta\delta(k-i-1)+ \widetilde{C}_d\sum_{j=0}^{k-1}\widetilde{A}_d^{(j)}\widetilde{B}_{cd}\dot{\psi}_{des}(k-j-1) \end{aligned}
ξ(1)ξ(2)ξ(3) ...ξ(k)y(k)=A
dξ(0)+B
dΔδ(0)+B
cdψ˙des(0)=A
dξ(1)+B
dΔδ(1)+B
cdψ˙des(1)=A
d[A
dξ(0)+B
dΔδ(0)+B
cdψ˙des(0)]+B
dΔδ(1)+B
cdψ˙des(1)=A
d2ξ(0)+A
dB
dΔδ(0)+B
dΔδ(1)+A
dB
cdψ˙des(0)+B
cdψ˙des(1)=A
dξ(2)+B
dΔδ(2)+B
cdψ˙des(2)=A
d[A
d2ξ(0)+A
dB
dΔδ(0)+B
dΔδ(1)+A
dB
cdψ˙des(0)+B
cdψ˙des(1)]+B
dΔδ(2)+B
cdψ˙des(2)=A
d3ξ(0)+A
d2B
dΔδ(0)+A
dB
dΔδ(1)+B
dΔδ(2)+A
d2B
cdψ˙des(0)+A
dB
cdψ˙des(1)+B
cdψ˙des(2)=A
d(k)ξ(0)+i=0∑k−1A
d(i)B
dΔδ(k−i−1)+j=0∑k−1A
d(j)B
cdψ˙des(k−j−1)=C
dξ(k)=C
dA
d(k)ξ(0)+C
di=0∑k−1A
d(i)B
dΔδ(k−i−1)+C
dj=0∑k−1A
d(j)B
cdψ˙des(k−j−1)
基于以上的预测方程,我们可以通过
t
t
t时刻的状态量初值
ξ
(
0
)
\xi(0)
ξ(0),建立状态量
ξ
\xi
ξ 在
k
k
k 个时域预测周期内与各周期控制增量
Δ
δ
\Delta\delta
Δδ 之间的联系,即
[
ξ
(
t
+
1
∣
t
)
ξ
(
t
+
2
∣
t
)
ξ
(
t
+
3
∣
t
)
⋮
ξ
(
t
+
k
∣
t
)
]
=
[
A
~
d
A
~
d
2
A
~
d
3
⋮
A
~
d
k
]
ξ
(
0
)
+
[
B
~
d
0
0
…
0
A
~
d
B
~
d
B
~
d
0
…
0
A
~
d
2
B
~
d
A
~
d
B
~
d
B
~
d
…
0
⋮
⋮
⋮
⋱
⋮
A
~
d
k
−
1
B
~
d
A
~
d
k
−
2
B
~
d
A
~
d
k
−
3
B
~
d
…
B
~
d
]
[
Δ
δ
(
0
)
Δ
δ
(
1
)
Δ
δ
(
2
)
⋮
Δ
δ
(
k
−
1
)
]
+
[
B
~
c
d
0
0
…
0
A
~
d
B
~
c
d
B
~
c
d
0
…
0
A
~
d
2
B
~
c
d
A
~
d
B
~
c
d
B
~
c
d
…
0
⋮
⋮
⋮
⋱
⋮
A
~
d
k
−
1
B
~
c
d
A
~
d
k
−
2
B
~
c
d
A
~
d
k
−
3
B
~
c
d
…
B
~
c
d
]
[
ψ
˙
d
e
s
(
0
)
ψ
˙
d
e
s
(
1
)
ψ
˙
d
e
s
(
2
)
⋮
ψ
˙
d
e
s
(
k
−
1
)
]
\begin{bmatrix} \xi(t+1 | t) \\ \xi(t+2 | t) \\ \xi(t+3 | t) \\ \vdots \\ \xi(t+k | t)\end{bmatrix} = \begin{bmatrix} \widetilde{A}_d \\ \widetilde{A}^2_d \\ \widetilde{A}^3_d \\ \vdots \\ \widetilde{A}^k_d\end{bmatrix}\xi(0)+ \begin{bmatrix} \widetilde{B}_d & 0 & 0 & \dots & 0 \\ \widetilde{A}_d\widetilde{B}_d & \widetilde{B}_d & 0 & \dots & 0 \\ \widetilde{A}_d^2\widetilde{B}_d & \widetilde{A}_d\widetilde{B}_d & \widetilde{B}_d & \dots & 0 \\ \vdots & \vdots & \vdots & \ddots & \vdots \\ \widetilde{A}_d^{k-1}\widetilde{B}_d & \widetilde{A}_d^{k-2}\widetilde{B}_d &\widetilde{A}_d^{k-3}\widetilde{B}_d & \dots & \widetilde{B}_d \end{bmatrix} \begin{bmatrix} \Delta\delta(0) \\ \Delta\delta(1) \\ \Delta\delta(2) \\ \vdots \\ \Delta\delta(k-1) \end{bmatrix} + \begin{bmatrix} \widetilde{B}_{cd} & 0 & 0 & \dots & 0\\ \widetilde{A}_d\widetilde{B}_{cd} & \widetilde{B}_{cd} & 0 & \dots & 0 \\ \widetilde{A}_d^2\widetilde{B}_{cd} & \widetilde{A}_d\widetilde{B}_{cd} & \widetilde{B}_{cd} & \dots & 0 \\ \vdots & \vdots & \vdots & \ddots & \vdots \\ \widetilde{A}_d^{k-1}\widetilde{B}_{cd} & \widetilde{A}_d^{k-2}\widetilde{B}_{cd} &\widetilde{A}_d^{k-3}\widetilde{B}_{cd} & \dots & \widetilde{B}_{cd} \end{bmatrix} \begin{bmatrix} \dot{\psi}_{des}(0) \\ \dot{\psi}_{des}(1) \\ \dot{\psi}_{des}(2) \\ \vdots \\ \dot{\psi}_{des}(k-1) \end{bmatrix}
⎣⎢⎢⎢⎢⎢⎡ξ(t+1∣t)ξ(t+2∣t)ξ(t+3∣t)⋮ξ(t+k∣t)⎦⎥⎥⎥⎥⎥⎤=⎣⎢⎢⎢⎢⎢⎡A
dA
d2A
d3⋮A
dk⎦⎥⎥⎥⎥⎥⎤ξ(0)+⎣⎢⎢⎢⎢⎢⎡B
dA
dB
dA
d2B
d⋮A
dk−1B
d0B
dA
dB
d⋮A
dk−2B
d00B
d⋮A
dk−3B
d………⋱…000⋮B
d⎦⎥⎥⎥⎥⎥⎤⎣⎢⎢⎢⎢⎢⎡Δδ(0)Δδ(1)Δδ(2)⋮Δδ(k−1)⎦⎥⎥⎥⎥⎥⎤+⎣⎢⎢⎢⎢⎢⎡B
cdA
dB
cdA
d2B
cd⋮A
dk−1B
cd0B
cdA
dB
cd⋮A
dk−2B
cd00B
cd⋮A
dk−3B
cd………⋱…000⋮B
cd⎦⎥⎥⎥⎥⎥⎤⎣⎢⎢⎢⎢⎢⎡ψ˙des(0)ψ˙des(1)ψ˙des(2)⋮ψ˙des(k−1)⎦⎥⎥⎥⎥⎥⎤
定义
X
=
[
ξ
(
t
+
1
∣
t
)
ξ
(
t
+
2
∣
t
)
⋮
ξ
(
t
+
k
∣
t
)
]
Δ
U
=
[
Δ
δ
(
0
)
Δ
δ
(
1
)
⋮
Δ
δ
(
k
−
1
)
]
Λ
=
[
A
~
d
A
~
d
2
⋮
A
~
d
k
]
Ψ
=
[
ψ
˙
d
e
s
(
0
)
ψ
˙
d
e
s
(
1
)
ψ
˙
d
e
s
(
2
)
⋮
ψ
˙
d
e
s
(
k
−
1
)
]
Γ
=
[
B
~
d
0
0
…
0
A
~
d
B
~
d
B
~
d
0
…
0
A
~
d
2
B
~
d
A
~
d
B
~
d
B
~
d
…
0
⋮
⋮
⋮
⋱
⋮
A
~
d
k
−
1
B
~
d
A
~
d
k
−
2
B
~
d
A
~
d
k
−
3
B
~
d
…
B
~
d
]
Υ
=
[
B
~
c
d
0
0
…
0
A
~
d
B
~
c
d
B
~
c
d
0
…
0
A
~
d
2
B
~
c
d
A
~
d
B
~
c
d
B
~
c
d
…
0
⋮
⋮
⋮
⋱
⋮
A
~
d
k
−
1
B
~
c
d
A
~
d
k
−
2
B
~
c
d
A
~
d
k
−
3
B
~
c
d
…
B
~
c
d
]
\begin{aligned} &X = \begin{bmatrix} \xi(t+1 | t) \\ \xi(t+2 | t) \\ \vdots \\ \xi(t+k | t)\end{bmatrix} \quad \Delta U = \begin{bmatrix} \Delta\delta(0) \\ \Delta\delta(1) \\ \vdots \\ \Delta\delta(k-1) \end{bmatrix} \quad \Lambda = \begin{bmatrix} \widetilde{A}_d \\ \widetilde{A}^2_d \\ \vdots \\ \widetilde{A}^k_d\end{bmatrix} \quad \Psi = \begin{bmatrix} \dot{\psi}_{des}(0) \\ \dot{\psi}_{des}(1) \\ \dot{\psi}_{des}(2) \\ \vdots \\ \dot{\psi}_{des}(k-1) \end{bmatrix}\\ \\ &\Gamma = \begin{bmatrix} \widetilde{B}_d & 0 & 0 & \dots & 0 \\ \widetilde{A}_d\widetilde{B}_d & \widetilde{B}_d & 0 & \dots & 0 \\ \widetilde{A}_d^2\widetilde{B}_d & \widetilde{A}_d\widetilde{B}_d & \widetilde{B}_d & \dots & 0 \\ \vdots & \vdots & \vdots & \ddots & \vdots \\ \widetilde{A}_d^{k-1}\widetilde{B}_d & \widetilde{A}_d^{k-2}\widetilde{B}_d &\widetilde{A}_d^{k-3}\widetilde{B}_d & \dots & \widetilde{B}_d \end{bmatrix}\\ \\ &\Upsilon = \begin{bmatrix} \widetilde{B}_{cd} & 0 & 0 & \dots & 0\\ \widetilde{A}_d\widetilde{B}_{cd} & \widetilde{B}_{cd} & 0 & \dots & 0 \\ \widetilde{A}_d^2\widetilde{B}_{cd} & \widetilde{A}_d\widetilde{B}_{cd} & \widetilde{B}_{cd} & \dots & 0 \\ \vdots & \vdots & \vdots & \ddots & \vdots \\ \widetilde{A}_d^{k-1}\widetilde{B}_{cd} & \widetilde{A}_d^{k-2}\widetilde{B}_{cd} &\widetilde{A}_d^{k-3}\widetilde{B}_{cd} & \dots & \widetilde{B}_{cd} \end{bmatrix} \end{aligned}
X=⎣⎢⎢⎢⎡ξ(t+1∣t)ξ(t+2∣t)⋮ξ(t+k∣t)⎦⎥⎥⎥⎤ΔU=⎣⎢⎢⎢⎡Δδ(0)Δδ(1)⋮Δδ(k−1)⎦⎥⎥⎥⎤Λ=⎣⎢⎢⎢⎡A
dA
d2⋮A
dk⎦⎥⎥⎥⎤Ψ=⎣⎢⎢⎢⎢⎢⎡ψ˙des(0)ψ˙des(1)ψ˙des(2)⋮ψ˙des(k−1)⎦⎥⎥⎥⎥⎥⎤Γ=⎣⎢⎢⎢⎢⎢⎡B
dA
dB
dA
d2B
d⋮A
dk−1B
d0B
dA
dB
d⋮A
dk−2B
d00B
d⋮A
dk−3B
d………⋱…000⋮B
d⎦⎥⎥⎥⎥⎥⎤Υ=⎣⎢⎢⎢⎢⎢⎡B
cdA
dB
cdA
d2B
cd⋮A
dk−1B
cd0B
cdA
dB
cd⋮A
dk−2B
cd00B
cd⋮A
dk−3B
cd………⋱…000⋮B
cd⎦⎥⎥⎥⎥⎥⎤
进一步的,定义
Y
=
[
y
(
t
+
1
∣
t
)
y
(
t
+
2
∣
t
)
⋮
y
(
t
+
k
∣
t
)
]
Ω
=
[
C
~
d
A
~
d
C
~
d
A
~
d
2
⋮
C
~
d
A
~
d
k
]
Θ
=
[
C
~
d
B
~
d
0
0
…
0
C
~
d
A
~
d
B
~
d
C
~
d
B
~
d
0
…
0
C
~
d
A
~
d
2
B
~
d
C
~
d
A
~
d
B
~
d
C
~
d
B
~
d
…
0
⋮
⋮
⋮
⋱
⋮
C
~
d
A
~
d
k
−
1
B
~
d
C
~
d
A
~
d
k
−
2
B
~
d
C
~
d
A
~
d
k
−
3
B
~
d
…
C
~
d
B
~
d
]
Φ
=
[
C
~
d
B
~
c
d
0
0
…
0
C
~
d
A
~
d
B
~
c
d
C
~
d
B
~
c
d
0
…
0
C
~
d
A
~
d
2
B
~
c
d
C
~
d
A
~
d
B
~
c
d
C
~
d
B
~
c
d
…
0
⋮
⋮
⋮
⋱
⋮
C
~
d
A
~
d
k
−
1
B
~
c
d
C
~
d
A
~
d
k
−
2
B
~
c
d
C
~
d
A
~
d
k
−
3
B
~
c
d
…
C
~
d
B
~
c
d
]
\begin{aligned} &Y = \begin{bmatrix} y(t+1 | t) \\ y(t+2 | t) \\ \vdots \\ y(t+k | t)\end{bmatrix} \quad \Omega = \begin{bmatrix} \widetilde{C}_d\widetilde{A}_d \\ \widetilde{C}_d\widetilde{A}^2_d \\ \vdots \\ \widetilde{C}_d\widetilde{A}^k_d\end{bmatrix} \\ \\ &\Theta = \begin{bmatrix} \widetilde{C}_d\widetilde{B}_d & 0 & 0 & \dots & 0 \\ \widetilde{C}_d\widetilde{A}_d\widetilde{B}_d & \widetilde{C}_d\widetilde{B}_d & 0 & \dots & 0 \\ \widetilde{C}_d\widetilde{A}_d^2\widetilde{B}_d & \widetilde{C}_d\widetilde{A}_d\widetilde{B}_d & \widetilde{C}_d\widetilde{B}_d & \dots & 0 \\ \vdots & \vdots & \vdots & \ddots & \vdots \\ \widetilde{C}_d\widetilde{A}_d^{k-1}\widetilde{B}_d & \widetilde{C}_d\widetilde{A}_d^{k-2}\widetilde{B}_d &\widetilde{C}_d\widetilde{A}_d^{k-3}\widetilde{B}_d & \dots & \widetilde{C}_d\widetilde{B}_d \end{bmatrix}\\ \\ &\Phi = \begin{bmatrix} \widetilde{C}_d\widetilde{B}_{cd} & 0 & 0 & \dots & 0\\ \widetilde{C}_d\widetilde{A}_d\widetilde{B}_{cd} & \widetilde{C}_d\widetilde{B}_{cd} & 0 & \dots & 0 \\ \widetilde{C}_d\widetilde{A}_d^2\widetilde{B}_{cd} & \widetilde{C}_d\widetilde{A}_d\widetilde{B}_{cd} & \widetilde{C}_d\widetilde{B}_{cd} & \dots & 0 \\ \vdots & \vdots & \vdots & \ddots & \vdots \\ \widetilde{C}_d\widetilde{A}_d^{k-1}\widetilde{B}_{cd} & \widetilde{C}_d\widetilde{A}_d^{k-2}\widetilde{B}_{cd} &\widetilde{C}_d\widetilde{A}_d^{k-3}\widetilde{B}_{cd} & \dots & \widetilde{C}_d\widetilde{B}_{cd} \end{bmatrix} \end{aligned}
Y=⎣⎢⎢⎢⎡y(t+1∣t)y(t+2∣t)⋮y(t+k∣t)⎦⎥⎥⎥⎤Ω=⎣⎢⎢⎢⎡C
dA
dC
dA
d2⋮C
dA
dk⎦⎥⎥⎥⎤Θ=⎣⎢⎢⎢⎢⎢⎡C
dB
dC
dA
dB
dC
dA
d2B
d⋮C
dA
dk−1B
d0C
dB
dC
dA
dB
d⋮C
dA
dk−2B
d00C
dB
d⋮C
dA
dk−3B
d………⋱…000⋮C
dB
d⎦⎥⎥⎥⎥⎥⎤Φ=⎣⎢⎢⎢⎢⎢⎡C
dB
cdC
dA
dB
cdC
dA
d2B
cd⋮C
dA
dk−1B
cd0C
dB
cdC
dA
dB
cd⋮C
dA
dk−2B
cd00C
dB
cd⋮C
dA
dk−3B
cd………⋱…000⋮C
dB
cd⎦⎥⎥⎥⎥⎥⎤
综上有
X
=
Λ
ξ
(
0
)
+
Γ
Δ
U
+
Υ
Ψ
Y
=
Ω
ξ
(
0
)
+
Θ
Δ
U
+
Φ
Ψ
\begin{aligned} &X=\Lambda\xi(0)+\Gamma\Delta U+\Upsilon\Psi\\ &Y=\Omega\xi(0)+\Theta\Delta U +\Phi\Psi \end{aligned}
X=Λξ(0)+ΓΔU+ΥΨY=Ωξ(0)+ΘΔU+ΦΨ
代价函数
我构造的代价函数惩罚输出量以及控制增量,因此
J
=
∣
∣
Y
−
Y
d
e
s
∣
∣
Q
c
+
∣
∣
Δ
U
∣
∣
R
c
=
(
Y
−
Y
d
e
s
)
T
Q
c
(
Y
−
Y
d
e
s
)
+
Δ
U
T
R
c
Δ
U
=
(
Ω
ξ
(
0
)
+
Θ
Δ
U
+
Φ
Ψ
−
Y
d
e
s
)
T
Q
c
(
Ω
ξ
(
0
)
+
Θ
Δ
U
+
Φ
Ψ
−
Y
d
e
s
)
+
Δ
U
T
R
c
Δ
U
\begin{aligned} J &= ||Y-Y_{des}||_{Q_c}+||\Delta U||_{R_c}\\ &=(Y-Y_{des})^TQ_c(Y-Y_{des})+\Delta U^TR_c\Delta U\\ &=(\Omega\xi(0)+\Theta\Delta U+\Phi\Psi-Y_{des})^TQ_c(\Omega\xi(0)+\Theta\Delta U+\Phi\Psi-Y_{des})+\Delta U^TR_c\Delta U \end{aligned}
J=∣∣Y−Ydes∣∣Qc+∣∣ΔU∣∣Rc=(Y−Ydes)TQc(Y−Ydes)+ΔUTRcΔU=(Ωξ(0)+ΘΔU+ΦΨ−Ydes)TQc(Ωξ(0)+ΘΔU+ΦΨ−Ydes)+ΔUTRcΔU
我们优化的对象参数是
Δ
U
\Delta U
ΔU,因此将代价函数中不含对象参数的部分定义为
N
N
N,则
N
=
Ω
ξ
(
0
)
+
Φ
Ψ
−
Y
d
e
s
N = \Omega\xi(0)+\Phi\Psi-Y_{des}
N=Ωξ(0)+ΦΨ−Ydes
将
N
N
N 代入到代价函数中可得
J
=
(
N
+
Θ
Δ
U
)
T
Q
c
(
N
+
Θ
Δ
U
)
+
U
T
R
c
U
=
N
T
Q
c
N
+
2
Δ
U
T
Θ
T
Q
c
N
+
Δ
U
T
Θ
t
Q
c
Θ
Δ
U
+
Δ
U
T
R
c
Δ
U
=
N
T
Q
c
N
+
2
Δ
U
T
Θ
T
Q
c
N
+
Δ
U
T
(
Θ
t
Q
c
Θ
+
R
c
)
Δ
U
\begin{aligned}J &= (N+\Theta\Delta U)^TQ_c(N+\Theta\Delta U)+U^TR_cU\\ &= N^TQ_cN + 2\Delta U^T\Theta^TQ_cN + \Delta U^T \Theta^tQ_c\Theta\Delta U + \Delta U^TR_c\Delta U\\ &= N^TQ_cN + 2\Delta U^T\Theta^TQ_cN + \Delta U^T(\Theta^tQ_c\Theta+R_c)\Delta U \end{aligned}
J=(N+ΘΔU)TQc(N+ΘΔU)+UTRcU=NTQcN+2ΔUTΘTQcN+ΔUTΘtQcΘΔU+ΔUTRcΔU=NTQcN+2ΔUTΘTQcN+ΔUT(ΘtQcΘ+Rc)ΔU
针对该优化问题,我们可以进一步简化代价函数
J
J
J,即消去代价函数中不含优化对象参数
Δ
U
\Delta U
ΔU的项
a
r
g
m
i
n
Δ
U
(
J
)
=
a
r
g
m
i
n
Δ
U
(
N
T
Q
c
N
+
2
Δ
U
T
Θ
T
Q
c
N
+
Δ
U
T
(
Θ
t
Q
c
Θ
+
R
c
)
Δ
U
)
=
a
r
g
m
i
n
Δ
U
(
2
Δ
U
T
Θ
T
Q
c
N
+
Δ
U
T
(
Θ
t
Q
c
Θ
+
R
c
)
Δ
U
)
=
a
r
g
m
i
n
Δ
U
(
J
∗
)
\begin{aligned} \mathop{argmin}\limits_{\Delta U}(J) &=\mathop{argmin}\limits_{\Delta U}(N^TQ_cN+2\Delta U^T\Theta^TQ_cN + \Delta U^T(\Theta^tQ_c\Theta+R_c)\Delta U)\\ &=\mathop{argmin}\limits_{\Delta U}(2\Delta U^T\Theta^TQ_cN + \Delta U^T(\Theta^tQ_c\Theta+R_c)\Delta U)\\ &=\mathop{argmin}\limits_{\Delta U}(J^*) \end{aligned}
ΔUargmin(J)=ΔUargmin(NTQcN+2ΔUTΘTQcN+ΔUT(ΘtQcΘ+Rc)ΔU)=ΔUargmin(2ΔUTΘTQcN+ΔUT(ΘtQcΘ+Rc)ΔU)=ΔUargmin(J∗)
其中,
J
∗
J^*
J∗ 为简化后的代价函数
定义
H
=
Θ
t
Q
c
Θ
+
R
c
K
=
2
Θ
T
Q
c
N
\begin{aligned} &H = \Theta^tQ_c\Theta+R_c \\ &K = 2\Theta^TQ_cN \end{aligned}
H=ΘtQcΘ+RcK=2ΘTQcN
则QP问题为
m
i
n
Δ
U
(
J
∗
)
=
m
i
n
Δ
U
(
Δ
U
T
H
Δ
U
+
Δ
U
T
K
)
\mathop{min}\limits_{\Delta U}(J^*) =\mathop{min}\limits_{\Delta U}(\Delta U^TH\Delta U+ \Delta U^TK)
ΔUmin(J∗)=ΔUmin(ΔUTHΔU+ΔUTK)
约束条件
控制增量约束
在增量式模型中,我们优化的对象参数是控制量量增量
Δ
U
\Delta U
ΔU,因此对控制量增量直接约束为
Δ
U
m
i
n
⪯
Δ
U
⪯
Δ
U
m
a
x
\Delta U^{min} \preceq \Delta U \preceq \Delta U^{max}
ΔUmin⪯ΔU⪯ΔUmax
其中
Δ
U
m
i
n
=
[
Δ
δ
(
0
)
m
i
n
Δ
δ
(
1
)
m
i
n
…
Δ
δ
(
k
−
1
)
m
i
n
]
T
Δ
U
m
a
x
=
[
Δ
δ
(
0
)
m
a
x
Δ
δ
(
1
)
m
a
x
…
Δ
δ
(
k
−
1
)
m
a
x
]
T
\begin{aligned} &\Delta U^{min} = \begin{bmatrix} \Delta\delta(0)^{min} & \Delta\delta(1)^{min} & \dots & \Delta\delta(k-1)^{min} \end{bmatrix} ^T \\ &\Delta U^{max} = \begin{bmatrix} \Delta\delta(0)^{max} & \Delta\delta(1)^{max} & \dots & \Delta\delta(k-1)^{max} \end{bmatrix} ^T \end{aligned}
ΔUmin=[Δδ(0)minΔδ(1)min…Δδ(k−1)min]TΔUmax=[Δδ(0)maxΔδ(1)max…Δδ(k−1)max]T
以上约束条件可改写成
[ − 1 0 … 0 0 0 − 1 … 0 0 ⋮ ⋮ ⋱ ⋮ ⋮ 0 0 … − 1 0 0 0 … 0 − 1 1 0 … 0 0 0 1 … 0 0 ⋮ ⋮ ⋱ ⋮ ⋮ 0 0 … 1 0 0 0 … 0 1 ] [ Δ δ ( 0 ) Δ δ ( 1 ) ⋮ Δ δ ( k − 2 ) Δ δ ( k − 1 ) ] ⪯ [ − Δ δ ( 0 ) m i n − Δ δ ( 1 ) m i n ⋮ − Δ δ ( k − 2 ) m i n − Δ δ ( k − 1 ) m i n Δ δ ( 0 ) m a x Δ δ ( 1 ) m a x ⋮ Δ δ ( k − 2 ) m a x Δ δ ( k − 1 ) m a x ] \left[\begin{array}{rrrrr} -1 & 0 & \dots & 0 & 0\\ 0 & -1 & \dots & 0 & 0\\ \vdots & \vdots & \ddots & \vdots & \vdots\\ 0 & 0 & \dots & -1 & 0\\ 0 & 0 & \dots & 0 & -1\\ 1 & 0 & \dots & 0 & 0\\ 0 & 1 & \dots & 0 & 0\\ \vdots & \vdots & \ddots & \vdots & \vdots\\ 0 & 0 & \dots & 1 & 0\\ 0 & 0 & \dots & 0 & 1 \end{array}\right] \begin{bmatrix} \Delta\delta(0)\\ \Delta\delta(1)\\ \vdots \\ \Delta\delta(k-2)\\ \Delta\delta(k-1) \end{bmatrix} \preceq \begin{bmatrix} -\Delta\delta(0)^{min}\\ -\Delta\delta(1)^{min}\\ \vdots\\ -\Delta\delta(k-2)^{min}\\ -\Delta\delta(k-1)^{min}\\ \Delta\delta(0)^{max}\\ \Delta\delta(1)^{max}\\ \vdots\\ \Delta\delta(k-2)^{max}\\ \Delta\delta(k-1)^{max}\\ \end{bmatrix} ⎣⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎡−10⋮0010⋮000−1⋮0001⋮00……⋱…………⋱……00⋮−1000⋮1000⋮0−100⋮01⎦⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎤⎣⎢⎢⎢⎢⎢⎡Δδ(0)Δδ(1)⋮Δδ(k−2)Δδ(k−1)⎦⎥⎥⎥⎥⎥⎤⪯⎣⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎡−Δδ(0)min−Δδ(1)min⋮−Δδ(k−2)min−Δδ(k−1)minΔδ(0)maxΔδ(1)max⋮Δδ(k−2)maxΔδ(k−1)max⎦⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎤
定义
S
d
u
l
=
[
−
1
0
…
0
0
0
−
1
…
0
0
⋮
⋮
⋱
⋮
⋮
0
0
…
−
1
0
0
0
…
0
−
1
1
0
…
0
0
0
1
…
0
0
⋮
⋮
⋱
⋮
⋮
0
0
…
1
0
0
0
…
0
1
]
=
[
−
I
I
]
S
d
u
r
=
[
−
Δ
δ
(
0
)
m
i
n
−
Δ
δ
(
1
)
m
i
n
⋮
−
Δ
δ
(
k
−
2
)
m
i
n
−
Δ
δ
(
k
−
1
)
m
i
n
Δ
δ
(
0
)
m
a
x
Δ
δ
(
1
)
m
a
x
⋮
Δ
δ
(
k
−
2
)
m
a
x
Δ
δ
(
k
−
1
)
m
a
x
]
=
[
−
Δ
U
m
i
n
Δ
U
m
a
x
]
\begin{aligned} &S^{l}_{du} = \left[\begin{array}{rrrrr} -1 & 0 & \dots & 0 & 0\\ 0 & -1 & \dots & 0 & 0\\ \vdots & \vdots & \ddots & \vdots & \vdots\\ 0 & 0 & \dots & -1 & 0\\ 0 & 0 & \dots & 0 & -1\\ 1 & 0 & \dots & 0 & 0\\ 0 & 1 & \dots & 0 & 0\\ \vdots & \vdots & \ddots & \vdots & \vdots\\ 0 & 0 & \dots & 1 & 0\\ 0 & 0 & \dots & 0 & 1 \end{array}\right] = \begin{bmatrix} -I \\ \quad I \end{bmatrix}\\ &S^{r}_{du} = \begin{bmatrix} -\Delta\delta(0)^{min}\\ -\Delta\delta(1)^{min}\\ \vdots\\ -\Delta\delta(k-2)^{min}\\ -\Delta\delta(k-1)^{min}\\ \Delta\delta(0)^{max}\\ \Delta\delta(1)^{max}\\ \vdots\\ \Delta\delta(k-2)^{max}\\ \Delta\delta(k-1)^{max}\\ \end{bmatrix} = \begin{bmatrix} -\Delta U^{min} \\ \quad\Delta U^{max} \end{bmatrix} \end{aligned}
Sdul=⎣⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎡−10⋮0010⋮000−1⋮0001⋮00……⋱…………⋱……00⋮−1000⋮1000⋮0−100⋮01⎦⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎤=[−II]Sdur=⎣⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎡−Δδ(0)min−Δδ(1)min⋮−Δδ(k−2)min−Δδ(k−1)minΔδ(0)maxΔδ(1)max⋮Δδ(k−2)maxΔδ(k−1)max⎦⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎤=[−ΔUminΔUmax]
则控制增量的约束可表示为
S
d
u
l
Δ
U
⪯
S
d
u
r
S^{l}_{du} \Delta U \preceq S^{r}_{du}
SdulΔU⪯Sdur
控制量约束
首先我们要将控制量矩阵
U
U
U 以控制增量
Δ
U
\Delta U
ΔU 的形式表达
U
=
[
δ
(
0
)
δ
(
1
)
⋮
δ
(
k
−
2
)
δ
(
k
−
1
)
]
=
[
1
1
⋮
1
1
]
δ
(
−
1
)
+
[
1
0
0
…
0
0
1
1
0
…
0
0
1
1
1
…
0
0
⋮
⋮
⋮
⋱
⋮
⋮
1
1
1
…
1
0
1
1
1
…
1
1
]
[
Δ
δ
(
0
)
Δ
δ
(
1
)
⋮
Δ
δ
(
k
−
2
)
Δ
δ
(
k
−
1
)
]
U = \begin{bmatrix} \delta(0)\\ \delta(1)\\ \vdots \\ \delta(k-2)\\ \delta(k-1) \end{bmatrix} = \begin{bmatrix} 1 \\ 1 \\ \vdots \\ 1 \\ 1 \end{bmatrix} \delta(-1)+ \begin{bmatrix} 1 & 0 & 0 & \dots & 0 & 0\\ 1 & 1 & 0 & \dots & 0 & 0\\ 1 & 1 & 1 & \dots & 0 & 0\\ \vdots & \vdots & \vdots & \ddots &\vdots & \vdots\\ 1 & 1 & 1 & \dots & 1 & 0\\ 1 & 1 & 1 & \dots & 1 & 1 \end{bmatrix} \begin{bmatrix} \Delta\delta(0)\\ \Delta\delta(1)\\ \vdots \\ \Delta\delta(k-2)\\ \Delta\delta(k-1) \end{bmatrix}
U=⎣⎢⎢⎢⎢⎢⎡δ(0)δ(1)⋮δ(k−2)δ(k−1)⎦⎥⎥⎥⎥⎥⎤=⎣⎢⎢⎢⎢⎢⎡11⋮11⎦⎥⎥⎥⎥⎥⎤δ(−1)+⎣⎢⎢⎢⎢⎢⎢⎢⎡111⋮11011⋮11001⋮11………⋱……000⋮11000⋮01⎦⎥⎥⎥⎥⎥⎥⎥⎤⎣⎢⎢⎢⎢⎢⎡Δδ(0)Δδ(1)⋮Δδ(k−2)Δδ(k−1)⎦⎥⎥⎥⎥⎥⎤
其中
δ
(
−
1
)
\delta(-1)
δ(−1) 代表
t
−
1
t-1
t−1 时刻的控制量,即上一周期的控制量
定义
E
=
[
1
1
⋮
1
1
]
F
=
[
1
0
0
…
0
0
1
1
0
…
0
0
1
1
1
…
0
0
⋮
⋮
⋮
⋱
⋮
⋮
1
1
1
…
1
0
1
1
1
…
1
1
]
E = \begin{bmatrix} 1 \\ 1 \\ \vdots \\ 1 \\ 1 \end{bmatrix} \quad F = \begin{bmatrix} 1 & 0 & 0 & \dots & 0 & 0\\ 1 & 1 & 0 & \dots & 0 & 0\\ 1 & 1 & 1 & \dots & 0 & 0\\ \vdots & \vdots & \vdots & \ddots &\vdots & \vdots\\ 1 & 1 & 1 & \dots & 1 & 0\\ 1 & 1 & 1 & \dots & 1 & 1 \end{bmatrix}
E=⎣⎢⎢⎢⎢⎢⎡11⋮11⎦⎥⎥⎥⎥⎥⎤F=⎣⎢⎢⎢⎢⎢⎢⎢⎡111⋮11011⋮11001⋮11………⋱……000⋮11000⋮01⎦⎥⎥⎥⎥⎥⎥⎥⎤
则上式可改写为
U
=
E
δ
(
−
1
)
+
F
Δ
U
U = E\delta(-1)+F\Delta U
U=Eδ(−1)+FΔU
控制量
U
U
U 需满足的约束条件为
U
m
i
n
⪯
U
⪯
U
m
a
x
U^{min} \preceq U \preceq U^{max}
Umin⪯U⪯Umax
其中
U
m
i
n
=
[
δ
(
0
)
m
i
n
δ
(
1
)
m
i
n
…
δ
(
k
−
1
)
m
i
n
]
T
U
m
a
x
=
[
δ
(
0
)
m
a
x
δ
(
1
)
m
a
x
…
δ
(
k
−
1
)
m
a
x
]
T
\begin{aligned} &U^{min} = \begin{bmatrix} \delta(0)^{min} & \delta(1)^{min} & \dots & \delta(k-1)^{min} \end{bmatrix} ^T \\ & U^{max} = \begin{bmatrix} \delta(0)^{max} & \delta(1)^{max} & \dots & \delta(k-1)^{max} \end{bmatrix} ^T \end{aligned}
Umin=[δ(0)minδ(1)min…δ(k−1)min]TUmax=[δ(0)maxδ(1)max…δ(k−1)max]T
根据以上关系,改写控制量约束
[
−
F
F
]
Δ
U
⪯
[
−
U
m
i
n
+
E
δ
(
−
1
)
U
m
a
x
−
E
δ
(
−
1
)
]
\begin{bmatrix} -F\\ \quad F \end{bmatrix} \Delta U \preceq \begin{bmatrix} -U^{min}+E\delta(-1)\\ \quad U^{max}-E\delta(-1) \end{bmatrix}
[−FF]ΔU⪯[−Umin+Eδ(−1)Umax−Eδ(−1)]
定义
S
u
l
=
[
−
F
F
]
S
u
r
=
[
−
U
m
i
n
+
E
δ
(
−
1
)
U
m
a
x
−
E
δ
(
−
1
)
]
\begin{aligned} &S^{l}_{u} = \begin{bmatrix} -F\\ \quad F \end{bmatrix}\\ &S^{r}_{u} = \begin{bmatrix} -U^{min}+E\delta(-1)\\ \quad U^{max}-E\delta(-1) \end{bmatrix} \end{aligned}
Sul=[−FF]Sur=[−Umin+Eδ(−1)Umax−Eδ(−1)]
则控制增量的约束可表示为
S u l Δ U ⪯ S u r S^{l}_{u} \Delta U \preceq S^{r}_{u} SulΔU⪯Sur
状态量约束与输出量约束
状态量约束和输出量约束我们可以直接根据增量式动力学模型推导,步骤如下
X
=
Λ
ξ
(
0
)
+
Γ
Δ
U
+
Υ
Ψ
Y
=
Ω
ξ
(
0
)
+
Θ
Δ
U
+
Φ
Ψ
X
l
b
−
Λ
ξ
(
0
)
−
Υ
Ψ
⪯
Γ
Δ
U
⪯
X
u
b
−
Λ
ξ
(
0
)
−
Υ
Ψ
Y
l
b
−
Ω
ξ
(
0
)
−
Φ
Ψ
⪯
Θ
Δ
U
⪯
Y
u
b
−
Ω
ξ
(
0
)
−
Φ
Ψ
[
−
Γ
Γ
]
Δ
U
⪯
[
−
X
l
b
+
Λ
ξ
(
0
)
+
Υ
Ψ
X
u
b
−
Λ
ξ
(
0
)
−
Υ
Ψ
]
[
−
Θ
Θ
]
Δ
U
⪯
[
−
Y
l
b
+
Ω
ξ
(
0
)
+
Φ
Ψ
Y
u
b
−
Ω
ξ
(
0
)
−
Φ
Ψ
]
\begin{aligned} &X=\Lambda\xi(0)+\Gamma\Delta U+\Upsilon\Psi\\ &Y=\Omega\xi(0)+\Theta\Delta U +\Phi\Psi \\ \\ &X_{lb}-\Lambda\xi(0)-\Upsilon\Psi \preceq \Gamma\Delta U \preceq X_{ub}-\Lambda\xi(0)-\Upsilon\Psi\\ &Y_{lb}-\Omega\xi(0)-\Phi\Psi \preceq \Theta\Delta U \preceq Y_{ub}-\Omega\xi(0)-\Phi\Psi\\\\ &\begin{bmatrix} -\Gamma\\ \quad\Gamma \end{bmatrix} \Delta U \preceq \begin{bmatrix} -X_{lb}+\Lambda\xi(0)+\Upsilon\Psi \\ \quad X_{ub}-\Lambda\xi(0)-\Upsilon\Psi \end{bmatrix}\\ &\begin{bmatrix} -\Theta\\ \quad\Theta \end{bmatrix} \Delta U \preceq \begin{bmatrix} -Y_{lb}+\Omega\xi(0)+\Phi\Psi \\ \quad Y_{ub}-\Omega\xi(0)-\Phi\Psi \end{bmatrix} \end{aligned}
X=Λξ(0)+ΓΔU+ΥΨY=Ωξ(0)+ΘΔU+ΦΨXlb−Λξ(0)−ΥΨ⪯ΓΔU⪯Xub−Λξ(0)−ΥΨYlb−Ωξ(0)−ΦΨ⪯ΘΔU⪯Yub−Ωξ(0)−ΦΨ[−ΓΓ]ΔU⪯[−Xlb+Λξ(0)+ΥΨXub−Λξ(0)−ΥΨ][−ΘΘ]ΔU⪯[−Ylb+Ωξ(0)+ΦΨYub−Ωξ(0)−ΦΨ]
定义
S X l = [ − Γ Γ ] S X r = [ − X l b + Λ ξ ( 0 ) + Υ Ψ X u b − Λ ξ ( 0 ) − Υ Ψ ] S Y l = [ − Θ Θ ] S Y r = [ − Y l b + Ω ξ ( 0 ) + Φ Ψ Y u b − Ω ξ ( 0 ) − Φ Ψ ] \begin{aligned} &S^{l}_{X} = \begin{bmatrix} -\Gamma\\ \quad \Gamma \end{bmatrix}\quad S^{r}_{X} = \begin{bmatrix} -X_{lb}+\Lambda\xi(0)+\Upsilon\Psi \\ \quad X_{ub}-\Lambda\xi(0)-\Upsilon\Psi \end{bmatrix}\\ &S^{l}_{Y}=\begin{bmatrix} -\Theta\\ \quad\Theta \end{bmatrix}\quad S^{r}_{Y} = \begin{bmatrix} -Y_{lb}+\Omega\xi(0)+\Phi\Psi \\ \quad Y_{ub}-\Omega\xi(0)-\Phi\Psi \end{bmatrix} \end{aligned} SXl=[−ΓΓ]SXr=[−Xlb+Λξ(0)+ΥΨXub−Λξ(0)−ΥΨ]SYl=[−ΘΘ]SYr=[−Ylb+Ωξ(0)+ΦΨYub−Ωξ(0)−ΦΨ]
则状态量与输出量的约束可表示为
S X l Δ U ⪯ S X r S Y l Δ U ⪯ S Y r \begin{aligned} &S^{l}_{X} \Delta U \preceq S^{r}_{X} \\ &S^{l}_{Y} \Delta U \preceq S^{r}_{Y} \end{aligned} SXlΔU⪯SXrSYlΔU⪯SYr
值得注意的是,因为本文中代价函数惩罚的是输出量,因此我们选择输出量约束作为QR问题的约束条件之一.
总结
综上,在每一个时间窗内MPC需求解的QR问题为:
m
i
n
Δ
U
(
J
∗
)
=
m
i
n
Δ
U
(
Δ
U
T
H
Δ
U
+
Δ
U
T
K
)
s
.
t
.
S
d
u
l
Δ
U
⪯
S
d
u
r
S
u
l
Δ
U
⪯
S
u
r
S
Y
l
Δ
U
⪯
S
Y
r
\begin{aligned} &\mathop{min}\limits_{\Delta U}(J^*) =\mathop{min}\limits_{\Delta U}(\Delta U^TH\Delta U+ \Delta U^TK)\\ \\ &s.t. \qquad \qquad \qquad \begin{aligned} &S^{l}_{du} &&\Delta U &&\preceq S^{r}_{du}\\ &S^{l}_{u} &&\Delta U &&\preceq S^{r}_{u}\\ &S^{l}_{Y} &&\Delta U &&\preceq S^{r}_{Y} \end{aligned} \end{aligned}
ΔUmin(J∗)=ΔUmin(ΔUTHΔU+ΔUTK)s.t.SdulSulSYlΔUΔUΔU⪯Sdur⪯Sur⪯SYr
其中
H
=
Θ
t
Q
c
Θ
+
R
c
K
=
2
Θ
T
Q
c
N
S
d
u
l
=
[
−
I
I
]
S
d
u
r
=
[
−
Δ
U
m
i
n
Δ
U
m
a
x
]
S
u
l
=
[
−
F
F
]
S
u
r
=
[
−
U
m
i
n
+
E
δ
(
−
1
)
U
m
a
x
−
E
δ
(
−
1
)
]
S
Y
l
=
[
−
Θ
Θ
]
S
Y
r
=
[
−
Y
l
b
+
Ω
ξ
(
0
)
+
Φ
Ψ
Y
u
b
−
Ω
ξ
(
0
)
−
Φ
Ψ
]
\begin{aligned} &H = \Theta^tQ_c\Theta+R_c \\ &K = 2\Theta^TQ_cN \\ \\ &S^{l}_{du} = \begin{bmatrix} -I \\ \quad I \end{bmatrix} &&S^{r}_{du} = \begin{bmatrix} -\Delta U^{min} \\ \quad\Delta U^{max} \end{bmatrix} \\ \\ &S^{l}_{u} = \begin{bmatrix} -F\\ \quad F \end{bmatrix} &&S^{r}_{u} = \begin{bmatrix} -U^{min}+E\delta(-1)\\ \quad U^{max}-E\delta(-1) \end{bmatrix}\\\\ &S^{l}_{Y}=\begin{bmatrix} -\Theta\\ \quad\Theta \end{bmatrix} &&S^{r}_{Y} = \begin{bmatrix} -Y_{lb}+\Omega\xi(0)+\Phi\Psi \\ \quad Y_{ub}-\Omega\xi(0)-\Phi\Psi \end{bmatrix} \end{aligned}
H=ΘtQcΘ+RcK=2ΘTQcNSdul=[−II]Sul=[−FF]SYl=[−ΘΘ]Sdur=[−ΔUminΔUmax]Sur=[−Umin+Eδ(−1)Umax−Eδ(−1)]SYr=[−Ylb+Ωξ(0)+ΦΨYub−Ωξ(0)−ΦΨ]
第二章完结撒花
标签：begin,系列,MPC,求解,cd,widetilde,bmatrix,Delta,end 来源： https://blog.csdn.net/weixin_42623186/article/details/114082510