模型预测控制器(MPC)系列: 2.求解车辆横向控制中的MPC

求解MPC: 在滚动时间窗内建立并求解QP问题

该小节旨在根据上一节得到的离散车辆横向动力学模型,在滚动时间窗内建立并求解QP问题,实现MPC横向控制.
在开始求解之前,我们需要回答以下两个问题:
$$\begin{array}{rl}& 1.什么是QP问题?其标准形式的什么样的?\\ & 2.怎么把MPC中的优化问题转化为求解QP问题?\end{array}$$
简要回答第一个问题: QP(Quadratic programming),即二次规划.旨在求解最小化一个带线性约束的多元二次型代价函数的最优变量值.其标准形式为
$$\begin{array}{rlrl}& \underset{x}{\min }J=\underset{x}{\min }& & \frac{1}{2}{x}^{T}Qx+x^{T}g\\ & s.t.& & Ax\le b\end{array}$$
其中,当且仅当 $Q⪰0$,即 $Q$ 为半正定矩阵时,QP问题为一个凸优化问题.

目前有很多开源的QP求解库,如 $OSQP/qpOASES$,以及 $Matlab$ 中的 $quadprog$ 都为以上QP问题提供了求解的API.

接下来回答第二个问题,如何将MPC转化为求解QP问题?
通常的,MPC控制算法包含以下四个基本步骤:
$$\begin{array}{rl}& \text{1.根据已知的系统模型,控制序列与当前的系统状态变量,对预测时域内各状态变量作出预测}\\ & \text{2.根据第一步的预测结果,计算预测时域内,满足约束的条件,最小化损失函数 J 的控制序列}\\ & \text{3.将第二步中得到的控制序列的第一个控制量输入到系统中}\\ & \text{4.在下一次采样时间,将时间往前推动一步,更新第一步中的当前的系统状态变量,重复步骤1-3}\end{array}$$
步骤一中,我们需要根据当前系统状态和已知的控制序列对整个时间窗内的状态变量值作出预测. 我们可以通过迭代系统的离散模型得到 $t+k$ 时刻的状态变量与 $t$ 时刻(当前时刻)的状态变量和控制序列之间的关系,即系统的预测方程.

步骤二中,我们将时间窗内的系统性能期望构造为一个二次型损失函数,并将控制过程中所涉及到的约束条件都改写为 $Ax⪯b$ 的形式.至此,我们便构造了一个标准QP问题,接下来就是采用合适的优化算法(GD,Newton,ADMM等)对问题进行求解,得到时间窗内的最优控制序列.

步骤三中,我们只将控制序列中的第一个控制量作为系统的真实输入.采用这种策略,我认为原因有二.
原因一是因为步骤一和步骤二中的预测以及优化过程均为开环.在系统不确定度的影响下,这种计算是粗略的.且预测时域越长,与实际系统的偏差越大.因此,我们需要引入真实系统的反馈去修正预测.那么在当前时刻,新的系统反馈抵达之前,采用控制序列中的第一个控制量作为系统的真实输入是合适的.

略作延伸, 我们可以把MPC算法与Kalman滤波算法作对比.Kalman滤波算法中也有相似的预测过程,同理,若想要预测结果收敛于真值,单有预测是不够的.我们需要持续将最新的观测纳入考虑,利用其中蕴含的信息对预测过程中的相关量进行修正.Kalman的做法是通过贝叶斯定理,利用观测量对协方差矩阵进行修正.

那么MPC是如何利用最新的观测的呢? MPC的策略是在进入下一次采样时间后, 将最新的观测量转化为预测步骤的初始条件,然后在新的时间窗内进行预测,优化,即步骤四.这种方法称为滚动时域优化,因此MPC也称作滚动时域控制.
由此,我们便引出了原因二.通过滚动时域的方法,利用真实系统的反馈不断修正控制量,保障步骤三的操作可以满足最终的控制目标.而每一个时间片的控制过程又符合优化损失函数的预期.两者相互配合,形成闭环.

