LUCRARE DE LICENTA. Aplicatie grafica pentru controlul unui pendul dublu neliniar. Cuprins: Absolvent. Alexandru Stefan.

Transcription

1 LUCRARE DE LICENTA Aplicatie grafica petru cotrolul uui pedul dublu eliiar Absolvet Alexadru Stefa Coordoator Asist.Ig. Dr. Valeti Taasa Bucuresti, 2013 Cupris: 1

2 Capitolul 1: Itroducere... 4 Capitolul 2: Modelul matematic, modelare eliiara Prezetare geerala Studiul problemei Exemplificare Tipuri de frecari...22 Capitolul 3: Cotrolul sistemului (strategii de comada) Strategii de cotrol... Ошибка! Закладка не определена Aspecte prelimiare... Ошибка! Закладка не определена Proiectarea schemei de cotrol CLF (Cotroler de Logica Fuzzy)...29 Capitolul 4: Aplicatie Cotiutul aplicatiei Sistemul de ecuatii Metoda Dormad-Price Iterfata...36 Capitolul 5: Cocluzii...38 Aexa A: Codul aplicatiei...39 A.1. Cotroler Fuzzy...39 A.2. Pedulul ivers...43 A.3. Pedulul ivers dublu...46 A.4. Starea sistemului...49 A.5. Metoda Dormad-Price...50 A.6. Egie-ul aplicatiei:...51 Aexa B: Iterfata aplicatiei...54 B.1. Pedulul simplu...54 B.2. Pedulul dublu

3 Bibliografie

4 Capitolul 1: Itroducere Scopul acestei lucrari este de a realiza o aplicatie grafica iteractiva pri itermediul carora studetii sa poata dezvolta si testa tehici de cotrol liiar sau eliiar petru sisteme de tip pedul ivers. Sistemele de tip pedul sut sisteme mecaice simple dar cu diamici eliiare si care sut dificil de stabilizat. Sut multe exemple di atura sau idustrie care se pot explica pe baza aalogiilor cu u system de tip pedul. Pri urmare aceste sisteme sut adeseori folosite atat i scop educativ cat si i cercetare petru a se ilustra cocepte specifice. Aceasta aplicatie grafica este dedicata studiului uui pedul simplu sau a uui pedul dublu ivers cu carucior. Aplicatia permite simularea traiectoriilor uui asemeea pedul, avad cuoscute setul de coditii iitiale. De asemeea lucrarea permite implemetarea de legi de comada potrivite. Î acestă lucrare sut derivate ecuaţiile de mişcare petru u pedul ivers multiplu. Sistemul pedulului este mai geeral şi iclude frecări de tip eliiar petru a se potrivi diverselor aplicaţii igiereşti. Ecuaţiile geeralizate sut dezvoltate î sistemul de coordoate absolut folosid tehicile lui Lagrage, apoi o trasformare liiară simplă se propue petru a obţie setul de ecuaţii eliiare î sistemul de coordiate Devait-Harteberg. 1.1 Aspecte prelimiare stabilizare sisteme de pedul ivers Stabilizarea sistemelor de pedul ivers reprezita o metodă aplicată i majoritatea ariilor de cotrol. Importaţa si frecveţa aalizelor acestor sisteme este defiită de faptul că acestea reprezită mai multe aspecte. Sistemul de pedul ivers este ecostisitor si poate fi costruit si istalat cu usurita i laboratoare i scop educatioal sau petru activitatile de cercetare. Acestea sut dezvoltate petru a dezvolta teorii de cotrol si algoritmi, itrucat sut i aalogie cu umeroase sisteme di viata reala. U propulsor de racheta la lasare poate fi simulate ca u pedul ivers cu o sigura coexiue rigida motat pe u carucior mobil. Studiul uui sistem cu trei coexiui poate 4

5 cotribui la dezvoltarea uor masii de locomotive bipede deoarece aceasta poate fi cosiderat u model simplificat al omului stad itr-u picior. Sistemul de pedul ivers cu multicoexiui poate fi tratat ca o aproximare bidimesioala petru maipulatoare de roboti cu capacitate de cotrol redusa. Di cauza ca aceste sisteme sut foarte istabile, multi-variabile si prezita eliiaritati care u pot fi eglijate date de structura lor diamica si fortele de frecare, pedulul iversat este adeseori utilizat i obtierea performatelor simularilor si i validarea tehicilor de cotrol. Figura 1.1 Lasare racheta Apollo 11 exemplu pedru pedul simplu Figura 1.2. Robot umaoid Asimo exemplu petru pedul dublu 5

6 I cotiuare, bazat pe lucrarea Eltohamy si Kuo (1999) s-a preluat studiul bibliografic legat de cotrolul sistemelor de pedul ivers care este redat i cotiuare. Lucrarile publicate legate de stabilizarea si cotrolul sistemelelor de tip pedul ivers simplu, dublu si triplu pot fi separate i doua categorii pricipale: Lucrari de cercetare care se bazeaza i totalitate pe simulari computerizate i vederea validarii abordarilor; Lucrari experimetale care ivestigheaza icertitudiile fizice cauzatoare de aparitia uor diferete de performata substatiale itre rezultatele simularii pe computer si implemetarile de cotrol i timp real. Ditre aceste lucrari care dezbat stabilizarea pedulului ivers cu o coexiue bazat pe simulari computerizate Chug si Litt (1986) au folosit o tehica de estimare de tipul least squares recursiva petru a idetifica diamicile sistemului. U cotroller de raspus autoreglabil si adaptiv utilizeaza aceasta iformatie petru a stabiliza sistemul i vederea starii de echilibru verticale istabile. U cotroller de raspus PD (proportioal si derivativ) este de asemeea coceput pe baza modelului diamic liiarizat al sistemului. Comparatia simularilor computerizate a ambelor cotrollere, decalajele ughiulare iitiale, perturbatiile de tip treapta si siusoidale dovedesc perfomata superioara a primului cotroller. I locul rezolvarii uei probleme eliiare cu doua pucte limita pri calculul variatioal, Adersi si Gratham (1989) combia miimizarea fuctiei cu metoda de stabilitate Lyapuov si o aplica uui pedul eliiar cu o sigura coexiue petru a produce u cotroller de raspus optimal. O costragere asupra cuplului de cotrol il limiteaza la mai puti de cuplul gravitatioal al modelului. Simularile computerizate promit rezultate de succes si u cotroller care aduce aproape toate traiectoriile la pucte de stabilitate. Heders si Soudak (1992) studiaza comportametul sistemului aterior cu ajutorul uui cotroller liiar pri aaliza plaului de faza. Acestia au dezvaluit existeta atractoarelor cu doua stari i sistem. Primul bazi de atractie este de o forma complexa si itidere limitata. Cu toate astea sistemul de cotrol creaza u puct stabil petru acest atractor la origiea plaului de faza. Al doilea atractor este u ciclu cu limita stabila si trage toate starile de sistem di afara baziului primului atractor. Petru sistemele de pedul ivers cu doua coexiui Sahba (1983) formuleaza criteriul de stabilitate al sistemului ca o costragere de iegalitate ifiit dimesioala petru a folosirea metodei Lyapuov. 6

7 Petru a obtie stabilitate asimptotica, fuctia Lyapuov a sistemelor eliiare permite icremetarea pe iterval scurt i locul descresterii cotiue de-a lugul traiectoriilor de stare. I fial Siha si Joseph (1994) utilizeaza trasformarea Lyapuo-Floquet petru a reduce sistemul de pedul ivers cu trei coexiui variat i timp, fara carucior mobil, la o forma potrivita petru aplicatia metodelor de cotrol ivariate i timp. Tehicile lor rezulta itr-u model de cotroller simulat pe computer pri trei itrari de cotrol. Cu toate acestea majoritatea tehicilor dicutate aterior sut dificil de implemetat deoarece formei cotrollerelor care duc i timp real la u volum de calcul extesiv. Pe de alta parte i experimetele i timp real u pedul ivers cu o sigura coexiue poate fi stabilizat i starea verticala de echilibru istabil folosid u cotroller PD simplu. Mori et al. (1976) a extis asta la modelul uui cotroller complex petru a aduce la pozitie verticala u pedul cu o coexiue si a-l metie acolo. I primul rad u cotroller de tipul feedforward este folosit i timpul perioadei de balas spre pozitia verticala apoi u regulator aalog de tipul observator petru stabilizarea diamica proiectat folosidu-se plasametul polilor care este activat i faza de stabilizare. O tehica mai simpla este folosita de Smith si Blackbur (1992). Acestia supu pivotul pedulului la oscilatii verticale de frecveta ialta si obti stari iversate stabile. Ostertag si Carvalho Ostertag (1993) compeseaza fortele de frecare di sistem folosid u compesator fuzzy, apoi dezvolta u cotroller fuzzy stabil. De aici iaite, u cotroller de raspus de stare este folosit i cocordata cu u itrerupator de prag costat ca o compesare a frecarii. Rezultatele de la ambele cotrollere sut comparabile. Cu toate astea, cotrollerul de logica fuzzy arata o stabilizare usor mai bua. Petru sistemele de pedul dublu ivers care sut mai complicate cotrollerul poate fi proiectat liiarizad modelul sistemului si gasid u regulator care miimizeaza u idex de performata patratica. Furuta el al. (1978) si Zu-re et al. (1984) folosesc cotrollere de stare aalogice petru a stabiliza astfel de sisteme i experimete. Zu-re et al. (1987) a folosit u cotroller digital care da o mai bua stabilizare. Maletisky et al. (1981) u masoara ughiul coexiuii secudare i mod direct si tot reusesc sa cotroleze pedulul dublu ivers itr-o pozitie stabila. U sistem cu doua coexiui pe o sie icliata este de asemeea cotrolat de Furuta et al.(1980). 7

