Tartalomjegyzék

Inverted pendulum with uncertain frictional coefficient

Teljes Matlab script kiegészítő függvényekkel.

File: ccs_nonlin_ipend_local_LPV.m
Directory: 4_gyujtemegy/11_CCS/Modellek/inverse_pendulum/2018b_nonlin_pannon
Author: Peter Polcz (ppolcz@gmail.com)

Created on 2018. August 26.

Nonlinear model

% Time
syms t real

% State variables
syms position v phi omega real

% Input
syms u real

% Frictional coefficient
syms b real
b_lims = [0,1];

% State vector
x = [
    position
    v
    phi
    omega
    ];

n = numel(x);

% Known model parameters
M = 0.5;
m = 0.2;
l = 1;
g = 9.8;

q = 4*(M+m) - 3*m*cos(phi)^2;

f_sym = [
    v
    (4*m*l*sin(phi)*omega^2 - 1.5*m*g*sin(2*phi) -4*b*v) / q
    omega
    3*(-m*l*sin(2*phi)*omega^2 / 2 + (M+m)*g*sin(phi) + b*cos(phi)*v) / (l*q)
    ];

g_sym = [
    0
    4*l
    0
    -3*cos(phi)
    ] / (l*q);

Linearized (LPV) model around the unstable equilibrium point

A_sym = subs(jacobian(f_sym,x), x, [0;0;0;0]);
A_fh = matlabFunction(A_sym);

% Matrix A is an affine parameter dependent matrix: A = A0 + A1*b, where
A0 = A_fh(0);
A1 = A_fh(1) - A0;

B = double(subs(g_sym, x, [0;0;0;0]));

C = [
    1 0 0 0
    0 0 1 0
    ];

D = [ 0 ; 0 ];

Static state feedback design with LMIs

Q = sdpvar(n);   % Q = inv(P)
N = sdpvar(1,n); % N = K*Q

AkQ = @(b) A_fh(b)*Q - B*N;

CONS = [
    AkQ(b_lims(1))' + AkQ(b_lims(1)) <= 0
    AkQ(b_lims(2))' + AkQ(b_lims(2)) <= 0
    Q >= eye(n)*0.001
    ];

sol = optimize(CONS)

N = value(N);
Q = value(Q);

K = N/Q

for b = linspace(b_lims(1),b_lims(2),11)
    Ak = A_fh(b) - B*K;
    fprintf('Eigenvalues of A(%3.1f)-B*K: [%5.2g,%5.2g,%5.2g,%5.2g]\n', b, eig(Ak))
end

Output:

Problem
  Name                   :                 
  Objective sense        : min             
  Type                   : CONIC (conic optimization problem)
  Constraints            : 14              
  Cones                  : 0               
  Scalar variables       : 0               
  Matrix variables       : 3               
  Integer variables      : 0               

Optimizer started.
Presolve started.
Linear dependency checker started.
Linear dependency checker terminated.
Eliminator - tries                  : 0                 time                   : 0.00            
Lin. dep.  - tries                  : 1                 time                   : 0.00            
Lin. dep.  - number                 : 0               
Presolve terminated. Time: 0.00    
Problem
  Name                   :                 
  Objective sense        : min             
  Type                   : CONIC (conic optimization problem)
  Constraints            : 14              
  Cones                  : 0               
  Scalar variables       : 0               
  Matrix variables       : 3               
  Integer variables      : 0               

