MPC(模型预测控制)_附matlab例程

写在前面：
本文为科研理论笔记的第二篇，其余笔记目录传送门：

理论笔记专栏目录

介绍结束下面开始进入正题：

1 基本概念

1.1 最优控制

最优控制(optimal control)：在约束条件下的最优表现，约束条件即物理限制，而对于最优的评判往往需要具体问题具体分析。

一个SISO系统的框图如下所示：

对于误差 e e e，从轨迹跟踪的角度出发， ∫ 0 t e 2 d t \int_{0}^{t} {e^2}dt ∫0te2dt 越小表示系统追踪效果越好；而从输入的角度出发， ∫ 0 t u 2 d t \int_{0}^{t}{u^2}dt ∫0tu2dt 越小表示系统的输入(能耗)越小。在上述条件下，即可构造系统的代价函数(cost function)： J = ∫ 0 t q e 2 + r u 2 d t J=\int_{0}^{t}{qe^2+ru^2}dt J=∫0tqe2+ru2dt。其中的 q q q和 r r r称为调节参数，最优化过程即使设计一个控制器使得 J = J m i n J=J_{min} J=Jmin。若使得 q > > r q>>r q>>r 则表示在最优化的过程中更看重误差，反之若 r > > q r>>q r>>q 则表示在最优化的过程中更看重输入。

