模型预测控制系列(MPC)系列: 2.求解MPC问题

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

该小节的旨在根据上一节得到的离散误差动力学模型,在MPC的滚动时间窗内建立并求解QP问题.
首先,我们要回答以下两个问题:
$$\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$ 需为半正定矩阵,因为当且仅当 $Q⪰0$时,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}$$
步骤一中,我们需要根据当前系统状态和已知的控制序列对时间窗内的状态变量值作出预测.这可以通过对系统的离散动力学模型作差分来实现,即计算得到系统的预测方程.

步骤二中,我们要将控制过程的性能期望构造为一个二次型损失函数,并把过程中的约束条件改写为 $Ax⪯b$ 的形式.至此,就完成了一个标准QP问题的构造,接下来就是通过合适的优化方法求解该问题.

步骤三中,我们只将控制序列中的第一项作为真实控制量输入.在我看来,这一步是MPC策略的精髓.采用这种策略,我认为原因有二.
原因一是因为步骤一和步骤二中的预测以及优化过程均为开环,在系统不确定度的影响下,这种计算是粗略的.
略作延伸, 前两步操作其实和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]A_d=\left[\begin{array}{cc}{A}_d& B_d\\ 0& I\end{array}\right]\\ & B_d=\left[\begin{array}{c}{B}_d\\ I\end{array}\right]B_{cd}=\left[\begin{array}{c}{B}_{cd}\\ 0\end{array}\right]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)=A_d\xi (k)+B_d\Delta \delta (k)+B_{cd}{\dot{\psi }}_{des}(k)\\ & y(k)=C_d\xi (k)\end{array}$$

假设预测步数为$k$,即MPC的预测时域为$k$个周期,则预测方程有
$$\begin{array}{rlrl}& \xi (1)& & =A_d\xi (0)+B_d\Delta \delta (0)+B_{cd}{\dot{\psi }}_{des}(0)\\ & & & \\ & \xi (2)& & =A_d\xi (1)+B_d\Delta \delta (1)+B_{cd}{\dot{\psi }}_{des}(1)\\ & & & =A_d[A_d\xi (0)+B_d\Delta \delta (0)+B_{cd}{\dot{\psi }}_{des}(0)]+B_d\Delta \delta (1)+B_{cd}{\dot{\psi }}_{des}(1)\\ & & & =A_d^2\xi (0)+A_d{B}_d\Delta \delta (0)+B_d\Delta \delta (1)+A_d{B}_{cd}{\dot{\psi }}_{des}(0)+B_{cd}{\dot{\psi }}_{des}(1)\\ & & & \\ & \xi (3)& & =A_d\xi (2)+B_d\Delta \delta (2)+B_{cd}{\dot{\psi }}_{des}(2)\\ & & & =A_d[A_d^2\xi (0)+A_d{B}_d\Delta \delta (0)+B_d\Delta \delta (1)+A_d{B}_{cd}{\dot{\psi }}_{des}(0)+B_{cd}{\dot{\psi }}_{des}(1)]+B_d\Delta \delta (2)+B_{cd}{\dot{\psi }}_{des}(2)\\ & & & =A_d^3\xi (0)+A_d^2{B}_d\Delta \delta (0)+A_d{B}_d\Delta \delta (1)+B_d\Delta \delta (2)+A_d^2{B}_{cd}{\dot{\psi }}_{des}(0)+A_d{B}_{cd}{\dot{\psi }}_{des}(1)+B_{cd}{\dot{\psi }}_{des}(2)\\ & & & \\ & ...& & \\ & & & \\ & \xi (k)& & =A_d^{(k)}\xi (0)+\sum _{i=0}^{k-1}{A}_d^{(i)}{B}_d\Delta \delta (k-i-1)+\sum _{j=0}^{k-1}{A}_d^{(j)}{B}_{cd}{\dot{\psi }}_{des}(k-j-1)\\ & y(k)& & =C_d\xi (k)=C_d{A}_d^{(k)}\xi (0)+C_d\sum _{i=0}^{k-1}{A}_d^{(i)}{B}_d\Delta \delta (k-i-1)+C_d\sum _{j=0}^{k-1}{A}_d^{(j)}{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}{A}_d\\ A_d^2\\ A_d^3\\ ⋮\\ A_d^k\end{array}\right]\xi (0)+\left[\begin{array}{ccccc}{B}_d& 0& 0& \dots & 0\\ A_d{B}_d& B_d& 0& \dots & 0\\ A_d^2{B}_d& A_d{B}_d& B_d& \dots & 0\\ ⋮& ⋮& ⋮& \ddots & ⋮\\ A_d^{k-1}{B}_d& A_d^{k-2}{B}_d& A_d^{k-3}{B}_d& \dots & 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}{B}_{cd}& 0& 0& \dots & 0\\ A_d{B}_{cd}& B_{cd}& 0& \dots & 0\\ A_d^2{B}_{cd}& A_d{B}_{cd}& B_{cd}& \dots & 0\\ ⋮& ⋮& ⋮& \ddots & ⋮\\ A_d^{k-1}{B}_{cd}& A_d^{k-2}{B}_{cd}& A_d^{k-3}{B}_{cd}& \dots & 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}{A}_d\\ A_d^2\\ ⋮\\ 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}{B}_d& 0& 0& \dots & 0\\ A_d{B}_d& B_d& 0& \dots & 0\\ A_d^2{B}_d& A_d{B}_d& B_d& \dots & 0\\ ⋮& ⋮& ⋮& \ddots & ⋮\\ A_d^{k-1}{B}_d& A_d^{k-2}{B}_d& A_d^{k-3}{B}_d& \dots & B_d\end{array}\right]\\ & \\ & {\rm Y}=\left[\begin{array}{ccccc}{B}_{cd}& 0& 0& \dots & 0\\ A_d{B}_{cd}& B_{cd}& 0& \dots & 0\\ A_d^2{B}_{cd}& A_d{B}_{cd}& B_{cd}& \dots & 0\\ ⋮& ⋮& ⋮& \ddots & ⋮\\ A_d^{k-1}{B}_{cd}& A_d^{k-2}{B}_{cd}& A_d^{k-3}{B}_{cd}& \dots & 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}{C}_d{A}_d\\ C_d{A}_d^2\\ ⋮\\ C_d{A}_d^k\end{array}\right]\\ & \\ & \Theta =\left[\begin{array}{ccccc}{C}_d{B}_d& 0& 0& \dots & 0\\ C_d{A}_d{B}_d& C_d{B}_d& 0& \dots & 0\\ C_d{A}_d^2{B}_d& C_d{A}_d{B}_d& C_d{B}_d& \dots & 0\\ ⋮& ⋮& ⋮& \ddots & ⋮\\ C_d{A}_d^{k-1}{B}_d& C_d{A}_d^{k-2}{B}_d& C_d{A}_d^{k-3}{B}_d& \dots & C_d{B}_d\end{array}\right]\\ & \\ & \Phi =\left[\begin{array}{ccccc}{C}_d{B}_{cd}& 0& 0& \dots & 0\\ C_d{A}_d{B}_{cd}& C_d{B}_{cd}& 0& \dots & 0\\ C_d{A}_d^2{B}_{cd}& C_d{A}_d{B}_{cd}& C_d{B}_{cd}& \dots & 0\\ ⋮& ⋮& ⋮& \ddots & ⋮\\ C_d{A}_d^{k-1}{B}_{cd}& C_d{A}_d^{k-2}{B}_{cd}& C_d{A}_d^{k-3}{B}_{cd}& \dots & C_d{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 = [ 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​