Implementation of Intelligent Control on TITech Pendulum M.Yamakita Dept. of Mechanical and Control Systems Eng. Tokyo Inst. Of Tech. 2018/11/20

TITech Pendulum Top View TITech pendulum is a rational type pendulum which was developed for control algorithm test bed by prof. K.Furuta at Tokyo Institute of Technology. Side View Coordinate System 2018/11/20

Dynamic Equation and Parameters Non-Linear Dynamic Equation Linearlized Dynamic Equation at Upright Position 2018/11/20

Stabilizing Control at Upright Position Standard Regulator Problem Linear Quadratic Optimal Regulator(LQR) Optimal Feedback Control State Observer (Minimal Order Observer) 2018/11/20

Swing Up Control of TITech Pendulum Assume Input Sequence (Bang-Bang Control) Determination of T1, T2, T3, and T4 Constrained Successive Optimization Open Loop Control 2018/11/20

Reduction of Degree of the System Assumption Reduced Model used for Non-Linear Control 2018/11/20

Non-Linear State Feedback Control Controlled Vector Field of the System U = 0 U = +Max U = -Max Combine ! 2018/11/20

Control Result Let’s use this controller as a expert. 2018/11/20

Fuzzy Set Allocation to the Problem ZO NS NM NB1 NB2 PS PM 2018/11/20

If-THEN Rule for the Problem PM If x is NM and y is ZO then z is PB. If x is NB1 and y is PS then z is NB. If x is NM and y is PS then z is NB. If x is NS and y is PS then z is NB. If x is NB1 and y is ZO then z is ZO. If x is NS and y is ZO then z is ZO. If x is NB2 then z is ZO. If x is ZO then z is ZO. If y is NS then z is ZO. If y is PM the z is ZO. PS ZO NS NB2 NB1 NM NS ZO 2018/11/20

Simulation Environment MaTX/RtMaTX： Prof.Masanobu　KOGA 　　　　　Matrix Operation Language Useful Functions for Control System Design (Similar to Matlab) Free Software Running on Windows, Unix, DOS … Download→http://www.matx.org/#machines Scherzo：Mr.　Masaaki　HARA 　　　　　　３D　Animation　Software(Free) X Window+Open GL(Mesa,glut) Download→http://www.matx.org/#machines 2018/11/20

2018/11/20

2018/11/20

2018/11/20

Matrix Element Operation Using MaTX at Glance (Matrix) A, B; A = Z(2,2); B = I(2,2); A(1,2) = 3.0; B(2,2) = A(2,2); 2018/11/20

Matrix Operation Using MaTX at Glance Matrix A, B,C; 1. Addition, Subtraction, Multiplication, Division C = A+B; C = A-B; C= A*B; C = A\B (C = A~*B) 2. Transpose, Complex Conjugate Transpose C = A’; C = B#; 3. Eignevalue, Eignevector eigenvalues(A); {D,X} = eig(A) ; 4. Determinant, Inversion, Rnak det(A); C = A~; rank(A); 5. Singular Decomposition, Schur Decomposition {U,S,V} = svd(A); {UT} = schur(A); 2018/11/20

Structure of the Simulator diff_eqs(dx,t,u,x) link_eqs(u,t,x) t x u Logging Data Simulation Functions: {t,x,u} = OdeAuto(ts,te,x0,diff_eqs,link_eqs); {t,x,u} = OdeHybrid(ts,te,T,x0,diff_eqs,link_eqs); 2018/11/20

Fuzzy Set Allocation on Real Line (Matrix Representation) 1 h1 l2 C2 h2 … x S1 S2 Sx(:,1) stands for fuzzy set S1 Sx(:,2) stands for fuzzy set S2 … S1’ S2’ 1 C1’ l1’ h1’ l2’ C2’ h2’ y 1 C1’’ C2’’ z (S1’’) (S2’’) (Singletons) 2018/11/20

IF-THEN Rules and Function Realization R1:If x is S1 and y is S1’ then z is S1’’’. R2:If x is S2 and y is S2’ then z is S2’’’. … We assume system output and control input are real values not are fuzzy sets. w2 w1 z C1’’ C2’’ (Normalized weighted sum) 2018/11/20

Functions and Variables in the Sample Program FIsMember(x0, Sx,1) Fand(a,b) w(1,i) 2018/11/20

