Implementation of Intelligent Control on TITech Pendulum

1. Implementation of Intelligent Control onTITech Pendulum M.Yamakita Dept. of Mechanical and Control Systems Eng. Tokyo Inst. Of Tech.

2. 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. TITech Pendulum Side View Coordinate System

3. Dynamic Equation and Parameters Non-Linear Dynamic Equation Linearlized Dynamic Equation at Upright Position

4. Stabilizing Control at Upright Position Standard Regulator Problem Linear Quadratic Optimal Regulator(LQR) Optimal Feedback Control State Observer (Minimal Order Observer)

5. 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

6. Reduction of Degree of the System Assumption Reduced Model used for Non-Linear Control

7. Combine ! Controlled Vector Field of the System Non-Linear State Feedback Control U = 0 U = +Max U = -Max

8. Control Result Let’s use this controller as a expert.

9. PM PS ZO NS NB2 ZO NB1 NM NS Fuzzy Set Allocation to the Problem

10. 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

11. Simulation Environment MaTX/RtMaTX： Prof.MasanobuKOGA 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.MasaakiHARA 　　　　　　３DAnimationSoftware(Free) X Window+Open GL(Mesa,glut) Download→http://www.matx.org/#machines

15. 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);

16. 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);

17. t diff_eqs(dx,t,u,x) x u Logging Data link_eqs(u,t,x) Structure of the Simulator 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);

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

19. 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 C2’’ C1’’ (Normalized weighted sum)

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

21. 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"; }

22. 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)]]; }

23. 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; } }

24. Func Matrix FControl(xx) Matrix xx; { 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); }

25. Open Loop Control Crisp Logic Control Fuzzy Logic Control

26. Func void Set_FSets() { Integer i,j; 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; }

27. Simulation Results (1)

28. Simulation Result (2)

29. 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)