Optimizer  - threads                : 4               
Optimizer  - solved problem         : the primal      
Optimizer  - Constraints            : 14
Optimizer  - Cones                  : 0
Optimizer  - Scalar variables       : 0                 conic                  : 0               
Optimizer  - Semi-definite variables: 3                 scalarized             : 30              
Factor     - setup time             : 0.00              dense det. time        : 0.00            
Factor     - ML order time          : 0.00              GP order time          : 0.00            
Factor     - nonzeros before factor : 101               after factor           : 105             
Factor     - dense dim.             : 0                 flops                  : 3.96e+03        
ITE PFEAS    DFEAS    GFEAS    PRSTATUS   POBJ              DOBJ              MU       TIME  
0   1.8e+00  1.0e+00  1.0e+00  0.00e+00   -4.000000000e-03  0.000000000e+00   1.0e+00  0.00  
1   4.2e-01  2.3e-01  4.7e-01  9.96e-01   -2.669752533e-03  0.000000000e+00   2.3e-01  0.01  
2   7.4e-02  4.1e-02  1.9e-01  9.81e-01   -2.424807185e-03  0.000000000e+00   4.1e-02  0.01  
3   1.5e-02  8.5e-03  7.7e-02  8.90e-01   -2.466832262e-03  0.000000000e+00   8.4e-03  0.01  
4   1.6e-03  9.0e-04  2.4e-02  8.06e-01   -2.570715608e-04  0.000000000e+00   9.0e-04  0.01  
5   2.7e-06  1.5e-06  9.4e-04  9.79e-01   -6.755147482e-07  0.000000000e+00   1.5e-06  0.01  
6   1.4e-14  8.4e-15  7.8e-15  1.00e+00   -1.249225875e-15  0.000000000e+00   7.9e-15  0.01  
Optimizer terminated. Time: 0.02    


Interior-point solution summary
  Problem status  : PRIMAL_AND_DUAL_FEASIBLE
  Solution status : OPTIMAL
  Primal.  obj: -1.2492258747e-15   nrm: 1e-12    Viol.  con: 4e-13    barvar: 0e+00  
  Dual.    obj: 0.0000000000e+00    nrm: 5e+00    Viol.  con: 0e+00    barvar: 1e-14  
Optimizer summary
  Optimizer                 -                        time: 0.02    
    Interior-point          - iterations : 6         time: 0.01    
      Basis identification  -                        time: 0.00    
        Primal              - iterations : 0         time: 0.00    
        Dual                - iterations : 0         time: 0.00    
        Clean primal        - iterations : 0         time: 0.00    
        Clean dual          - iterations : 0         time: 0.00    
    Simplex                 -                        time: 0.00    
      Primal simplex        - iterations : 0         time: 0.00    
      Dual simplex          - iterations : 0         time: 0.00    
    Mixed integer           - relaxations: 0         time: 0.00    

sol = 
  struct with fields:

    yalmiptime: 0.3347
    solvertime: 0.0291
          info: 'Successfully solved (MOSEK)'
       problem: 0
K =
   -1.7482   -7.6865  -56.9980  -19.5788
Eigenvalues of A(0.0)-B*K: [ -5.1, -5.1,-0.27, -2.2]
Eigenvalues of A(0.1)-B*K: [ -5.2, -5.2, -2.2,-0.28]
Eigenvalues of A(0.2)-B*K: [ -5.3, -5.3, -2.1,-0.28]
Eigenvalues of A(0.3)-B*K: [ -5.4, -5.4, -2.1,-0.29]
Eigenvalues of A(0.4)-B*K: [ -5.5, -5.5, -2.1,-0.29]
Eigenvalues of A(0.5)-B*K: [ -5.7, -5.7,   -2, -0.3]
Eigenvalues of A(0.6)-B*K: [ -5.8, -5.8,   -2,-0.31]
Eigenvalues of A(0.7)-B*K: [ -5.9, -5.9,   -2,-0.31]
Eigenvalues of A(0.8)-B*K: [   -6,   -6, -1.9,-0.32]
Eigenvalues of A(0.9)-B*K: [ -6.1, -6.1, -1.9,-0.33]
Eigenvalues of A(1.0)-B*K: [ -6.9, -5.5, -1.8,-0.34]

Published with MATLAB® R2017a, and sanitarized by BeautifulSoup (Python script using BSoup is written by Polcz).