Cuprins:

1
Capitolul 1: Introducere ................................................................................................................................4

Capitolul 2: Modelul matematic, modelare neliniara ..................................................................................11

2.1. Prezentare generala .....................................................................................................................11

2.2. Studiul problemei.........................................................................................................................12

2.3. Exemplificare................................................................................................................................20

2.4. Tipuri de frecari ............................................................................................................................22

Capitolul 3: Controlul sistemului (strategii de comanda) ............................................................................24

3.1. Strategii de control ............................................................. Ошибка! Закладка не определена.

3.2. Aspecte preliminare ............................................................ Ошибка! Закладка не определена.

3.3. Proiectarea schemei de control ...................................................................................................28

3.4. CLF (Controler de Logica Fuzzy)....................................................................................................29

Capitolul 4: Aplicatie ....................................................................................................................................34

4.1. Continutul aplicatiei ..........................................................................................................................34

4.2. Sistemul de ecuatii ............................................................................................................................34

4.3. Metoda Dormand­Prince ..................................................................................................................35

4.4. Interfata.............................................................................................................................................36

Capitolul 5: Concluzii ....................................................................................................................................38

Anexa A: Codul aplicatiei..............................................................................................................................39

A.1. Controler Fuzzy .................................................................................................................................39

A.2. Pendulul invers..................................................................................................................................43

A.3. Pendulul invers dublu .......................................................................................................................46

A.4. Starea sistemului...............................................................................................................................49

A.5. Metoda Dormand­Prince ..................................................................................................................50

A.6. Engine­ul aplicatiei:...........................................................................................................................51

Anexa B: Interfata aplicatiei .........................................................................................................................54

B.1. Pendulul simplu.................................................................................................................................54

B.2. Pendulul dublu ..................................................................................................................................54

2
Bibliografie ...................................................................................................................................................55

3
Capitolul 1: Introducere

n ă ecuaţiile de miş un
mai general şi include frecă
aplicaţii inginereş Ecuaţ n
liniară simplă se propune
obţine setul de ecuaţii n

reprezinta o metodă aplicată in majoritatea

Importanţa frecvenţa definită de faptul că
reprezintă
o

4
d

5
,

6
 un

7
 un

1.

8
2.

3.

9
.

10
Capitolul 2: Modelul matematic, modelare neliniara

2.1. Prezentare generala

± ( )

Tabel 1. Nomenclatura sistemului

mi

μi

11
 Figura 1 Pendul invers multi-link

(1)

− + = = 1, … , ( + 1) , = [ , 0,0, … ,0]

(2)

=( 1 … )

2.2. Studiul problemei

=1

12
 ∑ =1 ∑ ̇
1
2 =1

≠
1
2

(3)

1 2
= +
2
=1

−1

∗ ) + sin⁡
2

+ sin( ( )
=−

−1

) + cos⁡
2

′2
1
+ cos( ( ) +
=−
2

(4)

−1

"= ( )+
#( cos⁡ cos⁡
( )
=1 =−

(5)

$= %( − −1 ) + % ′2
1 2
1
2 2
=1

13
(6)

= − + + , ( = 1, … , ( + 1))

=[ − ( ) ,− ( 1) 1 1
,…,− ( ) ]

(7)

−1
⎧
= ! + "# $ ′′
+ " # " &' [cos( ') '
′′
− sin( ') ']
⎪
2

⎪
+
'= −
⎪
=1 =1

⎪ " # * [cos( ) ′′
− sin( ) 2
]

⎨
=1

⎪
⎪
=0

⎪
⎪
=0

⎩ =+

(8)

−1

!+"# $ ′′
+ " , " &' [cos( ') '
′′
− sin( ' ) ' ]- #
2

=1 =1 '= −

+ " # * [cos( ) ′′
− sin( ) 2
]++ = − ( )
=1

14
(9)

−1

= + + cos( ) + cos⁡
( − )
= +1 =−

2
+ +
= +1

−1

+ cos⁡
( − ) + cos⁡
( − )
= +1 = − = +1

(10)

= ′′
+ + cos( ) ′′
− + sin( ) ′

= +1 = +1

2 ′′
+ +
= +1

−1

+ cos( − ) ′′
− sin( − )( − )
=−

+ cos − ′′
− sin − ( − )
= +1

−1

+ [cos − ′′
− sin − ( − ) ]
= +1 = −

(11)

−1

=− + sin( ) ′
+ sin⁡
( − )
= +1 =−

−1

+ sin⁡
( − ) + ( −
sin⁡ )
= +1 = − = +1
15
(12)

=− + sin( )
= +1

(13)

=( + +1 )
′
− −1
′
+1 + ′
−1

(14)

+1 = + cos( ) ′′
+ 2
+ ′′

= +1 = +1

−1

+ [cos( − ) ′′
− sin( − ) ′′
]
=−

+ [cos − ′′
− sin − ′′
]
= +1

−1

+ [cos − ′′
− sin − 2
]
= +1 = −

− + sin( ) + ( + +1 )
′
− −1
′
+1 + ′
−1
= +1

(i = 1, ..., n)

(15)

( ) ′′
+ ( , ′) ′
+ ( )= ( , )
16
(16)

⎧
⎪
+ , =1
( )=
⎨
=1

⎪ −1 −1 −1 −1 , 1 ≤
⎩
2
+ + < +1
=

(17)

−2
⎧ ≤
⎪
⎪
cos⁡
( ) + cos⁡
( ) , = 1, 1 < +1
= −1 = − −1 = −1
⎪ −2
⎪ ≤
⎪
( ) +
cos⁡ cos⁡
( ) , = 1, 1 < +1
= −1 = − −1 = −1
( )= −2
⎨
⎪⎛ [cos( − −1 )] −1 −1 +" #cos$ − −1 %&' −1 ⎞
⎪⎜ = − −1 ⎟ , , -./
⎪⎜ −1 ⎟
=

