MPC-模型预测控制-公式推导

MPC-模型预测控制

MPC推导过程梳理，在此记录。
DR_CAN老师原视频链接：【MPC模型预测控制器】。

状态方程

离散系统状态方程
$$\mathbf{x}(k+1)=\mathbf{Ax}(k)+\mathbf{Bu}(k)\text{\hspace{1em}(1)}$$
其中
$$\begin{gathered} \underset{m\times 1}{\mathbf{x}}={\left(\begin{array}{cccc}{x}_1& x_2& \cdots & x_m\end{array}\right)}^T \\ \underset{n\times 1}{\mathbf{u}}={\left(\begin{array}{cccc}{u}_1& u_2& \cdots & u_n\end{array}\right)}^T \\ \underset{m\times m}{\mathbf{A}} \\ \underset{m\times n}{\mathbf{B}}\text{\hspace{1em}(2)} \end{gathered}$$
在第$k$时刻，对未来$N$步进行预测，采样时间间隔为$\Delta t$
$$\underset{nN\times 1}{{\mathbb{U}}_k}=\left(\begin{array}{c}\mathbf{u}(k|k)\\ \mathbf{u}(k+1|k)\\ ⋮\\ \mathbf{u}(k+i|k)\\ ⋮\\ \mathbf{u}(k+N-1|k)\end{array}\right)\underset{m(N+1)\times 1}{{\mathbb{X}}_k}=\left(\begin{array}{c}\mathbf{x}(k|k)\\ \mathbf{x}(k+1|k)\\ ⋮\\ \mathbf{x}(k+i|k)\\ ⋮\\ \mathbf{x}(k+N|k)\end{array}\right)\text{\hspace{1em}(3)}$$
为简化问题，输出量等于状态量$\mathbf{y}=\mathbf{x}$，参考量$\mathbf{R}=0$，误差可以表达为：
$$\mathbf{E}=\mathbf{y}-\mathbf{R}=\mathbf{x}$$
代价函数由误差代价加权和，输入代价加权和，终端误差加权和组成：
$$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)\text{\hspace{1em}(4)}$$

问题描述

二次规划的一般形式
$$\begin{array}{rlrl}& & \underset{\mathbf{x}}{\min }f(\mathbf{x})=\frac{1}{2}{\mathbf{x}}^T\mathbf{Hx}+{\mathbf{c}}^T\mathbf{x},& \mathbf{x}\in {\mathbb{R}}^n\\ & s.t.& \mathbf{Ax}\le \mathbf{b}& \\ & & \mathbf{Cx}=\mathbf{d}& \end{array}\text{\hspace{1em}(5)}$$

在下一部分内容中，将MPC问题标准化为二次规划的一般形式。

标准化