值得一提的是,滚动时域的策略使MPC的优化求解都是在一个有限时间的窗口下进行的,因此是无法保障得到整个时域的全局最优.但是因为持续优化,最终也能获得较好的结果. 虽然MPC策略损失了全局最优性,但其获得了更大灵活性.M因此相较于如LQR/LQG等最优控制器,MPC对于复杂非线性约束的对应更加自如,以及模型误差容忍度更高.

综上,回答第二个问题:通过滚动时域优化的方法,将MPC算法转化为在每一个时间窗下求解一个QP问题.

预测方程

我们将离散误差动力学模型改写为以控制量增量 $\Delta \delta$ 的输入的增量式模型.当然了你也可以不用增量式模型,直接用原有模型也是可以的.这并不影响预测方程的形式,只不过我选择了增量式模型作为例子,MPC的模型和损失函数的构造都是很灵活的.回到主题,增量式模型如下:
$$\begin{array}{rl}x(k+1)& =A_{d}x(k)+B_d\delta (k)+B_{cd}{\dot{\psi }}_{des}(k)\\ & =A_{d}x(k)+B_d[\delta (k-1)+\Delta \delta (k)]+B_{cd}{\dot{\psi }}_{des}(k)\end{array}$$

进一步的, 将以上模型改写为矩阵形式,可得
$$\begin{array}{rl}& \left[\begin{array}{c}x(k+1)\\ \delta (k)\end{array}\right]=\left[\begin{array}{cc}{A}_d& B_d\\ 0& I\end{array}\right]\left[\begin{array}{c}x(k)\\ \delta (k-1)\end{array}\right]+\left[\begin{array}{c}{B}_d\\ I\end{array}\right]\Delta \delta (k)+\left[\begin{array}{c}{B}_{cd}\\ 0\end{array}\right]{\dot{\psi }}_{des}(k)\\ & \\ & y(k)=\left[\begin{array}{cc}{C}_d& 0\end{array}\right]\left[\begin{array}{c}x(k)\\ \delta (k-1)\end{array}\right]\end{array}$$

定义
$$\begin{array}{rl}& \xi (k|t)=\left[\begin{array}{c}x(k)\\ \delta (k-1)\end{array}\right]{\tilde{A}}_d=\left[\begin{array}{cc}{A}_d& B_d\\ 0& I\end{array}\right]\\ & {\tilde{B}}_d=\left[\begin{array}{c}{B}_d\\ I\end{array}\right]{\tilde{B}}_{cd}=\left[\begin{array}{c}{B}_{cd}\\ 0\end{array}\right]{\tilde{C}}_d=\left[\begin{array}{cc}{C}_d& 0\end{array}\right]\end{array}$$

其中, $\xi (k|t)$ 代表在$t$时刻,预测$k$个周期得到的状态.为了简便, 在后续的使用中 $\xi (k)$ 与 $\xi (k|t)$ 通用.

综上,增量式离散误差动力学模型为
$$\begin{array}{rl}& \xi (k+1)={\tilde{A}}_d\xi (k)+{\tilde{B}}_d\Delta \delta (k)+{\tilde{B}}_{cd}{\dot{\psi }}_{des}(k)\\ & y(k)={\tilde{C}}_d\xi (k)\end{array}$$