⎪⎜ ⎟
⎪ +" " ( ( − −1 %]' ' −1
⎩⎝ ⎠
[cos$
= (= −

(18)

34 , = 1
0 ( , ′)
=2
3 −1 + 3 ,1 < ≤ +1

(19)

0 ( , ′)

≤
⎧ −2
0, = 1, 1 < +1
⎪
⎪ − −1 ) −1 − −1 ) −1
⎪
sin( sin⁡
(
= −1 = −1 = − −1
⎪ −2

⎨⎛
− − −1 ) −1 −1 −1 −" − −1 % −1 ' −1 ⎞
=
⎪⎜ ⎟ , , -./
sin( sin$
= − −1
⎪⎜ −1 ⎟
=

⎪⎜ ⎟
⎪ −" " ( sin⁡
( ( − −1 ) −1 ' ' −1
⎩⎝ = (= − ⎠

17
(20)

⎧
0, =1
( )=
⎨− −1 −1 −1 −1 ) ,
⎩
+ sin(
=

(21)

− ( )
( , )=
− ( −1 )
, =1
−1 −1
,

(22)

(1) ( ) ′′
= −!( , ′) ′
− ( )+ ( , ) " =( 1 … )$

(2) % = & ' , = 1, … , + 1

( (
(3) & ' %′ = & ' % + )−0 * + )0*
−!
0 0
0 0

Sau

(23)

(
%′ = & '% + + ,++ ,
− ! −
0 0
−1 −1 −1
0
0

Sau

(24)

% ′ ( ) = (%( ), ( ))

%0
%( )
18
(25)

=( 1 1 … −1 )

Stiind ca:

(26)

− , = 1, … , − 1
−1
= +1

= + , = 2, … ,
=1

γ + θ1 (i = 2, … , n)
−1
=1 k

( )

(27)

= −1, ≤ ≤ 2 + 2,
1, = ,
= + 1,2 < + 1, + 3 <
0,

( 1 2 1 2) ( 1 1 1 1)

1 0 00 0 0
⎛ ⎞
−1 10 0
0 1 00 0 0
= ⎜
⎜0 0⎟⎟
0 0
0 01 0

⎝0 0 00 −1 1⎠
0 0 00 1 0

19
 2.3. Exemplificare
,

(28)

( + + 2) ̈+( + 2 1 ) cos( 1 ) 1
̈ + cos( 2 ) 2̈ +
−( 2 1 ) sin( 1 ) 1 − 2 2 sin( 2 ) 2 − ( )μ
1 1 1 2 2
2 2
1 1 + =

( + 2 1) ̈+ + 2
+ 2 ̈ + cos( − 2) 2
̈ +( + 2) 1 −
− 2) 2 − ( 2 1 ) sin 1 =− ( 1 )μ
1 1 1 1 1 2 1 1 2 2 1 1 1 2 2
2
+ 2 2 1 sin( 1 1 1 +

̈ + ̈ −
1 1

cos( 2 ) ̈ + cos( − 2) 1 + 2
sin( − 2) 1
2
−
− sin( 2 ) = −
2 2 2 2 1 1 2 2 2 2 2 2 1 1 2 1

+ 2 2 2 2 ( 2 )μ 2 2

1− 2) 1 − 3)
cos 3 cos 2 4 cos 3

( )=
1 2 1

− 2) 21 cos( 2 − 3)
9 cos 1 10 11 cos( 12 cos(

1 − 3) − 3)
18 cos 2 19 cos( 1 20
28 cos 3 29 cos( 30 cos( 2 31

G(q, ̇ )
sin( 1 ) sin( 2 ) sin( 3 ) 3
14 sin( 1 − 2 ) 16 sin( 1 − 3 ) 3
=⎛ ⎞
5 6 2 7 2 8

sin( 1 − 2 ) 1 + 25 sin( 2 − 3 ) 3 +
0 13 2 + 15

⎝0 33 sin( 1 − 3 ) 1 − 3) 2 ⎠
0 22 23 24 26
35 sin( 2 + 36 31

1)
0
( )=
27 sin( 2 )
17 sin(

34 sin( 3 )

20
 Tabel 2. Constantele sistemului

1 1 2 + 3 19 ( 2 2 + 3 2) 1

2 1 1 + ( 2 + 3) 1 20 2 + 2 2
2
+ 3 2
2

3 2 2 + 3 2 21 3 3 2

4 3 3 22 − 1( 2 2 + 3 2)

5 23 − 2

6 − 1 1 −( 2 + 3) 1 24 2 + 3

7 −( 2 2 + 3 2) 25 3 3 2

8 − 3 3 26 − 3

9 1 1 + ( 2 + 3) 1 27 − ( 2 2 + 3 2)

2 2
10 1 + 1 1 +( 2 + 3) 1 28 3 3

11 ( 2 2 + 3 2) 1 29 3 3 1

12 3 3 1 30 3 3 2

2
13 1 + 2 31 3 + 3 3

14 ( 2 2 + 3 2) 1 32 3

15 − 2 33 − 3 3 1

16 3 3 1 34 − 3 3

17 − ( 1 1 + 2 + 3 1) 35 − 3 3 1

18 2 2 + 3 2 36 − 3

−
⎛− ⎞
( )μ

( , )=⎜ ⎟
−
( 1 )μ 1 1

−
( 2 )μ 2
⎝ ⎠
2
( 3 )μ 3 3

21
 2.4. Tipuri de frecari

un

̇ =0
̇ ≠0
=
+

− <μ
≥μ
=
μ

=μ ( ̇ ),

= −" ̇

μ ,μ ,"
=( + #) .

22
 0,08328
0,04287
0,3156

23
Capitolul 3: Controlul sistemului (strategii de comanda)

3.1. Strategii de control

24
 3.2. Aspecte preliminare
2

25
 Figura 2. Sistem pendul invers si carut

Figura 3. Carut si pendul separate

,θe
26
 2

2
=

F=M ̈ +b ̇ +N

2
1
2
= − −

2
=

2
1
2
= ( cos + sin )

2
= =

2
= 2

2
= = −

2
= ( 2
+ )

27
 = − sin

= − cos

2 2 2 2

2
= 2
+ sin − cos 2

= cos

= − sin

2 2 2

2
= − cos − sin 2

2 2 2 2
N = m 2
= 2
+ sin − cos 2

2 2 2
P = m 2
= − cos − sin 2

3.3. Proiectarea schemei de control

28
 Figura 4. Diagrama bloc controler fyzzy

un

– Fc.

3.4. CLF (Controler de Logica Fuzzy)

29
numerice. Variabilele lingvistice sunt definite in limbaj normal (cum ar fi “mic” sau “mare”) si

μ ∶ → [0,1]

0 μ ( 0

30
Figura 5. Comfigurare de baza CLF

31
 3

Figura 6 Functiile de apartenenta pendul

32
Tabel 3 Exemplu reguli control fuzzy

33
Capitolul 4: Aplicatie

4.1. Continutul aplicatiei

4.2. Sistemul de ecuatii

̈+ 12 Ө
̈ =
̈+ ̈
22 Ө =
11
21

̈+ 12 Ө1
̈ + ̈ =
13 Ө2
̈+ ̈
22 Ө1 +
̈
23 Ө2 =
11

̈+ 32 Ө1 +
̈ 33 Ө2 =
̈
21
31

34
 ̈ Ӫ

−
⎧ ̈=
22 −
22 12

⎨Ӫ = 11 −
11 21 12

⎩ 22 −
21

11 21 12

̈
Ө1 ̈
11 12 13

Ө2̈
21 22 23 =
31 32 33

−1

̈
Ө1 ̈ −1

Ө2̈
=

, ̇ ,Ө1 , Ө1̇ ,Ө2 , Ө2̇ –

Ө–

′
= ̇
⎧ ′̇ = ̈
⎪
⎪Ө1 ′ = Ө1̇
̇′ = Ө1̈
⎨Ө1
⎪Ө2
⎪
′
= Ө2̇
⎩Ө2̇ ′ = Ө2̈

4.3. Metoda Dormand­Prince

35
 Tabelul Butcher
0
1/5 1/5
3/10 3/40 9/40
4/5 44/45 ­56/ 15 32/9
­25360/
8/9 19372/6561 64448/6561 ­212/729
2187
1 9017/3168 ­355/33 46732/5247 49/176 ­5103/18656
1 35/384 0 500/113 125/192 ­2187/6784 11/84
35/384 0 500/113 125/192 ­2187/6784 11/84
5179/57600 0 7571/16695 393/640 ­92097/ 339200 187/2100 1/40

K1 = f(stare initiala, F)

K2 = f(stare initiala + DormandPrince1(k1) * delta, F);

K3 = f(stare initiala + DormandPrince2(k1, k2) * delta, F);

K4 = f(stare initiala + DormandPrince3(k1, k2, k3) * delta, F);

K5 = f(stare initiala + DormandPrince4(k1, k2, k3, k4) * delta, F);

K6 = f(stare initiala + DormandPrince5(k1, k2, k3, k4, k5) * delta, F);

K7 = f(stare initiala + DormandPrince6(k1, k2, k3, k4, k5, k6) * delta, F);

Stare finala = stare initiala + DormandPrince7(k1, k2, k3, k4, k5, k6, k7) * delta;

Unde Ki sunt stari intermediare, f functie incrementara folosita de algoritmul de evolutie, delta
timpul trecut de la pasul anterior si F forta aplicata de controlerul fuzzy.

4.4. Interfata
In interfata aplicatiei se pot observa schimbarile din sistemul pendulului invers. Utilizatorului i se
da posiblitatea de a interactiona cu procesul in timp real, modificand parametrii in care functioneaza
sistemul. Parametrii configurabili sunt: masa si coeficientii de frecare ai caruciorului, masele si
coeficientii de frecare ale pendulelor, starea initiala a sistemului, precum si perturbari in sistem
actionand cu un anumit impuls asupra pendulelor. De asemenea exista posibilitatea de a schimba din

36
sistem dublu in sistem simplu si invers, sau de a reporni sistemul din starea initiala si utilizatorului poate
observa evolutia sistemului fara reglare sau cu reglare.

37
Capitolul 5: Concluzii

un

d
,
.

38
Anexa A: Codul aplicatiei

A.1. Controler Fuzzy

publicclassFuzzyController
{
constbool DEBUG_FUZZY_CONTROLLER = false;

constuint DEFAULT_THETA_FUZZY_STATE_GRADE = 3;
constuint DEFAULT_D_THETA_FUZZY_STATE_GRADE = 2;
constuint DEFAULT_X_FUZZY_STATE_GRADE = 2;
constuint DEFAULT_D_X_FUZZY_STATE_GRADE = 3;

constuint DEFAULT_FORCE_PENDULUM_FUZZY_STATE_GRADE = 4;
constuint DEFAULT_FORCE_CART_FUZZY_STATE_GRADE = 4;

constdouble DEFAULT_THETA_MAX_RANGE = 0.02;
constdouble DEFAULT_D_THETA_MAX_RANGE = 0.4;
constdouble DEFAULT_X_MAX_RANGE = 2.0;
constdouble DEFAULT_D_X_MAX_RANGE = 0.03;

constdouble DEFAULT_FORCE_PENDULUM_MAX_RANGE = 150.0;
constdouble DEFAULT_FORCE_CART_MAX_RANGE = 80.0;

staticuint theta_fuzzy_state_grade;
staticuint d_theta_fuzzy_state_grade;
staticuint x_fuzzy_state_grade;
staticuint d_x_fuzzy_state_grade;

staticuint force_pendulum_fuzzy_state_grade;
staticuint force_cart_fuzzy_state_grade;

staticdouble theta_max_range;
staticdouble d_theta_max_range;
staticdouble x_max_range;
staticdouble d_x_max_range;

staticdouble force_pendulum_max_range;
staticdouble force_cart_max_range;

staticFuzzyController instance = null;

staticint [] debug_fuzzy_states = newint [6];

staticint [] saved_debug_fuzzy_states = newint [6];
staticint debug_same_result_count;

staticdouble [] debug_input_values = newdouble [4];

public FuzzyController()
{
theta_fuzzy_state_grade = DEFAULT_THETA_FUZZY_STATE_GRADE;
d_theta_fuzzy_state_grade = DEFAULT_D_THETA_FUZZY_STATE_GRADE;
x_fuzzy_state_grade = DEFAULT_X_FUZZY_STATE_GRADE;
d_x_fuzzy_state_grade = DEFAULT_D_X_FUZZY_STATE_GRADE;

force_pendulum_fuzzy_state_grade =
DEFAULT_FORCE_PENDULUM_FUZZY_STATE_GRADE;
force_cart_fuzzy_state_grade =
39
 DEFAULT_FORCE_CART_FUZZY_STATE_GRADE;

theta_max_range = DEFAULT_THETA_MAX_RANGE;
d_theta_max_range = DEFAULT_D_THETA_MAX_RANGE;
x_max_range = DEFAULT_X_MAX_RANGE;
d_x_max_range = DEFAULT_D_X_MAX_RANGE;

force_pendulum_max_range = DEFAULT_FORCE_PENDULUM_MAX_RANGE;
force_cart_max_range = DEFAULT_FORCE_CART_MAX_RANGE;

for (int i = 0; i < 6; i++)

{
saved_debug_fuzzy_states[i] = 0;
}
debug_same_result_count = 0;
}

privatestaticint GetFuzzyState(double value,

uint grade,
double max_range)
{
System.Diagnostics.Debug.Assert(max_range > 0.0 && grade > 0);

double fuzzy_state = value * (double)grade / max_range;
int result;

if (fuzzy_state > 0)
{
if (fuzzy_state > (double)grade ­ 0.5)
{
return (int)grade;
}
result = 0;
while (fuzzy_state > 0.5)
{
result++;
fuzzy_state ­= 1.0;
}
return result;
}
else
{
if (fuzzy_state < ­(double)grade + 0.5)
{
return ­(int)grade;
}
result = 0;
while (fuzzy_state < ­0.5)
{
result­­;
fuzzy_state += 1.0;
}
return result;
}
}

privatestaticint GetThetaFuzzyState(double theta)

{
return GetFuzzyState(theta, theta_fuzzy_state_grade,
theta_max_range);
40
 }

privatestaticint GetDThetaFuzzyState(double d_theta)

{
return GetFuzzyState(d_theta, d_theta_fuzzy_state_grade,
d_theta_max_range);
}

privatestaticint GetXFuzzyState(double x)
{
return GetFuzzyState(x, x_fuzzy_state_grade,
x_max_range);
}

privatestaticint GetDXFuzzyState(double d_x)

{
return GetFuzzyState(d_x, d_x_fuzzy_state_grade,
d_x_max_range);
}

privatestaticdouble GetForcePendulum(double theta, double d_theta)

{
System.Diagnostics.Debug.Assert(force_pendulum_fuzzy_state_grade >=
Math.Max(theta_fuzzy_state_grade,
d_theta_fuzzy_state_grade));

int theta_fuzzy_state = GetThetaFuzzyState(theta);

int d_theta_fuzzy_state = GetDThetaFuzzyState(d_theta);
int sum = theta_fuzzy_state + d_theta_fuzzy_state;
int force_pendulum_fuzzy_state =
(sum> 0) ?
(sum> (int)force_pendulum_fuzzy_state_grade ?
(int)force_pendulum_fuzzy_state_grade : sum) :
(sum< ­(int)force_pendulum_fuzzy_state_grade ?
­(int)force_pendulum_fuzzy_state_grade : sum);

if (DEBUG_FUZZY_CONTROLLER)
{
debug_fuzzy_states[0] = theta_fuzzy_state;
debug_fuzzy_states[1] = d_theta_fuzzy_state;
debug_fuzzy_states[4] = force_pendulum_fuzzy_state;
}

return force_pendulum_max_range *
(double)force_pendulum_fuzzy_state /
(double)force_pendulum_fuzzy_state_grade;
}

privatestaticdouble GetForceCart(double x, double d_x)

{
System.Diagnostics.Debug.Assert(force_cart_fuzzy_state_grade >=
Math.Max(x_fuzzy_state_grade,
d_x_fuzzy_state_grade));

int x_fuzzy_state = GetXFuzzyState(x);

int d_x_fuzzy_state = GetDXFuzzyState(d_x);
int sum = x_fuzzy_state + d_x_fuzzy_state;
int force_cart_fuzzy_state =
(sum> 0) ?
(sum> (int)force_cart_fuzzy_state_grade ?
41
 (int)force_cart_fuzzy_state_grade : sum) :
(sum< ­(int)force_cart_fuzzy_state_grade ?
­(int)force_cart_fuzzy_state_grade : sum);

if (DEBUG_FUZZY_CONTROLLER)
{
debug_fuzzy_states[2] = x_fuzzy_state;
debug_fuzzy_states[3] = d_x_fuzzy_state;
debug_fuzzy_states[5] = force_cart_fuzzy_state;
}

return force_cart_max_range *
(double)force_cart_fuzzy_state /
(double)force_cart_fuzzy_state_grade;
}

publicstaticdouble GetForce(SystemStatus s, double dt)

{
/* Make sure FuzzyController is initialized */
if (instance == null)
{
instance = newFuzzyController();
}

if (DEBUG_FUZZY_CONTROLLER)
{
debug_input_values[0] = s.theta;
debug_input_values[1] = s.d_theta;
debug_input_values[2] = s.x;
debug_input_values[3] = s.d_x;
}

double force_pendulum = GetForcePendulum(s.theta, s.d_theta);
double force_cart = GetForceCart(s.x, s.d_x);

if (DEBUG_FUZZY_CONTROLLER)
{
bool same = true;
for (int i = 0; i < 6; i++)
{
if (saved_debug_fuzzy_states[i] != debug_fuzzy_states[i])
{
same = false;
break;
}
}
if (same)
{
debug_same_result_count++;
Console.Out.Write(".");
Console.Out.Write("Input:  Theta = ");
Console.Out.Write(debug_input_values[0]);
Console.Out.Write(", DTheta = ");
Console.Out.Write(debug_input_values[1]);
Console.Out.Write(", X = ");
Console.Out.Write(debug_input_values[2]);
Console.Out.Write(", DX = ");
Console.Out.Write(debug_input_values[3]);
Console.Out.Write(", NEXT_DT = ");
Console.Out.Write(dt);
42
Console.Out.WriteLine();
}
else
{
Console.Out.Write(debug_same_result_count);
Console.Out.WriteLine();
Console.Out.WriteLine();

Console.Out.Write("Input: Theta = ");

Console.Out.Write(debug_input_values[0]);
Console.Out.Write(", DTheta = ");
Console.Out.Write(debug_input_values[1]);
Console.Out.Write(", X = ");
Console.Out.Write(debug_input_values[2]);
Console.Out.Write(", DX = ");
Console.Out.Write(debug_input_values[3]);
Console.Out.Write(", NEXT_DT = ");
Console.Out.Write(dt);
Console.Out.WriteLine();

Console.Out.Write("Fuzzy States: Theta = ");

Console.Out.Write(debug_fuzzy_states[0]);
Console.Out.Write(", DTheta = ");
Console.Out.Write(debug_fuzzy_states[1]);
Console.Out.Write(", X = ");
Console.Out.Write(debug_fuzzy_states[2]);
Console.Out.Write(", DX = ");
Console.Out.Write(debug_fuzzy_states[3]);
Console.Out.Write(", FP = ");
Console.Out.Write(debug_fuzzy_states[4]);
Console.Out.Write(", FC = ");
Console.Out.Write(debug_fuzzy_states[5]);
Console.Out.WriteLine();

Console.Out.Write("Force = ");
Console.Out.Write(force_pendulum ­ force_cart);
Console.Out.WriteLine();

for (int i = 0; i < 6; i++)

{
saved_debug_fuzzy_states[i] = debug_fuzzy_states[i];
}
}
}

return force_pendulum ­ force_cart;

}
}

A.2. Pendulul invers

classInvertedPendulum
{
constdouble DEFAULT_CART_MASS = 2.0;
constdouble DEFAULT_CART_VISCOUS_FRICTION = 0.0;
constdouble DEFAULT_CART_COULOMB_FRICTION = 0.0;

43
constdouble DEFAULT_PENDULUM_MASS = 0.1;
constdouble DEFAULT_PENDULUM_LENGTH = 2.0;
constdouble DEFAULT_PENDULUM_DISTANCE_TO_CENTER = 0.5;
constdouble DEFAULT_PENDULUM_INERTIA_MOMENTUM =
DEFAULT_PENDULUM_MASS * DEFAULT_PENDULUM_LENGTH * DEFAULT_PENDULUM_LENGTH
/ 3.0;
constdouble DEFAULT_PENDULUM_VISCOUS_FRICTION = 0.0;

constdouble G = 9.8;

// Let's note:
// Constants:
// M = cart mass
// m = pendulum mass
// l = distance from pendulum base to center of gravity
// I = pendulum moment of inertia
// cc = cart viscous friction
// cp = pendulum viscous friction
// u = cart Coulomb friction
// G = gravity
//
// Variables:
// pA = pendulum angular position
// pDA = pendulum angular velocity (derived position)
// pDDA = pendulum angular acceleration (derived velocity)
// cX = cart position
// cDX = cart velocity (derived position)
// cDDX = cart acceleration (derived velocity)
// cos = cos(pA)
// sin = sin(pA)
//
// The next differential equations must be solved:
// (M+m) * cDDX + (m * l * cos) * pDDA =
// ­cc * cDX + (m * l * sin) * pDA * pDA ­ (sgn(cDX) * u * (M + m) * G))
// + F;
// (m * l * cos) * cDDX + (I + m * l * l) * pDDA =
// ­cp * pDA + G * m * l * sin;
//
// By noting:
// A = M+m
// B = (m * l * cos)
// C = ­cc * cDX + (m * l * sin) * pDA * pDA ­ (sgn(cDX) * u * (M + m) * G))
// D = (m * l * cos)
// E = I + m * l * l
// H = ­cp * pDA + G * m * l * sin
// The next system is obtained:
// A * cDDX + B * pDDA = C + F
// D * cDDX + E * pDDA = H
// So cDDX and pDDA can be computed as:
// cDDX = ((C + F) * E ­ H * B) / (A * E ­ D * B)
// pDDA = ((C + F) * D ­ H * A) / (B * D ­ E * A)
// These results are used when computing system evolution

staticdouble cart_mass;
staticdouble pendulum_mass;
//static double length; // unused for single inverted pendulum
staticdouble l;
staticdouble inertia;
staticdouble cc;
44
staticdouble uc;
staticdouble cp;

staticdouble total_mass;
staticdouble m_l;
staticdouble i_and_m_l_l;
staticdouble u_total_mass_G;
staticdouble m_l_G;

staticInvertedPendulum instance = null;

private InvertedPendulum()
{
cart_mass = DEFAULT_CART_MASS;
pendulum_mass = DEFAULT_PENDULUM_MASS;
l = DEFAULT_PENDULUM_DISTANCE_TO_CENTER;
inertia = DEFAULT_PENDULUM_INERTIA_MOMENTUM;
cc = DEFAULT_CART_VISCOUS_FRICTION;
uc = DEFAULT_CART_COULOMB_FRICTION;
cp = DEFAULT_PENDULUM_VISCOUS_FRICTION;

ComputeConstants();
}

privatestaticvoid ComputeConstants()
{
total_mass = cart_mass + pendulum_mass;
m_l = pendulum_mass * l;
i_and_m_l_l = inertia + m_l * l;
u_total_mass_G = uc * total_mass * G;
m_l_G = m_l * G;
}

privatestaticvoid check()
{
if (instance == null)
{
instance = newInvertedPendulum();
}
}

privatestaticSystemStatus EvolutionHelper(SystemStatus s, double f)

{
SystemStatus result = newSystemStatus();
double sin = System.Math.Sin(s.theta);
double cos = System.Math.Cos(s.theta);
double sgn =
(s.d_x == 0.0) ?
0 : ((s.d_x > 0) ?
1.0 : ­1.0);

// A = M+m
// B = (m * l * cos)
// C = ­cc * cDX + (m * l * sin) * pDA * pDA ­ (sgn(cDX) * u * (M + m) * G))
// D = (m * l * cos)
// E = I + m * l * l
// H = ­cp * pDA + G * m * l * sin
double a = total_mass;
double b = m_l * cos;
double c =
45
 ­cc * s.d_x + m_l * sin * s.d_theta * s.d_theta ­ sgn * u_total_mass_G;
double d = b;
double e = i_and_m_l_l;
double h = ­cp * s.d_theta + m_l_G * sin;

// cDDX = ((C + F) * E ­ H * B) / (A * E ­ D * B)
// pDDA = ((C + F) * D ­ H * A) / (B * D ­ E * A)
double c_and_f = c + f;
double ae_minus_bd = a * e ­ b * d;

result.x = s.d_x;
result.d_x = (c_and_f * e ­ h * b) / ae_minus_bd;
result.theta = s.d_theta;
result.d_theta = (h * a ­ c_and_f * d) / ae_minus_bd;

return result;
}

publicstaticSystemStatus Evolution(SystemStatus s, double f, double dt)

{
check();

// Use Dormand–Prince method

SystemStatus [] temp = newSystemStatus [7];
temp[0] = EvolutionHelper(s, f);
temp[1] = EvolutionHelper(s + dt * DormandPrince.DP1(temp), f);
temp[2] = EvolutionHelper(s + dt * DormandPrince.DP2(temp), f);
temp[3] = EvolutionHelper(s + dt * DormandPrince.DP3(temp), f);
temp[4] = EvolutionHelper(s + dt * DormandPrince.DP4(temp), f);
temp[5] = EvolutionHelper(s + dt * DormandPrince.DP5(temp), f);
temp[6] = EvolutionHelper(s + dt * DormandPrince.DP6(temp), f);

return s + dt * DormandPrince.DP7(temp);
}

publicstaticdouble GetPendulumLength()
{
return DEFAULT_PENDULUM_LENGTH;
}
}

A.3. Pendulul invers dublu

classDoubleInvertedPendulum
{
constdouble DEFAULT_CART_MASS = 2.0;
constdouble DEFAULT_CART_VISCOUS_FRICTION = 0.0;
constdouble DEFAULT_CART_COULOMB_FRICTION = 0.0;

constdouble DEFAULT_PENDULUM_MASS_1 = 0.1;

constdouble DEFAULT_PENDULUM_LENGTH_1 = 2.0;
constdouble DEFAULT_PENDULUM_DISTANCE_TO_CENTER_1 = 0.5;
constdouble DEFAULT_PENDULUM_INERTIA_MOMENTUM_1 =
DEFAULT_PENDULUM_MASS_1 * DEFAULT_PENDULUM_LENGTH_1 *
DEFAULT_PENDULUM_LENGTH_1 /
3.0;
constdouble DEFAULT_PENDULUM_VISCOUS_FRICTION_1 = 0.0;

46
constdouble DEFAULT_PENDULUM_MASS_2 = 0.1;
constdouble DEFAULT_PENDULUM_LENGTH_2 = 2.0;
constdouble DEFAULT_PENDULUM_DISTANCE_TO_CENTER_2 = 0.5;
constdouble DEFAULT_PENDULUM_INERTIA_MOMENTUM_2 =
DEFAULT_PENDULUM_MASS_2 * DEFAULT_PENDULUM_LENGTH_2 *
DEFAULT_PENDULUM_LENGTH_2 /
3.0;
constdouble DEFAULT_PENDULUM_VISCOUS_FRICTION_2 = 0.0;

constdouble G = 9.8;

staticdouble cart_mass;
staticdouble cart_vf;
staticdouble cart_cf;

staticdouble m1;
staticdouble L1;
staticdouble l1;
staticdouble I1;
staticdouble vf1;

staticdouble m2;
staticdouble L2;
staticdouble l2;
staticdouble I2;
staticdouble vf2;

private DoubleInvertedPendulum()
{
cart_mass = DEFAULT_CART_MASS;
cart_vf = DEFAULT_CART_VISCOUS_FRICTION;
cart_cf = DEFAULT_CART_COULOMB_FRICTION;

m1 = DEFAULT_PENDULUM_MASS_1;
L1 = DEFAULT_PENDULUM_LENGTH_1;
l1 = DEFAULT_PENDULUM_DISTANCE_TO_CENTER_1;
I1 = DEFAULT_PENDULUM_INERTIA_MOMENTUM_1;
vf1 = DEFAULT_PENDULUM_VISCOUS_FRICTION_1;

m2 = DEFAULT_PENDULUM_MASS_2;
L2 = DEFAULT_PENDULUM_LENGTH_2;
l2 = DEFAULT_PENDULUM_DISTANCE_TO_CENTER_2;
I2 = DEFAULT_PENDULUM_INERTIA_MOMENTUM_2;
vf2 = DEFAULT_PENDULUM_VISCOUS_FRICTION_2;

ComputeConstants();
}

staticDoubleInvertedPendulum instance = null;

privatestaticvoid check()
{
if (instance == null)
{
instance = newDoubleInvertedPendulum();
}
}

staticdouble Mt;
staticdouble mt;
staticdouble I1t;
47
staticdouble I2t;
staticdouble m2l2;
staticdouble ml2L1;

privatestaticvoid ComputeConstants()
{
Mt = cart_mass + m1 + m2;
mt = m1 * l1 + m2 * L1;
I1t = I1 + m1 * l1 * l1 + m2 * L1 * L1;
I2t = I2 + m2 * l2 * l2;
m2l2 = m2 * l2;
ml2L1 = m2 * l2 * L1;
}

privatestaticSystemStatus EvolutionHelper (SystemStatus s, double f)

{
SystemStatus result = newSystemStatus();
double sin = System.Math.Sin(s.theta);
double cos = System.Math.Cos(s.theta);
double sin2 = System.Math.Sin(s.theta2);
double cos2 = System.Math.Cos(s.theta2);
double sin_diff = System.Math.Sin(s.theta ­ s.theta2);
double cos_diff = System.Math.Cos(s.theta ­ s.theta2);
double sign =
(s.x == 0.0) ?0 : (s.x > 0.0 ? 1.0 : ­1.0);

Matrix3x3 coefficients =
newMatrix3x3(Mt, mt * cos, m2l2 * cos2,
mt, I1t, ml2L1 * cos_diff,
m2l2, ml2L1 * cos_diff, I2t);

result.x = s.d_x;
result.d_x = 0;
result.theta = s.d_theta;
result.d_theta = 0;
result.theta2 = s.d_theta2;
result.d_theta2 = 0;

coefficients.Invert();
coefficients.RightMultiplyWithVector(
­cart_vf * s.d_x + mt * sin * s.d_theta * s.d_theta +
m2l2 * sin2 * s.d_theta2 * s.d_theta2 ­ sign * cart_cf * Mt * G,
­(vf1 + vf2) * s.d_theta + vf2 * s.d_theta2 ­
ml2L1 * sin_diff * s.d_theta2 * s.d_theta2 + G * mt * sin,
ml2L1 * sin_diff * s.d_theta * s.d_theta +
vf2 * (s.d_theta ­ s.d_theta2) + G * m2l2 * sin2,
ref result.d_x, ref result.d_theta, ref result.d_theta2);

return result;
}

publicstaticSystemStatus Evolution(SystemStatus s, double f, double dt)

{
check();

// Use Dormand–Prince method

SystemStatus[] temp = newSystemStatus[7];
temp[0] = EvolutionHelper(s, f);
temp[1] = EvolutionHelper(s + dt * DormandPrince.DP1(temp), f);
temp[2] = EvolutionHelper(s + dt * DormandPrince.DP2(temp), f);
48
temp[3] = EvolutionHelper(s + dt * DormandPrince.DP3(temp), f);
temp[4] = EvolutionHelper(s + dt * DormandPrince.DP4(temp), f);
temp[5] = EvolutionHelper(s + dt * DormandPrince.DP5(temp), f);
temp[6] = EvolutionHelper(s + dt * DormandPrince.DP6(temp), f);

return s + dt * DormandPrince.DP7(temp);
}

publicstaticdouble GetFirstPendulumLength()
{
return L1;
}

publicstaticdouble GetSecondPendulumLength()
{
return L2;
}
}

A.4. Starea sistemului

publicclassSystemStatus
{
publicdouble x; /* the position of the cart */
publicdouble d_x; /* velocity of the cart */

publicdouble theta; /* angular position of pendulum */

publicdouble d_theta; /* angular velocity of pendumul */

publicdouble theta2;
publicdouble d_theta2;

publicstaticSystemStatusoperator+(SystemStatus x, SystemStatus y)
{
SystemStatus result = newSystemStatus ();

result.x = x.x + y.x;

result.theta = x.theta + y.theta;
result.theta2 = x.theta2 + y.theta2;
result.d_x = x.d_x + y.d_x;
result.d_theta = x.d_theta + y.d_theta;
result.d_theta2 = x.d_theta2 + y.d_theta2;

return result;
}

publicstaticSystemStatusoperator ­(SystemStatus x, SystemStatus y)

{
SystemStatus result = newSystemStatus();

result.x = x.x ­ y.x;

result.theta = x.theta ­ y.theta;
result.theta2 = x.theta2 ­ y.theta2;
result.d_x = x.d_x ­ y.d_x;
result.d_theta = x.d_theta ­ y.d_theta;
result.d_theta2 = x.d_theta2 ­ y.d_theta2;

return result;

49
 }

publicstaticSystemStatusoperator *(SystemStatus x, double a)

{
SystemStatus result = newSystemStatus();

result.x = x.x * a;
result.theta = x.theta * a;
result.theta2 = x.theta2 * a;
result.d_x = x.d_x * a;
result.d_theta = x.d_theta * a;
result.d_theta2 = x.d_theta2 * a;

return result;
}

publicstaticSystemStatusoperator *(double a, SystemStatus x)

{
return x * a;
}
}

A.5. Metoda Dormand­Prince

classDormandPrince
{
staticdouble[] DP =
{
// DP1 (0):
(double)1/5,
// DP2 (1­2):
(double)3/40, (double)9/40,
// DP3 (3­5)
(double)44/45, ­(double)56/15, (double)32/9,
// DP4 (6­9)
(double)19372/6561, ­(double)25360/2187, (double)64448/6561, ­(double)212/729,
// DP5 (10­14)
(double)9017/3168, ­(double)355/33, (double)46732/5247, (double)49/176,
­(double)5103/18656,
// DP 6 (15­20)
(double)35/384, (double)0, (double)500/1113, (double)125/192,
­(double)2187/6784, (double)11/84,
// DP 7 (21­27)
(double)5179/57600, (double)0, (double)7571/16695, (double)393/640,
­(double)92097/339200, (double)187/2100, (double)1/40
};

publicstaticSystemStatus DP1(SystemStatus[] k)
{
return DP[0] * k[0];
}

publicstaticSystemStatus DP2(SystemStatus[] k)
{
return DP[1] * k[0] + DP[2] * k[1];
}

publicstaticSystemStatus DP3(SystemStatus[] k)

50
 {
return DP[3] * k[0] + DP[4] * k[1] + DP[5] * k[2];
}

publicstaticSystemStatus DP4(SystemStatus[] k)
{
return DP[6] * k[0] + DP[7] * k[1] + DP[8] * k[2] + DP[9] * k[3];
}

publicstaticSystemStatus DP5(SystemStatus[] k)
{
return DP[10] * k[0] + DP[11] * k[1] + DP[12] * k[2] + DP[13] * k[3] +
DP[14] * k[4];
}

publicstaticSystemStatus DP6(SystemStatus[] k)
{
return DP[15] * k[0] + DP[16] * k[1] + DP[17] * k[2] + DP[18] * k[3] +
DP[19] * k[4] + DP[20] * k[5];
}

publicstaticSystemStatus DP7(SystemStatus[] k)
{
return DP[21] * k[0] + DP[22] * k[1] + DP[23] * k[2] + DP[24] * k[3] +
DP[25] * k[4] + DP[26] * k[5] + DP[27] * k[6];
}
}

A.6. Engine­ul aplicatiei:

publicclassEngine
{
publicstaticSystemStatus systemStatus;

privatestaticEngine instance = null;

staticbool use_double;

long lastFrameTime;

constlong FRAME_RATE = 60;
constlong FRAME_LENGTH = 1000 / FRAME_RATE;

publicconstdouble WORLD_HEIGHT = 8.0;
publicconstdouble WORLD_WIDTH = 32.0;

public Engine()
{
if (instance != null)
{
return;
}

instance = this;
use_double = true;

systemStatus = newSystemStatus ();
systemStatus.x = 0.0;

51
 systemStatus.d_x = 0.0;
systemStatus.theta = 0.4;
systemStatus.d_theta = 0.0;
systemStatus.theta2 = 0.0;
systemStatus.d_theta2 = 0.0;

// start thread
Thread thread = newThread(newThreadStart(run));
thread.Start();
}

publicvoid run()
{
// start a stopwatch to count time
Stopwatch stopwatch = Stopwatch.StartNew();
stopwatch.Start();

// initialize last frame time

lastFrameTime = stopwatch.ElapsedMilliseconds;
while (true)
{
long frameTime;
do
{
// wait for at least frameLength
Thread.Sleep(0);
frameTime = stopwatch.ElapsedMilliseconds;
} while (frameTime ­ lastFrameTime < FRAME_LENGTH);

// get elapsed time

double elapsedTime = (double)(frameTime ­ lastFrameTime) / 1000;
lastFrameTime = frameTime;

/* just for debug */

//elapsedTime = 0.016;

if (use_double)
{
systemStatus =
DoubleInvertedPendulum.Evolution(systemStatus,
FuzzyController.GetForce(systemStatus,
elapsedTime),
elapsedTime);
}
else
{
systemStatus =
InvertedPendulum.Evolution(systemStatus,
FuzzyController.GetForce(systemStatus,
elapsedTime),
elapsedTime);
}

Main.Draw();
}
}

publicstaticSystemStatus GetSystemStatus()
{
return systemStatus;
52
 }

publicstaticvoid GetLengths(refdouble length1, refdouble length2)

{
if (use_double)
{
length1 = DoubleInvertedPendulum.GetFirstPendulumLength();
length2 = DoubleInvertedPendulum.GetSecondPendulumLength();
}
else
{
length1 = InvertedPendulum.GetPendulumLength();
length2 = 0.0;
}
}
}

53
Anexa B: Interfata aplicatiei

B.1. Pendulul simplu

B.2. Pendulul dublu

54
Bibliografie

K. Tanaka and M. Sugeno , “Stability analysis and design of fuzzy Control systems”,
156, 1992.
55
 C.W. Tao, et al., “Fuzzy
–cart system”, Fuzzy Sets and Systems (2008),ScienceDirect

L. A. Zadeh, “Outline of a new approach of the analysis of complex system and decision
processes,” IEEE Trans. Syst 44, 1963.

, ,

56