MPC learning notes (I): push MPC formula manually

1. Introduce

MPC(Model Predictive Control), Model predictive control , It is an advanced process control method . Compared with LQR and PID The algorithm can only consider various constraints of input and output variables ,MPC The algorithm can consider the constraints of state space variables . It can be applied to linear and nonlinear systems .
Reference material ：

1. MPC teaching
2. 421 Shi Gong's team MPC Teaching video

2.MPC Four elements

(1) Model : Adopt step response

(2) forecast

(3) Rolling optimization :

Second planning
$$J\left(x\right)=x^2+bx+c$$
Extreme points
$$J^{\prime }\left(x_0\right)=0$$
(4) Error compensation

Parameters are defined

P: Prediction step
$$y\left(k+1\right),y\left(k+2\right)\dots y(k+P)$$
M: Control step size
$$∆u(k),∆u(k+1)\dots ∆u(k+M)$$

3. Derivation process

(1) Model

Assume that T,2T…PT The output corresponding to the time model is
$$a_1,a_2\dots a_p$$
According to the superposition principle of linear system :
$$y\left(k\right)=a_1\ast u\left(k-1\right)+a_2\ast u\left(k-2\right)+a_M\ast u(k-M)$$

Incremental :
$$∆y\left(k\right)=a_1\ast ∆u\left(k-1\right)+a_2\ast ∆u\left(k-2\right)+a_M\ast ∆u(k-M)$$

(2) forecast : A total of P A moment

$∆y(k+1)=a_1\ast ∆u(k)$

$∆y(k+2)=a_1\ast ∆u(k)+1+a_2\ast ∆u(k)$

$∆y(k+P)=a_1\ast ∆u(k+P-1)+a_2\ast ∆u(k+P-2)\dots +a_M\ast ∆u(k+P-M)$

Turn to matrix representation :
$$\sum _{i=1}^M{a}_i\ast ∆u(k+P-i)$$

New forecast output = Original forecast output + Incremental input
$$\begin{gathered} {\hat{Y}}_0=A\ast ∆u+Y_0 \\ \left[\begin{array}{c}\begin{array}{c}{\hat{Y}}_0(k+1)\\ {\hat{Y}}_0(k+2)\end{array}\\ \begin{array}{c}⋮\\ {\hat{Y}}_0(k+P)\end{array}\end{array}\right]=\left[\begin{array}{cc}\begin{array}{cc}{a}_1& 0\\ a_2& a_1\end{array}& \begin{array}{cc}\cdots & 0\\ \cdots & 0\end{array}\\ \begin{array}{cc}⋮& ⋮\\ a_P& a_{P-1}\end{array}& \begin{array}{cc}\ddots & ⋮\\ \cdots & a_{P-M+1}\end{array}\end{array}\right]\left[\begin{array}{c}\begin{array}{c}∆u(k)\\ ∆u(k+1)\end{array}\\ \begin{array}{c}⋮\\ ∆u(k+M)\end{array}\end{array}\right]+\left[\begin{array}{c}\begin{array}{c}{Y}_0(k+1)\\ Y_0(k+2)\end{array}\\ \begin{array}{c}⋮\\ Y_0(k+P)\end{array}\end{array}\right] \end{gathered}$$

(3) Rolling optimization

notes ： Solved ∆u by M*1 Vector , Take the first one , namely $∆u_0$ Increment as input .

1. Reference trajectory
   Use first-order filtering :
   $$\omega \left(k+i\right)={\alpha }^{i}y\left(k\right)+(1-{\alpha }^i)y_r(k)$$

2. Cost function （ Similar to optimal control ）
   Form 1

   Goal one ： The closer you get to your goal, the better
   $$J_1=\sum _{i=1}^P{\left[y\left(k+i\right)-\omega \left(k+i\right)\right]}^2$$

   Goal two ： The smaller the energy consumption, the better
   $$J_2=i=\sum _{i=1}^M[∆u(k+i-1){]}^2$$

   The total cost function is obtained by combining
   $$J=q{J}_1+r{J}_2$$

   q and r Are weight coefficients , Their relative size indicates which indicator is more important , The bigger it is, the more important it is .

   Matrix form ：
   $$J={\left(r-\omega \right)}^{T}Q\left(R-\omega \right)+∆u^{T}R∆u$$
   among ,Q and R Most of them are diagonal matrices （ Generally, covariance matrix is not considered , That is, the interaction between different variables ）
   $$\left[\begin{array}{cccc}{q}_1& 0& ...& 0\\ 0& q_2& ...& 0\\ ⋮& ⋮& \ddots & ⋮\\ 0& 0& 0& q_n\end{array}\right]\left[\begin{array}{cccc}{r}_1& 0& ...& 0\\ 0& r_2& ...& 0\\ ⋮& ⋮& \ddots & ⋮\\ 0& 0& 0& r_n\end{array}\right]$$
   Optimal solution ：
   $$\frac{\partial \mathrm{J}(\Delta u)}{\partial (\Delta u)}=0\to \Delta u={\left({\mathrm{A}}^{\mathrm{T}}\mathrm{Q}\mathrm{A}+\mathrm{R}\right)}^{-1}{\mathrm{A}}^{\mathrm{T}}\left(\omega -{\mathrm{Y}}_0\right)$$
   Form 2

   • be based on $u_k,u_{k+1}...u_{k+N-1}Into\; theThat\text{'}s\; okmostoptimalturn$
     $$J=\sum _k^{N-1}(E_k^{T}Q{E}_k+u_k^{T}R{u}_k)+\underset{terminalpredictstatus}{\underbrace{E_N^{T}F{E}_N}}$$

(4) Correction feedback , Error compensation

• k moment , forecast P Outputs ：
  $${\hat{Y}}_0\left(k+1\right),{\hat{Y}}_0\left(k+2\right)...{\hat{Y}}_0\left(k+P\right)$$

• k+1 moment , The current output is ：
  $$y(k+1)$$

• error ：
  $$e\left(k+1\right)=y\left(k+1\right)-{\hat{Y}}_0\left(k+1\right)$$

h: Compensation coefficient , Value range 0-1, habit 0.5

compensate ：
$$\begin{gathered} y_{cor}\left(k+1\right)={\hat{Y}}_0\left(k+1\right)+h_1\ast e\left(k+1\right) \\ {y}_{cor}\left(k+2\right)={\hat{Y}}_0\left(k+2\right)+h_2\ast e\left(k+1\right) \\ ⋮ \\ {y}_{cor}\left(k+P\right)={\hat{Y}}_0\left(k+P\right)+h_P\ast e\left(k+1\right) \end{gathered}$$
The matrix represents ：
$$Y_{cor}={\hat{Y}}_0+H\ast e(k+1)$$

Rolling update forecast output ：

K+1 moment ：
$${\hat{Y}}_0=S\ast Y_{cor}$$

S: Shift matrix (PxP)
$$\left[\begin{array}{cc}\begin{array}{cc}0& 1\\ 0& 0\end{array}& \begin{array}{cc}\cdots & 0\\ \cdots & ⋮\end{array}\\ \begin{array}{cc}⋮& ⋮\\ 0& 0\end{array}& \begin{array}{cc}\ddots & 1\\ \cdots & 1\end{array}\end{array}\right]$$

Add : Derivation of cost function