Design of inverted pendulum controller (using pole configuration, LQR method, Robust H-infinity method respectively)

Design of G01 Inverted Pendulum Controller

• Author：Dargon
• Note date：2020/12/13
• Course Books:LMIs in Control Systems Analysis,Design and Applications

1. Brief introduction of inverted pendulum control system

• Inverted pendulum system is a complex control system with characteristics of nonlinearity, strong coupling, multivariable and instability. In the control process, it can effectively reflect many key problems in the control, such as stability problems, nonlinear problems, robustness problems, follow-up problems and tracking problems, etc., which can be studied with the inverted pendulum as the object. In addition, it has many similarities with rocket flight and robot joint motion, and its principle can be used to control the stable launch of rockets. The research of inverted pendulum has important practical significance for the research of modern high technology such as rocket flight control and robot control. Therefore, the control of inverted pendulum has become an enduring research topic in control theory. In the last 25 years, many classical and modern control theories have been used for the stability control of the inverted pendulum, such as pole placement control, LQR control, state feedback control, robust H ∞ H_{\infty $H_{\infty }$, Intelligent control, fuzzy control, artificial neuron control.
• Formally, the inverted pendulum system can be divided into the following types:
  1. Linear inverted pendulum system: or called "trolley-inverted pendulum system", it is a system composed of a trolley that can move along a linear guide rail and a uniform long rod with one end fixed on the trolley.
  2. Ring inverted pendulum system: It can be regarded as a system formed by bending the linear guide rail of the trolley.
  3. Planar inverted pendulum system: the bottom end of the homogeneous pendulum can move freely in the plane, and the pendulum can rotate along any axis in the plane.
  4. Flexible connection inverted pendulum system: On the basis of the original inverted pendulum system, a new free oscillation link is introduced: the free spring system.
  5. Flexible inverted pendulum system: It not only replaces the homogeneous rigid body with a flexible pendulum, but itself has become a nonlinear distributed parameter system.
  6. Linear flexible connection two-stage inverted pendulum: The so-called linear flexible connection two-stage inverted pendulum system is based on the linear rigid two-stage inverted pendulum, adding a free spring system: the motor is connected to an active trolley, and the active trolley acts on the driven trolley through a spring to control the two-stage inverted pendulum fixed on the driven trolley.

2. Analysis and establishment of the mathematical model of the inverted pendulum

2.1, model analysis

• The inverted pendulum system is shown in Figure 1. A wheeled trolley is hinged to a rigid inverted pendulum in the middle. The trolley slides left and right along a straight and smooth track, and the pendulum rod can move freely in the vertical plane. For the convenience of analysis, we assume that the friction force is small enough to be ignored. The equation of motion of the first-order inverted pendulum can be deduced from the theory of dynamics.
  
  M: mass of the trolley
  m: mass of the pendulum
  L: distance from the center of mass of the pendulum to the rotating shaft
  I: moment of inertia of the pendulum
  g: acceleration of gravity
  b: damping ratio between the trolley and the guide rail
  C: damping ratio between the pendulum and the trolley
  $\theta$ : The angle between the pendulum and the vertical direction
  F: The external force output by the controller
  
  The force
  $$M\ddot{x}=F-F_N-b\dot{x}\text{\hspace{1em}( 2.1 )}$$
  Horizontal force
  $$F_N=m\frac{d^2(x+Lsin\theta )}{d{t}^2}\text{\hspace{1em}( 2.2 )}$$ A kinematic equation F = ( M + m ) x ¨ + m L θ ¨ cos θ − m L θ ˙ 2 sin θ + bx ˙ (2.3) F =(M +m) \ddot{x} +mL\ddot{\theta}cos\theta -mL \dot{\theta}^{2}sin can be obtained according to formulas (2.1) and (2.2) \theta +b \dot{x}\tag{2.3
  }
  $$F=(M+m)\ddot{x}+mL\ddot{i}cos\theta -mL{\dot{i}}^{2}sin\theta +b\dot{x}\text{\hspace{1em}( 2.3 )}$$ FP − mg = md 2 ( L cos θ ) dt 2 (2.4) F_P -mg = m \frac{d^2(Lcos \theta)}{dt^2} \tag{2.4
  }
  $$F_P-mg=m\frac{d^2(Lcos\theta )}{d{t}^2}\text{\hspace{1em}( 2.4 )}$$
  Function
  $$I\ddot{i}=F_{P}Lsin\theta -F_{N}Lcos\theta -c\dot{i}\text{\hspace{1em}( 2.5 )}$$
  Substitute (2.2) and (2.4) into (2.5) to get another equation of the system
  $$(I+m{L}^2)\ddot{i}+mL\ddot{x}+c\dot{i}=mgL\theta \text{\hspace{1em}( 2.6 )}$$
  The equation is given by$sin\theta \approx i,cos\theta \approx 1,(\frac{d}{dt}{)}^2=0$得出
  $$\{\begin{array}{rl}F& =(M+m)\ddot{x}+mL\ddot{i}+b\dot{x}\\ mgL\theta & =(I+m{L}^2)\ddot{i}+mL\ddot{x}+c\dot{i}\end{array}\text{\hspace{1em}(2.7)}$$

2.2, System state equation modeling

• At any moment, the state of the system is described by four variables, which are the angle $\theta$ , the angular velocity θ of the pendulum$\dot{i}$ , the displacement of the trolley$x$ , the speed of the car$\dot{x}$
  令$X={[x\dot{x}i\dot{i}]}^T$,$Y={\left[x\dot{x}i\dot{i}\right]}^T$
  can describe the state space equation of the system
  $$\{\begin{array}{rl}\dot{X}& =AX+BU\\ Y& =CX+D\end{array}\text{\hspace{1em}(2.8)}$$
  $$A=\left[\begin{array}{cccc}0& 1& 0& 0\\ 0& a_1& a_2& a_3\\ 0& 0& 0& 1\\ 0& a_4& a_5& a_6\end{array}\right]B=\left[\begin{array}{c}0\\ b_1\\ 0\\ b_2\end{array}\right]C=\left[\begin{array}{cccc}1& 0& 0& 0\\ 0& 1& 0& 0\\ 0& 0& 1& 0\\ 0& 0& 0& 1\end{array}\right]D=\left[\begin{array}{c}0\\ 0\\ 0\\ 0\end{array}\right]$$
  其中$$a_1=\frac{-b(I+m{L}^2)}{(M+m)I+Mm{L}^2}{a}_2=\frac{-m^2{L}^{2}g}{(M+m)I+Mm{L}^2}{a}_3=\frac{mLc}{(M+m)I+Mm{L}^2}$$
  $$a_4=\frac{mLb}{(M+m)I+Mm{L}^2}{a}_5=\frac{mgL(M+m)}{(M+m)I+Mm{L}^2}{a}_6=\frac{-c(M+m)}{(M+m)I+Mm{L}^2}$$
  $$b_1=\frac{I+m{L}^2}{(M+m)I+Mm{L}^2}{b}_2=\frac{-mL}{(M+m)I+Mm{L}^2}$$

3, the design of the controller

3.1 Without controller

• For the case of no controller, we can judge the stability of A by
  $$eandg\left(A\right)=[00.0830-5.2780502726]$$
  For the case where the eigenvalues ​​are in the right half plane, the system is unstable, and its image can be seen to diverge

%---------------------------------without control-------------------
disp(eig(A));
The eigenvalue of A :
    0
-0.0830
-5.2780
5.2726

3.2 Pole configuration design controller

• Using matlab, all the poles are arranged in the left half plane, and the set poles $P=[-10-9-1-0.5]$ , use the place function of matlab to configure and solve the value of feedback gain K.

%---------------------------------pole placement---------------------
disp('Pole placement');
P=[-10,-9,-1,-1.5];
K=place(A,B2,P);
disp('Feedback gain K:');
disp(K);

Pole placement
Feedback gain K:
   -5.8454  -11.0766   72.9832   13.2371

3.3 LQR method design controller

• Use matlab to find the optimal pole configuration, which is the smallest cost function (Cost function) of the quadratic form. At the same time, set the corresponding weight matrix Q and R J = ∫ 0 ∞ ( XTQX + UTRU ) dt J =\int_0^{\infty}(X^TQX +U^TRU)
  $$J={\int }_0^{\infty }(X^{T}QX+U^{T}RU)dt$$
  can be solved by using the LQR solving function lqr corresponding to matlab

%--------------------------------LQR controll---------------------------
disp('LQR control');
Q=[200 0 0   0;
     0 1 0   0;
     0 0 200 0;
     0 0 0   1];
R=0.1;
[K,P,E] =lqr(A,B2,Q,R);
disp('Feedback gain K:');
disp(K);
disp('The eigenvalue:');
disp(E);

LQR control
Feedback gain K:
  -44.7214  -31.5945  119.3920   21.7285

The eigenvalue:
  -9.2460 + 5.3810i
  -9.2460 - 5.3810i
  -2.4487 + 1.7404i
  -2.4487 - 1.7404i

3.4 Robust $H_{\infty }$design controller

• Use the LMI toolbox in matlab to solve the problem of robust $H_{\infty }$Solving the control problem for $H_{\infty }$It is derived from the definite mathematical derivation and physical meaning. The matrix spectral norm (2-norm) form is derived from the Euler norm, which belongs to the form of a constant matrix. For H ∞ H_\ $H_{\infty }$The norm is promoted by the 2 norm, and the derived norm can be used to define $H_{\infty }$form. It can be seen that if the input is disturbed, it represents this kind of magnification $\gamma$ , of course our goal is to be$\gamma$ is as small as possible.
  Find$H_{\infty }$The problem can be solved by writing the following standard form:
  $$min\gamma$$
  $$subjectto:$$
  $$\{\begin{array}{r}\left[\begin{array}{ccc}AX+B_{2}W+(AX+B_{2}W{)}^T& B_1& (C_{1}X+D_{12}W{)}^T)\\ {B_1}^T& -I& D_{11}^T\\ C_{1}X+D_{12}W& D_{11}& -c^2\end{array}\right]<0\\ X>0\end{array}\text{\hspace{1em}(3.1)}$$

$$u=$$
K obtained by$$K$$$$x$$$$K=W{X}^{-1}$$
Use the feasp function of matlab LMI toolbox to find the feasibility problem, and use mincx to find the smallest$The\; smallest\; \gamma$
obtained by$$c=0.8834$$

%descripe LMI
setlmis([]);       %建立一个LMI
X=lmivar(1,[4,1]); %定义矩阵变量
W=lmivar(2,[1,4]);
r1=lmivar(1,[1,1]);

% The first inequation
lmiterm([1 1 1 X],A,1,'s'); %AX +(AX)'
lmiterm([1 1 1 W],B2,1,'s'); %B2W +(B2W)'
lmiterm([1 2 1 0],B1'); %B1
lmiterm([1 2 2 0],-1); %-I
lmiterm([1 3 1 X],C1,1); %C1X +D12W
lmiterm([1 3 1 W],D12,1);
lmiterm([1 3 2 0],D11); %D11
lmiterm([1 3 3 r1],-1,1); %-r^2 *I

% The second equation
lmiterm([-2 1 1 X],1,1); %X >0

lmisys=getlmis;

%----------------------------Robust mincx solver---------------------------------------

n = decnbr(lmisys);
c = zeros(n,1);
for j=1:n
     [r1j]=defcx(lmisys,j,r1);
      c(j)=trace(r1j);
end
%c=mat2dec(lmisys,zeros(4,4),zeros(1,4),eye(1))
[copt,xopt]=mincx(lmisys,c, [0 0 0 0 0]);

X =dec2mat(lmisys,xopt,X);
W =dec2mat(lmisys,xopt,W);
r1 =dec2mat(lmisys,xopt,r1);
disp(copt);
disp(r1);
K =W *X^(-1);
disp('The best value:');
disp(sqrt(r1));
disp('Feedback gain K:');
disp(K/100);

%----------------------------------------solutions result--------------------
Solver for linear objective minimization under LMI constraints 

 Iterations   :    Best objective value so far 
 
     1
     2
     3
     4
     5
     6
     7
     8                 114.267501
     9                  59.802174
    10                  59.802174
    11                  41.613389
    12                  41.613389
    13                  41.613389
    14                  20.346897
    15                  20.346897
    16                  20.346897
    17                  10.262175
    18                  10.262175
    19                  10.262175
    20                   5.791484
    21                   5.791484
***                 new lower bound:    -1.071889
    22                   3.718387
***                 new lower bound:    -0.447187
    23                   3.718387
***                 new lower bound:    -0.074088
    24                   1.044904
***                 new lower bound:    -0.040169
    25                   1.000228
***                 new lower bound:     0.148979
    26                   0.960729
***                 new lower bound:     0.580998
    27                   0.862623
***                 new lower bound:     0.618936
    28                   0.862623
***                 new lower bound:     0.670347
    29                   0.798370
***                 new lower bound:     0.677624
    30                   0.790666
***                 new lower bound:     0.760378
    31                   0.784260
***                 new lower bound:     0.763410
    32                   0.781884
***                 new lower bound:     0.768376
    33                   0.780371
***                 new lower bound:     0.772957

 Result:  feasible solution of required accuracy
          best objective value:     0.780371
          guaranteed absolute accuracy:  7.41e-03
          f-radius saturation:  0.003% of R =  1.00e+09 
 
    0.7804

    0.7804

The best value:
    0.8834

Feedback gain K:
  112.1078   99.9032 -364.5474  -72.8227

4, simulink simulation analysis

4.1, simulink construction diagram

• 

4.2, Each corresponding controller input

•