假设预测步数为$k$,即MPC的预测时域为$k$个周期,则预测方程有
$$\begin{array}{rlrl}& \xi (1)& & ={\tilde{A}}_d\xi (0)+{\tilde{B}}_d\Delta \delta (0)+{\tilde{B}}_{cd}{\dot{\psi }}_{des}(0)\\ & & & \\ & \xi (2)& & ={\tilde{A}}_d\xi (1)+{\tilde{B}}_d\Delta \delta (1)+{\tilde{B}}_{cd}{\dot{\psi }}_{des}(1)\\ & & & ={\tilde{A}}_d[{\tilde{A}}_d\xi (0)+{\tilde{B}}_d\Delta \delta (0)+{\tilde{B}}_{cd}{\dot{\psi }}_{des}(0)]+{\tilde{B}}_d\Delta \delta (1)+{\tilde{B}}_{cd}{\dot{\psi }}_{des}(1)\\ & & & ={\tilde{A}}_d^2\xi (0)+{\tilde{A}}_d{\tilde{B}}_d\Delta \delta (0)+{\tilde{B}}_d\Delta \delta (1)+{\tilde{A}}_d{\tilde{B}}_{cd}{\dot{\psi }}_{des}(0)+{\tilde{B}}_{cd}{\dot{\psi }}_{des}(1)\\ & & & \\ & \xi (3)& & ={\tilde{A}}_d\xi (2)+{\tilde{B}}_d\Delta \delta (2)+{\tilde{B}}_{cd}{\dot{\psi }}_{des}(2)\\ & & & ={\tilde{A}}_d[{\tilde{A}}_d^2\xi (0)+{\tilde{A}}_d{\tilde{B}}_d\Delta \delta (0)+{\tilde{B}}_d\Delta \delta (1)+{\tilde{A}}_d{\tilde{B}}_{cd}{\dot{\psi }}_{des}(0)+{\tilde{B}}_{cd}{\dot{\psi }}_{des}(1)]+{\tilde{B}}_d\Delta \delta (2)+{\tilde{B}}_{cd}{\dot{\psi }}_{des}(2)\\ & & & ={\tilde{A}}_d^3\xi (0)+{\tilde{A}}_d^2{\tilde{B}}_d\Delta \delta (0)+{\tilde{A}}_d{\tilde{B}}_d\Delta \delta (1)+{\tilde{B}}_d\Delta \delta (2)+{\tilde{A}}_d^2{\tilde{B}}_{cd}{\dot{\psi }}_{des}(0)+{\tilde{A}}_d{\tilde{B}}_{cd}{\dot{\psi }}_{des}(1)+{\tilde{B}}_{cd}{\dot{\psi }}_{des}(2)\\ & & & \\ & ...& & \\ & & & \\ & \xi (k)& & ={\tilde{A}}_d^{(k)}\xi (0)+\sum _{i=0}^{k-1}{\tilde{A}}_d^{(i)}{\tilde{B}}_d\Delta \delta (k-i-1)+\sum _{j=0}^{k-1}{\tilde{A}}_d^{(j)}{\tilde{B}}_{cd}{\dot{\psi }}_{des}(k-j-1)\\ & y(k)& & ={\tilde{C}}_d\xi (k)={\tilde{C}}_d{\tilde{A}}_d^{(k)}\xi (0)+{\tilde{C}}_d\sum _{i=0}^{k-1}{\tilde{A}}_d^{(i)}{\tilde{B}}_d\Delta \delta (k-i-1)+{\tilde{C}}_d\sum _{j=0}^{k-1}{\tilde{A}}_d^{(j)}{\tilde{B}}_{cd}{\dot{\psi }}_{des}(k-j-1)\end{array}$$