拓展到MIMO系统，系统的状态空间方程一般可以描述为：
{ x ˙ = A x + B u y = C x \begin{cases} \dot x=Ax+Bu\\ y=Cx \end{cases} {x˙=Ax+Buy=Cx
其中 x = [ x 1 , ⋯ , x n ] T x=[x_1,\cdots,x_n]^T x=[x1,⋯,xn]T为系统状态, u = [ u 1 , ⋯ , u n ] T u=[u_1,\cdots,u_n]^T u=[u1,⋯,un]T为系统输入。

类似SISO系统，可设 J = ∫ 0 x e T Q e + u T R u d t J=\int_0^x{e^TQe+u^TRu}dt J=∫0xeTQe+uTRudt 。其中对角阵 Q = [ q 1 ⋯ 0 ⋮ ⋱ ⋮ 0 ⋯ q n ] Q=\begin{bmatrix}q_1&\cdots &0 \\ \vdots&\ddots&\vdots \\0&\cdots&q_n\end{bmatrix} Q=⎣⎢⎡q1⋮0⋯⋱⋯0⋮qn⎦⎥⎤和 R = [ r 1 ⋯ 0 ⋮ ⋱ ⋮ 0 ⋯ r n ] R=\begin{bmatrix}r_1&\cdots &0 \\ \vdots&\ddots&\vdots \\0&\cdots&r_n\end{bmatrix} R=⎣⎢⎡r1⋮0⋯⋱⋯0⋮rn⎦⎥⎤称为调节矩阵， q 1 ⋯ q n 、 r 1 ⋯ r n q_1\cdots q_n、r_1\cdots r_n q1⋯qn、r1⋯rn 称为最优化过程中的权重系数，可相应的调整 x 1 ⋯ x n 、 u 1 ⋯ u n x_1\cdots x_n、u_1\cdots u_n x1⋯xn、u1⋯un 的优化权重。

1.2 MPC

MPC(model predictive control)：模型预测控制，指通过模型预测系统在某一未来时间段内的表现来进行优化控制，因为其多用于数位控制，所以在分析时主要采用离散型状态空间表达式来进行分析，例如对于四轮阿克曼底盘的小车，其离散状态空间表达式可参考前一篇笔记：

四轮移动机器人(小车)数学建模

基于模型的预测：在MPC算法中，需要一个描述对象动态行为的模型，这个模型的作用是预测系统未来的动态，即能够根据系统 k k k时刻的状态 x k x_k xk和 k k k时刻的控制输入 u k u_k uk，预测到 k + 1 k+1 k+1时刻的输出 y k + 1 y_{k+1} yk+1。在这里 k k k时刻的输入正是用来控制系统 k + 1 k+1 k+1时刻的输出，使其最大限度的接近 k + 1 k+1 k+1时刻的输出期望值。故我们强调的是该模型的预测作用，而不是模型的形式。

预测区间：规定了希望未来能被预测到多久。从当前时刻 k k k到 k + N k+N k+N的时间区间。

控制区间：控制区间的选择就是一个最优化的问题，其代价函数 J = ∑ i = 0 N − 1 e k + i T Q e k + i + u k + i T R u k + i + e k + N T F e k + N J=\sum_{i=0}^{N-1}{e_{k+i}^TQe_{k+i}+u_{k+i}^TRu_{k+i}+e_{k+N}^TFe_{k+N}} J=∑i=0N−1ek+iTQek+i+uk+iTRuk+i+ek+NTFek+N ，其中 e k + N T F e k + N e_{k+N}^TFe_{k+N} ek+NTFek+N 表示最终代价，即最后 k + N k+N k+N时刻的误差代价。

MPC主要分为三步：

step1:在 k k k时刻(当前时刻)，测量或估计系统的当前状态 x k x_k xk (若 x k x_k xk不可直接测得，则一般采用状态观测器进行估计)；

step2:基于预测控制量 u k 、 u k + 1 . . . u k + N − 1 u_k、u_{k+1}...u_{k+N-1} uk、uk+1...uk+N−1 来进行最优化控制；

step3:只将 u k u_k uk 施加给系统，进行滚动优化控制。

2 最优化建模

MPC的重点在于step2中如何进行最优化控制，最优化的方法很多，下面主要推导一种常用的二次规划(Quadratic Programming)的方法。

二次规划的一般形式为： m i n ( Z T Q Z + C T Z ) min(Z^TQZ+C^TZ) min(ZTQZ+CTZ)，其中 Z T Q Z Z^TQZ ZTQZ为二次型项， C T Z C^TZ CTZ为线性项。对于二次规划问题的处理当前Matlab、Python、C++等都有很成熟的方法了，所以关键就在于如何将自己的模型化为二次规划的一般形式。

设系统的线性离散状态空间表达式为 x ( k + 1 ) = A x ( k ) + B u ( k ) x_{(k+1)}=Ax_{(k)}+Bu_{(k)} x(k+1)=Ax(k)+Bu(k) ，其中 x = [ x 1 , ⋯ , x n ] T , u = [ u 1 , ⋯ , u p ] T x=[x_1,\cdots,x_n]^T,u=[u_1,\cdots,u_p]^T x=[x1,⋯,xn]T,u=[u1,⋯,up]T，状态矩阵 A A A为 n × n n\times n n×n矩阵，输入矩阵 B B B为 n × p n\times p n×p矩阵。

在 k k k时刻，定义：
X k = [ x ( k ∣ k ) x ( k + 1 ∣ k ) ⋮ x ( k + N ∣ k ) ] ( N + 1 ) n × 1 ； U k = [ u ( k ∣ k ) u ( k + 1 ∣ k ) ⋮ u ( k + N − 1 ∣ k ) ] N p × 1 X_k=\begin{bmatrix} x_{(k|k)}\\ x_{(k+1|k)}\\ \vdots\\ x_{(k+N|k)}\\ \end{bmatrix}_{(N+1)n\times1} ； U_k=\begin{bmatrix} u_{(k|k)}\\ u_{(k+1|k)}\\ \vdots\\ u_{(k+N-1|k)}\\ \end{bmatrix}_{Np\times1} Xk=⎣⎢⎢⎢⎡x(k∣k)x(k+1∣k)⋮x(k+N∣k)⎦⎥⎥⎥⎤(N+1)n×1；Uk=⎣⎢⎢⎢⎡u(k∣k)u(k+1∣k)⋮u(k+N−1∣k)⎦⎥⎥⎥⎤Np×1
其中 N N N为预测区间，以 x ( k + 1 ∣ k ) x{(k+1|k)} x(k+1∣k)为例，括号内|左边的 k + 1 k+1 k+1表示预测的 k + 1 k+1 k+1时刻的状态量，括号内|右边的 k k k表示在 k k k时刻做出的预测。

为了简化分析，设输入 y = x y=x y=x，参考量 r = 0 r=0 r=0，则误差 e = y − r = x e=y-r=x e=y−r=x，代价函数可化为：
J = ∑ i = 0 N − 1 x ( k + i ∣ k ) T Q x ( k + i ∣ k ) + u ( k + i ∣ k ) T R u ( k + i ∣ k ) + x ( k + N ∣ k ) T F x ( k + N ∣ k ) 即： J = 误差加权和 + 输入加权和 + 终端误差 J=\sum_{i=0}^{N-1}{x_{(k+i|k)}^TQx_{(k+i|k)}+u_{(k+i|k)}^TRu_{(k+i|k)}+x_{(k+N|k)}^TFx_{(k+N|k)}}\\ 即：J=误差加权和+输入加权和+终端误差 J=i=0∑N−1x(k+i∣k)TQx(k+i∣k)+u(k+i∣k)TRu(k+i∣k)+x(k+N∣k)TFx(k+N∣k)即：J=误差加权和+输入加权和+终端误差
由系统初始条件 x ( k ∣ k ) = x ( k ) x_{(k|k)}=x_{(k)} x(k∣k)=x(k)，可递推得：
x ( k + 1 ∣ k ) = A x ( k ∣ k ) + B u ( k ∣ k ) = A x ( k ) + B u ( k ∣ k ) ; x ( k + 2 ∣ k ) = A x ( k + 1 ∣ k ) + B u ( k + 1 ∣ k ) = A 2 x ( k ) + A B u ( k ∣ k ) + B u ( k + 1 ∣ k ) ; ⋮ x ( k + N ∣ k ) = A n x ( k ) + A N − 1 B u ( k ∣ k ) + ⋯ + B u ( k + N − 1 ∣ k ) ; x_{(k+1|k)}=Ax_{(k|k)}+Bu_{(k|k)}=Ax_{(k)}+Bu_{(k|k)};\\ x_{(k+2|k)}=Ax_{(k+1|k)}+Bu_{(k+1|k)}=A^2x_{(k)}+ABu_{(k|k)}+Bu_{(k+1|k)};\\ \vdots \\ x_{(k+N|k)}=A^nx_{(k)}+A^{N-1}Bu_{(k|k)}+\cdots+Bu_{(k+N-1|k)}; x(k+1∣k)=Ax(k∣k)+Bu(k∣k)=Ax(k)+Bu(k∣k);x(k+2∣k)=Ax(k+1∣k)+Bu(k+1∣k)=A2x(k)+ABu(k∣k)+Bu(k+1∣k);⋮x(k+N∣k)=Anx(k)+AN−1Bu(k∣k)+⋯+Bu(k+N−1∣k);
整理上面 N + 1 N+1 N+1个式子可得(其中的 0 0 0表示 n × p n\times p n×p 的零矩阵)：
X k = [ I A A 2 ⋮ A N ] x ( k ) + [ 0 0 ⋯ 0 B 0 ⋯ 0 A B B ⋯ 0 ⋮ ⋮ ⋱ ⋮ A N − 1 B A N − 2 B ⋯ B ] U k X_k=\begin{bmatrix}I\\ A\\ A^2\\ \vdots\\ A^N\end{bmatrix}x_{(k)}+ \begin{bmatrix}0&0&\cdots&0\\ B&0&\cdots&0\\ AB&B&\cdots&0\\ \vdots&\vdots&\ddots&\vdots\\ A^{N-1}B&A^{N-2}B&\cdots&B\end{bmatrix}U_k Xk=⎣⎢⎢⎢⎢⎢⎡IAA2⋮AN⎦⎥⎥⎥⎥⎥⎤x(k)+⎣⎢⎢⎢⎢⎢⎡0BAB⋮AN−1B00B⋮AN−2B⋯⋯⋯⋱⋯000⋮B⎦⎥⎥⎥⎥⎥⎤Uk
定义：
M = [ I A A 2 ⋮ A N ] ( N + 1 ) n × n ； C = [ 0 0 ⋯ 0 B 0 ⋯ 0 A B B ⋯ 0 ⋮ ⋮ ⋱ ⋮ A N − 1 B A N − 2 B ⋯ B ] ( N + 1 ) n × N p M=\begin{bmatrix}I\\ A\\ A^2\\ \vdots\\ A^N\end{bmatrix}_{(N+1)n\times n}； C=\begin{bmatrix}0&0&\cdots&0\\ B&0&\cdots&0\\ AB&B&\cdots&0\\ \vdots&\vdots&\ddots&\vdots\\ A^{N-1}B&A^{N-2}B&\cdots&B\end{bmatrix}_{(N+1)n\times Np} M=⎣⎢⎢⎢⎢⎢⎡IAA2⋮AN⎦⎥⎥⎥⎥⎥⎤(N+1)n×n；C=⎣⎢⎢⎢⎢⎢⎡0BAB⋮AN−1B00B⋮AN−2B⋯⋯⋯⋱⋯000⋮B⎦⎥⎥⎥⎥⎥⎤(N+1)n×Np
即有： X k = M x ( k ) + C U k X_k=Mx_{(k)}+CU_k Xk=Mx(k)+CUk，其意义是由 k k k时刻的状态量和预测输入量可得出预测状态量。

再来将代价函数 J J J化为紧凑形式，例如其中误差加权和及终断误差可化为：
∑ i = 0 N − 1 x ( k + i ∣ k ) T Q x ( k + i ∣ k ) + x ( k + N ∣ k ) T F x ( k + N ∣ k ) = x ( k ∣ k ) T Q x ( k ∣ k ) + x ( k + 1 ∣ k ) T Q x ( k + 1 ∣ k ) + ⋯ + x ( k + N − 1 ∣ k ) T Q x ( k + N − 1 ∣ k ) + x ( k + N ∣ k ) T F x ( k + N ∣ k ) = X k T [ Q ⋯ 0 ⋮ ⋱ ⋮ 0 ⋯ F ] X k \sum_{i=0}^{N-1}{x_{(k+i|k)}^TQx_{(k+i|k)}}+x_{(k+N|k)}^TFx_{(k+N|k)}\\ =x_{(k|k)}^TQx_{(k|k)}+x_{(k+1|k)}^TQx_{(k+1|k)}+\cdots+x_{(k+N-1|k)}^TQx_{(k+N-1|k)}+x_{(k+N|k)}^TFx_{(k+N|k)}\\ =X_k^T\begin{bmatrix}Q&\cdots&0\\ \vdots&\ddots&\vdots\\ 0&\cdots&F\end{bmatrix}X_k i=0∑N−1x(k+i∣k)TQx(k+i∣k)+x(k+N∣k)TFx(k+N∣k)=x(k∣k)TQx(k∣k)+x(k+1∣k)TQx(k+1∣k)+⋯+x(k+N−1∣k)TQx(k+N−1∣k)+x(k+N∣k)TFx(k+N∣k)=XkT⎣⎢⎡Q⋮0⋯⋱⋯0⋮F⎦⎥⎤Xk
定义：
Q ‾ = [ Q ⋯ 0 ⋮ ⋱ ⋮ 0 ⋯ F ] ； R ‾ = [ R ⋯ 0 ⋮ ⋱ ⋮ 0 ⋯ R ] \overline Q=\begin{bmatrix}Q&\cdots&0\\ \vdots&\ddots&\vdots\\ 0&\cdots&F\end{bmatrix}；\overline R=\begin{bmatrix}R&\cdots&0\\ \vdots&\ddots&\vdots\\ 0&\cdots&R\end{bmatrix} Q=⎣⎢⎡Q⋮0⋯⋱⋯0⋮F⎦⎥⎤；R=⎣⎢⎡R⋮0⋯⋱⋯0⋮R⎦⎥⎤
则有：
J = ∑ i = 0 N − 1 x ( k + i ∣ k ) T Q x ( k + i ∣ k ) + u ( k + i ∣ k ) T R u ( k + i ∣ k ) + x ( k + N ∣ k ) T F x ( k + N ∣ k ) = X k T Q ‾ X K + U k T R ‾ U k J=\sum_{i=0}^{N-1}{x_{(k+i|k)}^TQx_{(k+i|k)}+u_{(k+i|k)}^TRu_{(k+i|k)}+x_{(k+N|k)}^TFx_{(k+N|k)}}\\ =X_k^T\overline QX_K+U_k^T\overline RU_k J=i=0∑N−1x(k+i∣k)TQx(k+i∣k)+u(k+i∣k)TRu(k+i∣k)+x(k+N∣k)TFx(k+N∣k)=XkTQXK+UkTRUk
将 X k = M x ( k ) + C U k X_k=Mx_{(k)}+CU_k Xk=Mx(k)+CUk代入 J = X k T Q ‾ X k + U k T R ‾ U k J=X_k^T\overline QX_k+U_k^T\overline RU_k J=XkTQXk+UkTRUk：
J = X k T Q ‾ X K + U k T R ‾ U k = ( M x ( k ) + C U k ) T Q ‾ ( M x ( k ) + C U k ) + U k T R ‾ U k = ( x ( k ) T M T + U k T C T ) Q ‾ ( M x ( k ) + C U k ) + U k T R ‾ U k = x ( k ) T M T Q ‾ M x ( k ) + x ( k ) T M T Q ‾ C U k + U k T C T Q ‾ M x ( k ) + U k T C T Q ‾ C U k + U k T R ‾ U k = x ( k ) T M T Q ‾ M x ( k ) + 2 x ( k ) T M T Q ‾ C U k + U k T ( C T Q ‾ C + R ‾ ) U k J=X_k^T\overline QX_K+U_k^T\overline RU_k\\ =(Mx_{(k)}+CU_k)^T\overline Q(Mx_{(k)}+CU_k)+U_k^T\overline RU_k\\ =(x_{(k)}^TM^T+U_k^TC^T)\overline Q(Mx_{(k)}+CU_k)+U_k^T\overline RU_k\\ =x_{(k)}^TM^T\overline QMx_{(k)}+x_{(k)}^TM^T\overline QCU_k+U_k^TC^T\overline QMx_{(k)}+U_k^TC^T\overline QCU_k+U_k^T\overline RU_k\\ =x_{(k)}^TM^T\overline QMx_{(k)}+2x_{(k)}^TM^T\overline QCU_k+U_k^T(C^T\overline QC+\overline R)U_k J=XkTQXK+UkTRUk=(Mx(k)+CUk)TQ(Mx(k)+CUk)+UkTRUk=(x(k)TMT+UkTCT)Q(Mx(k)+CUk)+UkTRUk=x(k)TMTQMx(k)+x(k)TMTQCUk+UkTCTQMx(k)+UkTCTQCUk+UkTRUk=x(k)TMTQMx(k)+2x(k)TMTQCUk+UkT(CTQC+R)Uk
定义： G = M T Q ‾ M 、 E = M T Q ‾ C 、 H = C T Q ‾ C + R ‾ G=M^T\overline QM、E=M^T\overline QC、H=C^T\overline QC+\overline R G=MTQM、E=MTQC、H=CTQC+R；即有：
J = x ( k ) T G x ( k ) + U k T H U k + 2 x ( k ) T E U k J=x_{(k)}^TGx_{(k)}+U_k^THU_k+2x_{(k)}^TEU_k J=x(k)TGx(k)+UkTHUk+2x(k)TEUk
注意到上式第一项 x ( k ) T G x ( k ) x_{(k)}^TGx_{(k)} x(k)TGx(k)仅与初始状态 x ( k ) x_{(k)} x(k)有关，不影响后续优化，后两项形同 Z T Q Z + C T Z Z^TQZ+C^TZ ZTQZ+CTZ，即可用于二次规划得到理想的输入。
至此MPC算法就介绍完成啦，上面的公式希望大家初学的时候都能手推一遍以便于更好的理解，最后附上一个简单的matlab例程供大家参考。

MPC例程github传送门，注意科学上网