其他分享
首页 > 其他分享> > 模型预测控制系列(MPC)系列:2.求解MPC问题

模型预测控制系列(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} ​xmin​J=xmin​s.t.​​21​xTQx+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)​=Ad​x(k)+Bd​δ(k)+Bcd​ψ˙​des​(k)=Ad​x(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)​]=[Ad​0​Bd​I​][x(k)δ(k−1)​]+[Bd​I​]Δδ(k)+[Bcd​0​]ψ˙​des​(k)y(k)=[Cd​​0​][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​=[Ad​0​Bd​I​]B d​=[Bd​I​]B cd​=[Bcd​0​]C d​=[Cd​​0​]​

其中, ξ ( 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 d​B d​Δδ(0)+B d​Δδ(1)+A d​B cd​ψ˙​des​(0)+B cd​ψ˙​des​(1)=A d​ξ(2)+B d​Δδ(2)+B cd​ψ˙​des​(2)=A d​[A d2​ξ(0)+A d​B d​Δδ(0)+B d​Δδ(1)+A d​B cd​ψ˙​des​(0)+B cd​ψ˙​des​(1)]+B d​Δδ(2)+B cd​ψ˙​des​(2)=A d3​ξ(0)+A d2​B d​Δδ(0)+A d​B d​Δδ(1)+B d​Δδ(2)+A d2​B cd​ψ˙​des​(0)+A d​B cd​ψ˙​des​(1)+B cd​ψ˙​des​(2)=A d(k)​ξ(0)+i=0∑k−1​A d(i)​B d​Δδ(k−i−1)+j=0∑k−1​A d(j)​B cd​ψ˙​des​(k−j−1)=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 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 d​A d2​A d3​⋮A dk​​⎦⎥⎥⎥⎥⎥⎤​ξ(0)+⎣⎢⎢⎢⎢⎢⎡​B d​A d​B d​A d2​B d​⋮A dk−1​B d​​0B d​A d​B d​⋮A dk−2​B d​​00B d​⋮A dk−3​B d​​………⋱…​000⋮B d​​⎦⎥⎥⎥⎥⎥⎤​⎣⎢⎢⎢⎢⎢⎡​Δδ(0)Δδ(1)Δδ(2)⋮Δδ(k−1)​⎦⎥⎥⎥⎥⎥⎤​+⎣⎢⎢⎢⎢⎢⎡​B cd​A d​B cd​A d2​B cd​⋮A dk−1​B cd​​0B cd​A d​B cd​⋮A dk−2​B cd​​00B cd​⋮A dk−3​B 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 d​A d2​⋮A dk​​⎦⎥⎥⎥⎤​Ψ=⎣⎢⎢⎢⎢⎢⎡​ψ˙​des​(0)ψ˙​des​(1)ψ˙​des​(2)⋮ψ˙​des​(k−1)​⎦⎥⎥⎥⎥⎥⎤​Γ=⎣⎢⎢⎢⎢⎢⎡​B d​A d​B d​A d2​B d​⋮A dk−1​B d​​0B d​A d​B d​⋮A dk−2​B d​​00B d​⋮A dk−3​B d​​………⋱…​000⋮B d​​⎦⎥⎥⎥⎥⎥⎤​Υ=⎣⎢⎢⎢⎢⎢⎡​B cd​A d​B cd​A d2​B cd​⋮A dk−1​B cd​​0B cd​A d​B cd​⋮A dk−2​B cd​​00B cd​⋮A dk−3​B 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 d​A d​C d​A d2​⋮C d​A dk​​⎦⎥⎥⎥⎤​Θ=⎣⎢⎢⎢⎢⎢⎡​C d​B d​C d​A d​B d​C d​A d2​B d​⋮C d​A dk−1​B d​​0C d​B d​C d​A d​B d​⋮C d​A dk−2​B d​​00C d​B d​⋮C d​A dk−3​B d​​………⋱…​000⋮C d​B d​​⎦⎥⎥⎥⎥⎥⎤​Φ=⎣⎢⎢⎢⎢⎢⎡​C d​B cd​C d​A d​B cd​C d​A d2​B cd​⋮C d​A dk−1​B cd​​0C d​B cd​C d​A d​B cd​⋮C d​A dk−2​B cd​​00C d​B cd​⋮C d​A dk−3​B cd​​………⋱…​000⋮C d​B 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)+UTRc​U=NTQc​N+2ΔUTΘTQc​N+ΔUTΘtQc​ΘΔU+ΔUTRc​ΔU=NTQc​N+2ΔUTΘTQc​N+Δ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​(NTQc​N+2ΔUTΘTQc​N+ΔUT(ΘtQc​Θ+Rc​)ΔU)=ΔUargmin​(2ΔUTΘTQc​N+Δ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​Θ+Rc​K=2ΘTQc​N​

则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⋮00​0−1⋮0001⋮00​……⋱…………⋱……​00⋮−1000⋮10​00⋮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⋮00​0−1⋮0001⋮00​……⋱…………⋱……​00⋮−1000⋮10​00⋮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⋮11​011⋮11​001⋮11​………⋱……​000⋮11​000⋮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⋮11​011⋮11​001⋮11​………⋱……​000⋮11​000⋮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⪯SXr​SYl​Δ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.​Sdul​Sul​SYl​​​Δ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​Θ+Rc​K=2ΘTQc​NSdul​=[−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