基于以上的预测方程,我们可以通过$t$时刻的状态量初值 $\xi (0)$,建立状态量 $\xi$ 在 $k$ 个时域预测周期内与各周期控制增量 $\Delta \delta$ 之间的联系,即
$$\left[\begin{array}{c}\xi (t+1|t)\\ \xi (t+2|t)\\ \xi (t+3|t)\\ ⋮\\ \xi (t+k|t)\end{array}\right]=\left[\begin{array}{c}{\tilde{A}}_d\\ {\tilde{A}}_d^2\\ {\tilde{A}}_d^3\\ ⋮\\ {\tilde{A}}_d^k\end{array}\right]\xi (0)+\left[\begin{array}{ccccc}{\tilde{B}}_d& 0& 0& \dots & 0\\ {\tilde{A}}_d{\tilde{B}}_d& {\tilde{B}}_d& 0& \dots & 0\\ {\tilde{A}}_d^2{\tilde{B}}_d& {\tilde{A}}_d{\tilde{B}}_d& {\tilde{B}}_d& \dots & 0\\ ⋮& ⋮& ⋮& \ddots & ⋮\\ {\tilde{A}}_d^{k-1}{\tilde{B}}_d& {\tilde{A}}_d^{k-2}{\tilde{B}}_d& {\tilde{A}}_d^{k-3}{\tilde{B}}_d& \dots & {\tilde{B}}_d\end{array}\right]\left[\begin{array}{c}\Delta \delta (0)\\ \Delta \delta (1)\\ \Delta \delta (2)\\ ⋮\\ \Delta \delta (k-1)\end{array}\right]+\left[\begin{array}{ccccc}{\tilde{B}}_{cd}& 0& 0& \dots & 0\\ {\tilde{A}}_d{\tilde{B}}_{cd}& {\tilde{B}}_{cd}& 0& \dots & 0\\ {\tilde{A}}_d^2{\tilde{B}}_{cd}& {\tilde{A}}_d{\tilde{B}}_{cd}& {\tilde{B}}_{cd}& \dots & 0\\ ⋮& ⋮& ⋮& \ddots & ⋮\\ {\tilde{A}}_d^{k-1}{\tilde{B}}_{cd}& {\tilde{A}}_d^{k-2}{\tilde{B}}_{cd}& {\tilde{A}}_d^{k-3}{\tilde{B}}_{cd}& \dots & {\tilde{B}}_{cd}\end{array}\right]\left[\begin{array}{c}{\dot{\psi }}_{des}(0)\\ {\dot{\psi }}_{des}(1)\\ {\dot{\psi }}_{des}(2)\\ ⋮\\ {\dot{\psi }}_{des}(k-1)\end{array}\right]$$

定义
$$\begin{array}{rl}& X=\left[\begin{array}{c}\xi (t+1|t)\\ \xi (t+2|t)\\ ⋮\\ \xi (t+k|t)\end{array}\right]\Delta U=\left[\begin{array}{c}\Delta \delta (0)\\ \Delta \delta (1)\\ ⋮\\ \Delta \delta (k-1)\end{array}\right]\Lambda =\left[\begin{array}{c}{\tilde{A}}_d\\ {\tilde{A}}_d^2\\ ⋮\\ {\tilde{A}}_d^k\end{array}\right]\Psi =\left[\begin{array}{c}{\dot{\psi }}_{des}(0)\\ {\dot{\psi }}_{des}(1)\\ {\dot{\psi }}_{des}(2)\\ ⋮\\ {\dot{\psi }}_{des}(k-1)\end{array}\right]\\ & \\ & \Gamma =\left[\begin{array}{ccccc}{\tilde{B}}_d& 0& 0& \dots & 0\\ {\tilde{A}}_d{\tilde{B}}_d& {\tilde{B}}_d& 0& \dots & 0\\ {\tilde{A}}_d^2{\tilde{B}}_d& {\tilde{A}}_d{\tilde{B}}_d& {\tilde{B}}_d& \dots & 0\\ ⋮& ⋮& ⋮& \ddots & ⋮\\ {\tilde{A}}_d^{k-1}{\tilde{B}}_d& {\tilde{A}}_d^{k-2}{\tilde{B}}_d& {\tilde{A}}_d^{k-3}{\tilde{B}}_d& \dots & {\tilde{B}}_d\end{array}\right]\\ & \\ & {\rm Y}=\left[\begin{array}{ccccc}{\tilde{B}}_{cd}& 0& 0& \dots & 0\\ {\tilde{A}}_d{\tilde{B}}_{cd}& {\tilde{B}}_{cd}& 0& \dots & 0\\ {\tilde{A}}_d^2{\tilde{B}}_{cd}& {\tilde{A}}_d{\tilde{B}}_{cd}& {\tilde{B}}_{cd}& \dots & 0\\ ⋮& ⋮& ⋮& \ddots & ⋮\\ {\tilde{A}}_d^{k-1}{\tilde{B}}_{cd}& {\tilde{A}}_d^{k-2}{\tilde{B}}_{cd}& {\tilde{A}}_d^{k-3}{\tilde{B}}_{cd}& \dots & {\tilde{B}}_{cd}\end{array}\right]\end{array}$$