2018/11/20 Real I0,L0,C0,m1,J1,l1,C1, g; Matrix f; Real Umax;   Real MF, PF; Integer NO_OF_RULES, MAX_DIV; Matrix FSet1, FSet2, FSet3; Func void main() { 　　　Matrix x0; 　　　Array tc, xc, uc; 　　　Real T, h; 　　　Integer i, N; 　　　Matrix A, B, D, C, G, Q, r, P; 　　　void diff_eqn(), link_eqn(), Set_FSets(); 　　　Integer IsNear(); /* Setting for Fuzzy Control */ 　　　Set_FSets(); 　　　Umax = 1.25; L0 = 0.215; I0 = 1.75e-2; C0 = 0.118;   m1 = 5.38e-2; J1 = 1.98e-4; l1 = 0.113; C1 = 8.3e-5; g = 9.81; D = [[I0+m1*L0^2, m1*l1*L0] [m1*l1*L0, J1+m1*l1^2]]; C = [[C0, 0] [0, C1]]; G = [[0, 0] [0, -m1*l1*g]]; A = Z(4,4); A(1,3)= 1.0; A(2,4)= 1.0; A(3:4,1:4) = D~*[-G, -C]; B = Z(4,1); B(3:4,1) = D~*[1.0,0]'; r = I(1,1); Q = [[30,0,0,0][0,2000,0,0][0,0,0.1,0][0,0,0,10]]; {f,P} = lqr(A, B, Q, r); x0 = Z(4,1); x0(2,1) = -3.14;   h = 5.0e-3; {tc, xc, uc} 　　　= OdeHybrid(0.0, T, h, x0, diff_eqn, link_eqn,h,1/30.0); N =Cols(tc); for(i=1;i<=N;i++) { 　　　　xc(2,i) = -xc(2,i) + PI; ｝ print [[tc] [xc] [uc]] >> "results.mat"; } 2018/11/20

Func void diff_eqn(dx,t,x,u) Matrix dx,x,u; Real t; { Matrix D,C,G,th,dth; Real c1,s1, s2;   th = x(1:2,1); dth= x(3:4,1); s1 = sin(th(2,1)); c1 = cos(th(2,1)); s2 = sin(2*th(2,1)); D = [[I0 +m1*(L0^2+l1^2*s1*s1), m1*l1*L0*c1] [m1*l1*L0*c1, J1+m1*l1^2]]; C = [[C0+m1*l1^2*s2*dth(2,1)/2.0,-m1*l1*L0*s1+m1*l1^2*s2*dth(2,1)/2.0] [-m1*l1^2*s2*dth(2,1), C1]]; G = [0, -m1*l1*g*s1]'; dx = [[dth] [D~*([1.0,0.0]'*u - C*dth-G)]]; } 2018/11/20

Func void link_eqn(u, t, x) Real t; Matrix u, x; { Matrix FControl(); if(IsNear(x)) { u = -f*(x - [0, 0, 0, 0]'); } else { u = FControl(x); }   if(u(1,1) > Umax) { u(1,1) = Umax; if(u(1,1) < -Umax) { u(1,1) = -Umax; 2018/11/20

2018/11/20 Func Matrix FControl(xx) Matrix xx; { Func Real Defuzzy(S) Integer i; Real x,y, z, m; Real FIsMember(), FAnd(); Matrix FSet3d; Real Defuzzy(); void FOr();   NO_OF_RULES = 9; x = xx(2,1); y = xx(4,1); FSet3d = FSet3; for(i=1;i<=Cols(FSet3d);i++) { FSet3d(2,i) = 0.0; } FOr(FAnd(FIsMember(x, FSet1, 3), FIsMember(y, FSet2, 2)), FSet3d, 1); FOr(FAnd(FIsMember(x, FSet1, 2), FIsMember(y, FSet2, 3)), FSet3d, 3); FOr(FAnd(FIsMember(x, FSet1, 3), FIsMember(y, FSet2, 3)), FSet3d, 3); FOr(FAnd(FIsMember(x, FSet1, 4), FIsMember(y, FSet2, 3)), FSet3d, 3); FOr(FAnd(FIsMember(x, FSet1, 2), FIsMember(y, FSet2, 2)), FSet3d, 2); FOr(FIsMember(y, FSet2, 1), FSet3d, 2); FOr(FIsMember(y, FSet2, 4), FSet3d, 2); FOr(FIsMember(x, FSet1, 1), FSet3d, 2); FOr(FIsMember(x, FSet1, 5), FSet3d, 2); z = Defuzzy(FSet3d); return([z]); Func Real Defuzzy(S) Matrix S; { Integer i, n; Real z, w;   n = Cols(S); z = 0.0; w = 0.0; for(i=1;i<=n;i++) { z = z + S(1,i)*S(2,i); w = w + S(2,i); } return(z/w); 2018/11/20

Open Loop Control Crisp Logic Control Fuzzy Logic Control 2018/11/20

2018/11/20 Func void Set_FSets() { Integer i,j; MF = -1e+8; PF = 1e+8;   MF = -1e+8; PF = 1e+8; /* Fuzzy Set */ FSet1 = Z(3,5); FSet1(:,1) = [MF, -6.5, -3.85]'; FSet1(:,2) = [-5.3, -3.85, -3.3]'; FSet1(:,3) = [-3.5, -3.3, -2.66]'; FSet1(:,4) = [-3.3, -2.66, -1.11]'; FSet1(:,5) = [-2.66, 0.0, PF]'; FSet2 = Z(3,4); FSet2(:,1) = [MF, -26.0, 3.18]'; FSet2(:,2) = [-17.53, -1.82, 5.42]'; FSet2(:,3) = [3.0, 12.17, 26.0]'; FSet2(:,4) = [17.17, 26, PF]'; FSet3 = Z(1,3); FSet3(1,1) = -Umax; FSet3(1,2) = 0.0; FSet3(1,3) = Umax; } 2018/11/20

Simulation Results (1) 2018/11/20

Simulation Result (2) 2018/11/20

References K.Furuta, M.Yamakita, S.Kobayashi:Swing-up control of inverted pendulum using pseudo-state feedback, Proc. Inst. Mech. Engrs. Vol.206 (1992) M.Koga: MaTX reference manual (1995) 2018/11/20