2

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

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

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

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

2.3. Exemplificare ................................................................................................................................

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

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

3.1. Strategii de control .............................................................

3.2. Aspecte preliminare ............................................................

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

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

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

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

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

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

4.4. Interfata.............................................................................................................................................

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

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

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

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

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

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

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

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

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

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

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

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 faptulcă
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 ∑ =1 ̇
2

1
≠
2

(3)

1 2
= +
2
=1

2
−1

∗ + sin( ) + sin⁡( )
=−

2
−1
1 ′2
+ cos( ) + 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( ) ′′ ) 2
= ! + "# $ + "# "& ' ' ' − sin( ' ' ] +
⎪
⎪ =1 =1 '= −
⎪
′′ 2
⎪ "#* [cos( ) − sin( ) ]
=1
⎨
⎪ =0
⎪
⎪ =0
⎪
⎩ =+

(8)

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

′′ 2
+"#* [cos( ) − sin( ) ]++ = − ()
=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)

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

−1

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

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

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

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

(i = 1, ..., n)

(15)

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

⎧ + , =1
⎪
=1
( )=
⎨ 2
⎪ −1 + −1 −1 + −1 , 1< ≤ +1
⎩ =

(17)

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

(18)

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

(19)

0 ( , ′)
0, = 1, 1 < ≤ +1
⎧ −2
⎪
⎪ − sin( −1 ) −1 − sin⁡( −1 ) −1
⎪ = −1 = −1 = − −1
⎪ −2
=
⎨⎛
− sin( − −1 ) −1 −1 −1 −" sin$ − −1 % −1 ' −1 ⎞
⎪⎜ = − −1 = ⎟, , -./
⎪⎜ −1 ⎟
⎪⎜ ⎟
⎪ −" " ( sin⁡( ( − −1 ) −1 ' ' −1
⎩⎝ = (= − ⎠

17
(20)

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

(21)

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

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

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

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

Sau

(23)

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

Sau

(24)

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

%0
%( )
18
(25)

=( 1 1… −1 )

Stiind ca:

(26)

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

= + , = 2, … ,
=1

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

()

(27)

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

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

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

19
 2.3. Exemplificare
,

(28)

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

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

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

+ 2 2 − 22 sin( 2) =− ( 2 )μ 2 2

1 2 cos 1 3 cos 2 4 cos 3

9 cos 1 10 11 cos( 1− 2) 12 cos( 1− 3)
( )=
18 cos 2 19 cos( 1− 2) 20 21 cos( 2 − 3)
28 cos 3 29 cos( 1− 3) 30 cos( 2 − 3) 31

G(q, ̇)
5 6 sin( 1) 2 sin( 2 )7 2 sin( 3 ) 3
8
0 14 sin( 1 − 2 ) + 16 sin( 1 − 3 ) 3
= ⎛ 13 2 15 ⎞
0 22 sin( 1 − 2 ) 1 + 23 24 25 sin( 2 − 3 ) 3 + 26
⎝0 33 sin( 1 − 3) 1 35 sin( 2 − 3) 2 + 36 31 ⎠

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

20
 Tabel 2. Constantele sistemului

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

2
2 11 + ( 2 + 3) 1 20 2 + 22 + 3 2
2

3 22 + 32 21 33 2

4 33 22 − 1( 22 + 3 2)

5 23 − 2

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

7 −( 22 + 32 ) 25 33 2

8 − 33 26 − 3

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

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

11 ( 22 + 3 2) 1 29 33 1

12 33 1 30 33 2

2
13 1 + 2 31 3 + 33

14 ( 22 + 3 2) 1 32 3

15 − 2 33 − 33 1

16 33 1 34 − 33

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

18 22 + 32 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: Controlulsistemului (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. Proiectareaschemei 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

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

11 ̈+ ̈+
12 Ө1
̈=
13 Ө2
21 ̈+ ̈
22 Ө1 +
̈
23 Ө2 =
31 ̈+ ̈
32 Ө1 +
̈
33 Ө2 =

34
 ̈ Ӫ

22 − 12
⎧ ̈=
11 22 − 21 12
⎨Ӫ= 11 − 21
⎩ 11 22 − 21 12

11 12 13 ̈
21 22 23
̈
Ө1 =
31 32 33 Ө2̈
−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 ­ 6/ 15
5 32/9
­25360/
8/9 19372/6561 64448/6561 ­212/729
2187
1 9017/3168 ­355/33 46732/5247 49/176 ­ 103/18656
5
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