进一步的,定义
$$\begin{array}{rl}& Y=\left[\begin{array}{c}y(t+1|t)\\ y(t+2|t)\\ ⋮\\ y(t+k|t)\end{array}\right]\Omega =\left[\begin{array}{c}{\tilde{C}}_d{\tilde{A}}_d\\ {\tilde{C}}_d{\tilde{A}}_d^2\\ ⋮\\ {\tilde{C}}_d{\tilde{A}}_d^k\end{array}\right]\\ & \\ & \Theta =\left[\begin{array}{ccccc}{\tilde{C}}_d{\tilde{B}}_d& 0& 0& \dots & 0\\ {\tilde{C}}_d{\tilde{A}}_d{\tilde{B}}_d& {\tilde{C}}_d{\tilde{B}}_d& 0& \dots & 0\\ {\tilde{C}}_d{\tilde{A}}_d^2{\tilde{B}}_d& {\tilde{C}}_d{\tilde{A}}_d{\tilde{B}}_d& {\tilde{C}}_d{\tilde{B}}_d& \dots & 0\\ ⋮& ⋮& ⋮& \ddots & ⋮\\ {\tilde{C}}_d{\tilde{A}}_d^{k-1}{\tilde{B}}_d& {\tilde{C}}_d{\tilde{A}}_d^{k-2}{\tilde{B}}_d& {\tilde{C}}_d{\tilde{A}}_d^{k-3}{\tilde{B}}_d& \dots & {\tilde{C}}_d{\tilde{B}}_d\end{array}\right]\\ & \\ & \Phi =\left[\begin{array}{ccccc}{\tilde{C}}_d{\tilde{B}}_{cd}& 0& 0& \dots & 0\\ {\tilde{C}}_d{\tilde{A}}_d{\tilde{B}}_{cd}& {\tilde{C}}_d{\tilde{B}}_{cd}& 0& \dots & 0\\ {\tilde{C}}_d{\tilde{A}}_d^2{\tilde{B}}_{cd}& {\tilde{C}}_d{\tilde{A}}_d{\tilde{B}}_{cd}& {\tilde{C}}_d{\tilde{B}}_{cd}& \dots & 0\\ ⋮& ⋮& ⋮& \ddots & ⋮\\ {\tilde{C}}_d{\tilde{A}}_d^{k-1}{\tilde{B}}_{cd}& {\tilde{C}}_d{\tilde{A}}_d^{k-2}{\tilde{B}}_{cd}& {\tilde{C}}_d{\tilde{A}}_d^{k-3}{\tilde{B}}_{cd}& \dots & {\tilde{C}}_d{\tilde{B}}_{cd}\end{array}\right]\end{array}$$

综上有
$$\begin{array}{rl}& X=\Lambda \xi (0)+\Gamma \Delta U+{\rm Y}\Psi \\ & Y=\Omega \xi (0)+\Theta \Delta U+\Phi \Psi \end{array}$$

代价函数

我构造的代价函数惩罚输出量以及控制增量,因此
$$\begin{array}{rl}J& =||Y-Y_{des}|{|}_{Q_c}+||\Delta U|{|}_{R_c}\\ & =(Y-Y_{des}{)}^T{Q}_c(Y-Y_{des})+\Delta U^T{R}_c\Delta U\\ & =(\Omega \xi (0)+\Theta \Delta U+\Phi \Psi -Y_{des}{)}^T{Q}_c(\Omega \xi (0)+\Theta \Delta U+\Phi \Psi -Y_{des})+\Delta U^T{R}_c\Delta U\end{array}$$

我们优化的对象参数是 $\Delta U$,因此将代价函数中不含对象参数的部分定义为 $N$,则
$$N=\Omega \xi (0)+\Phi \Psi -Y_{des}$$

