Control-模型预测控制(Model Predict Control,MPC)_小作坊钳工_模型预测控制

irpas 02-07 3208

模型预测控制(Model Predict Control)利用一个已有的模型、系统当前的状态和未来的控制量去预测系统未来的输出；这个输出的长度是控制周期的整数倍；由于未来的控制量是未知的，需要根据一定的条件进行求解，以得到未来的控制量序列，并在每个控制周期结束后，系统根据当前实际状态重新预测系统未来的输出。因此模型预测控制有三个关键步骤，分别是：预测模型、滚动优化和反馈校正。

预测模型：预测模型是控制的基础，根据对象的历史信息和未来输入，预测系统未来的输出。预测模型有：状态空间方程、传递函数、阶跃响应、脉冲响应、神经网络模型等等。

滚动优化：模型预测控制通过控制某一性能指标的最优来确定控制序列，但优化不是一次离线进行，而是反复在线进行的。

反馈校正：为了防止模型失配或者干扰等引起控制对理想状态的偏差，在新的采样时刻，首先检测对象的实际输出，并利用这一实时信息对基于模型的预测结果进行修正，然后再进行新的优化。

在此，选择状态空间方程作为预测模型。

1 状态空间方程

状态空间方程为： { x ˙ = A x + B u + D d i s y = C x + D u (1) \begin{cases} \dot{x} = A x + B u + D_{dis} \\ y = C x + D u \end{cases} \tag{1} {x˙=Ax+Bu+Ddis?y=Cx+Du?(1) 连续状态空间方程需要离散化，常用的离散化方法有：欧拉公式离散化，后向差分和双线性变换离散化

MatrixEulerBackward RectBilinear Transform A d A_d Ad? I + A ? Δ t I + A*\Delta t I+A?Δt ( I ? A ? Δ t ) ? 1 (I - A*\Delta t)^{-1} (I?A?Δt)?1 ( I ? A ? Δ t 2 ) ? 1 ( I + A ? Δ t 2 ) (I - \frac{A*\Delta t}{2})^{-1} (I + \frac{A*\Delta t}{2}) (I?2A?Δt?)?1(I+2A?Δt?) B d B_d Bd? B ? Δ t B* \Delta t B?Δt ( I ? A ? Δ t ) ? 1 B ? Δ t (I - A*\Delta t)^{-1} B * \Delta t (I?A?Δt)?1B?Δt ( I ? A ? Δ t 2 ) ? 1 B ? Δ t (I - \frac{A*\Delta t}{2})^{-1} B * \Delta t (I?2A?Δt?)?1B?Δt D d , d i s D_{d,dis} Dd,dis? D d i s ? Δ t D_{dis} * \Delta t Ddis??Δt ( I ? A ? Δ t ) ? 1 D d i s ? Δ t (I - A*\Delta t)^{-1} D_{dis} * \Delta t (I?A?Δt)?1Ddis??Δt ( I ? A ? Δ t 2 ) ? 1 D d i s ? Δ t (I - \frac{A*\Delta t}{2})^{-1} D_{dis} * \Delta t (I?2A?Δt?)?1Ddis??Δt C d C_d Cd? C C C C ( I ? A ? Δ t ) ? 1 C (I - A*\Delta t)^{-1} C(I?A?Δt)?1 C ( I ? A ? Δ t 2 ) ? 1 C (I - \frac{A*\Delta t}{2})^{-1} C(I?2A?Δt?)?1 D d D_d Dd? D D D D + C ( I ? A ? Δ t ) ? 1 B ? Δ t D + C (I - A*\Delta t)^{-1} B * \Delta t D+C(I?A?Δt)?1B?Δt D + C ( I ? A ? Δ t 2 ) ? 1 B ? Δ t 2 D + C (I - \frac{A*\Delta t}{2})^{-1} \frac{B * \Delta t}{2} D+C(I?2A?Δt?)?12B?Δt?

离散的状态空间方程为： { x k + 1 = A d x k + B d u k + D d , d i s y k = C d x k + D d u k (2) \begin{cases} x_{k+1} = A_d x_k + B_d u_k + D_{d,{dis}} \\ y_k = C_d x_k + D_d u_k \end{cases} \tag{2} {xk+1?=Ad?xk?+Bd?uk?+Dd,dis?yk?=Cd?xk?+Dd?uk??(2)

2 预测模型

根据经验模型(状态空间方程)和当前状态、未来控制量，可以预测未来的输出量。 { x k + 1 = A d x k + B d u k + D d , d i s x k + 2 = A d x k + 1 + B d u k + 1 + D d , d i s = A d ( A d x k + B d u k + D d , d i s ) + B d u k + 1 + D d , d i s = A d 2 x k + ( A d B d u k + B d u k + 1 ) + A d D d , d i s + D d , d i s x k + 3 = A d x k + 2 + B d u k + 2 + D d , d i s = A d 2 x k + 1 + ( A d B d u k + 1 + B d u k + 2 ) + A d D d , d i s + D d , d i s = A d 3 x k + ( A d 2 B d u k + A d B d u k + 1 + B d u k + 2 ) + A d 2 D d , d i s + A d D d , d i s + D d , d i s ? x k + N p = A d N p x k + ( A d N p ? 1 B d u k + ? + A d N p ? N c + 1 B d u k + 1 + A d N p ? N c B d u k + N c ? 1 ) + ( A d N p ? 1 D d , d i s + ? + A d D d , d i s + D d , d i s ) (3) \begin{cases} x_{k+1} &= A_d x_k + B_d u_k + D_{d,{dis}} \\ x_{k+2} &= A_d x_{k+1} + B_d u_{k+1} + D_{d,{dis}} \\ &= A_d(A_d x_k + B_d u_k + D_{d,{dis}}) + B_d u_{k+1} + D_{d,{dis}} \\ &= A^2_{d} x_k + (A_d B_d u_k + B_d u_{k+1}) + A_d D_{d,{dis}} + D_{d,{dis}} \\ x_{k+3} &= A_d x_{k+2} + B_d u_{k+2} + D_{d,{dis}} \\ &= A^2_{d} x_{k+1} + (A_dB_du_{k+1} + B_d u_{k+2}) + A_dD_{d,{dis}} + D_{d,{dis}} \\ &= A^3_d x_k + (A^2_dB_du_{k} + A_dB_du_{k+1} + B_d u_{k+2}) + A^2_d D_{d,{dis}} + A_dD_{d,{dis}} + D_{d,{dis}} \\ \vdots \\ x_{k+N_p} &= A^{N_p}_d x_{k} + (A^{N_p -1}_dB_du_{k} + \cdots + A^{N_p-N_c+1}_dB_du_{k+1} + A^{N_p-N_c}_d B_d u_{k+N_c-1}) \\ &+(A^{N_p -1}_d D_{d,{dis}} + \cdots + A_d D_{d,{dis}} + D_{d,{dis}}) \end{cases} \tag{3} ??????????????????????????????????????????xk+1?xk+2?xk+3??xk+Np???=Ad?xk?+Bd?uk?+Dd,dis?=Ad?xk+1?+Bd?uk+1?+Dd,dis?=Ad?(Ad?xk?+Bd?uk?+Dd,dis?)+Bd?uk+1?+Dd,dis?=Ad2?xk?+(Ad?Bd?uk?+Bd?uk+1?)+Ad?Dd,dis?+Dd,dis?=Ad?xk+2?+Bd?uk+2?+Dd,dis?=Ad2?xk+1?+(Ad?Bd?uk+1?+Bd?uk+2?)+Ad?Dd,dis?+Dd,dis?=Ad3?xk?+(Ad2?Bd?uk?+Ad?Bd?uk+1?+Bd?uk+2?)+Ad2?Dd,dis?+Ad?Dd,dis?+Dd,dis?=AdNp??xk?+(AdNp??1?Bd?uk?+?+AdNp??Nc?+1?Bd?uk+1?+AdNp??Nc??Bd?uk+Nc??1?)+(AdNp??1?Dd,dis?+?+Ad?Dd,dis?+Dd,dis?)?(3) 其中， N p N_p Np?是预测时域， N c N_c Nc?是控制时域，并且 N p ≥ N c N_p \geq N_c Np?≥Nc?，在 N c < k ≤ N p Nc < k \leq N_p Nc<k≤Np?时域内， u k = 0 u_k = 0 uk?=0。

将公式(3)整理可得： X = F X 0 + Φ U + E (4) X = F X_{0} + \Phi U + E \tag{4} X=FX0?+ΦU+E(4) 其中： { X = [ x k + 1 , x k + 2 , ? ? , x k + N P ] T ; X 0 = x k ; U = [ u k , u k + 2 , ? ? , u k + N c ? 1 ] T ; F = [ A d , A d 2 , ? ? , A d N p ] T ; Φ = [ B d 0 ? 0 A d B d B d ? 0 ? ? ? ? A N p ? 1 d B d A N p ? 2 d B d ? A d N p ? N c B d ] E = [ D d , d i s , A d D d , d i s + D d , d i s , ? ? , ∑ i = 0 N p ? 1 A k i D d , d i s ] T (5) \begin{cases} X = [x_{k+1},x_{k+2},\cdots,x_{k+N_P}]^T; \\ X_0 = x_k; \\ U = [u_{k},u_{k+2},\cdots,u_{k+N_c-1}]^T; \\ F = [A_d, A^{2}_d,\cdots, A^{N_p}_d]^T; \\ \Phi = \left[\begin{matrix} B_d &0 & \cdots & 0 \\ A_d B_d & B_d & \cdots & 0 \\ \vdots & \vdots & \ddots & \vdots \\ A^{N_p -1}d B_d & A^{N_p -2}d B_d & \cdots & A^{N_p-N_c}_d B_d \\ \end{matrix} \right] \\ E = [D_{d,dis}, A_d D_{d,dis} + D_{d,dis},\cdots,\sum_{i=0}^{N_p-1}A^i_kD_{d,dis}]^T \\ \end{cases} \tag{5} ??????????????????????????????????X=[xk+1?,xk+2?,?,xk+NP??]T;X0?=xk?;U=[uk?,uk+2?,?,uk+Nc??1?]T;F=[Ad?,Ad2?,?,AdNp??]T;Φ=??????Bd?Ad?Bd??ANp??1dBd??0Bd??ANp??2dBd???????00?AdNp??Nc??Bd????????E=[Dd,dis?,Ad?Dd,dis?+Dd,dis?,?,∑i=0Np??1?Aki?Dd,dis?]T?(5) 假设 D d = 0 D_d=0 Dd?=0，由公(4)可以得到系统的预测输出量： Y = C d F X 0 + C d Φ U + C d E = F y X 0 + Φ y U + E y = [ y k + 1 , y k + 2 , ? ? , y k + N p ] T (6) Y = C_d FX_{0} + C_d \Phi U + C_d E = F_yX_0 +\Phi _{y} U + E_y = [y_{k+1},y_{k+2},\cdots,y_{k+N_p}]^T \tag{6} Y=Cd?FX0?+Cd?ΦU+Cd?E=Fy?X0?+Φy?U+Ey?=[yk+1?,yk+2?,?,yk+Np??]T(6)

3 代价函数

以系统的期望输出与预测输出的误差最小作为代价函数 J = ( Y ? Y r e f ) T Q e ( Y ? Y r e f ) + U T R e U = ( F y X 0 + Φ y U + E y ? Y r e f ) T Q e ( F y X 0 + Φ y U + E y ? Y r e f ) + U T R e U = U T ( Φ y T Q e Φ y + R e ) U + U T [ 2 Φ y T Q e ( F y X 0 + E y ? Y r e f ) ] + ( F y X 0 + E y ? Y r e f ) T Q e ( F y X 0 + E y ? Y r e f ) (7) \begin{aligned} J &= (Y - Y_{ref})^T Q_e (Y - Y_{ref}) + U^T R_e U \\ &= (F_yX_0 +\Phi _{y} U + E_y - Y_{ref})^T Q_e (F_yX_0 +\Phi _{y} U + E_y - Y_{ref}) + U^T R_e U \\ &= U^T(\Phi ^T_{y} Q_e \Phi _{y} + R_e)U + U^T[2\Phi ^T_{y} Q_e (F_y X_0 + E_y -Y_{ref})] + (F_y X_0 + E_y -Y_{ref})^T Q_e (F_y X_0 + E_y -Y_{ref}) \end{aligned} \tag{7} J?=(Y?Yref?)TQe?(Y?Yref?)+UTRe?U=(Fy?X0?+Φy?U+Ey??Yref?)TQe?(Fy?X0?+Φy?U+Ey??Yref?)+UTRe?U=UT(ΦyT?Qe?Φy?+Re?)U+UT[2ΦyT?Qe?(Fy?X0?+Ey??Yref?)]+(Fy?X0?+Ey??Yref?)TQe?(Fy?X0?+Ey??Yref?)?(7) 则代价函数可以简写为： J = U T H U + 2 U T G + P (8) J = U^T H U + 2U^TG+P \tag{8} J=UTHU+2UTG+P(8) 其中， Q Q Q是状态权重矩阵， R R R是控制输入权重矩阵， P P P是常量，显然代价函数是一个 Q P QP QP问题的求解。 H = Φ y T Q e Φ y + R e ; G = Φ y T Q e M ; P = M T Q e M ; M = F y X 0 + E y ? Y r e f Q e = [ Q 0 ? 0 0 Q ? 0 ? ? ? ? 0 0 ? Q ] N p × N p R e = [ R 0 ? 0 0 R ? 0 ? ? ? ? 0 0 ? R ] N c × N c (9) \begin{array}{cc} H = \Phi ^T_{y} Q_e \Phi _{y} + R_e; \\ G = \Phi ^T_{y} Q_e M; \\ P =M^T Q_e M; \\ M = F_y X_0 + E_y -Y_{ref} \\ Q_e=\left[\begin{matrix} Q &0 &\cdots &0 \\ 0 &Q &\cdots &0 \\ \vdots &\vdots &\ddots &\vdots \\ 0 &0 &\cdots &Q \\ \end{matrix} \right]_{N_p \times N_p} \\ R_e=\left[\begin{matrix} R &0 &\cdots &0 \\ 0 &R &\cdots &0 \\ \vdots &\vdots &\ddots &\vdots \\ 0 &0 &\cdots &R \\ \end{matrix} \right]_{N_c \times N_c} \end{array} \tag{9} H=ΦyT?Qe?Φy?+Re?;G=ΦyT?Qe?M;P=MTQe?M;M=Fy?X0?+Ey??Yref?Qe?=??????Q0?0?0Q?0??????00?Q???????Np?×Np??Re?=??????R0?0?0R?0??????00?R???????Nc?×Nc???(9)

4 约束条件

假设只考虑控制变量的上下界约束，则矩阵 U U U的约束为： U m i n ≤ U ≤ U m a x (10) U_{min} \leq U \leq U_{max} \tag{10} Umin?≤U≤Umax?(10) 或者写成以下形式： [ I ? I ] U ≥ [ U m i n ? U m a x ] (11) \left[ \begin{matrix} I \\ -I \end{matrix}\right]U \geq \left[ \begin{matrix} U_{min} \\ -U_{max} \end{matrix}\right] \tag{11} [I?I?]U≥[Umin??Umax??](11)

5 Q P QP QP问题

由上可知， M P C MPC MPC问题的求解最终转化为 Q P QP QP问题的求解，对于 Q P QP QP问题工程上可以求解的，有多种方法及开源库可以进行求解。 m i n : ???? J = 1 2 U T H U + U T s . t . ???? U m i n ≤ U ≤ U m a x (12) \begin{matrix} min: \; \; J = \frac{1}{2}U^T H U + U^T \\ s.t.\; \; U_{min} \leq U \leq U_{max} \end{matrix} \tag{12} min:J=21?UTHU+UTs.t.Umin?≤U≤Umax??(12)

将求解的控制量系列的第一个值作为控制量。

6 增量约束问题

将状态空间方程(2)改写为增量模式，以 Δ u \Delta u Δu为控制量： { ξ k + 1 = A m ξ k + B m Δ u k + D m , d i s θ k = C m ξ k (13) \begin{cases} \xi_{k+1} = A_m \xi_k + B_m \Delta u_k + D_{m,{dis}} \\ \theta_k = C_m \xi_k \end{cases} \tag{13} {ξk+1?=Am?ξk?+Bm?Δuk?+Dm,dis?θk?=Cm?ξk??(13) 其中： ξ = [ x k , u k ] T ; θ k = [ y k , u k ] T ; A m = [ A d B d 0 I ] ; B m = [ 0 I ] T ; D m , d i s = [ D d , d i s 0 ] T ; C m = [ C d 0 0 I ] ; (14) \begin{array}{cc} {}\xi = [x_k,u_k]^T; \\ \theta _k = [y_k, u_k]^T; \\ A_m = \left[\begin{matrix} A_d &B_d \\ 0 &I \end{matrix}\right]; \\ B_m = \left[\begin{matrix} 0 &I \end{matrix}\right]^T; \\ D_{m,dis} = \left[\begin{matrix} D_{d,dis} &0 \end{matrix}\right]^T; \\ C_m = \left[\begin{matrix} C_d &0 \\ 0 &I \end{matrix}\right]; \\ \end{array} \tag{14} ξ=[xk?,uk?]T;θk?=[yk?,uk?]T;Am?=[Ad?0?Bd?I?];Bm?=[0?I?]T;Dm,dis?=[Dd,dis??0?]T;Cm?=[Cd?0?0I?];?(14) 与上述相同的推到可以得到增量模型的 Q P QP QP形式： m i n : ???? J = 1 2 Δ U T H Δ U + Δ U T s . t . ???????????????????? U m i n ≤ U ≤ U m a x ???????????????????????????????????????? Δ U m i n ≤ Δ U ≤ Δ U m a x (15) \begin{matrix} min: \; \; J = \frac{1}{2} \Delta U^T H \Delta U + \Delta U^T \\ s.t.\; \; \; \; \; \; \; \; \; \; U_{min} \leq U \leq U_{max} \\ \; \; \; \; \; \; \; \; \; \; \; \; \; \; \; \; \; \; \; \; \Delta U_{min} \leq \Delta U \leq \Delta U_{max} \end{matrix} \tag{15} min:J=21?ΔUTHΔU+ΔUTs.t.Umin?≤U≤Umax?ΔUmin?≤ΔU≤ΔUmax??(15) 其中， R m R_m Rm?是 Δ U \Delta U ΔU的权重矩阵： Q e = [ Q 0 ? 0 0 0 ? 0 0 Q ? 0 0 0 ? 0 ? ? ? ? ? ? ? ? 0 0 ? Q 0 0 ? 0 0 0 ? 0 R 0 ? 0 0 0 ? 0 0 R ? 0 ? ? ? ? ? ? ? ? 0 0 ? 0 0 0 ? R ] ( N p + N C ) × ( N p + N C ) R e = [ R m 0 ? 0 0 R m ? 0 ? ? ? ? 0 0 ? R m ] N c × N c (16) \begin{array}{cc} Q_e=\left[\begin{matrix} Q &0 &\cdots &0 &0 &0 &\cdots &0\\ 0 &Q &\cdots &0 &0 &0 &\cdots &0\\ \vdots &\vdots &\ddots &\vdots &\vdots &\vdots &\ddots &\vdots\\ 0 &0 &\cdots &Q &0 &0 &\cdots &0\\ 0 &0 &\cdots &0 &R &0 &\cdots &0\\ 0 &0 &\cdots &0 &0 &R &\cdots &0\\ \vdots &\vdots &\ddots &\vdots &\vdots &\vdots &\ddots &\vdots\\ 0 &0 &\cdots &0 &0 &0 &\cdots &R\\ \end{matrix} \right]_{(N_p + N_C) \times (N_p + N_C)} \\ R_e=\left[\begin{matrix} R_m &0 &\cdots &0 \\ 0 &R_m &\cdots &0 \\ \vdots &\vdots &\ddots &\vdots \\ 0 &0 &\cdots &R_m \\ \end{matrix} \right]_{N_c \times N_c} \end{array} \tag{16} Qe?=???????????????Q0?000?0?0Q?000?0??????????00?Q00?0?00?0R0?0?00?00R?0??????????00?000?R????????????????(Np?+NC?)×(Np?+NC?)?Re?=??????Rm?0?0?0Rm??0??????00?Rm????????Nc?×Nc???(16)

根据 Δ U \Delta U ΔU来求解 U U U，使 U U U满足其约束： u k = u k ? 1 + Δ u k = u k ? 1 + [ 1 ?? 0 ?? ? ?? 0 ] Δ U u k + 1 = u k + Δ u k + 1 = u k ? 1 + Δ u k + Δ u k + 1 = u k ? 1 + [ 1 ?? 1 ?? ? ?? 0 ] Δ U ? u N c = u N c ? 1 + Δ u N c = u N c ? 2 + Δ u N c ? 1 + Δ u N c = u k ? 1 + [ 1 ?? 1 ?? ? ?? 1 ] Δ U (17) \begin{array}{cc} u_{k} &= u_{k-1} + \Delta u_{k} &= u_{k-1} + [1\;0\;\cdots\;0] \Delta U \\ u_{k+1} &= u_{k} + \Delta u_{k+1} &= u_{k-1} + \Delta u_{k} + \Delta u_{k+1} &= u_{k-1} + [1\;1\;\cdots\;0] \Delta U \\ \vdots \\ u_{N_c} &= u_{N_c -1} + \Delta u_{N_c} &= u_{N_c -2} + \Delta u_{N_c - 1} + \Delta u_{N_c} &= u_{k-1} + [1\;1\;\cdots\;1] \Delta U \\ \end{array} \tag{17} uk?uk+1??uNc???=uk?1?+Δuk?=uk?+Δuk+1?=uNc??1?+ΔuNc???=uk?1?+[10?0]ΔU=uk?1?+Δuk?+Δuk+1?=uNc??2?+ΔuNc??1?+ΔuNc???=uk?1?+[11?0]ΔU=uk?1?+[11?1]ΔU?(17) 即： [ u k u k + 1 u k + 2 ? u k + N c ] = [ I I I ? I ] u k ? 1 + [ I 0 0 ? 0 I I 0 ? 0 I I I ? 0 ? ? ? ? ? I I I ? I ] [ Δ u k Δ u k + 1 Δ u k + 2 ? Δ u k + N c ] (18) \left[ \begin{matrix} u_k \\u_{k+1} \\u_{k+2} \\ \vdots \\u_{k+N_c} \end{matrix}\right] = \left[ \begin{matrix} I \\I \\I \\ \vdots \\I \end{matrix}\right]u_{k-1} + \left[ \begin{matrix} I &0 &0 & \cdots &0 \\ I &I &0 & \cdots &0 \\ I &I &I & \cdots &0 \\ \vdots & \vdots & \vdots & \ddots & \vdots\\ I &I &I & \cdots &I \end{matrix}\right] \left[ \begin{matrix} \Delta u_k \\ \Delta u_{k+1} \\ \Delta u_{k+2} \\ \vdots \\ \Delta u_{k+N_c} \end{matrix}\right] \tag{18} ????????uk?uk+1?uk+2??uk+Nc???????????=????????III?I?????????uk?1?+????????III?I?0II?I?00I?I???????000?I?????????????????Δuk?Δuk+1?Δuk+2??Δuk+Nc???????????(18) 简化可得： [ C 1 ? C 1 ] Δ U ≥ [ U m i n ? C 2 u k ? 1 ? U m a x + C 2 u k ? 1 ] (19) \left[ \begin{matrix} C_1 \\ -C_1 \end{matrix}\right] \Delta U \geq \left[ \begin{matrix} U_{min} - C_2 u_{k-1} \\ -U_{max} + C_2 u_{k-1}\end{matrix}\right] \\ \tag{19} [C1??C1??]ΔU≥[Umin??C2?uk?1??Umax?+C2?uk?1??](19) 其中： C 1 = [ I 0 0 ? 0 I I 0 ? 0 I I I ? 0 ? ? ? ? ? I I I ? I ] ; C 2 = [ I I I ? I ] (20) C_1 = \left[ \begin{matrix} I &0 &0 & \cdots &0 \\ I &I &0 & \cdots &0 \\ I &I &I & \cdots &0 \\ \vdots & \vdots & \vdots & \ddots & \vdots\\ I &I &I & \cdots &I \end{matrix}\right]; C_2 =\left[ \begin{matrix} I \\I \\I \\ \vdots \\I \end{matrix}\right] \tag{20} C1?=????????III?I?0II?I?00I?I???????000?I?????????;C2?=????????III?I?????????(20)

7 matlab求解 function [flag, control_out] = SolveLinearMpc(A, B, C, D, Q, R, sample_period, upper, lower, state_k, reference, Np, Nc) if (size(A,1) ~= size(A,2) || ... size(B,1) ~= size(A,1) || ... size(D,1) ~= size(A,1) || ... size(Q,1) ~= size(Q,2) || ... size(R,1) ~= size(R,2) || ... size(Q,1) ~= size(C,1) || ... size(R,1) ~= size(B,2) || ... size(C,2) ~= size(A,1) || ... size(B,2) ~= size(lower,1) || ... size(lower,1) ~= size(upper,1) || ... size(state_k,1) ~= size(A,1)) flag = false; return; end %% à?é￠?ˉ matrix_i = eye(size(A,1)); % Ad = inv(matrix_i - sample_period * 0.5 * A) * (matrix_i + sample_period * 0.5 * A); Ad = matrix_i + A * sample_period; Bd = B * sample_period; Dd = D * sample_period; Cd = C; %% ?€2a???ó F = zeros(Np * size(Cd,1), size(Ad,2)); Phi = zeros(Np * size(Cd,1), Nc * size(Bd,2)); E = zeros(Np * size(Cd,1), size(Dd,2)); matrix_f = zeros(Np * size(C,1), size(A,2)); F(1:size(Cd,1), 1:size(Ad,2)) = Cd * Ad; matrix_f(1:size(Cd,1), 1:size(Ad,2)) = Cd; %% update F for i = 2:1:Np F((i-1) * size(Cd,1) + 1 : i * size(Cd,1), :) = ... F((i-2) * size(Cd,1) + 1 : (i-1) * size(Cd,1), :) * Ad; matrix_f((i-1) * size(Cd,1) + 1 : i * size(Cd,1), :) = ... matrix_f((i-2) * size(Cd,1) + 1 : (i-1) * size(Cd,1), :) * Ad; end %% update Phi % for i = 1:1:Np % for j = 1:1:i % Phi((i-1) * size(Cd,1) + 1 : i * size(Cd,1), (j-1) * size(Bd,2) + 1 : j * size(Bd, 2)) = ... % matrix_f((i-j) * size(Cd,1) + 1 : (i-j+1) * size(Cd,1), 1 : size(matrix_f,2)) * Bd; % if j == Nc % break; % end % end % end matrix_phi = matrix_f * Bd; Phi(: , 1 : size(Bd,2)) = matrix_phi; for i = 1 : Nc - 1 Phi(:, (i * size(Bd,2) + 1) : (i + 1) * size(Bd,2)) = [zeros(i * size(Cd,1), size(Bd,2)); ... matrix_phi(1 : (Np - i) * size(Cd,1), :)]; end %% update E E(1 : size(Cd,1), 1 : size(Dd,2)) = Cd * Dd; for i = 2:1:Np E((i-1) * size(Cd,1) + 1 : i * size(Cd,1), :) = ... matrix_f((i-1) * size(Cd,1) + 1 : i * size(Cd,1), :) * Dd +... E((i-2) * size(Cd,1) + 1 : (i-1) * size(Cd,1), :); end %% update Cost Function: min J = (Y_p - Ref)^T * Qe * (Y_p - Ref) + U^T * Re * U %% s.t. matrix_inequality_constraint * U ?ü matrix_inequality_boundary %% convert to QP problem: min J = 0.5 * U^T * H * U + U^T * G + P %% where: H = Phi^T * Qe * Phi + Re %% G = Phi^T * Qe * M %% P = M^T Qe * M %% M = F * X_k + E - Ref Qe = zeros(Np * size(Q,1), Np * size(Q,2)); Re = zeros(Nc * size(R,1), Nc * size(R,2)); for i = 1:1:Np Qe((i-1) * size(Q,1) + 1 : i * size(Q,1), (i-1) * size(Q,2) + 1 : i * size(Q,2)) = Q; end for i = 1:1:Nc Re((i-1) * size(R,1) + 1 : i * size(R,1), (i-1) * size(R,2) + 1 : i * size(R,2)) = R; end M = F * state_k + E - reference; H = Phi' * Qe * Phi + Re; G = Phi' * Qe * M; P = M' * Qe * M; %% update constraint matrix_ctrl_k = zeros(size(Bd,2), Nc * size(Bd,2)); matrix_ctrl_k(1 : size(Bd,2), 1 : size(Bd,2)) = eye(size(Bd,2)); %% use to get the fisrt contol value Upper_boundary = zeros(Nc * size(Bd,2), 1); Lower_boundary = zeros(Nc * size(Bd,2), 1); for i = 1:1:Nc Upper_boundary((i-1) * size(Bd,2) + 1 : i * size(Bd,2), :) = upper; Lower_boundary((i-1) * size(Bd,2) + 1 : i * size(Bd,2), :) = lower; end inequality_boundary = [Upper_boundary; - Lower_boundary]; matrix_inequality_constraint_upper = eye(Nc * size(Bd,2), Nc * size(Bd,2)); matrix_inequality_constraint_lower = eye(Nc * size(Bd,2), Nc * size(Bd,2)); matrix_inequality_constraint = [matrix_inequality_constraint_upper; ... -matrix_inequality_constraint_lower]; %% solve QP solve = - H \ G; %% optimal slove with no constraint is_satisfy_constraint = true; for i=1:1:size(matrix_inequality_constraint,1) if matrix_inequality_constraint(i, :) * solve > inequality_boundary(i) is_satisfy_constraint = false; end end if is_satisfy_constraint control_out = matrix_ctrl_k * solve; flag = true; else ppp = matrix_inequality_constraint * (H \ matrix_inequality_constraint'); ddd = (matrix_inequality_constraint * (H \ G) + inequality_boundary); lambda = zeros(size(ddd,1), size(ddd,2)); tolerance = 10; for i = 1:38 lambda_p = lambda; for j = 1:size(ddd,1) www = ppp(i,:) * lambda - ppp(i,i) * lambda(i,1); www = www + ddd(i,1); la = - www / ppp(i,i); lambda(i,1) = max(0, la); end tolerance = (lambda - lambda_p)' * (lambda - lambda_p); if tolerance < 10e-8 break; end end solve = - H \ G - H \ matrix_inequality_constraint' * lambda; control_out = matrix_ctrl_k * solve; flag = true; end