对（3）进行化简
$$\begin{array}{r}{\mathbb{X}}_k=\left(\begin{array}{c}\mathbf{x}(k|k)\\ \mathbf{x}(k+1|k)\\ \mathbf{x}(k+2|k)\\ ⋮\\ \mathbf{x}(k+N|k)\end{array}\right)=\left(\begin{array}{c}{\mathbf{x}}_k\\ \mathbf{Ax}(k|k)+\mathbf{Bu}(k|k)=\mathbf{A}{\mathbf{x}}_k+\mathbf{Bu}(k|k)\\ \mathbf{Ax}(k+1|k)+\mathbf{Bu}(k+1|k)={\mathbf{A}}^2{\mathbf{x}}_k+\mathbf{AB}\mathbf{u}(k|k)+\mathbf{Bu}(k+1|k)\\ ⋮\\ {\mathbf{A}}^N{\mathbf{x}}_k+{\mathbf{A}}^{N-1}\mathbf{Bu}(k|k)+\cdots +\mathbf{Bu}(k+N-1|k)\end{array}\right)\end{array}\text{\hspace{1em}(6)}$$
即
$$\begin{array}{r}{{\mathbb{X}}_k}_{nN\times 1}=\underset{m(N+1)\times m}{\left(\begin{array}{c}\mathbf{I}\\ \mathbf{A}\\ {\mathbf{A}}^2\\ ⋮\\ {\mathbf{A}}^N\end{array}\right)}\underset{m\times 1}{{\mathbf{x}}_k}+\underset{m(N+1)\times nN}{\left(\begin{array}{cccccc}0& 0& 0& \cdots & 0& \\ \mathbf{B}& 0& 0& \cdots & 0& \\ \mathbf{AB}& \mathbf{B}& 0& \cdots & 0& \\ {\mathbf{A}}^2\mathbf{B}& \mathbf{AB}& \mathbf{B}& \cdots & 0& \\ ⋮& ⋮& ⋮& \ddots & ⋮& \\ {\mathbf{A}}^{\mathbf{N}-1}\mathbf{B}& {\mathbf{A}}^{\mathbf{N}-2}\mathbf{B}& {\mathbf{A}}^{\mathbf{N}-3}\mathbf{B}& \cdots & 0& \end{array}\right)}\underset{nN\times 1}{\left(\begin{array}{c}\mathbf{u}(k|k)\\ \mathbf{u}(k+1|k)\\ \mathbf{u}(k+2|k)\\ ⋮\\ \mathbf{u}(k+N-1|k)\end{array}\right)}\end{array}\text{\hspace{1em}(7)}$$
表示为
$${\mathbb{X}}_k=\mathbf{M}{\mathbf{x}}_k+\mathbf{C}{\mathbb{U}}_k\text{\hspace{1em}(8)}$$
向量化
$$\begin{array}{rl}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{Ru}(k+i|k)\right)+\mathbf{x}(k+N|k{)}^T{\mathbf{Q}}_N\mathbf{x}(k+N|k)\\ & ={\left(\begin{array}{c}\mathbf{x}(k|k)\\ \mathbf{x}(k+1|k)\\ ⋮\\ \mathbf{x}(k+N|k)\end{array}\right)}^T\left(\begin{array}{ccccc}{\mathbf{Q}}_1& & & & \\ & {\mathbf{Q}}_2& & & \\ & & \ddots & & \\ & & & {\mathbf{Q}}_N& \end{array}\right)\left(\begin{array}{c}\mathbf{x}(k|k)\\ \mathbf{x}(k+1|k)\\ ⋮\\ \mathbf{x}(k+N|k)\end{array}\right)\\ & +{\left(\begin{array}{c}\mathbf{u}(k|k)\\ \mathbf{u}(k+1|k)\\ ⋮\\ \mathbf{u}(k+N|k)\end{array}\right)}^T\left(\begin{array}{ccccc}{\mathbf{R}}_1& & & & \\ & {\mathbf{R}}_2& & & \\ & & \ddots & & \\ & & & {\mathbf{R}}_N& \end{array}\right)\left(\begin{array}{c}\mathbf{u}(k|k)\\ \mathbf{u}(k+1|k)\\ ⋮\\ \mathbf{u}(k+N|k)\end{array}\right)\\ & ={\mathbb{X}}_k^T\mathbb{Q}{\mathbb{X}}_k+{\mathbb{U}}_k^T\mathbb{R}{\mathbb{U}}_k\end{array}\text{\hspace{1em}(9)}$$

将代价函数表示为$\mathbb{U}$的函数形式
$$\begin{array}{rl}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{array}\text{\hspace{1em}(10)}$$

由标准形式（5）
$$\begin{array}{rl}f({\mathbb{U}}_k)=& {\mathbb{U}}_k^T\mathbb{H}{\mathbb{U}}_k+2{\mathbf{x}}_k^T\mathbb{E}{\mathbb{U}}_k\end{array}\text{\hspace{1em}(11)}$$
对比（5）
$$\begin{array}{rlrl}& & \underset{\mathbf{x}}{\min }f(\mathbf{x})& =\frac{1}{2}{\mathbf{x}}^T\mathbf{Hx}+{\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{array}\text{\hspace{1em}(12)}$$