将 $N$ 代入到代价函数中可得
$$\begin{array}{rl}J& =(N+\Theta \Delta U{)}^T{Q}_c(N+\Theta \Delta U)+\Delta U^T{R}_c\Delta U\\ & =N^T{Q}_{c}N+2\Delta U^T{\Theta }^T{Q}_{c}N+\Delta U^T{\Theta }^t{Q}_c\Theta \Delta U+\Delta U^T{R}_c\Delta U\\ & =N^T{Q}_{c}N+2\Delta U^T{\Theta }^T{Q}_{c}N+\Delta U^T({\Theta }^t{Q}_c\Theta +R_c)\Delta U\end{array}$$

针对该优化问题,我们可以进一步简化代价函数$J$,即消去代价函数中不含优化对象参数 $\Delta U$的项
$$\begin{array}{rl}\underset{\Delta U}{\mathrm{argmin}}(J)& =\underset{\Delta U}{\mathrm{argmin}}(N^T{Q}_{c}N+2\Delta U^T{\Theta }^T{Q}_{c}N+\Delta U^T({\Theta }^t{Q}_c\Theta +R_c)\Delta U)\\ & =\underset{\Delta U}{\mathrm{argmin}}(2\Delta U^T{\Theta }^T{Q}_{c}N+\Delta U^T({\Theta }^t{Q}_c\Theta +R_c)\Delta U)\\ & =\underset{\Delta U}{\mathrm{argmin}}(J^{\ast })\end{array}$$
其中, $J^{\ast }$ 为简化后的代价函数

定义
$$\begin{array}{rl}& H={\Theta }^t{Q}_c\Theta +R_c\\ & K=2{\Theta }^T{Q}_{c}N\end{array}$$

则QP问题为
$$\underset{\Delta U}{\min }(J^{\ast })=\underset{\Delta U}{\min }(\Delta U^{T}H\Delta U+\Delta U^{T}K)$$

约束条件

控制增量约束

在增量式模型中,我们优化的对象参数是控制量量增量 $\Delta U$,因此对控制量增量直接约束为
$$\Delta U^{min}⪯\Delta U⪯\Delta U^{max}$$
其中
$$\begin{array}{rl}& \Delta U^{min}={\left[\begin{array}{cccc}\Delta \delta (0{)}^{min}& \Delta \delta (1{)}^{min}& \dots & \Delta \delta (k-1{)}^{min}\end{array}\right]}^T\\ & \Delta U^{max}={\left[\begin{array}{cccc}\Delta \delta (0{)}^{max}& \Delta \delta (1{)}^{max}& \dots & \Delta \delta (k-1{)}^{max}\end{array}\right]}^T\end{array}$$
以上约束条件可改写成

$$\left[\begin{array}{rrrrr}-1& 0& \dots & 0& 0\\ 0& -1& \dots & 0& 0\\ ⋮& ⋮& \ddots & ⋮& ⋮\\ 0& 0& \dots & -1& 0\\ 0& 0& \dots & 0& -1\\ 1& 0& \dots & 0& 0\\ 0& 1& \dots & 0& 0\\ ⋮& ⋮& \ddots & ⋮& ⋮\\ 0& 0& \dots & 1& 0\\ 0& 0& \dots & 0& 1\end{array}\right]\left[\begin{array}{c}\Delta \delta (0)\\ \Delta \delta (1)\\ ⋮\\ \Delta \delta (k-2)\\ \Delta \delta (k-1)\end{array}\right]⪯\left[\begin{array}{c}-\Delta \delta (0{)}^{min}\\ -\Delta \delta (1{)}^{min}\\ ⋮\\ -\Delta \delta (k-2{)}^{min}\\ -\Delta \delta (k-1{)}^{min}\\ \Delta \delta (0{)}^{max}\\ \Delta \delta (1{)}^{max}\\ ⋮\\ \Delta \delta (k-2{)}^{max}\\ \Delta \delta (k-1{)}^{max}\end{array}\right]$$