8 Desi toate cotrollerele i timp real listate mai sus petru pedulul ivers dublu sut proiectate liear, ele difera i filosofia care se afla i spatele deciziilor luate de catre regulator si tipul de observatori folositi. Problema mai dificila a stabilizarii uui pedul ivers triplu i experimete a fost studiata de Furuta et al. (1984), Meier zu Farwyg si Ubehaue (1990), Medrao-Cerda (1997) ad Eltohamy si Kuo (1997, 1998). I primele doua lucrari problema este simplificata pri folosirea a doua motoare petru a furiza itrari de cotrol si cotrollere sut di ou proiectate pri miimizarea idexului de performata patratic bazat pe modele liiarizate ale sistemelor lor. Cu toate astea sistemul triplu coectat al lui Furuta u iclude u carucior mobil. I schimb coexiuea de la baza este ascusa la sol si cotrolabilitatea sistemului este icremetata fixad bare orizotale pe coexiui petru a le icremeta iertia iertia si a facilita cotrolul. Desi Meier zu Farwig si Ubehaue au implemetat u sistem care u iclude u carucior mobil u alt doilea motor este localizat i apropierea coexiuii de sus petru a icremeta iertia si a asigura cupluri stabilizatoare petru coexiuile mijlocii si de sus. I Medrao-Cerda (1997) u pedul ivers triplu fixat pe u carucior care se poate misaca orizotal este stabilizat i pozitia verticala folosid u sigur actuator, adica, u motor DC care coduce caruciorul. I Eltohamy si Kuo ( ) u pedul ivers triplu similar cu cel folosit i Medrao-Cerda (1997) este de asemeea stabilizatfolosid o sigura itrare petru cotrol. Trebuie de asemeea metioat ca u observator de ordi redus este folosit i Medrao-Cerda (1997) petru a recostrui acele variabile de stare care u sut direct masurate. Pe de alta parte toate variabilele de stare sut furizate direct de sezori digitali cu zgomot foarte redus, i Eltohamy si Kuo ( ). Di rezultatele experimetelor i timp real raportate i aceste lucrari se pare ca stabilitatea uui sistem de cotrol de tip feedback petru pedul ivers cu mai multe coexiui poate fi afectat foarte puteric de urmatorii factori: 1. Zgomotul i timpul masuratorilor care poate fi ocolit pri mai multe metode. Prima metoda, se pot folosi filtre de zgomot desi adauga complexitate sistemului. Observatori pot sa fie de asemeea folositi petru a estima uele di stari fara zgomot. Cu toate astea rata de testare a uui sistem de cotrol de multe ori depide de tipul de observator folosit datorita extra computatiilor ecesare petru ecuatiile observatorului. O solutie alterativa este sa foloseasca sezori digitali aproape lipsiti de zgomot. Eltohamy si Kuo 8

9 ( ). Astfel de sezori pot avea mase comparabile cu ale coexiuilor. 2. Icertitudii care se maifesta i mare parte i coeficietii de frecare. Estimad valorile acestora pri date experimetale si modelare, efectul lor asupra sistemului poate fi vizualizat i simulare. Desi amelioreaza modelul sistemului uii di termeii de frecare u pot fi liiarizati deci u au o iflueta directa asupra proiectarii regulatorului liiar. 3. I Eltohamy si Kuo ( ) s-a gasit ca viteza de rotatie ialta a coexiuii de top a uui pedul ivers triplu poate afecta foarte mult efectul de cotrol pri saturarea amplificatorului aalog. Aceasta poate fi depasita redistribuid masa coexiuii de top petru a cobori cetrul de greutate mai aproape de puctul pivotului. Cu toate acestea paa acum, aceste probleme practice u au fost adresate suficiet i lucrarile de cercetare precedete care se bazeaza pe simulare computerizata. Petru a face rezultatele care au fost cercetate pe baza simularii pe computer mai substatiale, acesti factori ar trebui de asemeea iclusi i aceste simulari folosite i cercetarea cotrolului petru pedulul ivers. I Larcombe ( ) ecuatiile de miscare petru u pedul ivers multiplu sut formulate i sisteme de doua coordoate bazate pe presupueri de distributie a masei i coexiui uiforme si fara frecare. Primul este sistemul de coordoate absolut i care ughiurile coexiuilor sut calculate tiad cot de pozitia pe verticala. Al doilea sistem de coordoate se umeste Devait-Harteberg sau sistemul de coordoate alterativ, care este o metoda stadard petru descrierea sistemelor kiematice a maipulatoarelor robotice. Larcombe si Torsey (1992) au gasit o formulare matematica petru eumerarea si idetificarea starilor de echilibru ale sistemelor de pedul multiplu. Itr-o alta lucrare Larcombe (1993) a aplicat maipularea algebricii simbolice ecuatiilor diamice liiarizate de bucla deschisa ale sistemului de pedul dublu si au computat simbolic ecuatia sistemului caracteristic si polilor i bucla deschisa ca fuctii ale parametrilor sistemului (masa, lugime, etc.). Dupa cum am idicat mai sus modelele de sistem hardware folosite i majoritatea experimetelor i timp real i geeral au distributie de masa a coexiuilor euiforma datorita folosirii actuatoarelor, a sezorilor si alte cosideratii. De asemeea existeta frecarii poate afecta performata oricarui proiect de cotrol al sistemului itr-u mod semificativ. Cu toate astea ecuatiile de miscare furizate de Larcombe ( ) petru u sistem geeral de pedul 9

10 ivers multiplu u coti i mod explicit acesti factori. I plus, ecuatiile lui Larcombe, desi elegate di puct de vedere matematic u sut scrise i mod direct i termei de parametri fizici, cum ar fi masa si lugimea coexiuilor. I aceasta lucrare o formulare mai geerala a ecuatiilor de miscare petru u sistem de pedul ivers multiplu va fi aratata. Ecuatiile derivate aici sut formulate i termei de parametri fizici ca cei di Tabelul 1 de mai jos si pot fi folositi i mod direct cu scopul simularii computerizate. Iclud factori cum ar fi ajustarea locatiilor cetrelor de gravitate petru coexiui plus modele de frecare vascoasa si Coulomb. 10

11 Capitolul 2: Modelul matematic, modelare eliiara 2.1. Prezetare geerala Cosideram u pedul bidimesioal cu coexiui motat pe u carucior mobil, dupa cum este aratat i figura 1. Caruciorul este miscat orizotal de forta ±F(t). Nomeclatura sistemului este data i tabelul 1. Simbol Descriere Tabel 1. Nomeclatura sistemului M Masa carutului. mi Masa coexiuii i. li Distata de la cea mai joasa coexiue la cetrul de gravitate al coexiuii i. Li Luigimea totala a coexiuii i. μi Ughiul coexiuii i fata de pozitia verticala. gi Ughiul relativ ditre coexiuea (i + 1) si coexiuea i. r Pozitia carutului fata de referitat. Ii Mometul de iertie al coexiuii i. cr Coeficietul de frecare vascoasa itre carut si sia. ci Coeficietul de frecare vascoasa al coexiuii. ¹r Coeficietul dee frecare Coulomb itre carut si sia. ¹i Coeficietul de frecare al coexiuii i. Coexiuile se presupu ca sut rigide i formularile ce urmeaza. Petru u astfel de sistem este evoie de + 1 ecuatii. Modelul matematic este costruit folosid metoda lui Lagrage (Greewood 1988) dupa cum urmeaza: 11

12 Figura 1 Pedul ivers multi-lik (1) d dt T q j T q j + V q j = U j j = 1,, + 1, U = [F, 0,0,,0] T Ude T si V sut eergia cietica si respectiv potetiala ale sistemului. Starile sistemului, q, depid de sistemul de coordoate ales. Daca ughiurile coexiuilor sut masurate i cocordata cu pozitia verticala starile vor fi: (2) q = (r θ 1 θ ) T 2.2. Studiul problemei Furuta et al. (1980) a sugerat ca frecarile ditre carucior si siele acestuia si cele ditre coexiui sut o combiatie de frecari Coulomb si frecari vascoase. Coeficietii de frecare se pot afla miimizad difereta ditre raspusurile experimetelor i timp real si modelelor simulate. Efectul frecarii vascoase care amortizeaza petru u sistem cu u coeficiet de libertate, este pri forta totala de amortizare care actioeaza asupra sistemului si este egala cu - ude c ij sut coeficietii de amortizare. Amortizarea este iclusa i ecuatia lui Lagrage pri i=1 c ij q i 12