定义
$$\begin{array}{rl}& S_{du}^l=\left[\begin{array}{rrrrr}-1& 0& \dots & 0& 0\\ 0& -1& \dots & 0& 0\\ ⋮& ⋮& \ddots & ⋮& ⋮\\ 0& 0& \dots & -1& 0\\ 0& 0& \dots & 0& -1\\ 1& 0& \dots & 0& 0\\ 0& 1& \dots & 0& 0\\ ⋮& ⋮& \ddots & ⋮& ⋮\\ 0& 0& \dots & 1& 0\\ 0& 0& \dots & 0& 1\end{array}\right]=\left[\begin{array}{c}-I\\ I\end{array}\right]\\ & S_{du}^r=\left[\begin{array}{c}-\Delta \delta (0{)}^{min}\\ -\Delta \delta (1{)}^{min}\\ ⋮\\ -\Delta \delta (k-2{)}^{min}\\ -\Delta \delta (k-1{)}^{min}\\ \Delta \delta (0{)}^{max}\\ \Delta \delta (1{)}^{max}\\ ⋮\\ \Delta \delta (k-2{)}^{max}\\ \Delta \delta (k-1{)}^{max}\end{array}\right]=\left[\begin{array}{c}-\Delta U^{min}\\ \Delta U^{max}\end{array}\right]\end{array}$$
则控制增量的约束可表示为
$$S_{du}^l\Delta U⪯S_{du}^r$$

控制量约束

首先我们要将控制量矩阵 $U$ 以控制增量 $\Delta U$ 的形式表达
$$U=\left[\begin{array}{c}\delta (0)\\ \delta (1)\\ ⋮\\ \delta (k-2)\\ \delta (k-1)\end{array}\right]=\left[\begin{array}{c}1\\ 1\\ ⋮\\ 1\\ 1\end{array}\right]\delta (-1)+\left[\begin{array}{cccccc}1& 0& 0& \dots & 0& 0\\ 1& 1& 0& \dots & 0& 0\\ 1& 1& 1& \dots & 0& 0\\ ⋮& ⋮& ⋮& \ddots & ⋮& ⋮\\ 1& 1& 1& \dots & 1& 0\\ 1& 1& 1& \dots & 1& 1\end{array}\right]\left[\begin{array}{c}\Delta \delta (0)\\ \Delta \delta (1)\\ ⋮\\ \Delta \delta (k-2)\\ \Delta \delta (k-1)\end{array}\right]$$
其中 $\delta (-1)$ 代表 $t-1$ 时刻的控制量,即上一周期的控制量

定义
$$E=\left[\begin{array}{c}1\\ 1\\ ⋮\\ 1\\ 1\end{array}\right]F=\left[\begin{array}{cccccc}1& 0& 0& \dots & 0& 0\\ 1& 1& 0& \dots & 0& 0\\ 1& 1& 1& \dots & 0& 0\\ ⋮& ⋮& ⋮& \ddots & ⋮& ⋮\\ 1& 1& 1& \dots & 1& 0\\ 1& 1& 1& \dots & 1& 1\end{array}\right]$$

则上式可改写为
$$U=E\delta (-1)+F\Delta U$$

控制量 $U$ 需满足的约束条件为
$$U^{min}⪯U⪯U^{max}$$
其中
$$\begin{array}{rl}& U^{min}={\left[\begin{array}{cccc}\delta (0{)}^{min}& \delta (1{)}^{min}& \dots & \delta (k-1{)}^{min}\end{array}\right]}^T\\ & U^{max}={\left[\begin{array}{cccc}\delta (0{)}^{max}& \delta (1{)}^{max}& \dots & \delta (k-1{)}^{max}\end{array}\right]}^T\end{array}$$
根据以上关系,改写控制量约束
$$\left[\begin{array}{c}-F\\ F\end{array}\right]\Delta U⪯\left[\begin{array}{c}-U^{min}+E\delta (-1)\\ U^{max}-E\delta (-1)\end{array}\right]$$

定义
$$\begin{array}{rl}& S_u^l=\left[\begin{array}{c}-F\\ F\end{array}\right]\\ & S_u^r=\left[\begin{array}{c}-U^{min}+E\delta (-1)\\ U^{max}-E\delta (-1)\end{array}\right]\end{array}$$

则控制增量的约束可表示为

$$S_u^l\Delta U⪯S_u^r$$

状态量约束与输出量约束

状态量约束和输出量约束我们可以直接根据增量式动力学模型推导,步骤如下
$$\begin{array}{rl}& X=\Lambda \xi (0)+\Gamma \Delta U+{\rm Y}\Psi \\ & Y=\Omega \xi (0)+\Theta \Delta U+\Phi \Psi \\ & \\ & X_{lb}-\Lambda \xi (0)-{\rm Y}\Psi ⪯\Gamma \Delta U⪯X_{ub}-\Lambda \xi (0)-{\rm Y}\Psi \\ & Y_{lb}-\Omega \xi (0)-\Phi \Psi ⪯\Theta \Delta U⪯Y_{ub}-\Omega \xi (0)-\Phi \Psi \\ & \\ & \left[\begin{array}{c}-\Gamma \\ \Gamma \end{array}\right]\Delta U⪯\left[\begin{array}{c}-X_{lb}+\Lambda \xi (0)+{\rm Y}\Psi \\ X_{ub}-\Lambda \xi (0)-{\rm Y}\Psi \end{array}\right]\\ & \left[\begin{array}{c}-\Theta \\ \Theta \end{array}\right]\Delta U⪯\left[\begin{array}{c}-Y_{lb}+\Omega \xi (0)+\Phi \Psi \\ Y_{ub}-\Omega \xi (0)-\Phi \Psi \end{array}\right]\end{array}$$

定义

$$\begin{array}{rl}& S_X^l=\left[\begin{array}{c}-\Gamma \\ \Gamma \end{array}\right]S_X^r=\left[\begin{array}{c}-X_{lb}+\Lambda \xi (0)+{\rm Y}\Psi \\ X_{ub}-\Lambda \xi (0)-{\rm Y}\Psi \end{array}\right]\\ & S_Y^l=\left[\begin{array}{c}-\Theta \\ \Theta \end{array}\right]S_Y^r=\left[\begin{array}{c}-Y_{lb}+\Omega \xi (0)+\Phi \Psi \\ Y_{ub}-\Omega \xi (0)-\Phi \Psi \end{array}\right]\end{array}$$

则状态量与输出量的约束可表示为

$$\begin{array}{rl}& S_X^l\Delta U⪯S_X^r\\ & S_Y^l\Delta U⪯S_Y^r\end{array}$$

值得注意的是,因为本文中代价函数惩罚的是输出量,因此我们选择输出量约束作为QR问题的约束条件之一.

总结

综上,在每一个时间窗内MPC需求解的QR问题为:
$$\begin{array}{rl}& \underset{\Delta U}{\min }(J^{\ast })=\underset{\Delta U}{\min }(\Delta U^{T}H\Delta U+\Delta U^{T}K)\\ & \\ & s.t.\begin{array}{rlrlrl}& S_{du}^l& & \Delta U& & ⪯S_{du}^r\\ & S_u^l& & \Delta U& & ⪯S_u^r\\ & S_Y^l& & \Delta U& & ⪯S_Y^r\end{array}\end{array}$$
其中
$$\begin{array}{rlrl}& H={\Theta }^t{Q}_c\Theta +R_c& & \\ & K=2{\Theta }^T{Q}_{c}N& & \\ & & & \\ & S_{du}^l=\left[\begin{array}{c}-I\\ I\end{array}\right]& & S_{du}^r=\left[\begin{array}{c}-\Delta U^{min}\\ \Delta U^{max}\end{array}\right]\\ & & & \\ & S_u^l=\left[\begin{array}{c}-F\\ F\end{array}\right]& & S_u^r=\left[\begin{array}{c}-U^{min}+E\delta (-1)\\ U^{max}-E\delta (-1)\end{array}\right]\\ & & & \\ & S_Y^l=\left[\begin{array}{c}-\Theta \\ \Theta \end{array}\right]& & S_Y^r=\left[\begin{array}{c}-Y_{lb}+\Omega \xi (0)+\Phi \Psi \\ Y_{ub}-\Omega \xi (0)-\Phi \Psi \end{array}\right]\end{array}$$

_{第二章完结撒花}