13 Fuctia de disipare Raleigh, D= 1 2 i=1 j =1 q i q j. Petru sisteme care se disipeaza D este o forma patratica semidefiita sau defiita, pozitiva, a velocitatilor si este egala cu o jumatate di rata de disipare a eergiei mecaice istataee a sistemului. Petru a acomoda evoia de variatie a masei de distributie i sistemul hardware cetrul de gravitate petru fiecare coexiue u va fi presupus a fi i mijloc, i.e. l i 1 2 L i i tabelul 1. Folosid masuratoarea absoluta a ughiurilor, eergia de disipare cietica, potetiala si de frecare sut dupa cum urmeaza. (3) Eergia cietica: T = 1 2 i=1 I i θ i 2 + m i d dt i 1 r + L k si θ k + l i si (θ i ) k=i 2 + d dt i 1 k=i L k cos θ k + l i cos (θ i ) Mr 2 Eergie potetiala: (4) Eergia de disipare: i 1 V = m i g l i cos (θ i + L k cos (θ k ) i=1 k=i (5) D = 1 c 2 i (θ i θ i 1 ) c rr 2 i=1 Ecuatiile Lagrage ale sistemului sut date dupa cum urmeaza: 13

14 (6) U j = d T T + V + D, (j = 1,, + 1 ) dt q j q j q j q j U = [F sig r μ r N r, sig θ 1 μ θ1 N θ1,, sig θ μ θ N θ ] T Petru coordoata r, maipularea matematica a ecuatiilor e da: (7) d dt T r = M + m i i=1 i=1 r + i=1 m i m i l i cos θ i θ i i 1 k=i T r = 0 V r = 0 D r = c rr L k cos θ k θ k si θ k θ k 2 + si θ i θ i 2 Ecuatia diamica eliiara este data de: (8) M + m i i=1 i 1 r + L k cos θ k θ k si θ k θ k 2 i=1 k=i 2 + m i l i cos θ i θ i si θ i θ i + c r r = F sig r μ r N r i=1 m i Aplicatia ecuatiei lui Lagrage petru coordoata θ i da urmatoarele rezultate: 14

15 (9) T θ j = I i θ i + m i l i + j =i+1 m j L i i 1 cos θ i r + m i L k l i cos (θ k θ i )θ k k=i + m i l i 2 + m j L i j =i+1 θ i i 1 + L p cos (θ p θ i )θ p j =i+1 p=j m j L i + m j l j L i cos ( θ j θ i )θ j j =i+1 (10) d dt T = I θ i θ i + m i l i + m j L i i j =i+1 cos θ i r m i l i + m j L i j =i+1 si θ i θ i r + m i l i 2 + m j L i j =i+1 θ i 1 + L k cos θ k θ i θ k si θ k θ i (θ k θ i )θ k k=i m i l i + m j l j cos θ j θ i θ j j =i+1 si θ j θ i (θ j θ i )θ j L i i 1 + L p [cos θ p θ i θ p si θ p θ i (θ p θ i )θ p ] m j j =i+1 p=j L i (11) T θ i = m i l i + j =i+1 m j L i i 1 si θ i θ i r + m i L k l i si (θ k θ i )θ i θ k k=i i 1 + L m L i si (θ p θ i )θ i θ p j =i+1 p=j 15 m j + m j l j L i si (θ j θ i )θ i θ j j =i+1

16 (12) V θ i = g m i l i + j =i+1 m j L i si θ i (13) T = (c θ i + c i+1 )θ i c i 1 θ i+1 + c i θ i 1 i I fial cele ecuatii eliiare corespodete ughiurilor absolute θ i sut: (14) U i+1 = m i l i + m j L i j =i+1 cos θ i r I i + m i l 2 i + m j L i j =i+1 θ i i 1 + L k [cos θ k θ i θ k si θ k θ i θ k ] k=i m i l i + m j l j [cos θ j θ i θ j j =i+1 si θ j θ i θ j ] L i j 1 + L p [cos θ p θ i θ p si θ p θ i θ p 2 ] j =i+1 p=j m j L i g m i l i + m j L i j =i+1 si θ i + (c i + c i+1 )θ i c i 1 θ i+1 + c i θ i 1 (i = 1,..., ) Ecuatiile pot fi rearajate i forma dupa stare dupa cum urmeaza: (15) F q q + G q, q q + H q = L(q, u) 16

17 Ude F si G sut matrice patratice astfel: (16) F ww q = M + m i, w = 1 i=1 2 I w 1 + m w 1 I w 1 + m j L w 1, 1 < w + 1 j =w (17) F wv q = v 2 m i L k cos (θ k ) + m i l i cos (θ i ), w = 1, 1 < v + 1 i=v 1 k=v 1 i=v 1 2 m i L k cos (θ k ) + m i l i cos (θ i ), v = 1, 1 < w + 1 i=w 1 k=w 1 i=w 1 w 2 L k cos θ k θ v 1 m w 1 l w 1 + m j l j cos θ j θ v 1 k=w 1 j =w j 1 + L p [cos θ p θ v 1 ] m j L w 1 j =w p=j L w 1, altfel (18) G wv q, q = c r, w = 1 c w 1 + c w, 1 < w + 1 (19) G wv q, q 0, v = 1, 1 < w + 1 w 2 m i l i si θ v 1 θ v 1 L k si (θ v 1 )θ v 1 = w 2 i=w 1 L k si θ k θ v 1 θ v 1 i=w 1 m w 1 l w 1 k=w 1 m j l j si θ j θ v 1 θ v 1 L w 1 k=w 1 j 1 j =w L p si (θ p θ v 1 )θ v 1 m j L w 1, altfel j =w p=j 17

18 H si L sut vectori X 1 si pot fi gasiti astfel: (20) H w q = 0, w = 1 g m w 1 l w 1 + m j L w 1 j =w si θ w 1, altfel (21) L w q, u = F sig r μ r N r, w = 1 sig θ w 1 μ θw 1 N θw 1, altfel Petru a rezolva ecuatiile diamice eliiare forma spatiului de stare este extisa petru a iclude starile si primelelor derivate dupa cum urmeaza: (22) Ecuatia (15) va devei: (1) F q q = G q, q q H q + L q, u ude q = (r θ 1 θ ) T (2) X = q i q i, i = 1,, + 1 Noua forma a spatiului de stare este data de: (3) I 0 0 F X = 0 I 0 G X + 0 H + 0 L Sau (23) X = 0 I 0 F 1 G X + 0 F 1 H + 0 F 1 L Sau (24) X t = f(x t, u t ) Fiid data o stare iitialax 0 si folosid metoda de itegrare umerica Ruge-Kutta de orice ordi, vectorul de stare X t este usor de simulat. 18

19 Di motive practice ecuatiile de miscare eliiare pot fi formulate i sisteme de coordoate diferite. I geeral sezorii de miscare ughiulara masoara ughiurile relative ditre diferite coexiui cosecutive. Astfel, diamica sistemului poate fi reformulata i sistemul de coordoate Devait-Harteberg, i care starile cut: (25) q = (r θ 1 γ 1 γ 1 ) T Stiid ca: (26) γ i = θ i+1 θ i, i = 1,, 1 i 1 θ i = γ k + θ i, i = 2,, k=1 Larcombe (1991) a reusit asta ilocuid θ i cu γ k + θ 1 (i = 2,, ) i ecuatiile de miscare formulate i coordoate absolute. Di momet ce ecuatiile obtiute pot fi foarte complicate se sugereaza ca ar fi mai simplu sa se rezolve ecuatia (23) mai itai dupa care se va trasforma vectorul de stare X t i celalalt sistem de coordoate folosid o matrice de trasformare T defiita ca: (27) i 1 k=1 T wv = 1, w = v, 1, w = v + 1,2 < w + 1, + 3 < w 2 + 2, 0, altfel Ca exemplu petru u sistem de pedul ivers dublu petru a trasforma de la vectorul de stare X=(r θ 1 θ 2 r θ 1 θ 2 ) T la X ou =(r θ 1 γ 1 r θ 1 γ 1 ) T, matricea de trasformare va fi: T = Si X ew = TX 19

20 2.3. Exemplificare Ca validare a ecuatiilor prezetate aterior, ecuatiile de miscare petru pedulul ivers dublu si triplu sut extrase di setul de ecuatii diamice eliiare geerale si verificate cu ecuatii specificate aterior derivate specific petru fiecare sistem. Ecuatiile diamice eliiare petru u sistem de pedul ivers cu doua coexiui, icluzad modelele de frecare vascoasa si Coulomb, sut dupa cum urmeaza: (28) m + m 1 + m 2 r + m 1 l 1 + m 2 L 1 cos θ 1 θ 1 + m 2 l 2 cos θ 2 θ 2 + c r m 1 l 1 + m 2 L 1 si θ 1 θ 1 2 m 2 l 2 si θ 2 θ 2 2 = F sig r µ r N r m 1 l 1 + m 2 L 1 r + I 1 + m 1 l m 2 L 1 2 θ 1 + m 2 l 2 L 1 cos θ 1 θ 2 θ 2 + c 1 + c 2 θ 1 c 2 θ 2 + m 2 l 2 L 1 si θ 1 θ 2 θ 2 2 g m 1 l 1 + m 2 L 1 si θ 1 = sig θ 1 µ θ1 N θ1 m 2 l 2 cos θ 2 r + m 2 l 2 L 1 cos θ 1 θ 2 θ 1 + I 2 + m 2 l 2 2 θ 2 m 2 l 2 L 1 si θ 1 θ 2 θ 2 1 c 2 θ 1 + c 2 θ 2 gm 2 l 2 si θ 2 = sig(θ 2 )µ θ2 N θ2 Ecuatiile corespud cu cele prezetate i Furuta et al. (1987, 1980) care sut derivate specific petru cofigurarea sistemelor lor hardware. Petru sistemul cu trei coexiui, ecuatiile diamice eliiare pot fi scrise i forma ecuatiei (15) i care: F q = A 1 A 2 cos θ 1 A 3 cos θ 2 A 4 cos θ 3 A 9 cos θ 1 A 10 A 11 cos θ 1 θ 2 A 12 cos θ 1 θ 3 A 18 cos θ 2 A 19 cos θ 1 θ 2 A 20 A 21 cos θ 2 θ 3 A 28 cos θ 3 A 29 cos θ 1 θ 3 A 30 cos θ 2 θ 3 A 31 G(q, q ) = A 5 A 6 si θ 1 θ 2 A 7 si θ 2 θ 2 A 8 si θ 3 θ 3 0 A 13 A 14 si θ 1 θ 2 θ 2 + A 15 A 16 si θ 1 θ 3 θ 3 0 A 22 si θ 1 θ 2 θ 1 + A 23 A 24 A 25 si θ 2 θ 3 θ 3 + A 26 0 A 33 si θ 1 θ 3 θ 1 A 35 si θ 2 θ 3 θ 2 + A 36 A 31 H q = 0 A 17 si θ 1 A 27 si θ 2 A 34 si θ 3 20

21 Tabel 2. Costatele sistemului Costata Valoare Costata Valoare A 1 M + m 1 +m 2 + m 3 A 19 m 2 l 2 + m 3 L 2 L 1 A 2 m 1 l 1 + m 2 + m 3 L 1 A 20 I 2 + m 2 l m 3 L 2 A 3 m 2 l 2 + m 3 l 2 A 21 m 3 l 3 L 2 A 4 m 3 l 3 A 22 L 1 m 2 l 2 + m 3 L 2 A 5 c r A 23 c 2 A 6 m 1 l 1 m 2 + m 3 L 1 A 24 c 2 + c 3 A 7 m 2 l 2 + m 3 l 2 A 25 m 3 l 3 L 2 A 8 m 3 l 3 A 26 c 3 A 9 m 1 l 1 + m 2 + m 3 L 1 A 27 g m 2 l 2 + m 3 L 2 A 10 I 1 + m 1 l (m 2 + m 3 )L 1 2 A 28 m 3 l 3 A 11 m 2 l 2 + m 3 L 2 L 1 A 29 m 3 l 3 L 1 A 12 m 3 l 3 L 1 A 30 m 3 l 3 L 2 A 13 c 1 + c 2 A 31 I 3 + m 3 l 3 2 A 14 m 2 l 2 + m 3 L 2 L 1 A 32 c 3 A 15 c 2 A 33 m 3 l 3 L 1 A 16 m 3 l 3 L 1 A 34 gm 3 l 3 A 17 g m 1 l 1 + m 2 L + m 3 L 1 A 35 m 3 l 3 L 1 A 18 m 2 l 2 + m 3 l 2 A 36 c 3 L q, u = F sig(r)µ r N r sig(θ 1 )µ θ1 N θ1 sig(θ 2 )µ θ2 N θ2 sig(θ 3 )µ θ3 N θ3 21

22 A i -urile sut costate ale sistemului si sut date i tabelul 2. Aceste ecuatii sut coform celor procesate i Meier zu Farwig si Ubehaue (1990) cad masa motorului superior al sistemului lor este combiata cu masa coexiuii Tipuri de frecari Desi frecarea, care disipeaza eergie itr-u sistem, ar parea sa fie o forta stabilizatoare s- a aratat ca subestimarea marimii coeficietului de frecare poate cateodata sa duca la istabilitate itr-u sistem de cotrol cu feedback. Deasemeea frecarea a fost asociata cu u comportamet oscilatoriu. Cicluri de limitare geerate de frecare au fost aratate itr-u sistem de carucior simplu, simulat, si au fost observata experimetal itr-u aparat petru balasare. Comportametul oscilatoriu a fost prevazut teoretic itr-u sistem de balasare cu reactie. U model de frecare mai precis itre carucior si sia iclude frecare statica si Coulomb (aluecare) cat si frecare vascoasa. Frecarea statica este frecarea care trebuie depasita petru a icepe miscarea, este astfel prezeta umai cad obiectul u se misca. Frecarile Coulomb si vascoasa sut prezete doar cad sistemul se misca. Astfel cel mai simplu model care icorporeaza toate acestea este: F fric = F static F Coulomb + F vascoasa daca x = 0 daca x 0 Compoetele vascoase au modele bie cuoscute: F fric = F aplicata µ s F sg F aplicata daca F aplicata < µ s F N daca F aplicata µ s F N F Coulomb = µ c F sg x, F vascoasa = εx Ude µ s, µ c, εsut coeficietii frecarilor statice, Coulomb si vascoase, iar F N este magitudiea fortei ormale. I modelul ostru obiectul se misca orizotal deci F N = m + M g. Valoarea a diversi parametrii se pot determia experimetal ca i tebelul urmator: 22

23 Parametrii Descriere Estimare μ s coeficiet frecare statica 0,08328 μ c coeficiet frecare Coulomb 0,04287 ε coeficiet frecare vascoasa 0,

24 Capitolul 3: Cotrolul sistemului (strategii de comada) 3.1. Strategii de cotrol Datorita faptului ca pedulul ivers prezita u sistem istabil este deseori folosit ca u puct de reper petru a verifica performatele si eficieta uui algoritm petru reglare. Pedulul ivers este o combiatie de diverse arii de studiu, robotica, teoria cotrolului etc. Scopul este de a aduce pedulul di pozitia de stare vertical jos i pozitia vertical sus si sa il metiem acolo odata atisa. Caruciorul va trebui de asemeea sa fie pozitioat i pozitia iitiala. Toate astea trebuie facuta miscad caruciorul staga si dreapta pe siele acestuia evidet fara a depasi margiile. I teorie putem gasi o multime de algoritmi petru stabilizare pe verticala. Wei et al a prezetat u model de strategie eliiara petru cotrol pri descompuerea legii de cotrol itro secveta de pasi. Chug si Houser au propus o lege de stare eliiara bazata pe feedback petru a regla pozitia caruciorului cat si eergia de balasare a pedulului. Zhao si Spog au aplicat o strategie de cotrol hibrida care stabilizeaza asimptotic global petru toate coditiile iitiale. Metoda lor u ecesita sa ia i cosiderare lugimea siei. Chatterjee et al. propue o lege de cotrol bazata pe eergie care ridica si stabilizeaza u sistem carucior-pedul cu lugime a siei limitata si de asemeea forta de cotrol limitata i simulare cat si i sistem real. O problema majora cu metoda lor este proiectarea potetialelor si coeficietilor folosid ituitia si iteratii care cosuma timp. Toate aceste metode ecesita o cuoastere amautita a diamicii sistemului. Folosirea logicii de cotrol Fuzzy u ecesita cuoasterea amautita a sistemului ceea ce devie foarte importad cad avem de aface cu sisteme eliiare complexe. Mai mult decat asta modelarea diamica a sistemelor arata o depedeta de parametrii mecaici ai acestora, acestia fiid supusi multor modificari pe percursul vietii (uele di cauza micilor deteriorari), si deasemeea o depedeta de parametrii diamici care variaza odata cu actiuea performata. Aceste cosiderete dau cotrolului fuzzy u avataj asupra altor metode eliiare. S-au propus mai multe tehici de cotrol fuzzy. Wog a adoptat algoritmul geetic petru a tua toate fuctiile membre ale sistemului fuzzy petru a metie u pedul ivers erect. Yamakawa a proiectat u sistem hardware de cotrol fuzzy foarte rapid si a folosit umai sapte reguli de cotrol petru a regla u pedul ivers. Dar aceste metode u iau i cosiderare lugimea siei. Matsuura si Yasuobu au folosit amadoi iformatia veita de la carucior petru a costrui u set de 49 de reguli petru a cotrola ughiul virtual dorit si apoi au folosit iformatii despre ughiul virtual si iformatiile despre pedul petru a costrui u alt set de 49 de reguli fuzzy petru 24

25 stabilizare totala. Majoritatea algoritmilor petru cotrol si stabilizare fuzzy folosesc u umar mare de fuctii membru. Parametrii acestor fuctii sut greu de gasit. Mai mult, umarul mare de fuctii membru creste dificultatea gasirii regulilor.ca o alterativa la abordarea covetioala de cotrol cotrolul fuzzy itroduce iformatii ligvistice. Cea mai dificila parte e cotrolului fuzzy este costructia regulilor bazate pe experieta si ituitia operatorului. De asemeea ajustarea fuctiilor membru ale cotrolului fuzzy este u pas importat i strategia de cotrol. I aceasta lucrare se va prezeta o oua structura de cotrol petru coducerea uui sistem cu u pedul ivers eliiar cu frecare. Petru a simplifica desig-ul cotrolerului fuzzy petru balasarea cotrolerului si miscarea caruciorului la origie, sistemul de pedul ivers este descompus i doua sub sisteme uul petru pedul si uul petru carucior. Petru aceste subsisteme doua cotrolere petru stabilizare sut propuse, tiad cot de forma eliiara a sistemului parite. Schema de stare are o structura simpla care scade umarul de reguli fuzzy Aspecte prelimiare Figura 2 este deseul schematic al sistemului de pedul ivers si carucior. Caruciorul este capabil sa se miste pe o sia cu dimesiue limitata. 25

26 separate. Figura 2. Sistem pedul ivers si carut Figura 3 arata diagrama a celor doua compoete ale sistemului, pedulul si carutul, Figura 3. Carut si pedul separate I acest sistem tija pedulului este cosiderata fara masa, F este forta aplicata petru cotrol, x este deplasarea carutului, θ este ughiul ditre pozitia pedulului cureta si cea 26

27 verticala, g este acceleratia gravitatioala. Alti parametri sut masa carutului M, masa pedulului m, coeficietul de frecare al carutului b, lugimea de la cetrul pedulului la ax si iertia pedulului I. Petru simulare ecuatiile diamice ale pedulului ivers sut dupa cum urmeaza: M d2 x dt 2 = cart F F = Mx + bx + N Ude N si P sut fortele de iteractue ditre carut si pedul. d 2 x dt 2 = 1 M F N b dx dt Petru pedul I d2 θ dt 2 = ped τ d 2 θ dt 2 = 1 I Nl cos θ + Pl si θ Este ecesar sa itroducem fortele de iteractiue N si P itre carut si pedul petru a modela diamica sistemului corect. Icluderea acestor forte ecesita modelarea diamicilor x si y ale pedulului i plus fata de theta. Astfel ecuatiile aditioale x si y petru pedul sut modelate astfel: m d2 x p dt 2 = F x = N ped N = m d2 x p dt 2 m d2 y p dt 2 = F y = P mg ped P = m( d2 y p dt 2 + g) Cu toate astea x p si y p sut fuctii exacte ale lui theta. Astfel derivatele lor sut prezetate i fuctie de derivatele lui theta. 27

28 x p = x l si θ dx p dt = dx dθ l cos θ dt dt d 2 x p dt 2 = d2 x dθ + l si θ dt2 dt 2 l cos θ d2 θ dt 2 Si y p = l cos θ dy p dt = l si θ dθ dt d 2 y p dt 2 dθ = l cos θ dt 2 l si θ d2 θ dt 2 Dupa ilocuirea i ecuatiile 7 si 9, avem: N = m d2 x p dt 2 = m d2 x dθ + l si θ dt2 dt 2 l cos θ d2 θ dt 2 P = m d2 y p dt 2 dθ = m l cos θ dt 2 l si θ d2 θ dt 2 Folosid aceste ecuatii eliiare am simulat u sistem de pedul ivers cu carucior itr-o aplicatie vizuala Proiectarea schemei de cotrol Desig-ul cotrolerului este u factor cheie al sistemului de pedul iversat. Cotrolerele sut folosite petru a stabiliza u sistem istabil si petru a-l face robust impotriva perturbatiilor. Framework-ul acestui cotroler fuzzy este prezetat i figura urmatoare. 28

29 Figura 4. Diagrama bloc cotroler fyzzy I figura umarul 4 sistemul de pedul ivers cu carucior este cotrolat de doua cotrollere separate, cotrolerul petru ughiul pedulului si cel petru pozitia carutului. Di ecuatiile diamice petru acest sistem descoperim ca sut doua obiecte diamice, uul este pedulul iar celalalt este carutul. Sa cosideram cotrolul petru carut si petru pedul separate ar facilita dezvoltarea cotrolerului cu u mecaism de reguli. Cu toate astea este permisa o sigura actiue petru sistemul de pedul ivers cu carucior si aume forta aplicata asupra carutului. Astfel forta Fp a pedulului si forta Fc a caruciorului vor trebui sa fie combiate itr-o sigura forta de cotrol F petru sistem. Putem observa ca petru a suplimeta o forta de actiue petru a impige carutul i partea staga va misca pedulul i partea dreapta. Di aceasta cuostita istictiva este usor de vazut ca cele doua forte Fp si respectiv Fc au seme opuse. Tiad cot ca scopul pricipal al acestui experimet este de a balasa pedulul i stare erecta combiatia de forte va fi reprezetata astfel, F = Fp Fc CLF (Cotroler de Logica Fuzzy) CLF este ua ditre cele mai de succes aplicatii di setul de teorii fuzzy, itrodus de catre Zadeh i 1965, caracteristicile sale majore fiid utilizarea variabilelor ligvistice i locul celor 29

30 umerice. Variabilele ligvistice sut defiite i limbaj ormal (cum ar fi mic sau mare ) si pot fi reprezetate iseturi fuzzy. U set fuzzy este o extesie a uui se de tip crisp ude u elemet poate apartie umai uui sigur set (aparteeta totala) sau a u apartie ici uui set (zero aparteeta). U set fuzzy permite aparteeta partiala ceea ce iseama ca u elemet poate apartie mai multor seturi cocomitet. U set fuzzy A este caracterizat pritr-o fuctie de aparteeta µ A care asigeaza fiecarui obiect al uei clase o ota de aparteeta care poate varia itre 0 (zero aparteeta) si 1 (aparteeta totala) astfel vom avea: µ A X 0,1 Ceea ce iseama ca setul fuzzy A apartie uui set uiversal X (umit i geeral uivers de tratare) defiit itr-o problema specifica. U set fuzzy A este umit u sigleto fuzzy ude este u sigur elemet x 0 cu µ A (x 0 ) = 1 i timp ce gradul de aparteeta al tuturor celorlalte va fi zero. Aceasta abordare permite caracterizarea comportametului sistemului pri relatii simple (reguli fuzzy) itre variabile ligvistice. I geeral regulile fuzzy sut exprimate i forma expresiilor coditioale fuzzy de tipul: If x mic THEN y este mare Ude x si y sut variabile fuzzy si mic sau mare sut etichete ale setului fuzzy. Daca sut de la i = 1 la reguli setul de regult reprezetat de uiuea acestor reguli ar fi: R = R1 else R2 else...r U cotroler de logica fuzzy este bazat pe o colectie de R reguli de cotrol. Executia acestor reguli este guverata de catre regula de ifereta compozitioala. Structura geerala a uui CLF este reprezetata i Figura 5 si este compusa di compoetele pricipale: - O iterfata de fuzzyficare care coverteste date de itrare i variabile ligvistice corespuzatoare, - O baza de cuostite care costa i o baza de date cu defiitii ligvistice geerale si u set de reguli de cotrol, 30

31 - O logica de decizie care face folos de cuostitele dii baza de date si de defiitiile ligvistice petru a crea cotrolul fuzzy, - Si i fial o iterfata de defuzzyficare care iterpreteaza actiuea data de catre logica de decizie petru a spacifica o actiue sictemului. Figura 5. Comfigurare de baza CLF I aceasta lucrare vom creea doua cotrolere de tip fuzzy petru pedul respectiv carucior, fiecare cotroler va folosi doua variabile de itrare: cotrolerul petru ughiul pedulului va folosi doua erori ua petru ughiul fata de verticala si ua care reiese di derivata acesteia edot, iar carutul va folosi deasemeea doua erori ua cu referita la pozitia fata de starea iitiala x si ua formata pri derivarea acesteia xdot. Iesirea acestor cotrolere e vor da forta cu care vom actioa asupra caruciorului. Cotrolerele de logica fuzzy propuse petru a cotrola pedulul ivers folosesc fuctii de aparteeta covetioale triughiulare. Mai mult motorul de ifereta fuzzy implemeteaza u set de reguli IF-THEN-ELSE petru eroarea ughiulara e si cea edot petru forta Fp si eroarea de pozitie a carutului x si respectiv cea edot petru forta Fc. Toate sase uiversele de tratare sut 31

32 impartite i trei seturi de valori fuzzy care se itrepatrud N (egativ), Z (zero) si P (pozitiv). Figura 5 care urmeaza arata u exemplu luat ditr-o simulare Mathlab care arata fuctiile de aparteeta si uiversurile de tratare petru pedul. Petru carucior vor fi similare. Crearea regulilor este bazata pe cuostita euristica a comportametului sistemului si criterii teoretice. Petru a geera regulile acestei lucrari criteriul de baza este cosiderat ca: daca pedulul cade itr-o aumita directie, impige carutul i aceeasi directie petru a cotracara miscarea pedulului. Noua reguli sut defiite petru fiecare cotroler si tiute itr-o matrice. Matricea de reguli petru cotrolul ughiului pedulului este aratata i Tabelul 3 si este similara cu cea petru carucior. Figura 6 Fuctiile de aparteeta pedul 32

33 Tabel 3 Exemplu reguli cotrol fuzzy e edot N Z P N N N Z Z N Z P P Z P P 33

34 Capitolul 4: Aplicatie Aceasta aplicatie are scopul de a simula itr-u mediu digital u sistem cu pedul ivers cu carucior cu ua sau doua coexiui (simplu sau dublu). U beeficiu al acestei aplicatii este ca permite studiul implemetarii uui regulator de tip fuzzy/ dar si a altor cotrollere petru coducerea sistemului cu pedul ivers fara a ecesita compoetele experimetale costisitoare ale uui astfel de mecaism sau diverse licete petru programele folosite ca mediu de reglare Cotiutul aplicatiei Aplicatia este costruita i jurul uui Egie (de vazut aexa A.6.) care face legatura itre diverse compoete. Acesta iitializeaza programul si imparte executia itr-u umar predefiit de frame-uri per secuda, repetad aumiti pasi i fiecare frame. Primul pas este de a trasmite cotrolerului de logica fuzzy (clasa FuzzyCotroller, de vazut aexa A.1.) starea cureta a sistemului. Cotrolerul fuzzy va calcula i fuctie de aceasta stare u raspus, sub forma uei forte care va fi aplicata asupra carutului. Egie-ul va prelua raspusul si il va trasmite i fuctie de caz ueia ditre clasele IvertedPedulum (aexa A.2.) sau DoubleIvertedPedulum (aexa A.3.), acestea urmad sa calculeze evolutia sistemului.petru a determia schimbarile i starea sistemului, vor fi rezolvatesisteme de ecuatii diferetiale obtiute di modelul matematic al pedului ivers, prezetat i capitolul 2.Petru rezolvarea sistemelor de ecuatii este folosita metoda Dormad- Price Sistemul de ecuatii Ilocuid costatele si variabilele cuoscute di modelul matematic al pedului ivers se obti sisteme de ecuatii diferetiale de forma: Pedulul simplu: Pedulul dublu: c 11 r + c 12 Ө = A c 21 r + c 22 Ө = B c 11 r + c 12 Ө 1 + c 13 Ө 2 = A c 21 r + c 22 Ө 1 + c 23 Ө 2 = B c 31 r + c 32 Ө 1 + c 33 Ө 2 = C 34

35 Petru a obtie r si Ө i cazul pedulului simplu, se rezolva sistemul pri a elimia pe rad cate ua ditre variabile. Rezultatul: r = Ac 22 Bc 12 c 11 c 22 c 21 c 12 Ө = Bc 11 Ac 21 c 11 c 22 c 21 c 12 I cazul pedului dublu ecuatia se rescrie ca: c 11 c 12 c 13 c 21 c 22 c 23 c 31 c 32 c 33 r Ө 1 Ө 2 = A B C Notad matricea de coeficieti cu M si imultid la staga cu M 1, se obtie: r Ө 1 Ө 2 = M 1 A B C Starea sistemului este defiita de multimea (r, r,ө 1, Ө 1,Ө 2, Ө 2 ) i cazul pedului simplu este folosit u sigur Ө si se poate defii fuctia icremetala de schimbare a starii sistemului astfel: sistemului. r = r r = r Ө 1 = Ө 1 Ө 1 = Ө 1 Ө 2 = Ө 2 Ө 2 = Ө 2 Folosid acest sistem si metoda Dormad-Price se va putea obtie starea urmatoare a I ultimul pas ditr-u frame, starea sistemului este trasmisa clase de deseare petru a putea observa schimbarile si i iterfata aplicatiei Metoda Dormad-Price Numita si metoda DOPRI, metoda Domad-Price este folosita i rezolvarea ecuatiilor diferetiale ordiare. Metoda este membru al familiei de solutii Ruge-Kutta, si foloseste sase evaluari de fuctii petru a calcula solutii de precizie de ordi 4 si 5. Diferetele itre aceste solutii este apoi cosiderata ca fiid eroare solutiei (de ordi 4). Estimarea acestei erori 35

36 estecoveabila petru algoritmii de itegrare adaptiva pri pasi. Cele 6 evaluari de fuctii sut executate folosid coeficietii di tabelul urmator: Tabelul Butcher 0 1/5 1/5 3/10 3/40 9/40 4/5 44/45-56/ 15 32/9 8/ / / / / / / / / / / / / / /84 35/ / / / / / / / / /2100 1/40 Iar fuctia de evolutie va arata astfel: K1 = f(stare iitiala, F) K2 = f(stare iitiala + DormadPrice1(k1) * delta, F); K3 = f(stare iitiala + DormadPrice2(k1, k2) * delta, F); K4 = f(stare iitiala + DormadPrice3(k1, k2, k3) * delta, F); K5 = f(stare iitiala + DormadPrice4(k1, k2, k3, k4) * delta, F); K6 = f(stare iitiala + DormadPrice5(k1, k2, k3, k4, k5) * delta, F); K7 = f(stare iitiala + DormadPrice6(k1, k2, k3, k4, k5, k6) * delta, F); Stare fiala = stare iitiala + DormadPrice7(k1, k2, k3, k4, k5, k6, k7) * delta; Ude Ki sut stari itermediare, f fuctie icremetara folosita de algoritmul de evolutie, delta timpul trecut de la pasul aterior si F forta aplicata de cotrolerul fuzzy Iterfata I iterfata aplicatiei se pot observa schimbarile di sistemul pedulului ivers. Utilizatorului i se da posiblitatea de a iteractioa cu procesul i timp real, modificad parametrii i care fuctioeaza sistemul. Parametrii cofigurabili sut: masa si coeficietii de frecare ai caruciorului, masele si coeficietii de frecare ale pedulelor, starea iitiala a sistemului, precum si perturbari i sistem actioad cu u aumit impuls asupra pedulelor. De asemeea exista posibilitatea de a schimba di 36

37 sistem dublu i sistem simplu si ivers, sau de a repori sistemul di starea iitiala si utilizatorului poate observa evolutia sistemului fara reglare sau cu reglare. 37

38 Capitolul 5: Cocluzii Crearea uui mediu digital petru studiul aplicarii uuei logici de reglare de tip fuzzy uui sistem de pedul(e) ivers(e) prezita diverse probleme si trebuie tratat systematic petru a le rezolva pe fiecare i parte. Ua ditre probleme este implemetarea uui set de ecuatii diamice geeralizat cu care sa se poata itocmi modelul matematic petru sistemul ostru de pedul ivers cu carucior. Petru a face asta se va folosi metoda lui Lagrage (Greewood 1988). Trebuie de asemeea sa luam i cosiderare diversele tipuri de frecare ce vor aparea itr-u sistem real si sa le implemetam ca atare, iar aceasta poate prezeta propriile ei dificultati. Desi frecarea este u tip de forta care disipa eergie itr-u sistem si ar parea sa fie o forta stabilizatoare, subestimarea marimii coeficietului de frecare poate cateodata sa duca la istabilitate itr-u sistem de cotrol cu feedback. Referitor la desig-ul cotrolerului, acesta este u factor cheie al sistemului de pedul ivers si prezita propriile lui probleme la implemetare. Petru a itegra u tip de cotrol fuzzy i aplicatia oastra vor trebui izolati factorii de iteres care e vor permite sa maipulam sistemul creat. Petru aceasta observam ca vor fi ecesare patru itrari si aume ughiul pedulului cu referita la verticala, viteza ughiulara a pedulului, distata ditre referita carutului si pozitia cureta a acestuia (pe sia de dimesiue fiita) si viteza carutului. Petru fiecare ditre acesti factori va trebui defiit cate u uivers de tratare. Actiuea propriu zisa asupra sistemului se va face cu ajutorul uei forte aplicata asupra carutului, care este calculata cu ajutorul celor patru factori metioati mai devreme. Aplicatia, desi u prezita gradul de flexibilitate al mediilor de reglare oferite de programe profesioale, este de sjus petru a studia si a testa reglarea fuzzy (petru u sistem cu pedul ivers) si permite o bua itelegere a felului i care diferite forte (frecari, greutate, forta care actioeaza asupra carutului etc.) actioeaza asupra sistemului. 38

39 Aexa A: Codul aplicatiei A.1. Cotroler Fuzzy publicclassfuzzycotroller costbool DEBUG_FUZZY_CONTROLLER = false; costuit DEFAULT_THETA_FUZZY_STATE_GRADE = 3; costuit DEFAULT_D_THETA_FUZZY_STATE_GRADE = 2; costuit DEFAULT_X_FUZZY_STATE_GRADE = 2; costuit DEFAULT_D_X_FUZZY_STATE_GRADE = 3; costuit DEFAULT_FORCE_PENDULUM_FUZZY_STATE_GRADE = 4; costuit DEFAULT_FORCE_CART_FUZZY_STATE_GRADE = 4; costdouble DEFAULT_THETA_MAX_RANGE = 0.02; costdouble DEFAULT_D_THETA_MAX_RANGE = 0.4; costdouble DEFAULT_X_MAX_RANGE = 2.0; costdouble DEFAULT_D_X_MAX_RANGE = 0.03; costdouble DEFAULT_FORCE_PENDULUM_MAX_RANGE = 150.0; costdouble DEFAULT_FORCE_CART_MAX_RANGE = 80.0; staticuit theta_fuzzy_state_grade; staticuit d_theta_fuzzy_state_grade; staticuit x_fuzzy_state_grade; staticuit d_x_fuzzy_state_grade; staticuit force_pedulum_fuzzy_state_grade; staticuit force_cart_fuzzy_state_grade; staticdouble theta_max_rage; staticdouble d_theta_max_rage; staticdouble x_max_rage; staticdouble d_x_max_rage; staticdouble force_pedulum_max_rage; staticdouble force_cart_max_rage; staticfuzzycotroller istace = ull; staticit [] debug_fuzzy_states = ewit [6]; staticit [] saved_debug_fuzzy_states = ewit [6]; staticit debug_same_result_cout; staticdouble [] debug_iput_values = ewdouble [4]; public FuzzyCotroller() 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_pedulum_fuzzy_state_grade = DEFAULT_FORCE_PENDULUM_FUZZY_STATE_GRADE; force_cart_fuzzy_state_grade = 39

40 DEFAULT_FORCE_CART_FUZZY_STATE_GRADE; theta_max_rage = DEFAULT_THETA_MAX_RANGE; d_theta_max_rage = DEFAULT_D_THETA_MAX_RANGE; x_max_rage = DEFAULT_X_MAX_RANGE; d_x_max_rage = DEFAULT_D_X_MAX_RANGE; force_pedulum_max_rage = DEFAULT_FORCE_PENDULUM_MAX_RANGE; force_cart_max_rage = DEFAULT_FORCE_CART_MAX_RANGE; for (it i = 0; i < 6; i++) saved_debug_fuzzy_states[i] = 0; debug_same_result_cout = 0; privatestaticit GetFuzzyState(double value, uit grade, double max_rage) System.Diagostics.Debug.Assert(max_rage > 0.0 && grade > 0); double fuzzy_state = value * (double)grade / max_rage; it result; if (fuzzy_state > 0) if (fuzzy_state > (double)grade - 0.5) retur (it)grade; result = 0; while (fuzzy_state > 0.5) result++; fuzzy_state -= 1.0; retur result; else if (fuzzy_state < -(double)grade + 0.5) retur -(it)grade; result = 0; while (fuzzy_state < -0.5) result--; fuzzy_state += 1.0; retur result; privatestaticit GetThetaFuzzyState(double theta) retur GetFuzzyState(theta, theta_fuzzy_state_grade, theta_max_rage); 40

41 privatestaticit GetDThetaFuzzyState(double d_theta) retur GetFuzzyState(d_theta, d_theta_fuzzy_state_grade, d_theta_max_rage); privatestaticit GetXFuzzyState(double x) retur GetFuzzyState(x, x_fuzzy_state_grade, x_max_rage); privatestaticit GetDXFuzzyState(double d_x) retur GetFuzzyState(d_x, d_x_fuzzy_state_grade, d_x_max_rage); privatestaticdouble GetForcePedulum(double theta, double d_theta) System.Diagostics.Debug.Assert(force_pedulum_fuzzy_state_grade >= Math.Max(theta_fuzzy_state_grade, d_theta_fuzzy_state_grade)); it theta_fuzzy_state = GetThetaFuzzyState(theta); it d_theta_fuzzy_state = GetDThetaFuzzyState(d_theta); it sum = theta_fuzzy_state + d_theta_fuzzy_state; it force_pedulum_fuzzy_state = (sum> 0)? (sum> (it)force_pedulum_fuzzy_state_grade? (it)force_pedulum_fuzzy_state_grade : sum) : (sum< -(it)force_pedulum_fuzzy_state_grade? -(it)force_pedulum_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_pedulum_fuzzy_state; retur force_pedulum_max_rage * (double)force_pedulum_fuzzy_state / (double)force_pedulum_fuzzy_state_grade; privatestaticdouble GetForceCart(double x, double d_x) System.Diagostics.Debug.Assert(force_cart_fuzzy_state_grade >= Math.Max(x_fuzzy_state_grade, d_x_fuzzy_state_grade)); it x_fuzzy_state = GetXFuzzyState(x); it d_x_fuzzy_state = GetDXFuzzyState(d_x); it sum = x_fuzzy_state + d_x_fuzzy_state; it force_cart_fuzzy_state = (sum> 0)? (sum> (it)force_cart_fuzzy_state_grade? 41

42 (it)force_cart_fuzzy_state_grade : sum) : (sum< -(it)force_cart_fuzzy_state_grade? -(it)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; retur force_cart_max_rage * (double)force_cart_fuzzy_state / (double)force_cart_fuzzy_state_grade; publicstaticdouble GetForce(SystemStatus s, double dt) /* Make sure FuzzyCotroller is iitialized */ if (istace == ull) istace = ewfuzzycotroller(); if (DEBUG_FUZZY_CONTROLLER) debug_iput_values[0] = s.theta; debug_iput_values[1] = s.d_theta; debug_iput_values[2] = s.x; debug_iput_values[3] = s.d_x; double force_pedulum = GetForcePedulum(s.theta, s.d_theta); double force_cart = GetForceCart(s.x, s.d_x); if (DEBUG_FUZZY_CONTROLLER) bool same = true; for (it i = 0; i < 6; i++) if (saved_debug_fuzzy_states[i]!= debug_fuzzy_states[i]) same = false; break; if (same) debug_same_result_cout++; Cosole.Out.Write("."); Cosole.Out.Write("Iput: Theta = "); Cosole.Out.Write(debug_iput_values[0]); Cosole.Out.Write(", DTheta = "); Cosole.Out.Write(debug_iput_values[1]); Cosole.Out.Write(", X = "); Cosole.Out.Write(debug_iput_values[2]); Cosole.Out.Write(", DX = "); Cosole.Out.Write(debug_iput_values[3]); Cosole.Out.Write(", NEXT_DT = "); Cosole.Out.Write(dt); 42

43 Cosole.Out.WriteLie(); else Cosole.Out.Write(debug_same_result_cout); Cosole.Out.WriteLie(); Cosole.Out.WriteLie(); Cosole.Out.Write("Iput: Theta = "); Cosole.Out.Write(debug_iput_values[0]); Cosole.Out.Write(", DTheta = "); Cosole.Out.Write(debug_iput_values[1]); Cosole.Out.Write(", X = "); Cosole.Out.Write(debug_iput_values[2]); Cosole.Out.Write(", DX = "); Cosole.Out.Write(debug_iput_values[3]); Cosole.Out.Write(", NEXT_DT = "); Cosole.Out.Write(dt); Cosole.Out.WriteLie(); Cosole.Out.Write("Fuzzy States: Theta = "); Cosole.Out.Write(debug_fuzzy_states[0]); Cosole.Out.Write(", DTheta = "); Cosole.Out.Write(debug_fuzzy_states[1]); Cosole.Out.Write(", X = "); Cosole.Out.Write(debug_fuzzy_states[2]); Cosole.Out.Write(", DX = "); Cosole.Out.Write(debug_fuzzy_states[3]); Cosole.Out.Write(", FP = "); Cosole.Out.Write(debug_fuzzy_states[4]); Cosole.Out.Write(", FC = "); Cosole.Out.Write(debug_fuzzy_states[5]); Cosole.Out.WriteLie(); Cosole.Out.Write("Force = "); Cosole.Out.Write(force_pedulum - force_cart); Cosole.Out.WriteLie(); for (it i = 0; i < 6; i++) saved_debug_fuzzy_states[i] = debug_fuzzy_states[i]; retur force_pedulum - force_cart; A.2. Pedulul ivers classivertedpedulum costdouble DEFAULT_CART_MASS = 2.0; costdouble DEFAULT_CART_VISCOUS_FRICTION = 0.0; costdouble DEFAULT_CART_COULOMB_FRICTION = 0.0; 43

44 costdouble DEFAULT_PENDULUM_MASS = 0.1; costdouble DEFAULT_PENDULUM_LENGTH = 2.0; costdouble DEFAULT_PENDULUM_DISTANCE_TO_CENTER = 0.5; costdouble DEFAULT_PENDULUM_INERTIA_MOMENTUM = DEFAULT_PENDULUM_MASS * DEFAULT_PENDULUM_LENGTH * DEFAULT_PENDULUM_LENGTH / 3.0; costdouble DEFAULT_PENDULUM_VISCOUS_FRICTION = 0.0; costdouble G = 9.8; // Let's ote: // Costats: // M = cart mass // m = pedulum mass // l = distace from pedulum base to ceter of gravity // I = pedulum momet of iertia // cc = cart viscous frictio // cp = pedulum viscous frictio // u = cart Coulomb frictio // G = gravity // // Variables: // pa = pedulum agular positio // pda = pedulum agular velocity (derived positio) // pdda = pedulum agular acceleratio (derived velocity) // cx = cart positio // cdx = cart velocity (derived positio) // cddx = cart acceleratio (derived velocity) // cos = cos(pa) // si = si(pa) // // The ext differetial equatios must be solved: // (M+m) * cddx + (m * l * cos) * pdda = // -cc * cdx + (m * l * si) * pda * pda - (sg(cdx) * u * (M + m) * G)) // + F; // (m * l * cos) * cddx + (I + m * l * l) * pdda = // -cp * pda + G * m * l * si; // // By otig: // A = M+m // B = (m * l * cos) // C = -cc * cdx + (m * l * si) * pda * pda - (sg(cdx) * u * (M + m) * G)) // D = (m * l * cos) // E = I + m * l * l // H = -cp * pda + G * m * l * si // The ext system is obtaied: // A * cddx + B * pdda = C + F // D * cddx + E * pdda = H // So cddx ad pdda ca 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 whe computig system evolutio staticdouble cart_mass; staticdouble pedulum_mass; //static double legth; // uused for sigle iverted pedulum staticdouble l; staticdouble iertia; staticdouble cc; 44

45 staticdouble uc; staticdouble cp; staticdouble total_mass; staticdouble m_l; staticdouble i_ad_m_l_l; staticdouble u_total_mass_g; staticdouble m_l_g; staticivertedpedulum istace = ull; private IvertedPedulum() cart_mass = DEFAULT_CART_MASS; pedulum_mass = DEFAULT_PENDULUM_MASS; l = DEFAULT_PENDULUM_DISTANCE_TO_CENTER; iertia = DEFAULT_PENDULUM_INERTIA_MOMENTUM; cc = DEFAULT_CART_VISCOUS_FRICTION; uc = DEFAULT_CART_COULOMB_FRICTION; cp = DEFAULT_PENDULUM_VISCOUS_FRICTION; ComputeCostats(); privatestaticvoid ComputeCostats() total_mass = cart_mass + pedulum_mass; m_l = pedulum_mass * l; i_ad_m_l_l = iertia + m_l * l; u_total_mass_g = uc * total_mass * G; m_l_g = m_l * G; privatestaticvoid check() if (istace == ull) istace = ewivertedpedulum(); privatestaticsystemstatus EvolutioHelper(SystemStatus s, double f) SystemStatus result = ewsystemstatus(); double si = System.Math.Si(s.theta); double cos = System.Math.Cos(s.theta); double sg = (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 * si) * pda * pda - (sg(cdx) * u * (M + m) * G)) // D = (m * l * cos) // E = I + m * l * l // H = -cp * pda + G * m * l * si double a = total_mass; double b = m_l * cos; double c = 45

46 -cc * s.d_x + m_l * si * s.d_theta * s.d_theta - sg * u_total_mass_g; double d = b; double e = i_ad_m_l_l; double h = -cp * s.d_theta + m_l_g * si; // cddx = ((C + F) * E - H * B) / (A * E - D * B) // pdda = ((C + F) * D - H * A) / (B * D - E * A) double c_ad_f = c + f; double ae_mius_bd = a * e - b * d; retur result; result.x = s.d_x; result.d_x = (c_ad_f * e - h * b) / ae_mius_bd; result.theta = s.d_theta; result.d_theta = (h * a - c_ad_f * d) / ae_mius_bd; publicstaticsystemstatus Evolutio(SystemStatus s, double f, double dt) check(); // Use Dormad Price method SystemStatus [] temp = ewsystemstatus [7]; temp[0] = EvolutioHelper(s, f); temp[1] = EvolutioHelper(s + dt * DormadPrice.DP1(temp), f); temp[2] = EvolutioHelper(s + dt * DormadPrice.DP2(temp), f); temp[3] = EvolutioHelper(s + dt * DormadPrice.DP3(temp), f); temp[4] = EvolutioHelper(s + dt * DormadPrice.DP4(temp), f); temp[5] = EvolutioHelper(s + dt * DormadPrice.DP5(temp), f); temp[6] = EvolutioHelper(s + dt * DormadPrice.DP6(temp), f); retur s + dt * DormadPrice.DP7(temp); publicstaticdouble GetPedulumLegth() retur DEFAULT_PENDULUM_LENGTH; A.3. Pedulul ivers dublu classdoubleivertedpedulum costdouble DEFAULT_CART_MASS = 2.0; costdouble DEFAULT_CART_VISCOUS_FRICTION = 0.0; costdouble DEFAULT_CART_COULOMB_FRICTION = 0.0; costdouble DEFAULT_PENDULUM_MASS_1 = 0.1; costdouble DEFAULT_PENDULUM_LENGTH_1 = 2.0; costdouble DEFAULT_PENDULUM_DISTANCE_TO_CENTER_1 = 0.5; costdouble DEFAULT_PENDULUM_INERTIA_MOMENTUM_1 = DEFAULT_PENDULUM_MASS_1 * DEFAULT_PENDULUM_LENGTH_1 * DEFAULT_PENDULUM_LENGTH_1 / 3.0; costdouble DEFAULT_PENDULUM_VISCOUS_FRICTION_1 = 0.0; 46

47 costdouble DEFAULT_PENDULUM_MASS_2 = 0.1; costdouble DEFAULT_PENDULUM_LENGTH_2 = 2.0; costdouble DEFAULT_PENDULUM_DISTANCE_TO_CENTER_2 = 0.5; costdouble DEFAULT_PENDULUM_INERTIA_MOMENTUM_2 = DEFAULT_PENDULUM_MASS_2 * DEFAULT_PENDULUM_LENGTH_2 * DEFAULT_PENDULUM_LENGTH_2 / 3.0; costdouble DEFAULT_PENDULUM_VISCOUS_FRICTION_2 = 0.0; costdouble 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 DoubleIvertedPedulum() cart_mass = DEFAULT_CART_MASS; cart_vf = DEFAULT_CART_VISCOUS_FRICTION; cart_cf = DEFAULT_CART_COULOMB_FRICTION; ComputeCostats(); 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; staticdoubleivertedpedulum istace = ull; privatestaticvoid check() if (istace == ull) istace = ewdoubleivertedpedulum(); staticdouble Mt; staticdouble mt; staticdouble I1t; 47

48 staticdouble I2t; staticdouble m2l2; staticdouble ml2l1; privatestaticvoid ComputeCostats() 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 EvolutioHelper (SystemStatus s, double f) SystemStatus result = ewsystemstatus(); double si = System.Math.Si(s.theta); double cos = System.Math.Cos(s.theta); double si2 = System.Math.Si(s.theta2); double cos2 = System.Math.Cos(s.theta2); double si_diff = System.Math.Si(s.theta - s.theta2); double cos_diff = System.Math.Cos(s.theta - s.theta2); double sig = (s.x == 0.0)?0 : (s.x > 0.0? 1.0 : -1.0); Matrix3x3 coefficiets = ewmatrix3x3(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; coefficiets.ivert(); coefficiets.rightmultiplywithvector( -cart_vf * s.d_x + mt * si * s.d_theta * s.d_theta + m2l2 * si2 * s.d_theta2 * s.d_theta2 - sig * cart_cf * Mt * G, -(vf1 + vf2) * s.d_theta + vf2 * s.d_theta2 - ml2l1 * si_diff * s.d_theta2 * s.d_theta2 + G * mt * si, ml2l1 * si_diff * s.d_theta * s.d_theta + vf2 * (s.d_theta - s.d_theta2) + G * m2l2 * si2, ref result.d_x, ref result.d_theta, ref result.d_theta2); retur result; publicstaticsystemstatus Evolutio(SystemStatus s, double f, double dt) check(); // Use Dormad Price method SystemStatus[] temp = ewsystemstatus[7]; temp[0] = EvolutioHelper(s, f); temp[1] = EvolutioHelper(s + dt * DormadPrice.DP1(temp), f); temp[2] = EvolutioHelper(s + dt * DormadPrice.DP2(temp), f); 48

49 temp[3] = EvolutioHelper(s + dt * DormadPrice.DP3(temp), f); temp[4] = EvolutioHelper(s + dt * DormadPrice.DP4(temp), f); temp[5] = EvolutioHelper(s + dt * DormadPrice.DP5(temp), f); temp[6] = EvolutioHelper(s + dt * DormadPrice.DP6(temp), f); retur s + dt * DormadPrice.DP7(temp); publicstaticdouble GetFirstPedulumLegth() retur L1; publicstaticdouble GetSecodPedulumLegth() retur L2; A.4. Starea sistemului publicclasssystemstatus publicdouble x; /* the positio of the cart */ publicdouble d_x; /* velocity of the cart */ publicdouble theta; /* agular positio of pedulum */ publicdouble d_theta; /* agular velocity of pedumul */ publicdouble theta2; publicdouble d_theta2; publicstaticsystemstatusoperator+(systemstatus x, SystemStatus y) SystemStatus result = ewsystemstatus (); retur result; 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; publicstaticsystemstatusoperator -(SystemStatus x, SystemStatus y) SystemStatus result = ewsystemstatus(); 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; retur result; 49

50 publicstaticsystemstatusoperator *(SystemStatus x, double a) SystemStatus result = ewsystemstatus(); retur result; 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; publicstaticsystemstatusoperator *(double a, SystemStatus x) retur x * a; A.5. Metoda Dormad-Price classdormadprice 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) retur DP[0] * k[0]; publicstaticsystemstatus DP2(SystemStatus[] k) retur DP[1] * k[0] + DP[2] * k[1]; publicstaticsystemstatus DP3(SystemStatus[] k) 50

51 retur DP[3] * k[0] + DP[4] * k[1] + DP[5] * k[2]; publicstaticsystemstatus DP4(SystemStatus[] k) retur DP[6] * k[0] + DP[7] * k[1] + DP[8] * k[2] + DP[9] * k[3]; publicstaticsystemstatus DP5(SystemStatus[] k) retur DP[10] * k[0] + DP[11] * k[1] + DP[12] * k[2] + DP[13] * k[3] + DP[14] * k[4]; publicstaticsystemstatus DP6(SystemStatus[] k) retur 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) retur 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. Egie-ul aplicatiei: publicclassegie publicstaticsystemstatus systemstatus; privatestaticegie istace = ull; staticbool use_double; log lastframetime; costlog FRAME_RATE = 60; costlog FRAME_LENGTH = 1000 / FRAME_RATE; publiccostdouble WORLD_HEIGHT = 8.0; publiccostdouble WORLD_WIDTH = 32.0; public Egie() if (istace!= ull) retur; istace = this; use_double = true; systemstatus = ewsystemstatus (); systemstatus.x = 0.0; 51

52 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 = ewthread(ewthreadstart(ru)); thread.start(); publicvoid ru() // start a stopwatch to cout time Stopwatch stopwatch = Stopwatch.StartNew(); stopwatch.start(); // iitialize last frame time lastframetime = stopwatch.elapsedmillisecods; while (true) log frametime; do // wait for at least framelegth Thread.Sleep(0); frametime = stopwatch.elapsedmillisecods; 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 = DoubleIvertedPedulum.Evolutio(systemStatus, FuzzyCotroller.GetForce(systemStatus, elapsedtime), elapsedtime); else systemstatus = IvertedPedulum.Evolutio(systemStatus, FuzzyCotroller.GetForce(systemStatus, elapsedtime), elapsedtime); Mai.Draw(); publicstaticsystemstatus GetSystemStatus() retur systemstatus; 52

53 publicstaticvoid GetLegths(refdouble legth1, refdouble legth2) if (use_double) legth1 = DoubleIvertedPedulum.GetFirstPedulumLegth(); legth2 = DoubleIvertedPedulum.GetSecodPedulumLegth(); else legth1 = IvertedPedulum.GetPedulumLegth(); legth2 = 0.0; 53

54 Aexa B: Iterfata aplicatiei B.1. Pedulul simplu B.2. Pedulul dublu 54