ALGORYTMY STEROWANIA QUADROTORA

ALGORYTMY STEROWANIA QUADROTORA Poliechnika Wrocławska 23

Auorzy Doroa Moskal rozdziały 4.2, 6.4, 7.2 Michał Kre rozdział 4.2 Maeusz Sawicki, Michał Dziergwa rozdziały 6.6, 6.7, 7.5 Mariusz Orda, Krzyszof Szydłowski rozdziały, 2, 4., 5, 6.3, 7.. Naalia Czop, Maciej Gawron rozdziały 3., 4.3, 6.5, 7.3 Michał Adamczyk, Paweł Kułakowski rozdziały 3.2, 6., 6.2, 7.4 Skład raporu wykonano w sysemie L A TEX Praca udosępniana na licencji Creaive Commons: Uznanie auorswa-użycie niekomercyjne-na ych samych warunkach 3., Wrocław 22. Pewne prawa zasrzeżone na rzecz Auorów. Zezwala się na niekomercyjne wykorzysanie reści pod warunkiem wskazania Auorów jako właścicieli praw do eksu oraz zachowania niniejszej informacji licencyjnej ak długo, jak ylko na uwory zależne będzie udzielana aka sama licencja. Teks licencji dosępny na sronie: hp://creaivecommons.org/licenses/by-nc-sa/3./pl/

Spis reści Spis oznaczeń........................................ 4. Wsęp............................................ 7.. Badany obiek..................................... 7... Własności obieku.............................. 7.2. Układ współrzędnych................................. 8.3. Cel pracy........................................ 8 I. Podsawy eoreyczne 2. Model dynamiki quadroora.............................. 3 2.. Wprowadzenie..................................... 3 2.2. Równania Eulera-Lagrange a............................. 3 2.3. Momeny niezachowawcze.............................. 4 2.4. Współrzędne położenia................................ 6 2.5. Współrzędne kąowe................................. 7 2.5.. Energia kineyczna.............................. 7 2.5.2. Energia poencjalna............................. 8 2.5.3. Równanie Eulera-Lagrange a........................ 8 2.6. Przyjęy model.................................... 2 3. Modelowanie maemayczne.............................. 2 3.. Modelowanie maemayczne ciągu.......................... 2 3... Modelowanie ciągu.............................. 2 3..2. Modelowanie silników prądu sałego.................... 22 3..3. Zaburzenia oddziałujące na quadroor................... 22 3.2. Numeryczne meody rozwiązywania równań różniczkowych............ 24 3.2.. Wsęp do całkowania numerycznego.................... 24 3.2.2. Klasy równań różniczkowych........................ 24 3.2.3. Szacowanie błędu............................... 24 3.2.4. Meoda Eulera................................ 25 3.2.5. Meoda Rungego-Kuy........................... 25 3.2.6. Równania szywne.............................. 25 3.2.7. Podwajanie kroku.............................. 25 3.2.8. Zmienny krok całkowania.......................... 26 4. Algorymy serowania.................................. 27 4.. Regulaor PID..................................... 27 4... Implemenacja regulaora PID dla quadroora............... 29 4.2. Algorym całkowania wsecznego.......................... 32 4.2.. Przegląd lieraury.............................. 32 4.2.2. Schema algorymu.............................. 32 4.2.3. Serownik................................... 33 4.3. Algorym H..................................... 37 4.3.. Serowanie układu algorymem H..................... 38 4.3.2. Serowanie układu quadroora algorymem H.............. 4

4 Spis reści II. Implemenacja 5. Zadania realizowane przez quadroor........................ 43 5.. Śledzenie rajekorii.................................. 43 5... Krzywe Lissajous............................... 43 5..2. Krzywe Béziera................................ 43 5..3. Krzywa łańcuchowa............................. 45 5.2. Serowanie do punku................................. 46 6. Realizacja w środowisku MATLAB......................... 47 6.. Inerfejs danych.................................... 47 6... Wymiana danych w modelu......................... 47 6..2. Dobór inerfejsu danych........................... 47 6..3. Opis srukur zawarych w projekcie.................... 48 6..4. Funkcja pomocnicza generująca srukury................. 48 6.2. Model......................................... 5 6.2.. Opis wejść/wyjść............................... 52 6.2.2. Inicjalizacja.................................. 52 6.2.3. Przykład uruchomienia............................ 52 6.2.4. Implemenacja................................ 53 6.3. Implemenacja serownika PID............................ 57 6.4. Algorym całkowania wsecznego.......................... 59 6.4.. Inicjalizacja.................................. 62 6.5. Implemenacja algorymu H dla quadroora................... 64 6.5.. Model dynamiki quadroora opisany równaniami sanu.......... 64 6.5.2. Archiekura serownika H......................... 64 6.6. Inerfejs użykownika................................. 74 6.6.. Wprowadzenie................................ 74 6.6.2. Srukura GUI................................ 74 6.6.3. Uruchamianie GUI.............................. 76 6.6.4. Srukura GUI z punku widzenia użykownika.............. 76 6.6.5. Generaor rajekorii............................. 8 6.6.6. Połączenie algorymu z GUI......................... 82 6.7. Obsługa inerfejsu użykownika........................... 84 6.7.. Uruchamianie GUI.............................. 84 6.7.2. Usawienia paramerów modelu quadroora................ 84 6.7.3. Wybór algorymu serowania........................ 84 6.7.4. Usawienia paramerów algorymu serowania............... 85 6.7.5. Generowanie rajekorii zadanej....................... 85 6.7.6. Uruchomienie symulacji........................... 87 6.7.7. Prezenacja wyników............................. 88 6.7.8. Zapisywanie i odczyywanie wyników.................... 88 7. Badania........................................... 9 7.. Regulaor PID..................................... 9 7... Człon proporcjonalny P........................... 9 7..2. Człon całkujący I............................... 94 7..3. Człon różniczkujący D............................ 97 7..4. Badania serowanie do punku i śledzenie wybranych rajekorii.... 99 7..5. Podsumowanie................................ 3 7.2. Algorym całkowania wsecznego.......................... 6 7.2.. Trajekorie.................................. 6 7.2.2. Dobór paramerów serownika........................ 7

Spis reści 5 7.2.3. Przegląd wyników symulacji dla różnych nasaw serownika....... 7.2.4. Przegląd wyników symulacji dla różnych rajekorii............ 2 7.2.5. Podsumowanie................................ 4 7.3. Badania jakości serowania H dla quadroora.................. 5 7.3.. Wpływ zmian rajekorii współrzędnej z i kąa myszkowania na jakość serowania............................. 8 7.3.2. Wpływ zmian rajekorii współrzędnych x i y na jakość serowania... 2 7.3.3. Wpływ zmian kąa myszkowania na jakość serowania.......... 24 7.3.4. Przypadek na granicy sabilności układu.................. 27 7.3.5. Podsumowanie................................ 34 7.4. Badania meod numerycznych............................ 35 7.4.. Sałe serowania............................... 35 7.4.2. Serowanie zmienne.............................. 36 7.4.3. Wyniki badań................................. 36 7.4.4. Wnioski.................................... 39 7.5. Badania porównanwcze algorymów serowania.................. 42 7.5.. Wsęp..................................... 42 7.5.2. Porównanie algorymów serowania..................... 42 7.5.3. Wnioski.................................... 64 Bibliografia........................................... 7

6 Spis reści Spis oznaczeń Oznaczenia zosały uszeregowane alfabeycznie. W pierwszej kolejności przedsawiono symbole rozpoczynające się lierami alfabeu łacińskiego. Nasępnie wprowadzono oznaczenia wykorzysujące alfabe grecki. A X, A Y, A Z przekrój roboa odpowiednio w osi OX, OY, OZ b współczynnik ciągu d współczynnik oporu środowiska powierza F () siła elekrodynamiczna F X, F Y, F Z siła oporu powierza działająca odpowiednio w osi OX, OY, OZ f współczynnik arcia lepkiego G obiek serowania g przyspieszenie ziemskie I X, I Y i I Z momeny główne (bezwładności) ramy quadroora i() naężenie prądu płynącego przez silnik J r momen bezwładności pojedynczego śmigła K sała konsrukcyjna silnika K serownik H do śledzenia sygnału W i (eż Ki) K 2 serownik H do pomiaru sygnału zwronego (eż K2) L indukcyjność uzwojenia wornika silnika l odległość od środka ciężkości quadroora do każdego z roorów M() momen elekryczny M f momen arcia suchego M r () momen obciążenia m masa całkowia quadroora Q(s) funkcja przenoszenia q współrzędne uogólnione R rezysancja uzwojenia wornika silnika R r promień śmigieł (eż r) T energia kineyczna T ciąg z uwzględnionym efekem ziemi (podłoża) T ciąg uzyskiwany przez silniki na nieskończenie dużej wysokości U napięcie na zaciskach silnika U, U 2, U 3, U 4 serowania modelu quadroora u wekor wejścia serowania v warość zmierzona na wyjściu układu serowania V energia poencjalna W i model dokładny obieku wyznaczony przy sanie sacjonarnym (eż W i) w sygnały egzogenne (zakłócenia zewnęrzne) ẋ prędkość w płaszczyźnie dla kórej liczymy opory powierza x, y, z współrzędne układu związanego z ziemią. X, Y, Z współrzędne układu związanego z quadroorem. z sygnał błędu z h pionowa odległość quadroora od powierzchni płaskiej (podłoża/sufiu) ρ air gęsość powierza φ, θ, ψ kąy KKM (kiwanie, kołysanie, myszkowanie), inaczej kąy RPY (roll, pich, yaw) wyrażone w radianach Ω suma prędkości obroowych śmigieł Ω, Ω 2, Ω 3, Ω 4 warości prędkości obroowej śmigieł

Spis oznaczeń 7 ω() prędkość obroowa silnika ω X, ω Y, ω Z prędkości obroowe quadroora względem osi X, Y, Z.

. Wsęp.. Badany obiek W pracy rozparzono zagadnienia doyczące laającego roboa ypu quadroor (quadrocoper). Napędzany jes on za pomocą czerech śmigieł umieszczonych w jednej płaszczyźnie, kórych osie obrou są do niej prosopadłe. Taka konsrukcja umożliwia konrolowany ruch w rójwymiarowej przesrzeni. Wygląd przykładowej realizacji fizycznej obieku przedsawia rysunek.. Rysunek.. Przykład roboa ypu quadroor [5]... Własności obieku Obiek badany w pracy posiada nasępujące własności: Ciało roboa jes bryłą szywną. Robo posiada czery ramiona o równej długości, ułożone pod kąem 9 sopni względem siebie. Na ich końcach zamonowane są śmigła. Środek ciężkości roboa znajduje się w jego środku geomerycznym. Śmigła są bryłami szywnymi. Ciąg śmigieł jes proporcjonalny do kwadrau ich prędkości obroowych. Opór aerodynamiczny jes proporcjonalny do kwadrau prędkości ciała. Ponieważ praca śmigieł dosarcza dodakowej energii do układu quadroora, układ en jes niezachowawczy. Schema poglądowy badanego obieku zosał przedsawiony na rysunku.2.

. Wsęp Rysunek.2. Schema poglądowy quadroora..2. Układ współrzędnych Środowisko działania modelu zosało opisane za pomocą dwóch układów karezjańskich, związanych odpowiednio z ooczeniem oraz z ciałem roboa. Począe Do opisu współrzędnych pierwszego układu wykorzysano symbole x,y,z, naomias dla drugiego użyo X,Y,Z. Począek układu współrzędnych związanego z ciałem roboa umieszczono w środku geomerycznym quadroora, zaś osie X, Y w płaszczyźnie śmigieł. W celu określenia orienacji roboa w przesrzeni wykorzysano kąy obrou poprzecznego kołysania (ang. roll), wzdłużnego kiwania (pich) oraz pionowego myszkowania (yaw), w skrócie KKM. Reprezenację ą przedsawia rysunek.3. Jako oznaczenie wprowadzonych kąów przyjęo odpowiednio symbole φ, θ, ψ. W dalszej części dla przejrzysości zapisu wykorzysano skróconą noację zapisu funkcji rygonomerycznych: cos φ c φ, sin θ s θ ip. Przyjęy wekor zmiennych sanu o q = (x, y, z, φ, θ, ψ). Rysunek.3. Ilusracją kąów KKM.3. Cel pracy Głównym celem pracy jes opracowanie środowiska umożliwiającego porównywanie wybranych algorymów serowania dla quadroorów i realizacji zadania śledzenia rajekorii, uwzględniając wybrane zjawiska fizyczne działające na roboa. W rozdziale 2 przedsawiono sposób uzyskania przykładowego modelu dynamiki quadroora przy użyciu formalizmu Eulera-Lagrange a. Nasępnie zaprezenowano model

.3. Cel pracy z nowym serowaniem, wykorzysany w dalszej części pracy, oraz zwrócono uwagę na różnice w porównaniu do innych publikacji naukowych. Kolejny rozdział omawia kwesie związane z modelowaniem maemaycznym ciągu (sekcja 3.) oraz numerycznym rozwiązywaniem równań różniczkowych 3.2. Modelowanie maemayczne ciągu doyczy modelowania silników prądu sałego używanych w rzeczywisym modelu obieku oraz modelowania (lub ich uzasadnionego pomijania) zaburzeń oddziałujących na quadroor. Rozdział 4 wprowadzono podsawy eoreyczne rzech algorymów wykorzysanych do późniejszego serowania modelu quadroora. W sekcji 4. opisano regulaor PID (proporcjonalno całkująco różniczkujący), kóry wykorzysywany jes do realizacji w środowisku symulacyjnym określonych zadań, w szczególności śledzenia rajekorii roboa. Kolejno, w 4.2, omówiony jes serownik wykorzysujący algorym całkowania wsecznego w zasosowaniu dla quadroora. Sekcja 4.3 wprowadza eorię serowania algorymem H podsawowego układu serowania podając przekszałcenia i warunki niezbędne do jego poprawnego zaimplemenowania. Część drugą rozpoczyna rozdział 5, w kórym opisano zadania śledzenia rajekorii oraz serowania do punku. W rozdziale 6. przedsawiono opis inerfejsów danych używanych w aplikacji w celu wymiany informacji pomiędzy modelem, serownikami oraz wizualizacją. W kolejnym rozdziale 6.2 przedsawiono szczegółowy opis implemenacji modelu quadroora z wykorzysaniem pakieu Simulink, opis wejść i wyjść, insrukcję inicjalizacji oraz przykład wdrożenia. Sekcje PID, 6.4 oraz 6.5 przybliżają czyelnikowi sposób zamodelowania w pakiecie Malab/Simulink rozparywanych algorymów serowania quadroorem, ze szczególnym zwróceniem uwagi na pojawiające się problemy implemenacji i sposób ich rozwiązania. Rozdziały 7. - 7.3 prezenują badania serowania quadroorem przy pomocy rzech algorymów serowania opisanych w niniejszej publikacji. Rozdział 7.4 przedsawia esy numerycznych meod rozwiązywania układów równań różniczkowych, a w szczególności zaimplemenowanego modelu quadroora.

Część I Podsawy eoreyczne

2. Model dynamiki quadroora 2.. Wprowadzenie Kompuerowa symulacja roboów w konekście serowania wymaga implemenacji modelu fizycznego obieku o wymaganej dokładności. W celu jego uzyskania sosuje się formalizm Newona Eulera bądź Eulera Lagrange a. Pierwszy z nich opiera się o zasadę zachowania sił oraz zasadę zachowania momenów. Jego zaleą jes ławość uzyskania dynamika odwronej, dzięki czemu jes wykorzysywany w regulaorach oparych na modelu. Formalizm Eulera Lagrange a jes podejściem energeycznym. Może być sosowany na dowolnych współrzędnych opisujących układ, a uzyskane przez niego równania dynamiki są w posaci jawnej. Będzie on wykorzysany w niniejszym rozdziale do uzyskania modelu dynamiki quadroora. Wyprowadzenie zosanie przeprowadzone w oparciu o pracę [5]. 2.2. Równania Eulera-Lagrange a W ujęciu mechaniki klasycznej lagranżjan L jes różnicą energii kineycznej T i poencjalnej V obieku L = T V. (2.) Według Zasady Najmniejszego Działania Hamilona układ zachowawczy przyjmuje rajekorie dla kórych funkcjonał I = L(q(), q())d (2.2) osiąga warość najmniejszą z możliwych. Praca [5] przedsawia rozwiązanie problemu minimalizacji przy pomocy rachunku wariacyjnego. W rezulacie uzyskuje się równania Eulera Lagrange a d d ( ) δl δ q i δl δq i =. (2.3) Zasadniczą zaleą formalizmu Eulera Lagrange a jes niezmienniczość formy równań, bez względu na wybór współrzędnych uogólnionych q i. Badanie układów niezachowawczych, akich jak quadroor, wymaga skorzysania z uogólnionych równań Eulera-Lagrange a. Po uwzględnieniu działania sił niezachowaczych F i względem współrzędnych q i równanie (2.3) przyjmuje posać: ( ) δl d d δ q i δl δq i = F i (2.4) Siły uogólniony nie zawsze są ożsame z siłami fizycznymi. Ich posać różni się ze względu na dobór współrzędnych uogólnionych. W przypadku współrzędnych kąowych mają

6 2. Model dynamiki quadroora wymiar momenów. Nie dla każdego doboru współrzędnych uogólnionych isnieje inerpreacja fizyczna sił uogólnionych. Powyższe równania można zapisać w ypowej formie równań dynamiki M(q i ) q i + C(q i, q i ) q i + D(q i ) = F i, (2.5) gdzie M,C i D są odpowiednio macierzami bezwładności, sił odśrodkowych i Coriolisa oraz sił grawiacji obieku. 2.3. Momeny niezachowawcze Układ quadroroora jes niezachowawczy ze względu na pracę wykonywaną przez silniki wirników. W wyprowadzanym modelu uwzględniono nasępujące siły i momeny niezachowawcze: Suma ciągów silników: F c = b(ω 2 + Ω 2 2 + Ω 2 3 + Ω 2 4), (2.6) gdzie Ω i jes prędkością obroową śmigła i, a b współczynnikiem ciągu. Różnica pomiędzy ciągiem generowanym przez silniki 2 i 4 (rys. 2.): F () φ = bl( Ω 2 2 + Ω 2 4), (2.7) gdzie l jes długością ramienia. Różnica pomiędzy ciągiem generowanym przez silniki i 3 (rys. 2.2): F () θ = bl( Ω 2 + Ω 2 3). (2.8) Różnica pomiędzy ciągiem generowanym przez pary silników i 3 oraz 2 i 4 (rys. 2.3): F ψ = d( Ω 2 + Ω 2 2 Ω 2 3 + Ω 2 4), (2.9) gdzie d jes współczynnikiem oporu powierza. Efek żyroskopowy względem osi X(ką φ) (rys. 2.4): F (2) φ = J r ω Y ( Ω + Ω 2 Ω 3 + Ω 4 ), (2.) gdzie J r jes macierzą bezwładności śmigiła, ω Y prędkością obroową ciała względem osi Y. Efek żyroskopowy względem osi Y (ką θ) (rys. 2.5): F (2) θ = J r ω X (Ω Ω 2 + Ω 3 Ω 4 ). (2.) Rysunek 2.. Momen spowodowany różnicą ciągów generowanych przez silniki 2 i 4.

2.3. Momeny niezachowawcze 7 Rysunek 2.2. Momen spowodowany różnicą ciągów generowanych przez silniki i 3. Rysunek 2.3. Momen spowodowany różnicą ciągów generowanych przez pary silników i 3 oraz 2 i 4. Rysunek 2.4. Efek żyroskopowy względem osi X(ką φ)

8 2. Model dynamiki quadroora Rysunek 2.5. Efek żyroskopowy względem osi Y (ką θ) 2.4. Współrzędne położenia Do wyznaczenia równań dynamiki dla współrzędnych położenia wysarczy skorzysać z drugiej zasady dynamiki Newona a = m F, (2.2) gdzie m jes masą obieku, F siłą wypadkową działającą na niego, zaś a przyśpieszeniem ciała. Wypadkowa siła działającą na quadroora składa się z siły grawiacji F g = mg oraz sumy ciągów silników (2.3) F c = b(ω 2 + Ω 2 2 + Ω 2 3 + Ω 2 4), (2.4) gdzie Ω i jes prędkością obroową śmigła i. Załóżmy, że siła grawiacji oddziaływuje wzdłuż globalnej osi z, i zauważmy, że kierunek siły ciągu silników jes zgodny z normalną quadroora Z. W celu wyrażenia wekora opisanego we współrzędnych quadroora w układzie współrzędnych związanym z ziemią, posłużono się wzorem X r x,y,z (X, Y, Z) = R(φ, θ, ψ) Y, (2.5) Z gdzie c ψ c θ c ψ s θ s φ s ψ c φ c ψ s θ c φ + s ψ s φ R(φ, θ, ψ) = s ψ c θ s ψ s θ s φ + c ψ c φ s ψ s θ c φ c ψ s φ. (2.6) s θ c θ s φ c θ c φ Podsawiając siłę wypadkową do równania (2.2) uzyskujemy ẍ ÿ = + R(φ, θ, ψ). (2.7) m z g b(ω 2 + Ω 2 2 + Ω 2 3 + Ω 2 4) Po uproszczeniu równanie (2.7) orzymuje nasępującą posać ẍ ÿ = + b c ψ s θ c φ + s ψ s φ m (Ω2 + Ω 2 2 + Ω 2 3 + Ω 2 4) s ψ s θ c φ c ψ s φ. (2.8) z g c θ c φ

2.5. Współrzędne kąowe 9 2.5. Współrzędne kąowe Do wyprowadzenia równań dynamiki dla współrzędnych kąowych zosaną użye uogólnione równania Eulera Lagrange a (2.4). Jako współrzędne uogólnione posłużą kąy KKM (φ, θ, ψ). 2.5.. Energia kineyczna Pierwszą składową lagranżjanu jes energia kineyczna. Ponieważ współrzędnymi uogólnionymi są kąy KKM, należy uzyskać wzór energii w odpowiadającym im prędkościach obroowych. W ym celu różniczkowane jes wyrażenie (2.5). Dla przejrzysości zapisu rozdzielono zapis macierzowy do układu równań. v x (X, Y, Z) = ( s θ c ψ θ cθ s ψ ψ)x +( c ψ c φ ψ + s ψ s φ φ sψ s φ s θ ψ + c ψ c φ s θ φ + cψ s φ c θ θ)y +(c ψ s φ ψ + s ψ c φ φ sψ c φ s θ ψ c ψ s φ s θ φ + cψ c φ c θ θ)z v y (X, Y, Z) = ( s θ s ψ θ cθ c ψ ψ)x +( s ψ c φ ψ c ψ s φ φ + cψ s φ s θ ψ + s ψ c φ s θ φ + sψ s φ c θ θ)y +(s ψ s φ ψ c ψ c φ φ + cψ c φ s θ ψ s ψ s φ s θ φ + sψ c φ c θ θ)z (2.9) v z (X, Y, Z) = ( c θ θ)x +(c φ c θ φ sφ s θ θ)y +( s φ c θ φ cφ s θ θ)z Kwadra prędkości dowolnego punku układu quadroora w układzie ziemi przyjmuje posać: v 2 (X, Y, Z) = v 2 x(x, Y, Z) + v 2 y(x, Y, Z) + v 2 z(x, Y, Z) (2.2) Po wykonaniu przekszałceń uzyskano nasępujące wyrażenia: v 2 (X, Y, Z) = (Y 2 + Z 2 )( φ ψs θ ) 2 + (Z 2 + X 2 )( θc φ + ψs φ c θ ) 2 + (X 2 + Y 2 )( θs φ + ψc φ c θ ) 2 + 2XY ( ψ 2 s φ s θ c θ + ψ(c φ s θ θ sφ c θ φ) cφ φ θ) + 2ZX( ψ 2 c φ s θ c θ + ψ( s φ s θ θ cφ c θ φ) + sφ φ θ) + 2Y Z( ψ 2 s φ c θ c φ + ψ(s 2 φc θ θ c 2 φc θ θ) + sφ c φ θ2 ) (2.2) Energia kineycznego dla bryły szywnej jes wyrażona wzorem [5] T = 2 m q v 2 dm (2.22)

2 2. Model dynamiki quadroora Całkując wyrażenie (2.2) po masie na obszarze quadroora m q uzyskano wzór na energie kineyczną ruchu obroowego roboa (2.22). T = (Y 2 + Z 2 )dm ( 2 φ ψs θ ) 2 m q + (Z 2 + X 2 )dm ( 2 θc φ + ψs φ c θ ) 2 m q + (X 2 + Y 2 )dm ( 2 θs φ + ψc φ c θ ) 2 m q + XY dm ( ψ 2 s φ s θ c θ + ψ(c φ s θ θ sφ c θ φ) cφ φ θ) m q + ZXdm ( ψ 2 c φ s θ c θ + ψ( s φ s θ θ cφ c θ φ) + sφ φ θ) m q + Y Zdm ( ψ 2 s φ c θ c φ + ψ(s 2 φc θ θ c 2 φc θ θ) + sφ c θ2 φ ). (2.23) m q Ze względu na założoną symeryczną budowę quadroora, wyrażenia związane z momenami dewiacyjnymi można pominąć. Momeny główne zosały oznaczone I X, I Y, I Z. Osaecznie energia kineyczna: T = 2 I X( φ ψs θ ) 2 + 2 I Y ( θc φ + ψs φ c θ ) 2 + 2 I Z( θs φ + ψc φ c θ ) 2. (2.24) 2.5.2. Energia poencjalna Drugą składową lagranżjanu jes energia poencjalna układu. Wyraża się nasępującym wzorem: V = g zdm m q = g Xdm (s θ ) m q g Y dm (s φ c θ ) m q g Zdm (c φ c θ ). (2.25) m q 2.5.3. Równanie Eulera-Lagrange a Posiadając analiyczną posać energii kineycznej (2.24) i poencjalnej (2.25) oraz uogólnionych sił niezachowawczych (2.3) możliwe jes zasosowanie uogólnionych równań Eulera-Lagrange a (2.4). F () φ + F (2) φ = d ( ) δl d δ φ F ψ = d ( ) δl d δ ψ F () θ + F (2) θ = d ( ) δl d δ θ δl δφ, δl δψ, δl δθ. (2.26)

2.5. Współrzędne kąowe 2 W fizycznym quadroorze niemożliwe jes bezpośrednie mierzenie kąów KKM. Powszechnie wykorzysywane jes urządzenie IMU (z ang. inerial measuremen uni), będące kombinacją akceleromerów oraz żyroskopów. Za jego pomocą odczyywane są prędkości i przyśpieszenia kąowe związane z układem ciała roboa.z ego względu dokonujemy w modelu (2.26) zamiany prędkości φ, hea, psi ma przez IMU i oznaczone przez ω X, ω Y, ω Z. Transformacja wykonywana jes nasępującym wzorem: ω X s θ φ ω Y = c φ s φ c θ θ (2.27) ω Z s φ c φ c θ ψ W en sposób orzymujemy d d d d d d ( ) δl δ φ δl δφ = I X ω X (I Y I Z )ω Y ω Z + Y dm ( gc φ c θ ) m q + Zdm (gs φ c θ ) m q ( ) δl δ θ δl δθ = s φ(i Z ω Z (I X I Y )ω X ω Y ) ( ) δl δ φ + c φ (I Y ω Y (I Z I X )ω X ω Z ) + Xdm ( gc θ ) m q + Y dm ( gs φ s θ ) m q + Zdm ( gc φ s θ ) m q δl δφ = s θ(i X ω X (I Y I Z )ω Y ω Z ) + s φ c θ (I Y ω Y (I Z I X )ω X ω Z ) + c φ c θ (I Z ω Z (I X I Y )ω X ω Y ) (2.28) Po zasosowaniu w wyrażeniach (2.28) przybliżenia liniowego dla małych warości kąów kiwania i kołysania (α sin α, cos α ) uzyskano równania dynamiki układu dla kąów KKM. φ θ ψ = bl(ω2 2 Ω2 4 )+Jr θ( Ω 2 +Ω2 2 Ω3 +Ω2 4 )+(I Y I Z ) ψ θ I X = bl(ω2 3 Ω2 )+Jr φ(ω 2 Ω2 2 +Ω3 Ω2 4 )+(I Z I X ) ψ φ I Y = d(ω2 Ω2 2 +Ω2 3 Ω2 4 )+(I X I Y ) θ φ I Z (2.29)

22 2. Model dynamiki quadroora 2.6. Przyjęy model W celu nadania wyjściom serującym sensu sił (momenów oddziałujących na quadroor) opisanych w podrozdziale 2.3 wprowadzimy nowy wekor serowań U = ( U U 2 U 3 U 4 ) T oraz zmienną pomocniczą Ω w nasępujący sposób [5] U = b(ω 2 + Ω 2 2 + Ω 2 3 + Ω 2 4) U 2 = b( Ω 2 2 + Ω 2 4) U 3 = b( Ω 2 + Ω 2 3) U 4 = d( Ω 2 + Ω 2 2 Ω 2 3 + Ω 2 4) Ω = Ω 2 + Ω 4 Ω Ω 3. (2.3) Ławo można wyznaczyć warości prędkości obroowych śmigieł Ω i w zależności od wekora U, a mianowicie Ω 2 = U U 3 U 4 4b 2bl 4d Ω 2 2 = U U 2 + U 4 4b 2bl 4d Ω 2 3 = U + U 3 U 4 4b 2bl 4d Ω 2 4 = U + U 2 + U 4. 4b 2bl 4d (2.3) Zesawiając ze sobą równania (2.8) oraz (2.29) z uwzględnieniem zależności (2.3) orzymujemy kompleny model dynamiki quadroora w posaci ẍ = (c ψ s θ c φ + s ψ s φ ) U m ÿ = (s ψ s θ c φ c ψ s φ ) U m z = g + (c θ c φ ) U m φ = θ ψ( I Y I Z I X ) Jr I X θω + l I X U 2 θ = φ ψ( I Z I X I Y ) + Jr I Y φω + l I Y U 3 ψ = φ θ( I X I Y I Z ) + I Z U 4. (2.32) W lieraurze pojawia się kilka wersji modelu quadroora. Ważne, aby w przypadku wszyskich działań porównawczych zawsze opierać się na ej samej podsawie eoreycznej. Tuaj zdecydowano się zaimplemenować powyższy model z kilku powodów. Po pierwsze, jes o jeden z pierwszych przedsawionych modeli, wszyskie kolejne odwołują się do niego i na nim bazują. Model en jes bardzo dobrze opisany w lieraurze, zdecydowana większość publikacji znanych auorom przyjmuje en opis. Należy jednakże przy próbie powórzenia symulacji zwrócić uwagę na różnice wysępujące w używanych modelach. Przykładowo praca [8] wprowadza przeskalowanie serowań U 2 oraz U 3 przez odległość śmigła od środka modelu l, dzięki czemu prędkość zmian kąów kiwania i myszkowania zosaje powiązana z modelem fizycznym. Z kolei w [] oprócz ego przyjęo odwrony kierunek osi z, czego skukiem jes zmiana znaku przed serowaniem U 3.

3. Modelowanie maemayczne 3.. Modelowanie maemayczne ciągu Siła nośna quadroora uzyskiwana jes z 4 śmigieł kręcących się parami (usawionymi przeciwlegle) w przeciwnych kierunkach. Śmigła napędzane są silnikami elekrycznymi, kóre są serowane przez serownik. Każde z nich generuje własny ciąg zależny od prędkości obroowej. Poniżej przedsawiono modelowanie ciągu dla badanego obieku jakim jes quadroor. Nasępnie przybliżono czyelnikowi problem modelowania silników prądu sałego (gdyż aki yp silników sosowany jes w badanym obiekcie). Osania zaś sekcja niniejszego działu omawia zaburzenia jakie mogą oddziaływać na quadroor. 3... Modelowanie ciągu W przypadku prac rozważających jedynie algorymy serowania i samolokalizacji roboa częso en aspek jes pomijany [22]. W przypadku, gdy auorzy chcą jedynie pokazać, że akie zjawisko isnieje, sosowana jes kwadraowa zależność pomiędzy prędkością obroową silników, a ciągiem śmigieł aki model silników jes najczęściej spoykany w lieraurze [4]. Jes o uproszczony model wyprowadzony z eorii momenu [7]. W przypadku, gdy prace omawiają wpływ ciągu na działanie algorymów sosowany jes pełny model wyprowadzony z eorii momenu [2]. Isnieją akże prace poświęcone jedynie modelowaniu ciągu quadroorów [3]. W ich przypadku model wyprowadzony z eorii momenu uzupełniany jes o nieliniowe współczynniki wynikające z licznych zjawisk aerodynamicznych wysępujących na łopaach śmigieł obracających się z dużą prędkością. W modelu quadroora sworzonym na porzeby ego projeku zosała zamodelowana zależność kwadraowa pomiędzy warością prędkości obroowej łopa śmigieł (Ω, Ω 2, Ω 3, Ω 4 ) a ciągiem przez nie generowanym (U, U 2, U 3, U 4 ). Jes ona opisana nasępującymi równaniami: U = b(ω 2 + Ω 2 2 + Ω 2 3 + Ω 2 4) U 2 = b( Ω 2 2 + Ω 2 4) U 3 = b( Ω 2 + Ω 2 3) U 4 = d( Ω 2 + Ω 2 2 Ω 2 3 + Ω 2 4). (3.) Przyjęy model pozwala na wyliczenie prędkości obroowych śmigieł dla narzuconego serowania. Transformacja aka jes opisana równaniami: Ω 2 = U 4b U 2b 3 U 4d 4 Ω 2 2 = U 4b U 2b 2 + U 4d 4 Ω 2 3 = U 4b + U. (3.2) 2b 3 4d U 4 Ω 2 4 = 4b U + 2b U 2 + 4d U 4

24 3. Modelowanie maemayczne Ponieważ wszyskie prędkości obroowe są dodanie, ransformacja (3.2) daje jednoznaczne rozwiązanie. Dzięki emu możliwe jes symulowanie efeków oddziaływających na quadroor wynikających z prędkości obroowych śmigieł w przypadku algorymów, kóre serują jedynie ciągiem silników. 3..2. Modelowanie silników prądu sałego Poza modelowaniem zależności pomiędzy prędkością obroową a ciągiem śmigła, model quadroora można uzupełnić o model maemayczny napędów zazwyczaj silników prądu sałego. Podobnie jak w przypadku modelowania ciągu i uaj zależność pomiędzy napięciem zadawanym na silniki, a prędkością obroową wału może zosać pominięa [22]. Model maemayczny silników prądu sałego wynikający z praw Ohma, Kirchoffa i Newona przedsawiony w [2] można opisać równaniami: F ()r = Ki() = M() U = Ri() + L di() + Kω() (3.3) d M() = J dω() + fω() + M d f(ω()) + M r () Z równań (3.3) można wyprowadzić nasępującą zależność pomiędzy wyjściem ω a wejściami U i M r : JL d2 ω () + (JR + fl)dω d2 d () + (Rf + K2 )ω() = L dm r d () RM r() + KU(). (3.4) Częso, zważywszy na niewielką warość indukcyjności silników sosowanych w konsrukcjach ego ypu, równanie (3.4) jes upraszczane do JR dω d () + (Rf + K2 )ω() = RM r () + KU(). (3.5) Podobne, uproszczone modele są sosowane w [2] i [4]. Ponado w [4] zosała pokazana linearyzacja modelu silnika w punkcie pracy. Model quadroora przyjęy na porzeby ego projeku nie uwzględnia maemaycznego modelowania napędów. 3..3. Zaburzenia oddziałujące na quadroor Na unoszący się quadroor oddziałuje duża liczba sił i momenów, kóre mogą zosać uwzględnione w modelu. Poniżej przedsawiono e, kóre są najczęściej opisywane w lieraurze, np. w [3].. Zaburzenia ciągu. a) Zaburzenia ciągu wynikające z bliskości ziemi/sufiu quadroor na niewielkich wysokościach (rzędu kilku cm) doświadcza zwiększonej siły nośnej wynikającej z odbiego ciągu generowanego przez łopay śmigieł od ziemi. Zjawisko o można opisać równaniem: T T = ( Rr 4z h ). (3.6) 2 Podobne zjawisko obserwowane jes w przypadku bliskości sufiu. W ym przypadku nad śmigłami worzy się podciśnienie zwiększając siłę nośną quadroora. Efek sufiu jes częso modelowany ym samym wzorem co efek bliskości ziemi. b) Zaburzenia ciągu wynikające ze zmiany kąa naarcia łopa siła nośna zależy od kąa naarcia śmigła. Wraz ze zmianą wiaru lub eż ruchem quadroora ką naarcia może ulec zmianie, co wpływa na siłę ciągu generowaną przez śmigła.

3.. Modelowanie maemayczne ciągu 25 c) Zaburzenia ciągu wynikające z drgania łopa (blade flapping) zmiana geomerii śmigieł wynikająca z drgania łopa podczas lou poziomego. Efek powoduje akże zmianę kąa naarcia śmigła. d) Zaburzenia ciągu wynikające z oddziaływania sił horyzonalnych na elemeny śmigła (hub force) pomijalna warość przy wysępowaniu wcześniej opisanych oddziaływań. e) Zaburzenia ciągu wynikające z innego udźwigu łopay aakującej i łopay cofającej podczas lou do przodu (rolling momen) pomijalna warość przy wysępowaniu innych efeków. f) Zaburzenia ciągu wynikające z zależności gęsości powierza od wysokości również są pomijalne, gdyż quadroory poruszają się na sosunkowo niewielkich wysokościach i podczas pojedynczego lou nie zmieniają znacznie swojej wysokości. Większe zmiany wysokości są niemożliwe ze względu na niewielką pojemność akumulaorów sosowanych w popularnych konsrukcjach. g) Zaburzenia ciągu wynikające z zawirowań powierza generowanych przez inne maszyny laające zauważalne w przypadku laania w zwarej formacji. 2. Efek żyroskopowy ramy quadroora każde ciało obracające się ma zdolność do urzymania swojej orienacji względem płaszczyzny prosopadłej do osi obrou. Wynika o z zasady zachowania pędu. Podobnie jes z obracającym się quadrocoperem. Wpływ efeku żyroskopowego na kąy KKM jes zazwyczaj zawary bezpośrednio w równaniach opisujących modele maemayczne quadroorów Iy Iz φ = θ ψ I x Iz Ix θ = φ ψ I y. Ix Iy ψ = θ ψ I z 3. Efek żyroskopowy śmigieł obok efeku opisanego wcześniej jes o najczęściej modelowane zjawisko zaburzające quadroor. Bezwładność obracających się z dużą prędkością śmigieł przeciwdziała kiwaniu i kołysaniu się quadrokopera. Wpływ ego efeku na kąy KKM zazwyczaj modelowany jes nasępująco: θ = ψ = Jr φ = θ I x Ω φ Jr I y Ω. 4. Opory powierza zazwyczaj modelowane są jako: (3.7) (3.8) F X = 2 dρ airẋ 2 A X, (3.9) gdzie oznaczenia zgodnie ze spisem oznaczeń umieszczonym na począku pracy. 5. Wiar częso modelowany jako dodakowa prędkość dodawana do oporów powierza opisanych równaniem (3.9). Zakłóceniami zawarymi w analizowanym modelu są efeky żyroskopowe ramy quadroora i łopa jego śmigieł opisane równaniami (3.7) - (3.8).

26 3. Modelowanie maemayczne 3.2. Numeryczne meody rozwiązywania równań różniczkowych 3.2.. Wsęp do całkowania numerycznego Równania różniczkowe są częsym sposobem opisu rozmaiych procesów i zjawisk akich jak np. mechanika płynów, rozchodzenie się fal dźwiękowych. W niniejszym dokumencie podsawą naszych zaineresowań jes rozwiązywanie układów dynamiki kóre również zapisane są za pomocą równań różniczkowych. Gdy analizujemy układy równań eoreycznie najlepszym sposobem ich rozwiązywania (pod względem jakościowym) jes rozwiązanie ich w sposób analiyczny dzięki emu orzymujemy wzór kóry może być dalej badany. Niesey częso wyprowadzenie rozwiązania jes skomplikowane, czasochłonne lub eż niemożliwe. W akich należy zasosować meody numeryczne orzymując numeryczne rozwiązanie problemu. 3.2.2. Klasy równań różniczkowych Równania różniczkowe ze względu na meody rozwiązywania równań numerycznych dzielą się na dwie klasy: Równania różniczkowe zwyczajne ODE - ordinary differenial equaions posiadają funkcje jednej zmiennej (np. zmiennej czasu ) Równania różniczkowe cząskowe PDE - parial differenial equaions posiadają funkcje wielu zmiennych Równania ODE mają ogólną formę przedsawioną w równaniu 3. F (, q, dq d, d2 q d 2,..., dk q d k ) = (3.) gdzie zmienna, q q C k ((a, b), R n ) jes funkcją zmiennej. Przykładem akiego równania może być równanie 3.. d 2 q d 2 + adq + bq = (3.) d Równania PDE różnią się od ODE ym że zmiennych po kórych nasępuje całkowanie jes więcej niż w przypadku ODE przykładem akiego równania może być 3.2 d 2 q dx + d2 q dy = (3.2) gdzie q o funkcja zmiennych x, y. W ej pracy będziemy sosowane będą równania ypu ODE, gdzie zmienną po kórej różniczkujemy będzie czas, w związku z ym omówione zosaną meody numeryczne dla ej klasy równań. 3.2.3. Szacowanie błędu Aby oszacować błąd poszczególnych meod rozwiązywania równań różniczkowych sosuje się noację O(h) jej sosowanie jes bardzo inuicyjne: Jeśli po zmniejszeniu kroku o połowę błąd całkowania maleje o połowę o będzie o klasa O(h) jeżeli zaś maleje czerokronie o jes o klasa O(h 2 ) i ak dalej. Klasę błędu szacujemy z nasępującego wzoru: e h Ch p+ (3.3) Gdzie e h o błąd lokalny ( local runcaion error ), C jes sałą dodanią, a p jes rzędem danej meody.

3.2. Numeryczne meody rozwiązywania równań różniczkowych 27 3.2.4. Meoda Eulera Chcąc wyznaczyć rozwiązanie równania różniczkowego w posaci 3.4 wyznaczamy jego przybliżenie dla przyrosu h > : y () f(, y()) = y( + h) y() h Przekszałcając równanie 3.4 do posaci (3.4) y( + h) = y() + h f(, y()) (3.5) Orzymujemy podsawową meodę Eulera. Meoda a jes najprosszą i najbardziej inuicyjną meodą całkowania, jej błąd silnie zależy od przyjęego kroku całkowania a akże od szywności równania. Meoda Eulera jes meodą pierwszego rzędu O(h 2 ) 3.2.5. Meoda Rungego-Kuy Meoda Rungego-Kuy (w skrócie RK) drugiego rzędu bazuje na wyznaczeniu wyniku na podsawie średniego przyrosu (jak w m. Eulera) oraz punku pośredniego [23]. k = hf(x n, y n ) k 2 = hf(x n + 2 h, y n + 2 k ) y n+ = y n + k 2 (3.6) Na założeniu przyjęcia jednego punku pośredniego bazuje cała rodzina meod Runge-Kuy drugiego rzędu (np. w meodzie Heuna wyliczamy warość kroku z wzoru y n+ = y n + 2 (k +k 2 )). Błąd polepsza się do rzędu drugiego O(h 3 ). Meoda Rungego-Kuy rzędu czwarego bazuje naomias na czerech punkach pośrednich a błąd polepsza się do klasy czwarej O(h 5 ). Isnieje wiele odmian meod Rungego-Kuy różnią się one współczynnikami dla długości kroku (przed h), sposobie uwzględniania poprzednich kroków w krokach kolejnych i sposobie wyliczania wyniku z kroków, jednak ogólna idea pozosaje a sama. 3.2.6. Równania szywne Największym zagrożeniem dla meod całkowania numerycznego są równania szywne (ang. siff equaions). Niesey nie ma jednoliej definicji dla równań szywnych. Możemy jednak zdefiniować o inuicyjnie: są o akie równania kóre przy całkowaniu niekórymi meodami numerycznymi okazują się numerycznie niesabilne. Aby zapewnić numeryczną sabilność należy albo znacząco zmniejszyć krok całkowania lub zmienić meodę. 3.2.7. Podwajanie kroku Aby zwiększyć dokładność meod całkowania można zasosować meodę podwajania kroku. Meoda a jes prosa w zasosowaniu: Wykonujemy krok całkowania h, a poem powarzamy obliczenia wykonując podwójne całkowanie z krokiem 2h, różnica będzie esymaorem błędu całkowania kóry pomoże nam poprawić wynik: = y 2 y y(x + h) = y 2 + 5 Gdzie y o całkowanie pojedyncze z krokiem h a y 2 o dwa całkowania z krokiem h 2.

28 3. Modelowanie maemayczne 3.2.8. Zmienny krok całkowania Na schemacie podwójnego obliczania kroku bazują meody rozszerzone. y n+ = y n + c k + c 2 k 2 +... + c m k m yn+ = yn + c k + c 2k 2 +... + c mk m W meodach podsawowych RK bazujemy jedynie na y n+ naomias w rozszerzonych (embedded RK mehods) korzysamy dodakowo z yn+ (czyli całkujemy z krokiem o połowę mniejszym) co pozwala na esymację błędu za pomocą błędu meody (runcaion error): = y n+ y n+ (3.7) dzięki emu mamy szansę na poprawę kroku algorymu. Określamy dokładność za pomocą, nasępnie obliczamy bieżącą warość błędu, jeśli jes ona znacząco mniejsza od zadanej dokładności o podwajamy krok. Jeśli niewiele mniejsza o zosawiamy bieżący krok. Jeśli naomias przekracza błąd zmniejszamy krok całkowania o połowę. Innym sposobem doboru kroku jes wyznaczanie jego warości na podsawie wzoru:.2 h = h (3.8) W przypadku gdy > powarzamy całkowanie z nowym krokiem h, w przeciwnym wypadku wykonujemy krok naprzód o warości h. Ten sposób posępowania nazywany jes adapacyjnym krokiem całkowania.

4. Algorymy serowania 4.. Regulaor PID Ogólnie, w układzie regulacji (rysunek 4.) uchybem regulacji e() nazywamy różnicę pomiędzy warością zadaną r() a wielkością wyjściową obieku y(). Uchyb (błąd serowania) jes Rysunek 4.. Układ regulacji sygnałem, na podsawie kórego regulaor generuje sygnał serujący u() dla obieku. Na obiek dodakowo działają jeszcze zakłócenia z() []. Regulaor na podsawie warości e() seruje obiekem w pożądany przez nas sposób (na przykład minimalizując błąd śledzenia rajekorii). Jednym z najbardziej znanych przykładów serowników, obecnie najczęściej wykorzysywanym w przemyśle, jes regulaor PID rozparywany pod względem zasosowania w roboach ypu quadroor m. in. w [3, 8, ]. Składa się on z rzech członów kompensujących uchyb proporcjonalnego, całkującego oraz różniczkującego, pracujących w pęli sprzężenia zwronego. Zachowanie akiego serownika opisuje równanie [] u() = K p e() + K i de() e(τ)dτ + K d, (4.) d gdzie jako K p określamy wzmocnienie części proporcjonalnej, jako K i wzmocnienie części całkującej a jako K d wzmocnienie części różniczkującej. W dziedzinie liczb zespolonych po zasosowaniu ransformacji Laplace a (co jes porzebne przy implemenacji algorymu w środowisku MATLAB Simulink), można zapiać równanie (4.) jako u(s) = (K p + K i s + sk d)e(s). (4.2) Regulaor PID jes szeroko wykorzysywany w konsrukcjach roboów ypu quadrocoper, ponieważ: posiada bardzo prosą srukurę, jes ławo implemenowany sprzęowo, jes wydajnym obliczeniowo regulaorem nawe dla wielu procesów, posiada szereg algorymów oparych na meodach Zieglera-Nicholsa pozwalających dosroić go nawe bez znajomości modelu.

3 4. Algorymy serowania Modyfikacje regulaora PID Równanie (4.2) opisuje regulaor PID w zw. posaci równoległej, w kórej wszyskie składowe są od siebie niezależne. Z ego powodu posać a jes zwana algorymem PID IND (od INDependen algorihm) i używana najczęściej w pracach naukowych (chociażby w [8,]). Przedsawia ją rysunek (4.2). Rysunek 4.2. Schema ideowy regulaora PID IND Drugą posać regulaora PID, zwaną algorymem PID ISA (od Ideal Sandard Algorihm), określa równanie u() = k r ( e() + T i ) de() e(τ)dτ + T d, (4.3) d gdzie T i jes czasem zdwojenia (całkowania, po kórym przy działaniu członu proporcjonalnego i całkującego od podania sygnału skokowego wyjście ulega zdwojeniu). T d jes czasem wyprzedzenia (różniczkowania, po kórym przy działaniu członu proporcjonalnego i różniczkującego w czasie liniowo narasającego sygnału wyjściowego ulega on zdwojeniu). k r jes współczynnikiem skalującym wzmocnienie regulaora. W dziedzinie liczb zespolonych po zasosowaniu ransformacji Laplace a równanie (4.3) przyjmuje posać u(s) = k r ( + T i + st d )e(s). (4.4) Jak ławo zauważyć w celu przeliczania nasaw do posaci (4.2) należy wykorzysać wzory K p = k r K i = k r T i K d = k r T d. (4.5) Posać PID ISA, częso sosowana w przemyśle [2], przedsawiono na rysunku 4.3. Rysunek 4.3. Schema ideowy regulaora PID ISA

4.. Regulaor PID 3 Rysunek 4.4. Regulaor PID z sauraorem Pomimo szerokiej popularności i wielu meod doboru nasaw regulaor PID nie zawsze działa zadowalająco. Opracowano szereg usprawnienień przeciwdziałającym najczęściej wysępującym niepożądanym zjawiskom. Pierwszym akim wysępującym zjawiskiem jes inegral windup [2] (łumaczone jako nawijanie całkowania). Wszyskie układy rzeczywise charakeryzują się pewnymi osiągalnymi warościami granicznymi. Zdarza się, iż kiedy zmienna serująca osiągnie warość maksymalną, błąd dalej jes całkowany. Modyfikację emu zapobiegającą implemenuje się dodając do członu całkującego blok sauraora. Wyrażenie całkujące osiąga wówczas bardzo dużą warość, czemu przeciwdziała blok sauraora wprowadzający nasycenie. Kolejna modyfikacja wynika z faku, że w czasie działania algorymu regulacji zwykłego PID mogą wysąpić gwałowne zmiany zmiennych (np. kiedy zmienne zadane są w sposób skokowy). By zminimalizować o zjawisko częsokroć w miejsce pochodnej błędu do regulaora podawana jes pochodna wyjścia obieku. Konsrukcję regulaora PID, uwzględniającą oba usprawnienia, przedsawiono na rysunku 4.4. Zdarza się, iż w prakyce sosowane są regulaory PID nie posiadające wszyskich wysępujących w nich członów. I ak regulaor PI jes regulaorem, w kórym całkowicie pominięo blok różniczkujący, dzięki czemu sysem jes mniej wrażliwy na szybkie, rzeczywise zmienne sanu (nie będące szumami). Jes bardziej sabilny w przypadku zaszumianych danych. Dzieje się ak, gdyż pochodna jes wrażliwa na wysokie częsoliwości. Jes o szczególnie isone przy implemenacji na rzeczywisym robocie ypu quadroor, kóry nie jes wyposażony w wiele czujników lub nie sosuje ich fuzji. W przypadku, kiedy dynamika obieku jes całkowicie lub częściowo nieznana (lub wysępują duże różnice pomiędzy modelem maemaycznym a rzeczywisym obiekem) sosuje się regulaor PD. Obecność członu całkującego nie daje w akim przypadku wymiernej poprawy serowania. Wadą akiego rozwiązania jes fak, że regulaor PD daje niezerowy uchyb usalony. 4... Implemenacja regulaora PID dla quadroora Dysponując w quadroorze 4 wejściami serującymi wybieramy 4 składowe wekora sanu, kórych przebieg będziemy konrolować za pomocą regulaora PID. Będą o kąy φ, θ i ψ oraz położenie z [5,8,,]. W celu doboru srukury i nasaw ych regulaorów zauważmy, że równanie dynamiki (2.32) może zosać uproszczone po założeniu małych warości kąów myszkowania ψ i kołysania θ do posaci ẍ = ÿ = z = g + (c φ c θ ) U m φ = lu 2 I X θ = lu 3 I Y φ = U 4 I Z. (4.6)

32 4. Algorymy serowania Jak widać dla współrzędnych kąowych uzyskano w en sposób liniowy model dynamiki. Przy założeniu małych warości kąów φ i θ podobnie możemy spojrzeć na równanie dla składowej z (ze sałym pochodzącym od grawiacji wymuszeniem g, kóre w procesie serowania należy kompensować). Poniżej zosanie omówiony sposób implemenacji regulaora dla poszczególnych współrzędnych. Srukurę serowników dla kąów kiwania i kołysania pokazano na rysunkach 4.5 i 4.6. Są o ypowe regulaory PID z nasyceniem w części całkującej, z dodakowym czło- Rysunek 4.5. Serownik kąa φ Rysunek 4.6. Serownik kąa θ nem wzmacniającym o współczynniku wzmocnienia wyznaczonym na podsawie modelu (4.6). K P φ [s 2 ], K Iφ [s 3 ], i K Dφ [s ] są paramerami serownika kąa φ, zaś K P θ [s 2 ], K Iθ [s 3 ], i K Dθ [s ] są paramerami serownika kąa θ. Dla kąa myszkowania srukura serownika jes analogiczna z ą różnicą, że uaj ze względu na nieciągłość reprezenacji kąa myszkowania (przy ±π), pojawiła się porzeba zasosowania dodakowych bloków FOLDER i SIGN COHERENCE. Pozwalają one na zachowanie ciągłości kąa ψ. (zobacz rysunek 4.7). Serownik współrzędnej z posiada na wyjściu wynikający z modelu (4.6) współczynnik skalujący Rysunek 4.7. Serownik kąa myszkowania

4.. Regulaor PID 33 oraz człon służący do kompensacji grawiacji. Zosał przedsawiony na rysunku 4.8. Rysunek 4.8. Serownik współrzędnej z Zesawiając w jedną całość opisane serowniki orzymujemy kompleny układ serowania quadroorem o srukurze pokazanej na rysunku 4.9. Rysunek 4.9. Przykład implemenacji algorymu (za: []) Algorym serowania orzymuje jako wejścia dane o warościach akualnych zmiennych sanu, kóre pochodzą z sensorów - inercyjnej jednoski pomiarowej IMU, mierzącej akualne warości kąów φ, θ i ψ oraz danych z sonaru mierzących warość współrzędnej z. Jako Aliude se value przyjmujemy warości zadane dla ych zmiennych. Rysunek przedsawia serowanie za pomocą prędkościami kąowymi Ω, kóre pochodzi bezpośrednio ze wzoru (2.3). W implemenacji na fizycznym obiekcie można uprościć dynamikę quadroora do posaci z równania (4.6) - praca [8] powierdza pożądane działanie regulaora PID w zadaniu śledzenia rajekorii przez modelowany w en sposób quadroor.

34 4. Algorymy serowania 4.2. Algorym całkowania wsecznego W niniejszym podrozdziale omówiony zosanie algorym całkowania wsecznego poocznie nazywany algorymem backsepingu (od pierwonej nazwy angielskiej inegral backsepping) w zasosowaniu do serowania quadroorem. W kolejnych podrozdziałach czyelnik znajdzie: omówienie meody całkowania wsecznego, przegląd isniejących prac na ema serowania quadroorem przy użyciu algorymu backseppingu; opis serownika wykorzysującego ideę całkowania wsecznego. 4.2.. Przegląd lieraury Zagadnienie serowania quadroorem za pomocą algorymu wsecznego całkowania było wielokronie podejmowane w lieraurze fachowej. Jako jedni z pierwszych emaykę ę podjęli S. Bouabdallah oraz R. Siegwar. W pracy [6] wyprowadzają oni model dynamiki quadroora i podają jego równania (por. rozdział 6.2). Nasępnie przedsawiają dynamikę w przesrzeni sanu i, w jasny dla czyelnika sposób, przekszałcają ją do posaci łańcuchowej 2 (por. rozdział 4.2.3). Zdefiniowanie funkcji błędów dla kolejnych podsysemów serownika pozwala na czyelną konsrukcję kolejnych funkcji Lapunowa, a w efekcie prowadzi do orzymania dwusopniowego algorymu serowania quadroorem. Praca a sała się swoisą inspiracją dla kolejnych naukowców jes cyowana w niemalże każdym kolejnym arykule doyczącym omawianego zagadnienia. Rok po opublikowaniu pracy Bouabdallaha i Siegwara pojawiły się prace naukowców z Francji Madiniego oraz Benallequeá [8, 9]. Przedsawione w nich zosało inne podejście do zagadnienia dynamiki, m.in. położono większy nacisk na opisanie sił składających się na dynamikę quadroora, akich jak opory powierza czy efek żyroskopowy. Zaproponowany algorym znacznie odbiegał od ypowego schemau całkowania wsecznego, waro naomias zwrócić uwagę na fak, iż dokonano jego implemenacji na laboraoryjnym modelu quadroora. Prace [4, 24] również rakują o zagadnieniu serowania obiekem ypu quadroor, niemniej algorym całkowania wsecznego jes omówiony pobieżnie i przyjmuje posać podobną do ej, jaką zaproponowali Bouabdallah i Siegwar. Auorzy podanych prac skupiają się w nich na dwóch innych algorymach, odpowiednio na serowaniu ślizgowym oraz algorymie H. Spośród omówionych prac za najlepszą podsawę do opracowania i zamodelowania serownika uznano ą Bouabdallaha i Siegwara [6]. Zdecydowano się na akie podejście ponieważ: przedsawiona konsrukcja algorymu całkowania wsecznego jes najbliższa duchowi prac Kokoovića, jes o podsawowa praca, do kórej odwołują się kolejni badacze, algorym opisany jes czyelnie i nie ma w nim żadnych niejasności. 4.2.2. Schema algorymu Algorym całkowania wsecznego zaproponowany zosał w 99 przez Peara V. Kokoovića [6, 7]. Pozwala on na sabilizację układów o posaci kaskadowej i znalazł zasosowanie przy realizacji zadania serowania różnorodnych obieków. Rozważamy nasępujący układ kaskadowy [2] ẋ = f (x ) + g (x )x 2, ẋ 2 = f 2 (x, x 2 ) + g 2 (x, x 2 )x 3,. ẋ n = f n (x,..., x n ) + g n (x,..., x n )u. Polska nazwa całkowanie wseczne zaproponowana zosała przez prof. K. Tchonia w [26]. 2 zwanej również posacią kaskadową; nazwy e będą używane zamiennie (4.7)

4.2. Algorym całkowania wsecznego 35 W akim układzie funkcje f i i g i w i-ym podsysemie zależą ylko od ych zmiennych x j, dla kórych j i, zaś zmienna x i+ pełni rolę serowania. W osanim podsysemie, zamias kolejnej zmiennej wysępuje rzeczywise serowanie sysemu. Projekowanie serownika odbywa się w sposób rekurencyjny. Zaczynamy od sabilizacji podsysemu ẋ = f (x ) + g (x )x 2. (4.8) Definiujemy dla niego odpowiednią funkcję Lapunowa zależną ylko od zmiennej x. Zwykle przyjmuje ona formę kwadraową V (x ) = 2 zt z, (4.9) gdzie z = x lub z = x x d (x d rajekoria zadana). Funkcja a musi spełniać dwa warunki: V (x ) = x =, V (x ) > x. (4.) Kolejno wyliczamy V (x ) wzdłuż rajekorii układu. Na mocy zasady niezmienniczości LaSalle a, układ jes sabilny, jeśli spełniona jes nierówność V (x ). (4.) Z nierówności (4.) orzymujemy wyrażenie opisujące x 2, dane z dokładnością do pewnego parameru, zwykle oznaczanego jako α. W kolejnym kroku algorymu, podsysem składa się z dwóch równań { ẋ = f (x ) + g (x )x 2, ẋ 2 = f 2 (x, x 2 ) + g 2 (x, x 2 )x 3. (4.2) Ponownie definiujemy funkcję Lapunowa, pamięając, że należy zachować wyniki poprzedniego kroku. I ak V 2 przyjmuje posać V 2 (x, x 2 ) = V (x ) + 2 zt 2 z 2, (4.3) gdzie z 2 = x 2 α x lub z 2 = x 2 x 2d α (x x d ). Wykorzysując nierówność V 2 (x, x 2 ). (4.4) uzyskujemy posać x 3, ponownie daną z dokładnością do pewnego parameru, oznaczanego zwykle jako α 2. Posępując analagicznie dla układów obejmujących coraz więcej podsysemów dochodzimy do momenu, w kórym uzyskujemy posać serowania u. 4.2.3. Serownik Jak przyjęo w rozdziale.2 san quadroora określa sześciowymiarowy wekor q = (x, y, z, φ, θ, ψ). Dosępne są jednak ylko czery serowania, obiek jes więc niedoserowany. Nie jes więc możliwe śledzenie rajekorii dla więcej niż czerech składowych wekora q. Logicznym rozwiązaniem jes zadawanie położenia w przesrzeni (x, y, z) oraz orienacji w płaszczyźnie XY ψ. Kąy kołysania i kiwania przyjmują warości zależne od pozosałych współrzędnych. Schema serownika implemenującego algorym całkowania wsecznego pokazuje rysunek 4.. Serownik podzielony jes na dwa moduły pierwszy wylicza kąy θ i φ konieczne do uzyskania

36 4. Algorymy serowania Rysunek 4.. Schema serownika. zadanej pozycji (x, y) oraz łączną siłę ciągu silników porzebną do uzyskania zadanej wysokości z. Drugi wylicza pozosałe serowania pozwalające na uzyskanie wymaganych warości kąów. Do wyznaczenia serowań oba moduły serownika wykorzysują algorym całkowania wsecznego. W celu sprowadzenia układu quadroora danego równaniem (2.32) do przesrzeni sanu sosuje się podsawienie wekora x = (x, x 2,..., x 2 ) T, gdzie: x = φ, x 2 = ẋ = φ, x 3 = θ, x 4 = ẋ 3 = θ, x 5 = ψ,, x 6 = ẋ 5 = ψ, (4.5) x 7 = x, x 8 = ẋ 7 = ẋ, x 9 = y, x = ẋ 9 = ẏ, x = z, x 2 = ẋ = ż. Z (4.5) i (2.32) orzymujemy: ẋ = x 2, ẋ 2 = x 4 x 6 a + x 4 a 2 Ω + b U 2, ẋ 3 = x 4, ẋ 4 = x 2 x 6 a 3 + x 2 a 4 Ω + b 2 U 3, ẋ 5 = x 6, ẋ 6 = x 4 x 2 a 5 + b 3 U 4, ẋ 7 = x 8, ẋ 8 = g + (cos x cos x 3 ) m U, ẋ 9 = u x m U, ẋ = u x m U, ẋ = x 2, ẋ 2 = u y m U, (4.6) gdzie: a = I y I z, a 2 = J R, a 3 = I z I x, a 4 = J R, a 5 = I x I y, b = l, b 2 = l, b 3 = l, I x I x I y I y I z I x I y I z oraz u x = cos x sin x 3 cos x 5 + sin x sin x 5, (4.7) u x = cos x sin x 3 sin x 5 sin x cos x 5. (4.8)

4.2. Algorym całkowania wsecznego 37 Orzymany układ ma posać łańcuchową a ściślej można go porakować jako sześć łańcuchowych podukładów o zmiennych x i i x i+. Serowanie kąami naarcia Serowanie U 2 można wyliczyć wykorzysując pierwszy ze wspomnianych podukładów opisanych równaniami ẋ = x 2, (4.9) ẋ 2 = x 4 x 6 a + x 4 a 2 Ω + b U 2. (4.2) Dla zadanej rajekorii x d, błąd śledzenia e b zdefiniowany jes jako e b = x d x. (4.2) W celu usabilizowania (4.9) zdefiniujmy nasępującą funkcję Lapunowa (czyelnik powiniem zwrócić uwagę, że uaj wszyskie współrzędne są skalarne) V (x ) = 2 et be b = 2 e2 b. (4.22) Funkcja a spełnia warunek nieujemności (4.). Jej pochodna wyliczona wzdłuż rajekorii układu wynosi V (e b ) = e b ė b = e b (ẋ d ẋ ) = e b (ẋ d x 2 ). (4.23) Spełnienie warunku na niedodanią pochodną (4.) zapewnia x 2 = ẋ d + α e b, (4.24) przy paramerze α >. Dla drugiego sopnia kaskady (4.2) błąd ma posać e b2 = x 2 ẋ d α 2 e b. (4.25) Zdefiniujmy funkcję Lapunowa spełniającą (4.) V 2 (x, x 2 ) = V (x ) + 2 e2 b2 = 2 (e2 b + e 2 b2), (4.26) i wyliczmy jej pochodną wzdłuż rajekorii układu V 2 (e b, e b2 ) = e b2 (x 4 x 6 a + x 4 a 2 Ω + b U 2 ) e b2 (ẍ d α (e b2 + α e b )) e b e b2 α e 2 b = e b2 (b U 2 + x 4 x 6 a + x 4 a 2 Ω ẍ d + α (e b2 + α e b ) e b ) α e 2 b. (4.27) Spełnienie warunku (4.4) w punkcie równowagi (ẍ = ) zapewnia serowanie U 2 = b (e b x 4 x 6 a x 4 a 2 Ω α (e b2 + α e b ) α 2 e b2 ), (4.28) gdzie α 2 >, zaś człon α 2 e b2, przeciwdziała zmianom e b. W analogiczny sposób można wyznaczyć serowania U 3 i U 4 uzyskując U 3 = b 2 (e b3 x 2 x 6 a 3 x 2 a 4 Ω α 3 (e b3 + α 3 e b3 ) α 4 e b4 )), U 4 = b 3 (e b5 x 2 x 4 a 5 α 5 (e b6 + α 5 e b5 ) α 6 e b6 ), (4.29) gdzie e b3 = x d3 x 3, e b4 = x 4 ẋ d3 αx 3 e b3, e b5 = x d5 x 5, e b6 = x 6 ẋ d5 αx 5 e b5. (4.3)

38 4. Algorymy serowania Serowanie położeniem Wyznaczenie U odbywa się idenycznie jak dla pozosałych serowań. Przy błędach e b7 = x d7 x 7, e b8 = x 8 ẋ d7 α 7 e b7, (4.3) przyjmuje ono posać U = m cos x cos x 3 (e b7 + g α 7 (e b8 + α 7 e b7 ) α 8 e b8 ). (4.32) Przemieszczenie quadroora w płaszczyźnie XY związane jes bezpośrednio z kąami kiwania i kołysania. Znając serowanie U, dwa osanie podukłady sabilizujemy podobnie jak poprzednie, rakując jako serowanie odpowiednio u x i u y. Orzymujemy u x = m U (e b9 α 9 (e b + α 9 e b9 ) α e b ), u y = m U (e b α (e b2 + α e b ) α 2 e b2 ), (4.33) przy e b9 = x d9 x 9, e b = x ẋ d9 α 9 e b9, e b = x d x, e b2 = x 2 ẋ d α e b. (4.34) Kąy θ i φ wyznaczyć można przyrównując uzyskane wyniki do zależności (4.7) i (4.8). Osaecznie uzyskany zosał układ serowania o dwunasu regulowanych paramerach α α 2, pozwalający na śledzenie rajekorii w przesrzeni zadaniowej określonej położeniem i kąem myszkowania quadroora.

4.3. Algorym H 39 4.3. Algorym H Rysunek 4. przedsawia podsawowy układ serowania [25] zbudowany z regulaora P oraz obieku K. w P u v K z Rysunek 4.. Podsawowy układ serowania Równania opisujące układ można zapisać nasępująco: [ ] [ ] [ ] [ ] z w P (s) P = P (s) = 2 (s) w v u P 2 (s) P 22 (s) u u = K(s)v. (4.35) Równanie zamknięej pęli sprzężenia zwronego dla powyższego układu serowania z w do z opisuje ransformacja: z = F (P, K)w (4.36) gdzie F (P, K) = P + P 2 K(I P 22 K) P 2 (4.37) Algorym H ma za zadanie minimalizować warość normy F (P, K). Algorym serowania H bazuje na rozwiązywaniu równań sanu przedsawionych w 988 roku przez Glovera i Doyle a. Zakładając reprezenację P równaniami sanu posaci A B B 2 P = C D D 2 C 2 D 2 D 22 (4.38) można określić najczęściej wysępujące założenia odnośnie sosowania algorymu H : (I) A, B 2, C 2 są sabilne i wykrywalne, (II) D[ 2 i D 2 są pełnego ] rzędu, A jωi B2 (III) jes pełnego rzędu dla wszyskich ω, C D 2 [ ] A jωi B (IV) jes pełnego rzędu dla wszyskich ω, C 2 D 2 (V) D = i D 22 =.

4 4. Algorymy serowania Założenie (I) wymaga isnienia serownika K sabilizującego układ. Drugie założenie wysarcza, aby zapewnić isnienie akiego serownika. Założenia (III) i (IV) wysarczają aby zapewnić, by układ nie zosał wyrącony z równowagi i jego pierwiaski nie wpadały w obszar niesabilny. Założenie (V) nie jes wymagane dla algorymu H, sosuje się go raczej w algorymie H 2, pozwala jednak na znaczne uproszczenie formuł algorymu. Dodakowo dla uproszczeń częso zakłada się (VI), iż D 2 = [ I ] oraz D 2 = [ I ]. 4.3.. Serowanie układu algorymem H Sandardowy problem serowania algorymem H dla układu przedsawionego na rysunku 4. sprowadza się do znalezienia wszyskich serowników K, kóre minimalizują F (P, K) = max δ(f (P, K)(jω)) (4.39) ω W prakyce zazwyczaj nie jes wymagane uzyskanie opymalnego serownika dla problemu H. Za wysarczająco dobry serownik uznaje się aki, kóry spełnia nierówność: F (P, K) < γ (4.4) gdzie γ min jes minimalną warością F (P, K) spośród wszyskich serowników K oraz γ > γ min. Tak posawiony problem serowania można rozwiązać sosując algorym przedsawiony przez Doyle a w 989 roku. Dla układu 4. przy założeniach (I)-(VI) serownik spełniający nierówność (4.4) isnieje wedy i ylko wedy gdy: (i) X jes rozwiązaniem algebraicznego równania Riccaiego A T X + X A + C T C + X (γ 2 B B T B 2 B T 2 )X = (4.4) akim, że Reλ i [A + (γ 2 B B T B 2B T 2 )X ] <, i (ii) Y jes rozwiązaniem algebraicznego równania Riccaiego AY + X A T + B B T + Y (γ 2 C T C C T 2 C 2 )Y = (4.42) akim, że Reλ i [A + Y (γ 2 C T C C T 2 C 2)] <, i (iii) ρ(x Y ) < γ 2 Wszyskie serowniki spełniające powyższe warunki dane są przez K = F (K c, Q), gdzie: A Z L Z B 2 K c (s) = F I C 2 I F = B T 2 X, L = Y C T 2, Z = (I γ 2 Y X ) (4.43) A = A + γ 2 B B T X + B 2 F + Z L C 2 oraz Q(s) jes dowolną sabilną funkcją przenoszenia spełniającą nierówność Q < γ. Dla Q(s) = mamy: K(s) = K c (s) = Z L (si A ) F (4.44) K(s) nazywane jes głównym serownikiem i ma aki sam wymiar co P (s). Serownik en może zosać podzielony na część odpowiedzialną za esymację sanu: ˆx = Aˆx + B γ 2 B T X ˆx + B 2 u + Z L (C 2ˆx y) (4.45)

r y 4.3. Algorym H 4 oraz sprzężenie zwrone: u = F ˆx (4.46) Wiele układów wymagających serowania określa się jako układy 2DOF czyli układy o dwóch sopniach swobody, gdzie pierwszy doyczy pomiaru sygnału uzyskanego ze sprzężenia zwronego, a drugi podążania za warością referencyjną. Podsawowy układ ego ypu zosał przedsawiony na rysunku 4.2 (r sygnał referencyjny, G obiek, y wyjście). regulaor G r y Rysunek 4.2. Podsawowy układ 2DOF W przypadku algorymu kszałowania pęli H układ z rysunku 4.2 można przekszałcić do posaci z rysunku 4.3. Wi K W G K2 serownik Rysunek 4.3. Serowanie kszałowaniem pęli H układu 2DOF Kszałowanie pęli H łączy klasyczne meody eorii serowania ze serowaniem opymalnym zapewniając odporność sabilności sysemu. Niniejsze rozwiązanie zosało zaprezenowane w 99 roku przez McFarlane a i Glovera. Kszałowanie pęli H można zasadniczo podzielić na dwa eapy. Najpierw w owarej pęli sygnały z obieku serowania zwiększone są poprzez pre- i pos-kompensaory, aby uzyskać pożądany kszał odpowiedzi częsoliwościowej owarej pęli z dokładnością do pojedynczych warości. Nasępnie w drugim eapie algorymu uzyskany kszał jes poddany sabilizacji odpornościowej przy użyciu opymalizacji H. Kolejne kroki algorymu aby uzyskać układ z rysunku 4.3 można zapisać jako:. Zaprojekuj serownik kszałowania pęli H dla układu DOF bez pre-kompensaora W i. 2. Wybierz funkcję przenoszenia T ref między W /W i a konrolowanymi wyjściami. 3. Usaw paramer skalowania ρ na małą warość większą niż (najlepsza byłaby warość z przedziału [,3]).

42 4. Algorymy serowania 4. Dla G s = GW ], T ref oraz ρ rozwiąż sandardowy problem opymalizacji H. Wyznacz K = [K K 2. 5. Zasąp prefilr K przez K W i aby uzyskać dokładny model w sanie usalonym. 6. Przeanalizuj układ i jeśli jes o wymagane popraw ρ, W oraz T ref. 4.3.2. Serowanie układu quadroora algorymem H W przypadku quadroorów sosuje się zazwyczaj algorym kszałowania pęli H. Srukura podsawowego układu serowania kszałowaniem pęli H przedsawiona jes na rysunku 4.4. r y W W2 Wi K K2 G Rysunek 4.4. Podsawowy układ serowania kszałowaniem pęli H Niech dany będzie nieliniowy model quadroora UAV. Za serowania układu przyjęo równania (2.3), gdzie F i zgodnie z rysunkiem.2. W związku z ym równania dynamiki quadroora opisane są równaniami (2.32). Aby móc zasosować serowanie quadroora kszałowaniem pęli H należy obiek serowania poddać linearyzacji, ukszałować jego odpowiedź częsoliwościową w owarej pęli sprzężenia zwronego i nasępnie nałożyć na ukszałowany sysem pęlę serowania wysynezowaną za pomocą opymalizacji H. Zgodnie z [9] zasosowano prekompensaor W i poskompensaor W 2, kóre opisują sysem z owarą pęlą jako G s = W 2 GW. Zasosowanie kompensaorów pozwala dowolnie kszałować odpowiedź częsoliwościową sysemu w owarej pęli sprzężenia zwronego. Diagonalny kompensaor W zawiera filry całkujące i proporcjonalne. Filry proporcjonalne odpowiadają za zredukowanie opóźnienia fazowego sysemu w okolicach częsoliwości krzyżowania. Część całkująca W pomaga w filrowaniu zakłóceń. Poskompensaor W 2 składa się z filru dolnoprzepusowego pierwszego sopnia odrzucającego szumy polepszając odporność układu. Algorym posępowania oraz implemenację serownika przy użyciu oprogramowania Malab/Simulink przedsawiono w sekcji 6.5

Część II Implemenacja

5. Zadania realizowane przez quadroor Do zadań serowania realizowanych przez robo mobilny jakim jes quadroor należą: śledzenie rajekorii, śledzenie ścieżki, serowanie do punku. W celu analizy algorymów serowania, przeprowadzono badania nad pierwszym i osanim zaganieniem. Poniżej zosaną omówione e zadania i zaprezenowane krzywe użye przy śledzeniu rajekorii. 5.. Śledzenie rajekorii Zadanie śledzenia rajekorii polega na wykonaniu przez robo mobilny ruchu wzdłuż krzywej z określoną prędkością. Do określenia zadawanego położenia wykorzysuje się krzywe parameryzowane czasem. Zagadnienie zrealizowano przy wykorzysaniu rodzin krzywych zapewniających wysoką konrole nad generowanym kszałem. Krzywe Lissajous umożliwiają worzenie orów sinusoidalnych w przesrzeni karezjańskiej. W szczególności mogą posłużyć do generacji rajekorii kołowej bądź elipycznej. Krzywa Béziera zapewnia możliwość worzenia gładkich orów o dowolnym kszałcie. Jes funkcją wielomianową i może posłużyć do zbadania ruchu po prosej, czy paraboli. Jako uzupełnienie zosała zaprezenowana krzywa łańcuchowa, posiadająca kszał opymalny energeycznie. 5... Krzywe Lissajous Krzywe Lissajosu są rodziną krzywych realizowanych w przesrzeni karezjańskiej. Dla współrzędnych x i y są dane równaniem paramerycznym { x() = A sin ( 2πf x T + ϕ) + x y() = B sin 2πfy, (5.) T + y gdzie A i B są ampliudami krzywej,t jej okresem, x i y służą wypozycjonowaniu w przesrzeni, naomias f x i f y są częsoliwościami składowych i razem z opóźnieniem fazowym ϕ określają charaker kszału. Dla sosunku fx f y = orzymywane są dwa szczególne przypadki, dla ϕ = π 2 jes o elipsa, zaś przy ϕ = odcinek. Gamy kszałów w zależności od ych paramerów przedsawiono na rysunku 5.. Implemenacja rajekorii sprowadza się do przypisania równań (5.) dwóm współrzędnym przesrzeni karezjańskiej. 5..2. Krzywe Béziera Krzywe Béziera są wielomianowymi krzywymi paramerycznymi. Dzięki prosej konsrukcji, możliwości odwzorowania dowolnego kszału oraz wysokiej konroli nad krzywizną są powszechnie używane m.in. w grafice kompuerowej, środowiskach CAD czy wzornicwie przemysłowym. Niech P i P będą dowolnymi punkami w przesrzeni. Wedy krzywa Béziera pierwszego sopnia z parameryzacją [, ] jes wyrażona wzorem B P P () = ( )P + P. (5.2)

46 5. Zadania realizowane przez quadroor Rysunek 5.. Krzywe Lissajous Jak ławo zauważyć jes o odcinek łączący oba punky. Dołączmy do naszego zbioru punk P 2. Krzywa Béziera sopnia drugiego jes analogiczną konsrukcją przy wykorzysaniu krzywych B P P () i B P 2 P (). B P 2 P () = ( )B P P () + B P 2 P () (5.3) Uzyskana w powyższy sposób krzywa łączy punky P i P 2 oraz jes syczna na końcach do odcinków P P i P P 2. Osaecznie krzywą sopnia n jes rekurencyjnie określona równaniami { P B i+ P i () = ( )P i + P i+ B Pn P () = ( )B P. (5.4) n () + B Pn P () P Sopień krzywej jes liczbą punków pomniejszoną o jeden. Proces powsawania krzywej zosał zaprezenowany na rysunku 5.2 W celu zapewnienia możliwości zadawania rajekorii o swobod- Rysunek 5.2. Ilusracja rekurencji krzywej Béziera nym kszałcie zaimplemenowano krzywe Béziera w środowisku Simulink. Układ reprezenuje rysunek 5.3. Wejścia i wyjścia układu: Rysunek 5.3. Implemenacja krzywych Béziera w środowisku Simulink

5.. Śledzenie rajekorii 47 CV wekor wierzchołków (punków) konrolnych, zmienną czasu, czas saru, k czas końca, y krzywa Béziera, yp pierwsza pochodna krzywej Béziera, yb druga pochodna krzywej Béziera. Sopień krzywej jes określany na podsawie długości wekora CV. Przykładową realizacje przedsawia rysunek 5.4. Rysunek 5.4. Krzywa Béziera zrealizowana w środowisku Simulink 5..3. Krzywa łańcuchowa Krzywa łańcuchowa odzwierciedla krzywiznę nierozciągliwej, idealnie wiokiej liny o niezerowej masie, zawieszonej pomiędzy dwiema podporami. Jej kszał minimalizuje energię poencjalną akiego układu, i zaem może zosać uzyskany z równania Eulera-Lagrange a (2.3). Zosała przedsawiona na rysunku 5.5. Rysunek 5.5. Wykres krzywych łańcuchowych w zależności od parameru a

48 5. Zadania realizowane przez quadroor Krzywa łańcuchowa jes dana wzorem ( ) x x y(x) = a cosh + y, (5.5) a gdzie y oraz x są wpółrzędnymi karezjańskimi, x i y odpowiadają za umiejscowienie w przesrzeni, zaś a deerminuje zarówno wierchołek krzywej, jak i empo jej owarcia. Wzór (5.5) można przekszałcić do posaci paramerycznej { x() = + x y() = a cosh ( ), (5.6) a + y gdzie jes przesunięciem parameryzacji. 5.2. Serowanie do punku Serowanie do punku ma na celu osiągniecie i urzymanie przez roboa mobilnego zadanego punku przesrzeni zadaniowej w badanych współrzędnych. Regulacja może doyczyć zarówno współrzędnych położenia jak i kąowych (urzymanie orienacji). Implemenacja zadania sprowadza się do zadania rajekorii sałej w czasie. Nie jes wymagane, aby serowanie do punku doyczyło wszyskich serowalnych współrzędnych. Powszechnie łączy się je z zadaniem śledzenia rajekorii, zadając rajekorie, w kórej ruch jes dozwolono jedynie dla części składowych. Przykładową sałą rajekorie dla regulaora PID zaprezenowano równaniu (5.7) z = 2 φ = pi 6 θ = pi ψ = pi 2. Waro zauważyć, iż pomimo w przykładzie zadawane są warości sałe w czasie, quadroor będzie się poruszał we współrzędnych x oraz y. (5.7)

6. Realizacja w środowisku MATLAB 6.. Inerfejs danych 6... Wymiana danych w modelu Zadanie symulacji modelu wraz z jego serowaniem wymaga sworzenia wielu różnych srukur, akich jak model obieku, serowniki, wyniki symulacji, inerfejs użykownika. Każda z ych srukur zawiera w sobie paramery, pobiera oraz zwraca dane. Aby uniknąć chaosu związanego z wysępowaniem ylu różnych paramerów, (konflikom nazw, dublowaniu paramerów, braku zrozumienia przeznaczenia paramerów), należy przyjąć jednoliy sposób agregacji danych. Wymiana danych nasępuje w rzech punkach:. Model-Algorym 2. Wizualizacja-Model 3. Wizualizacja-Algorymy W przypadku, czyli wymiany danych pomiędzy modelem i algorymami inerfejsem danych są połączenia pomiędzy bloczkami modelu i serownika, dane am przekazywane zosały opare o model usalony pomiędzy grupami projekowymi. Naomias w dwóch pozosałych przypadkach (2, 3) zasosowano inerfejs dobrany i omówiony w kolejnej części raporu. 6..2. Dobór inerfejsu danych Agregacja danych w Malabie może odbywać się na różne sposoby, każdy z nich posiada swoje wady i zaley. W ej części zosały omówione podsawowe meody agregacji danych w Malabie i zosał uzasadniony wybór meody zasosowanej w projekcie. Zmienne w przesrzeni roboczej Malaba Podsawowa, najczęściej sosowana meoda. Zmienne zapisujemy bezpośrednio w przesrzeni roboczej, sarając się przy ym nadawać im czyelne nazwy. Ten sposób jes dobry przy niewielkich rozmiarach modelu, przy rozbudowanym modelu prowadzi do bałaganu w przesrzeni roboczej doarcie do porzebnej zmiennej nie jes prose a jeszcze rudniejsze jes znalezienie zbioru paramerów określonej srukury ponado powoduje konfliky nazw. Zasosowanie srukur Zasosowanie srukur pozwala na zagregowanie danych w większe bloki co porządkuje przesrzeń roboczą. Trzymanie się szablonu przy implemenacji pozwala na ławy dosęp do przechowywanych am warości, i ich modyfikację np. przez GUI. Do wad należy zaliczyć brak konroli nad zmianami co może prowadzić do błędów pracy oprogramowania oraz brak możliwości zagregowania w en sposób funkcji i meod. Programowanie obiekowe z użyciem klas Przy porządkowaniu inerfejsu możliwe jes wykorzysanie klas co umożliwia zasosowanie meod sosowanych przy programowaniu obiekowym: konrolę dosępu arybuów (public, pri- Tę wadę można zmniejszyć przez zasosowanie odpowiednich funkcji i konrolę warości - zosało o opisane w dalszej części dokumenu

5 6. Realizacja w środowisku MATLAB Tabela 6.. Srukura paramerów modelu i serowania Nazwa val min_val max_val uni_sym desc Opis Warość bieżąca minimalna warość maksymalna warość symbol jednoski SI opis parameru vae), sosowanie meod. Niesey prócz zale e rozwiązanie posiada znaczące wady. Implemenacja zmienia się wraz z kolejnymi wersjami Malaba. Tworzenie klas jes również skomplikowane i powoduje problemy przy wykorzysaniu przez inne osoby. 6..3. Opis srukur zawarych w projekcie W projekcie zasosowano srukury, jes o kompromis pomiędzy zaleami sosowania klas a prosoą zapisywania zmiennych bezpośrednio w przesrzeni roboczej. Jak okazało się później dokonany wybór był właściwy - pozwalał na szybkie zapoznanie się z nim pozosałych grup projekowych. Dane zaware w modelu zosały zagregowane w srukurach przedsawionych na rys. 6.. Krókie sreszczenie znaczenia poszczególnych srukur: Simopions Simulaion Opions opcje bieżącej symulacji, są one częścią modelu implemenowanego w Simulinku 2 składają się na nie m.in. meoda całkowania, krok całkowania, maksymalny błąd. QP Quadroor Parameers paramery modelu, w ej części znajdują się paramery związane z modelem quadrokopera np. warunki począkowe, paramery fizyczne, opcje serowania. CP Conroler Parameers paramery konrolera, obsługiwane i dosarczane przez grupy algorymiczne. QL Qualiy wskaźniki jakości służące ocenie algorymów, nie implemenowane na ym eapie prac. raj rajecory zapis rajekorii, służy do zapisywania zadanej/orzymanej rajekorii simou simulaion oupu agregacja wszyskich wymienionych powyżej srukur sosowanych/orzymanych przy pojedyńczej symulacji, oraz zapis przebiegu serowania Paramery wchodzące w skład srukur QP i CP posiadają usrukuryzowaną formę (abela 6.) pozwalającą na ich ławą obsługę zarówno w module GUI jak i z poziomu skrypów. 6..4. Funkcja pomocnicza generująca srukury W celu unifikacji i uniknięcia błędów przy implemenacji srukur należy korzysać z funkcji pomagającej worzyć srukur (lising 6.). Celem ej funkcji jes wsępna walidacja danych. Dane sprawdzane są pod względem poprawności ypu (np. eks, liczba) nasępnie sprawdzany jes zakres (czy bieżąca warość znajduje się w zadeklarowanym zakresie). Na końcu zadane warości przypisywane są do poszczególnych paramerów srukury danych. Funkcja pozwala więc na prose uworzenie parameru jes o znacząco krósze niż wypisywanie poszczególnych pól srukury. Jej konsrukcja wyłapuje błędy co pomaga w poprawnym uworzeniu parameru. Wydruk 6.. Funkcja wspomagająca worzenie paramerów dla srukur funcion N = nparam ( val, min_val, max_val, uni_sym, desc ) 3 VarC = class ( val ); 2 Te paramery można uzyskać za pomocą komendy simge( nazwa_pliku_modelu ) i usawiać za pomocą simse

6.. Inerfejs danych 5 Rysunek 6.. Srukury

52 6. Realizacja w środowisku MATLAB if ( ~( srcmp (VarC, f )... 5 srcmp (VarC, ss )... srcmp (VarC, logical )) ) 7 if ( min_val > max_val ) error ( New param. error! min_val > max_val ); 9 end if (( val < min_val ) ( val > max_val )) error ( New param. error! Curren value ou of range ); 3 end end 5 if (~ ischar ( desc )) 7 error ( nparam. error! Descripion ( desc ) is no char! ); end 9 if (~ ischar ( uni_sym )) 2 error ( nparam. error! Uni symbol ( uni_sym ) is no char! ); end 23 N = sruc ( val, val,... % curren vaule 25 min_val, min_val,... % value >= min_val max_val, max_val,... % value <= max_val 27 uni_sym, uni_sym,... % SI symbol (m, rad ) desc, desc ); % parameer descripion 29 end Podanie paramerów przebiega w nasępujący sposób (uaj na skróconym przykładzie 6.2). Wydruk 6.2. Przykład zadania paramerów % %% Quadroor parameers 2 % Source : " Design and Conrol of an Indoor Micro Quadroor " 4 % nparam ( val, min_val, max_val, uni_sym, desc ) QP.m= nparam (5,,, kg, Masa quadrokopera ); 6 QP.l= nparam (.25,,, m,... Dlugosc ramienia quadrokopera ); 8 QP.g= nparam (9.8, 9.78, 9.85, m/s^2,... Przyspieszenie ziemskie ); % (...)

6.2. Model 53 6.2. Model Model zaimplemenowano z wykorzysaniem środowiska Simulink będącego częścią pakieu numerycznego Malab. Środowisko o umożliwia szybką i efekywną realizację zadań z zakresu Teorii serowania. Implemenacja modeli i algorymów serowania odbywa się z wykorzysaniem inuicyjnych narzędzi graficznych. Efekem prac, oprócz poprawnie działających modeli i serowników umożliwiających przeprowadzanie badań, jes przejrzysa srukura całego układu serowania przedsawiająca różne bloki funkcjonalne oraz przepływ informacji. Efekem realizacji zadania jes blok quadcoper przedsawiony na rysunku 6.2. Reprezenuje w w U w OMEGA quadcoper Rysunek 6.2. Blok programu Simulink z zaimplemenowanym modelem obiek serowania z wekorem serowania na wejściu i zmiennymi sanu oraz jedną ze zmiennych wejściowych modelu wyliczaną na podsawie wekora serowania U (zmienna wymagana przez niekóre algorymy serowania) na wyjściu. Opis wejść i wyjść przedsawiono w podrozdziale 6.2.. Wykorzysanie modelu wymaga wcześniejszej inicjalizacji kilku zmiennych, kórych opis przedsawiono w podrozdziale 6.2.2. Umożliwiają one usawienie odpowiednich paramerów fizycznych modelu i warości począkowych symulacji oraz wybór ypu serowania (serowanie U i omega, podrozdział 6.2.). Implemenację modelu oparo na przedsawionym w pierwszej części publikacji równaniu dynamiki quadroora (2.32) opisującego w wymaganym sopniu zachowanie rzeczywisego obieku. Model zawiera opis efeków przedsawionych w rozdziale 3. i opisanych równaniami (3.7) i (3.8). Opisują one efeky żyroskopowe ramy quadroora oraz łopa śmigieł. Paramery fizyczne modelu wczyywane są poprzez srukurę QP param, kóra musi zosać zainicjowana odpowiednimi warościami przed uruchomieniem symulacji. Tabela 6.2 zawiera warości oraz opis paramerów fizycznych zasosowanych domyślnie podczas symulacji w niniejszej publikacji. W podrozdziale 6.2.3 przedsawiono przykład uruchomienia symulacji z wykorzysaniem bloku quadcoper wraz z przykładowym zesawem paramerów wczyanych przy pomocy skrypu 6.4. W podrozdziale 6.2.4 przedsawiono szczegóły implemenacji. Tabela 6.2. Przykładowe paramery fizyczne modelu Nazwa Warość Opis m.686 masa [kg] g 9.8 g [ m ] S 2 Ix.75 body ineria [ kg Iy.75 body ineria [ kg Iz.4 body ineria [ kg ] m 2 ] m 2 ] m 2 ] m 2 Jr. body ineria [ kg l.35 lever [m] b 2.7 hrus facor d.35 drag facor isuconrol rue wybór serowania ( - omega, - U )

54 6. Realizacja w środowisku MATLAB 6.2.. Opis wejść/wyjść Wejściem modelu (U) jes czeroelemenowy wekor serowań. W zależności od usawienia parameru QP.isUconrol (6.2.2) model serowany jes wekorem prędkości obroowych silników [Ω, Ω 2, Ω 3, Ω 4 ] (serowanie omega ) lub wekorem serowań [U, U 2, U 3, U 4 ] (serowanie U ). W zależności od wyboru serowania wekory wejściowe są zadawane wpros ( U ) lub w przypadku serowania omega warości są przeliczane na wekor [U, U 2, U 3, U 4 ] przy pomocy równania (2.3). Warość Ω w zależności od serowania jes wyliczana jako suma prędkości obroowych silników z odpowiednimi znakami (2.3) lub wyliczana na podsawie wekora serowań U przy pomocy równania (2.3). Wyjściem modelu są sześcioelemenowe wekory w, w oraz w reprezenujące zmienne sanu (x, y, z, φ, θ, ψ) oraz ich prędkości i przyspieszenia. Dodakowo wyprowadzono zmienną OMEGA reprezenującą zmienną Ω z modelu dynamiki (2.32). 6.2.2. Inicjalizacja Uruchomienie modelu wymaga wcześniejszej inicjalizacji paramerów począkowych pobieranych z przesrzeni roboczej Malaba. W ym celu należy zainicjować nasępujące zmienne: W - wekor [x6] - położenia począkowe zmiennych sanu, Wp - wekor [x6] - prędkości począkowe zmiennych sanu, paramsruc - srukura paramerów (Tab. 6.2). 6.2.3. Przykład uruchomienia Wydruk 6.3 przedsawia przykładowy skryp realizujący przygoowanie i uruchomienie symulacji z modelem. Obiek umieszczono w pliku obiekuni.mdl przedsawionym na rysunku 6.3. simu U U w w w OMEGA quadcoper w w wp wp wb wb omega omega Uou Uou Rysunek 6.3. Model obiekuni.mdl Inicjalizację srukury QP param przedsawiono na wydruku 6.4. QP_param ; 2 PP_param ; % === ini ===================== 4 W=PP. W; Wp =PP. Wp ; 6 % === paramsruc ============== paramsruc.m = QP.m. val ; 8 paramsruc.g = QP.g. val ; paramsruc. Ix = QP.Ix.val ; paramsruc. Iy = QP. Iy. val ; paramsruc. Iz = QP.Iz.val ; 2 paramsruc. Jr = QP. J. val ; paramsruc.l = QP.l. val ; 4 paramsruc.b = QP.b. val ; paramsruc.d = QP.d. val ; Wydruk 6.3. Przykład inicjalizacji i uruchomienia modelu

6.2. Model 55 6 paramsruc. isuconrol = QP. isuconrol. val ; % === parambus ================= 8 clear slbus *; businfo = Simulink. Bus. creaeobjec ( paramsruc ); 2 % === sim ====================== = sim ( obiekuni, ); Wydruk 6.4. QP param % %% Quadroor parameers % 3 % Source : " Design and Conrol of an Indoor Micro Quadroor " (325. pdf ) 5 % nparam ( val, min_val, max_val, uni_sym, desc ) QP. isuconrol = nparam ( rue, false, rue,,... 7 Przelacznik miedzy sposobem serowania wirnikami ( omega ) a (U) ); 9 QP.m = nparam (.686,,, kg, Masa quadrokopera ); QP. l = nparam (.35,,, m, Dlugosc ramienia quadrokopera ); QP.b = nparam (2.7,,,,... Wspolczynnik obciazenia ( hrus facor ) ); 3 QP.d = nparam (.35,,,,... Wspolczynnik oporu areodynamicznego ( drag facor ) ); 5 QP.g = nparam (9.8, 9.78, 9.85, m/s ^2, Przyspieszenie ziemskie ); 7 QP. Ix = nparam (.75,,, kg* m^2,... Momen bezwladnosci wzdloz osi x quadrokopera ); 9 QP. Iy = nparam (.75,,, kg* m^2,... Momen bezwladnosci wzdloz osi y quadrokopera ); 2 QP.Iz = nparam (.4,,, kg*m ^2,... Momen bezwladnosci wzdloz osi z quadrokopera ); 23 QP.J = nparam (.,,, kg*m ^2,... Momen bezwladnosci wirnika quadrokopera ); 25 QP.x = nparam (, -,, m, Poczakowe polozenie x ); 27 QP.y = nparam (, -,, m, Poczakowe polozenie y ); QP.z = nparam (, -,, m, Poczakowe polozenie z ); 29 QP. xprim = nparam (, -,, m/ s,... 3 Poczakowa predkosc liniowa x ); QP. yprim = nparam (, -,, m/ s,... 33 Poczakowa predkosc liniowa y ); QP. zprim = nparam (, -,, m/ s,... 35 Poczakowa predkosc liniowa z ); 37 QP. phi = nparam (,, 2*pi, rad,... Poczakowy ka kolysania ( roll ) phi ); 39 QP. hea = nparam (,, 2*pi, rad,... Poczakowy ka kiwania ( pich ) hea ); 4 QP. psi = nparam (,, 2*pi, rad,... Poczakowy ka myszkowania ( yaw ) psi ); 43 QP. phiprim = nparam (,, 2*pi, rad /s,... 45 Poczakowa predkosc kolysania ( roll ) phiprim ); QP. heaprim = nparam (,, 2*pi, rad /s,... 47 Poczakowa predkosc kiwania ( pich ) heaprim ); QP. psiprim = nparam (,, 2*pi, rad /s,... 49 Poczakowa predkosc myszkowania ( yaw ) psiprim ); 5 assignin ( base, QP,QP ); 6.2.4. Implemenacja Wydruk 6.5. prepinpu funcion [u, OMEGA ] = prepinpu ( UW, isu, b, d) 2 % prepinpu % 4 % param : % - UW = [U, U2, U3, U4 ] lub [ omega, omega2, omega3, omega4 ]

U u w u 56 6. Realizacja w środowisku MATLAB 6 % - isu - rue / false % - b - scalar param 8 % - d - scalar param u = zeros (4,); OMEGA = ; 2 A = [ /(4* b),, -/(2* b), -/(4* d); 4 /(4* b), -/(2* b),, /(4* d); /(4* b),, /(2* b), -/(4* d); 6 /(4* b), /(2* b),, /(4* d )]; 8 if isu == u () = b * ( UW ()^2 + UW (2)^2 + UW (3)^2 + UW (4)^2); 2 u (2) = b * ( UW (4)^2 - UW (2)^2); u (3) = b * (UW (3)^2 - UW ()^2); 22 u (4) = d * ( UW (2)^2 + UW (4)^2 - UW ()^2 - UW (3)^2); OMEGA = UW (2) + UW (4) - UW () - UW (3); 24 else u = UW; 26 om = A* UW; om = sqr (om ); 28 OMEGA = om (2) + om (4) - om () - om (3); end paramsruc paramsruc W w Wp w <isuconrol> <b> <d> UW isu b d prepinpu prepinpu OMEGA u OMEGA w w param quadcoper model w w w 2 w 3 w 4 OMEGA Rysunek 6.4. Wnęrze bloku quadcoper 3 w 4 w 2 OMEGA 5 param w w w w w inegraor w wp u OMEGA param quadcoper quadcoperfcn d2wd w 2 w 3 w Rysunek 6.5. Wnęrze bloku quadcoper model Wydruk 6.6. quadcoperfcn funcion d2wd = quadcoper (w, wp, u, OMEGA, param ) % quadcoper 3 % % w = [x, y, z, phi, hea, psi ] 5 % wp = [ xprim, yprim, zprim, phiprim, heaprim, psiprim ] % u = [U, U2, U3, U4 ]

6.2. Model 57 7 % OMEGA = scalar param % param = paramsruc 9 % x = w (); xprim = wp (); y = w (2); yprim = wp (2); 3 z = w (3); zprim = wp (3); phi = w (4); phiprim = wp (4); 5 hea = w (5); heaprim = wp (5); psi = w (6); psiprim = wp (6); 7 U = u (); U2 = u (2); U3 = u (3); U4 = u (4); 9 omega = OMEGA ; 2 m = param. m; g = param.g; 23 Ix = param. Ix; Iy = param. Iy; 25 Iz = param. Iz; Jr = param. Jr; 27 l = param. l; 29 dwd = zeros (6,); d2wd = zeros (6,); 3 dwd () = xprim ; 33 d2wd () = ( cos ( phi ) * sin ( hea ) * cos ( psi ) +... 35 sin ( phi ) * sin ( psi )) * (/ m) * U; 37 dwd (2) = yprim ; 39 d2wd (2) = ( cos ( phi ) * sin ( hea ) * sin ( psi ) -... sin ( phi ) * cos ( psi )) * (/ m) * U; 4 43 45 dwd (3) = zprim ; d2wd (3) = -g + ( cos ( phi ) * cos ( hea )) * (/ m) * U; dwd (4) = phiprim ; 47 d2wd (4) = heaprim * psiprim * (( Iy -Iz )/ Ix) -... 49 ( Jr / Ix) * heaprim * omega +... (l/ix ) * U2 ; 5 dwd (5) = heaprim ; 53 d2wd (5) = phiprim * psiprim * (( Iz - Ix )/ Iy) +... 55 ( Jr / Iy) * phiprim * omega +... (l/iy ) * U3 ; 57 dwd (6) = psiprim ; 59 d2wd (6) = phiprim * heaprim * (( Ix - Iy )/ Iz) +... 6 (/ Iz) * U4;

58 6. Realizacja w środowisku MATLAB 3 w w 2 w x o s x x o s y x o s z x o s phi x o s hea x o s psi xp yp zp phip heap psip x o s x x o s y x o s z x o s phi x o s hea x o s psi x y z phi hea psi 2 w w Rysunek 6.6. Wnęrze bloku inegraor

6.3. Implemenacja serownika PID 59 6.3. Implemenacja serownika PID Zaimplemenowany serownik reguluje czery zmienne - wysokość z oraz kąy φ, θ i ψ. Jesgo srukura zakłada pobieranie przez niego akualnych warości położenia i prędkości wszyskich sześciu współrzędnych z modułu modelu quadroora. Są one zapisywane w wekorze ω i ω. Analogicznie, z modułu generującego rajekorię regulaor poprzez wekory ω d ω d pobiera zadawane przez użykownika położenia i prędkości. Dodakowo, regulaor odczyuje poprzez wekor OME- GA prędkości obroowe śmigieł Ω i. Efekem działania bloku jes wygenerowanie serowanie dla quadroora w wekorze U. Wygląd sworzonego bloku w środowisku Malab Simulink pokazuje rysunek 6.7. Rysunek 6.7. Posać bloku sworzonego regulaora PID Wekor OMEGA i ω d służą do kompensacji efeku żyroskopowego. Zaimplemenowany człon spowalnia działanie symulacji, ale umożliwia dokładniejsze odwzorowanie rzeczywisego obieku. Blok en przedsawiono na rysunku 6.8. Rysunek 6.8. Człon kompensujący efek żyroskopowy Różnicą warości wekorów ω d i ω jes uchyb. Trafia on do bloków paramerów poszczególnych części regulaora PID. Serownik wykorzysuje ich 2. Odpowiadają one za warości wzmocnień części proporcjonalnych K p, całkujących K i i różniczkujących K d dla każdej współrzędnej. Na wejście członów różniczkujących podawana jes pochodna zmiennej w celu uniknięcia gwałownych zmian zmiennych. Dla zapewnienia większej dokładności wszyskie współrzędne kompensowane są przez odpowiednie sałe fizyczne (momeny bezwładności I xx, I yy, I zz i odległość od środka ciężkości ll) wynikające ze wzorów. Regulacja wysokości quadroora dodakowo uwzględnia wpływ grawiacji oraz zmian kąów θ i φ. Model serownika w środowisku Malab Simulink przedsawia rysunek 6.9.

6 6. Realizacja w środowisku MATLAB Rysunek 6.9. Zrealizowany regulaor PID

6.4. Algorym całkowania wsecznego 6 6.4. Algorym całkowania wsecznego W ej części przedsawiona zosanie implemenacja serownika wykorzysującego całkowanie wseczne (opisanego w 4.2.3) w środowisku MATLAB/Simulink. Podczas realizacji serowania pojawiły się pewne problemy, o kórych nie wspominano w lieraurze, a kóre są isone z punku widzenia implemenacji. Zosaną one uwzględnione w kolejnych paragrafach. Poszczególne elemeny serownika zamodelowane zosały w posaci zagnieżdżonych podsysemów. Rysunek 6. pokazuje blok serownika podłączony do generaora rajekorii zadanej oraz modelu. Rysunek 6.. Podłączenie serownika do układu Obliczanie wekora sanu quadroora q, jego pochodnych q i q oraz serowania zamknięe jes w pęli algebraicznej 3. Z ego eż powodu konieczne sało się dodanie do sysemu opóźnienia. Aby model działał dla różnych meod całkowania (zarówno sało- jak zmiennokrokowych), zdecydowano się na wykorzysanie bloków pamięci (odczy, zapis i rezerwacja przesrzeni na dane), w kórych można usawić zmienny czas próbkowania (w przeciwieńswie do popularniejszego bloku opóźnienia). Rysunek 6. pokazuje wnęrze bloku serownika. Widoczna jes na nim modułowa budowa pokazana wcześniej na rysunku 4.. Bloki reprezenujące serownik położenia i kąów posiion conrol i orienaion conrol zbierają sygnały im sygnnały, by przekazać je do funkcji wyliczających warości zadawane U zaimplemenowanych w MATLAB Funcion Blok. Serownik położenia pokazany jes na rysunku 6.2, zaś funkcja wyliczająca serowanie U i kąy φ d oraz θ d na wydruku 6.7. W rakcie esowania sysemu okazało się, że konieczne jes nałożenie na en serownik dodakowych ograniczeń, ak aby żaden z zadawanych kąów φ i θ nie przekroczył warości bezwzględnej π/4. Ograniczenia e są zgodne z inuicją przechylenie quadroora o większy ką zapocząkowałoby pełny obró wokół osi X lub Y. Takie zachowanie nie jes uwzględnione ani w modelu serownika, ani samego quadroora, szybko prowadzi więc do desabilizacji sysemu oraz dziwnych zachowań sysemu (np. serownik zadaje nierealizowalne serowania, definiujące zespolone prędkości obroowe śmigieł). Ze względu na możliwy błąd śledzenia rajekorii, a więc możliwość uzyskania większych kąów niż zadawane, wprowadzony zosał margines bezpieczeńswa i warość bezwzględna kąów zosała ograniczona do π/5. 3 ang. algebraic loop

62 6. Realizacja w środowisku MATLAB Rysunek 6.. Ogólny model serownika Rysunek 6.2. Serownik położenia

6.4. Algorym całkowania wsecznego 63 Wydruk 6.7. Funkcja serpos funcion [ U, phi_d, hea_d ] = serpos ( alpha, w, wp, w_d, w_dp, m, g) 3 % serpos % 5 % param : % w = [x, y, z, phi, hea, psi ] 7 % wp = [ xprim, yprim, zprim, phiprim, heaprim, psiprim ] % - m - quadrocoper mas ( scalar ) 9 % - BSP - backsepping algorihm parameers % - w, wp - rajecory and is derivaive % - w_d, w_dp - given rajecory and is derivaive 3 x = w (); xprim = wp (); y = w (2); yprim = wp (2); 5 z = w (3); zprim = wp (3); phi = w (4); 7 hea = w (5); psi = w (6); 9 x_d = w_d (); x_dprim = w_dp (); y_d = w_d (2); y_dprim = w_dp (2); 2 z_d = w_d (3); z_dprim = w_dp (3); 23 e_bs7 = z_d - z; e_bs8 = zprim - z_dprim - alpha (7) * e_bs7 ; 25 e_bs9 = x_d - x; e_bs = xprim - x_dprim - alpha (9) * e_bs9 ; 27 e_bs = y_d - y; e_bs2 = yprim - y_dprim - alpha () * e_bs ; 29 3 33 35 U = ( e_bs7 +g- alpha (7)*( e_bs8 + alpha (7)* e_bs7 )- alpha (8)* e_bs8 )* m / ( cos ( phi )* cos ( hea )); ux = ( e_bs9 - alpha (9)*( e_bs + alpha (9)* e_bs9 )- alpha ()* e_bs )* m / U; uy = ( e_bs - alpha ()*( e_bs2 + alpha ()* e_bs )- alpha (2)* e_bs2 )* m / U; s = ux*sin ( psi )+ uy*cos ( psi ); 37 % angle bounds 39 if s > sin ( pi /5) phi_d = - pi /5; 4 elseif s < sin (- pi /5) phi_d = pi /5; 43 else phi_d = -asin (s ); 45 end 47 s3 = (ux*cos ( psi )+ uy*sin ( psi )) / cos ( phi_d ); 49 % angle bounds if s3 > sin (pi /5) 5 hea_d = - pi /5; elseif s3 < sin (-pi /5) 53 hea_d = pi /5; else 55 hea_d = asin ( s3 ); end Serownik orienacji pokazany jes na rysunku 6.3, funkcja wyliczająca serowania U 2, U 3 i U 4 na wydruku 6.8. W przypadku serowania orienacją wsawione zosały dodakowe bloki na wejściach serownika. Wyliczenie błędów rajekorii zadanych równaniami (4.25) i (4.3) wymaga znajomości pochodnych kąów φ i θ, a serownik położenia ich nie dosarcza. Konieczne okazało się wykorzysanie bloków pochodnej. Zazwyczaj unika się różniczkowania numerycznego, ze względu na duże błędy w obliczeniach, jednak w przypadku quadroora wspomniane kąy są wolnozmienne i ich różniczkowanie nie sanowi problemu. Wydruk 6.8. Funkcja serorien funcion [ U2, U3, U4 ] = serorien ( quadp, alpha, OMEGA, w, wp, phi_d, phi_dp, hea_d, hea_dp, psi_d, psi 2 % serorien %

64 6. Realizacja w środowisku MATLAB Rysunek 6.3. Serownik orienacji 4 % param : % - quadp - quadrocoper parameers srucure 6 % - BSP - backsepping algorihm parameers % - OMEGA - scalar 8 % - w, wp - rajecory and is derivaive % - psi_d,..., phi_dp - given rajecory and is derivaive phi = w (4); phip = wp (4); 2 hea = w (5); heap = wp (5); psi = w (6); psip = wp (6); 4 a = ( quadp.iy - quadp.iz) / quadp.ix; 6 a2 = - quadp. Jr / quadp. Ix ; a3 = ( quadp.iz - quadp.ix) / quadp.iy; 8 a4 = quadp. Jr / quadp. Iy ; a5 = ( quadp.ix - quadp.iy) / quadp.iz; 2 b = quadp.l / quadp.ix; 22 b2 = quadp. l / quadp. Iy; b3 = quadp.l / quadp.iz; 24 26 e_bs = phi_d - phi ; e_bs2 = phip - phi_dp - alpha () * e_bs ; 28 e_bs3 = hea_d - hea ; e_bs4 = heap - hea_dp - alpha (3) * e_bs3 ; 3 e_bs5 = psi_d - psi ; e_bs6 = psip - psi_dp - alpha (5) * e_bs5 ; 32 U2 = ( e_bs -a* heap *psip -a2* heap * OMEGA -... 34 alpha ()*( e_bs2 + alpha ()* e_bs )- alpha (2)* e_bs2 ) / b; 36 U3 = ( e_bs3 -a3* phip *psip -a4* phip * OMEGA -... alpha (3)*( e_bs4 + alpha (3)* e_bs3 )- alpha (4)* e_bs4 ) / b2; 38 U4 = ( e_bs5 -a5* heap *phip - alpha (5)*( e_bs6 + alpha (5)* e_bs5 )- alpha (6)* e_bs6 ) / b3; 6.4.. Inicjalizacja W celu uruchomienia serownika (poza dosarczonym GUI) należy: zainicjalizować model quadroora,

6.4. Algorym całkowania wsecznego 65 uworzyć wekor paramerów serownika BSparam z wybranymi nasawami serownika, usawić wybraną rajekorię w generaorze. Uworzenie paramerów może się odbyć za pomocą przygoowanych skrypów: BackSeppingParam.m worzącego srukurę BSP z opisem paramerów serownika oraz BackSep.m inicjalizującego wekor BSparam warościami zadanymi w BSP, co pokazano na wydrukach 6.9 i 6.. Wydruk 6.9. Uworzenie srukury BSP % %% Backsepping algorihm parameers % 3 % Source : S. Bouabdallah, R. Siegwar \ % " Backsepping and Sliding - mode Techniques Applied 5 % o an Indoor Micro Quadroor " 7 % nparam ( val, min_val, max_val, uni_sym, desc ) BSP. alpha = nparam (,, Inf,,... 9. paramer sabilizujacy ka fi ); BSP. alpha2 = nparam (,, Inf,,... 2. paramer sabilizujacy ka fi ); BSP. alpha3 = nparam (,, Inf,,... 3. paramer sabilizujacy ka ea ); BSP. alpha4 = nparam (,, Inf,,... 5 2. paramer sabilizujacy ka ea ); BSP. alpha5 = nparam (,, Inf,,... 7. paramer sabilizujacy ka psi ); BSP. alpha6 = nparam (,, Inf,,... 9 2. paramer sabilizujacy ka psi ); BSP. alpha7 = nparam (3.,, Inf,,... 2. paramer sabilizujacy wsp. z ); BSP. alpha8 = nparam (3.4,, Inf,,... 23 2. paramer sabilizujacy wsp. z ); BSP. alpha9 = nparam (,, Inf,,... 25. paramer sabilizujacy wsp. x ); BSP. alpha = nparam (,, Inf,,... 27 2. paramer sabilizujacy wsp. x ); BSP. alpha = nparam (2,, Inf,,... 29. paramer sabilizujacy wsp. y ); BSP. alpha2 = nparam (,, Inf,,... 3 2. paramer sabilizujacy wsp. y ); BSparam = zeros (,2); 3 BSparam () = BSP. alpha. val ; BSparam (2) = BSP. alpha2. val ; 5 BSparam (3) = BSP. alpha3. val ; BSparam (4) = BSP. alpha4. val ; 7 BSparam (5) = BSP. alpha5. val ; BSparam (6) = BSP. alpha6. val ; 9 BSparam (7) = BSP. alpha7. val ; BSparam (8) = BSP. alpha8. val ; BSparam (9) = BSP. alpha9. val ; BSparam () = BSP. alpha. val ; 3 BSparam () = BSP. alpha. val ; BSparam (2) = BSP. alpha2. val ; 5 paramsruc. isuconrol = rue ; Wydruk 6.. Inicjalizacja wekora paramerów

66 6. Realizacja w środowisku MATLAB 6.5. Implemenacja algorymu H dla quadroora Niech dany będzie model dynamiki quadroora zgodny z (2.32), na podsawie kórego dokonano implemenacji badanego obieku w pakiecie Malab/Simulink. Jak zosało wspomniane wcześniej aby umożliwić implemenację algorymu H model quadroora musi zosać przedsawiony w posaci układu zapisanego przy użyciu równań sanu, w kórych macierze współczynników są funkcjami zewnęrznych zmiennych szedulujących α, α. Zgodnie z [9] reprezenacja układu macierzowego akiego układu wygląda nasępująco: ẋ [ ] A(α, α) B (α, α) B 2 (α, α) [ ] [ ] x ż = C (α, α) D (α, α) D 2 (α, α) v, (6.) ẏ C 2 (α, α) D 2 (α, α) D 22 (α, α) u gdzie x jes wekorem sanu, u wejścia, a z sygnału błędu, y oznacza mierzalne wyjściem, v zakłócenia. UWAGA: noacja zasosowana w niniejszym rozdziale zgodna jes z [9], aby uławić czyelnikowi porównanie zaprezenowanego rozwiązania z lieraurą i może momenami powielać wcześniej wprowadzone oznaczenia należy je zaem od siebie odróżniać. Jeśli dla modelu (6.) dobrze dobierze się zmienne α i α sysem będzie charakeryzował się odpornością sabilności w zamknięej pęli sprzężenia zwronego. Jednakże ak przedsawiony model nie jes akcepowalny przez problem serowania quadraorem. Należy przekszałcić go do quasi-modelu podającego formę, kóra jes od razu goowa do linearyzacji. α A 2 (α) B (α) α q q eq (α) = A 22 d dα [q eq(α)]a 2 (α) B 2 d dα [q eq(α)]b (α) q q eq (α) (6.2) δ δ eq (α) d dα [q eq(α)]a 2 (α) d dα [q eq(α)]b (α) δ δ eq (α) 6.5.. Model dynamiki quadroora opisany równaniami sanu d d Model (2.32) po przekszałceniu do quasi-modelu (6.2) przyjmuje posać: cos ψ sin θ cos φ+sin ψ sin φ ẋ ẋ ẏ ẏ m cos φ sin θ sin ψ sin φ cos ψ ż m ż cos φ cos θ θ m φ θ l = φ I y U l ψ + I ψ y U 2 q θ q I z U 3 θ U 4 φ φ ψ ψ gdzie kąy KKM [ θ φ ψ] T pełnią rolę zmiennych szedulujących. (6.3) 6.5.2. Archiekura serownika H Zaimplemenowaną archiekurę serownika H dla quadroora ilusruje rysunek 6.4.

6.5. Implemenacja algorymu H dla quadroora 67 serowany pela wewnerzna model_quadroora pela_wewnerzna druga_pela_zewnerzna pierwsza_pela_zewnerzna serowany druga pela zewnerzna serowany pierwsza pela zewnerzna Rysunek 6.4. Archiekura serownika H Linearyzacja modelu Wewnęrzna pęla serownika ma za zadanie zapewnić odporność sabilności układu quadroora przy wznoszeniu się i podczas lou zakładając wysępowanie zewnęrznych zakłóceń w powierzu (np. wiar). Jej drugim zadaniem jes odsprzęgnięcie sysemu, kóry jes sysemem MIMO. Wekor serowań przyjmuje posać: u = [ z y z ψ] T. Aby wewnęrzna pęla serownika zadziałała poprawnie model quadroora musiał zosać zlinearyzowany. Plik quadnh.mdl (rysunek 6.5) zawiera model samego quadroora (bez pęli kszałowania). Nie jes o en sam model co wyprowadzony w (2.32), a kórego implemenacja zosała przedsawiona w rozdziale 6.2. Zasosowany w nim blok fcn do obliczania modelu, powodował wykrywanie powiązania między całkowaniem położeń wejściowych x, y, a wyjściami (położenia, kąy KKM) co do linearyzacji modelu nie było porzebne, a powodowało problemy podczas rozwiązywania podwójnego algebraicznego równania Riccaiego (wydruk 6.). Usunięcie nieużywanych na ym eapie bloków całkujących oraz wyzerowanie warości x i y wraz z ich pochodnymi dalej generowało przy linearyzacji nieprawidłowy model nawe podczas wybrania w opcjach linearyzacji, aby pomijał niewykorzysane bloki. Dopiero sworzenie nowego modelu quadnh.mdl rozwiązało en problem i pozwoliło swobodnie generować serowniki H dla quadroora. Przy budowie modelu H pominięo efek żyroskopowy śmigieł. Wydruk 6.. Wyliczenie równań Riccaiego funcion [K gam ]= hinf2dof (Gs, Tref ) 2 % HINF2DOF synhesizes he H_inf 2- DOF conroller in (9.8) % Uses MATLAB RCAST oolbox 4 % Usage : K= hinf2dof (Gs, Tref ); % INPUTS : Shaped plan Gs and reference model Tref 6 % OUTPUT : Two degrees - of - freedom conroller K % 8 % Coprime facorizaion of Gs [As,Bs,Cs,Ds] = unpck (Gs ); [Ar,Br,Cr,Dr] = unpck ( Tref ); [nr,nr] = size (Ar ); [lr,mr] = size (Dr ); 2 [ns,ns] = size (As ); [ls,ms ] = size (Ds ); Rs = eye (ls )+ Ds*Ds. ; Ss = eye (ms )+ Ds * Ds ; 4 A = (As - Bs*inv (Ss )* Ds * Cs ); R = Cs * inv (Rs )* Cs ; Q = Bs*inv (Ss )* Bs ; 6 % [Z, Z2, fail, reig_min ] = ric_schr ([ A -R ; -Q -A ]); 8 % fail % [Z,Z2,eig, zerr, wellposed, Zs ] = aresolv (A,Q, R ); 2 % Zs = Z2 / Z ; % Al. Robus Conrol oolbox : 22 [Z,Z2,eig,zerr, wellposed,zs] = aresolv (A,Q, R ); % 24 % Choose rho = ( Designer s choice ) and (6.4)

s s s s s s 68 6. Realizacja w środowisku MATLAB In 2 In2 (l/ix) (l/ix) (Iy Iz)/Ix (Iy Iz)/Ix 3 In3 (Iz Ix)/Iy (Iz Ix)/Iy K /m nesum2 Produc (l/ix) (l/ix) Produc2 Inegraor nesum Produc 9.8 Consan Inegraor2 Inegraor3 nesum4 Inegraor4 2 phi Inegraor 3 hea W cos(u) Fcn cos(u) Fcn 4 In4 (Ix Iy)/Iz (Iz Ix)/Iy (l/ix) (l/ix)2 Produc3 nesum3 Inegraor5 4 yaw rae Terminaor Rysunek 6.5. Linearyzowany model quadroora (quadnh.mdl) % build he generalized plan P in (9.87) 26 % 28 rho =; A = blkdiag (As,Ar ); 3 B = [ zeros (ns,mr) (( Bs*Ds )+( Zs*Cs ))* inv ( sqrm (Rs )); Br zeros (nr,ls )]; 32 B2 = [Bs; zeros (nr,ms )]; C = [ zeros (ms,ns+nr ); Cs zeros (ls,nr ); rho *Cs -rho * rho *Cr ]; 34 C2 = [ zeros (mr,ns+nr ); Cs zeros (ls,nr )]; D = [ zeros (ms,mr+ls ); zeros (ls,mr ) sqrm (Rs ); 36 -rho * rho * Dr rho * sqrm (Rs )]; D2 = [ eye (ms ); Ds ; rho *Ds ]; 38 D2 = [ rho * eye (mr ) zeros (mr,ls ); zeros (ls,mr ) sqrm (Rs )]; D22 = [ zeros (mr,ms ); Ds ]; 4 B = [B B2 ]; C = [C;C2 ]; D = [ D D2 ; D2 D22 ]; P = ss(a,b,c, D); 42 % Alernaive : Use sysic o generae P from Figure 9.2 % bu may ge exra saes, since saes from Gs may ener wice. 44 % % Gamma ieraions o obain H - infiniy conroller 46 % [l,m2] = size ( D2 ); [l2,m] = size ( D2 ); 48 nmeas = l2; ncon = m2; gmin = ; gmax = 5; gol =.; 5 % [K, Gnclp, gam ] = hinfsyn (P, nmeas, ncon, gmin, gmax, gol ); % Al. Robus oolbox, use command : hinfop 52 [K, Gnclp, gam ] = shinfop (A,B,C,D, nmeas, ncon, gmax, gmin, gol ); Wydruk 6.2. Linearyzacja modelu i analiza warości dla modelu bez kszałowania pęli (appendixca.m) % Quad Roor UAV 2- DOF H - I n f i n i y Analysis and Synhesis 2 % Designed by Ming Chen % Dae : A p r i l 23 4 % updaed implemenaion and adapaion o he projec 6 % by Naalia Czop & Maciej Gawron

6.5. Implemenacja algorymu H dla quadroora 69 8 % Dae : January 23 % Secion One : Calcuae he H - inf conroller for he inner loop % Sep : Linearize he plan and analysis 2 % he singular values of he unshaped model [x,u] = rim ( quadnh ); 4 [A,B,C,D] = linmod ( quadnh,x,u ); sys = ss (A,B,C, D); 6 sys_f = f ( sys ); G = pck (A,B,C, D); 8 figure (); clf ; 2 hold on; sigma (A,B,C,D,{.,}) 22 ile ( Unshaped Plan Open Loop Singular Values ); grid pause ; Wykresy na rysunku 6.6 przedsawiają porównanie odpowiedzi częsoliwościowej modelu quadroora przed i po filracji dokonanej za pomocą W i W 2 zgodnie ze wskazówkami zawarymi w sekcji 4.3.2 oraz [25] i [9]. Warości dobranych paramerów umieszczone są w wydruku 6.2. Kompensaory pozwoliły skupić częsoliwości krzyżowania serowanych warości w okolicy 5 rad. Niesey w przypadku niższych częsoliwości krzyżowania układ zaczynał być niesabilny. W prekompensaorze W 2 konieczne okazało się dodanie zer umieszczonych w lewej półpłaszczyźnie płaszczyzny liczb zespolonych w częściach odpowiedzialnych za kszałowanie kąów kiwania i kołysania. Pozwoliły one zagwaranować nachylenie zbocza wzmocnienia ych kąów w okolicach częsoliwości krzyżowania na poziomie -. Unshaped Plan Open Loop Singular Values Shaped Plan Open Loop Singular Values wih Referance Model 5 5 Singular Values (db) 5 Singular Values (db) 5 5 5 2 2 3 Frequency (rad/s) 5 2 2 3 Frequency (rad/s) Rysunek 6.6. Analiza warości dla owarej pęli przed i po kszałowaniu Wydruk 6.3. Wybranie funkcji przenoszenia (appendixca.m) % Sep 2: Choose weighs and Tref and analysis he singular % values of he shaped plan sysem marix Gs 3 WA = [.2 ]; w = nd2sys ([7 7], WA ); 5 w3 = nd2sys (8, ); w4 = nd2sys ([.2, 3.6], WA ) ; 7 W = daug (w,w3,w3, w4 ); 9 WB = [ 35]; w2 = nd2sys ([55], WB ); w23 = nd2sys (75*[ 2.3], WB ); W2 = daug (w2,w23,w23, w2 ); 3 [Aw,Bw,Cw, Dw ] = unpck ( W ); 5 [Aw2,Bw2,Cw2, Dw2 ] = unpck (W2 ); [W2A,W2B,W2C, W2D ] = unpck (W ); 7 [W22A,W22B,W22C, W22D ] = unpck (W2 );

7 6. Realizacja w środowisku MATLAB 5 W_r Gain [/.272725 /.5 /.5 /.4387] w w x 3 z 6 y 2 PHI_r 3 Thea_r K Gain x = Ax+Bu y = Cx+Du K3 nesum x = Ax+Bu y = Cx+Du W2 w w U w OMEGA quadcoper wp wp 4 yaw angle 4 R_r Terminaor U 2 Terminaor V K32 x = Ax+Bu y = Cx+Du W22 x = Ax+Bu y = Cx+Du Terminaor Terminaor Rysunek 6.7. Wewnęrzna pęla serowania dla serownika H (quad.mdl) 9 Tref_model = nd2sys (2^2, [ 2*.95* ^2]); Tref_model = nd2sys (3^2,[ 2*.95*3 3^2]); 2 Tref_model2 = nd2sys (2^2,[ 2*.95*3 3^2]); Tref = daug ( Tref_model, Tref_model, Tref_model, Tref_model2 ); 23 Gs = mmul (W2,G,W ); 25 [As,Bs,Cs,Ds] = unpck (Gs ); figure (2); 27 clf ; hold on; 29 sigma ( As, Bs, Cs, Ds, {.,}) sigma ( Tref, {.,}) 3 ile ( Shaped Plan Open Loop Singular Values wih Referance Model ) 33 grid Kolejnym eapem posępowania jes syneza serownika oraz wyznaczenie K = Wydruk 6.4. Syneza serownika i wyznaczenie K (appendixca.m) % Sep 3: Synhesize 2- DOF conroller K and ge gamma = 2.8444 K = hinf2dof ( Gs, Tref ) ; 3 % Sep 4: Seperae K o K l and K2 5 minfo ( K) % sysem : 2 saes 4 oupus 8 inpus K = sel ( K, [ 2 3 4], [ 2 3 4]) ; 7 K2 = sel (K, [ 2 3 4], [5 6 7 8]); [Ak,Bk,Ck, Dk ] = unpck (K ); 9 [Ak2,Bk2,Ck2, Dk2 ] = unpck (K2 ); [K3A,K3B,K3C, K3D ] = unpck (K ); [K32A,K32B,K32C, K32D ] = unpck (K2 ); [K K 2 ]. Zlinearyzowanie modelu pozwala na podłączenie do obieku serowania pęli wewnęrznej. Implemenacja wewnęrznej pęli w pakiecie Malab/Simulink przedsawiona jes na rysunku 6.7. Musi ona spełniać nasępujące wymagania: wszyskie bieguny zamknięej pęli muszą leżeć w lewej półpłaszczyźnie płaszczyzny zespolonej, aby zapewnić sabilność układu; układ musi być w sanie odrzucić szybkie (króko rwające) zakłócenia impulsowe i krokowe. Układ z rysunku 6.7 również należy ] zlinearyzować, nasępnie przeprowadzić synezę serownika i wyznaczyć K = [K K 2. Ponownie konieczne było wyeliminowanie błędów modelu związanych z całkowaniem niewykorzysanych na ym eapie projekowania serownika współrzędnych. Schema całkowania oryginalny i po wprowadzeniu zmian przedsawiono na rysunku 6.8. Wydruk 6.5. Obliczenie paramerów serownika H dla pierwszej pęli zewnęrznej linearyzacja pęli wewnęrznej i wyznaczenie współczynników (appendix2ca.m) % Secion Two : Calcuae he H_inf conroller he firs ouer loop

6.5. Implemenacja algorymu H dla quadroora 7 Rysunek 6.8. Schema różniczkowania dla modelu oryginalnego i H % Sep : Linearize he plan and analysis 3 % he singular values of he unshaped model 5 [x,u] = rim ( quad_work ); [Aoi,Boi,Coi, Doi ] = linmod ( quad_work,x,u, ); 7 syso = ss(aoi,boi,coi, Doi ); sys_fo = f ( syso ) ; 9 Go = pck (Aoi,Boi,Coi, Doi ); figure () 3 clf sigma (Go, {.,}); 5 ile ( Unshaped Plan Open Loop Singular Values of he firs ouer loop ) 7 grid 9 % Sep 2: Choose weigh Wl ( s) and Tref and analysis % he singular values of he shaped plan 2 WA = 5; WB =.5; 23 wo = nd2sys ( WA,); 25 wo2 = nd2sys ( WB,); wo3 = wo2 ; 27 wo4 = wo ; 29 Wo = daug (wo,wo2,wo3, wo4 ); [WA,WB,WC, WD ] = unpck ( Wo ); 3 Tref_modelo = nd2sys (2^2, [ 2**.3.3^2]); 33 Tref_modelo2 = nd2sys (3^2,[ 2**2. 2^2]); Trefo = daug ( Tref_modelo, Tref_modelo, Tref_modelo, Tref_modelo2 ); 35 Gso = mmul (Go, Wo ); 37 figure (2) 39 clf hold on; 4 sigma (Gso, {.,}); sigma ( Trefo, {.,}, r ); 43 ile ( Shaped Plan Open Loop Singular Values of he firs ouer loop wih Referance Model ) 45 grid

72 6. Realizacja w środowisku MATLAB 47 % Sep 3: Synhesize 2- DOF conroller K and ge gamma = 3.656 Ko = hinf2dof ( Gso, Trefo ) ; 49 5 % Sep 4: Reduce Ko and seperae o Kol and Ko2 minfo ( Ko ) % sysem : 64 saes 4 oupus 8 inpus 53 % Reduce Ko o 2 saes, 4 oupus and 8 inpus 55 [ Ko_cf, sigko ] = sncfbal (Ko ); Ko_2 = cf2sys ( hankmr ( Ko_cf, sigko,2, d )); 57 gapko_2 = nugap ( Ko, Ko_2 ) % gap is closed o zero. 59 Ko = sel ( Ko_2, [ 2 3 4], [ 2 3 4]); Ko2 = sel ( Ko_2, [ 2 3 4], [5 6 7 8]); 6 [K2A,K2B,K2C, K2D ] = unpck ( Ko ); 63 [K22A,K22B,K22C, K22D ] = unpck ( Ko2 ); Unshaped Plan Open Loop Singular Values of he firs ouer loop Shaped Plan Open Loop Singular Values of he firs ouer loop wih Referance Model 5 5 Singular Values (db) 5 Singular Values (db) 5 5 5 2 2 2 3 Frequency (rad/s) 2 2 2 3 Frequency (rad/s) Rysunek 6.9. Analiza warości dla pęli wewnęrznej przed i po kszałowaniu Pęle zewnęrzne Kolejne dwie pęle zgodne z archiekurą serownika z rysunku 6.4 przedsawią rysunki 6.2 i 6.2. Posępowanie z nimi jes analogiczne do posępowania z pęlą wewnęrzną najpierw należy dokonać linearyzacji, a nasępnie kszałowania pęli. Jednak zważywszy, że sam model quadroora zosał ukszałowany w pęli wewnęrznej sosowany jes jedynie proporcjonalny prekompensaor W, kórego zadaniem jes ukszałowanie częsoliwości krzyżowania w okolicach 3 rad. Wykresy odpowiedzi częsoliwościowych sysemu z zamknięą wewnęrzną pęlą sprzężenia zwronego przed i po wsępnym kszałowaniu kompensaorem W uzyskane po wykonaniu kodu z wydruku 6.5 widoczne są na rysunku 6.9. Wydruk 6.6. Obliczenie paramerów serownika H dla drugiej pęli zewnęrznej linearyzacja pierwszej pęli zewnęrznej i wyznaczenie współczynników (appendix3ca.m) % Sep : Linearize he plan and analysis % he singular values of he unshaped model 3 [x,u] = rim ( quad22_work ); [A_4,B_4,C_4, D_4 ] = linmod ( quad22_work,x,u); 5 G_4 = pck (A_4,B_4,C_4, D_4 ); Gxy = sel (G_4,[2 3], [2 3]); 7 [Axy,Bxy,Cxy, Dxy ] = unpck ( Gxy ); 9 % Sep 2: Choose Tref Tref_model = nd2sys (2^2,[^2 2** ^2]); Tref = daug ( Tref_model, Tref_model ); 3 figure ();

U V z x y 2 x 3 y z U V z s s Ur z x y 6.5. Implemenacja algorymu H dla quadroora 73 z_r 2 U_r 3 V_r 4 yav angle r Gain [/92.392 /.5294 /.5294 /9.335] K Gain x = Ax+Bu y = Cx+Du K2 nesum x = Ax+Bu y = Cx+Du W U22 w W_r PHI_r Thea_r yaw angle R_r Innerloop and Dragon Flyer III K22 x = Ax+Bu y = Cx+Du 4 yaw angle yaw Rysunek 6.2. Pierwsza zewnęrzna pęla serowania dla serownika H (quad22.mdl) Clock ime o workspace cos(u/) sin(u/)*2 rajekoria referencyjna Gain [/.63547 /.63547] K Gain x = Ax+Bu y = Cx+Du K 6.28*u/25 referencyjny k¹ myszkowania nesum o workspace z_r U_r V_r yav angle r yaw angle quad22_work K2 x = Ax+Bu y = Cx+Du Terminaor Terminaor Rysunek 6.2. Druga zewnęrzna pęla serowania dla serownika H (quad33.mdl) 5 clf ; hold on; 7 sigma (Axy,Bxy,Cxy,Dxy,{.,}) sigma ( Tref_model,{.,}, r ) 9 ile ( Unshaped Plan Open Loop Singular Values wih Referance Model ) grid 2 23 % Sep 3: Synhesize 2- DOF conroller Kxy and ge gamma =3.659 Kxy = hinf2dof ( Gxy, Tref ) ; 25 % Sep 4: Reduce he Kxy and seperae o Kxyl and Kxy2 27 minfo ( Kxy ) % sysem : 2 saes 2 oupus 4 inpu 29 % Reduc Kxy model o 2 - saes, 2 oupus and 4 inpus [ Kxy_cf, sigkxy ] = sncfbal ( Kxy ); 3 Kxy_2 = cf2sys ( hankmr ( Kxy_cf, sigkxy,2, d )); gapkxy_2 = nugap ( Kxy, Kxy_2 ) % gap is closed o zero 33 Kxy = sel ( Kxy_2, [ 2], [ 2]); 35 Kxy2 = sel ( Kxy_2, [ 2 ], [3 4 ] ) ; [KA,KB,KC, KD ] = unpck ( Kxy ); 37 [K2A,K2B,K2C, K2D ] = unpck ( Kxy2 ); Analiza serowania Zarówno pęla wewnęrzna jak i pierwsza pęla zewnęrzna związane są bezpośrednio z algorymem serowania kszałowaniem pęli H.

74 6. Realizacja w środowisku MATLAB.2 W. zadana W. rzeczywisa.2 W. zadana W. rzeczywisa.8.8 ż [m/s].6.4 φ [rad].6.4.2.2.2 5 5 2 czas [s].2 5 5 2 czas [s].2 W. zadana W. rzeczywisa.2 W. zadana W. rzeczywisa.8.8 θ [rad].6.4 ψ [rad/s].6.4.2.2.2 5 5 2 czas [s].2 5 5 2 czas [s] Rysunek 6.22. Odpowiedzi skokowe dla quad.mdl Pęla wewnęrzna odpowiada za serowanie kąów kiwania i kołysania oraz sabilizację odchylenia i prędkości wznoszenia. Pierwsza pęla zewnęrzna zapewnia konrolę pozosałych prędkości (wzdłużnych i poprzecznych) oraz konrolę kąa myszkowania. Druga pęla zewnęrzna zapewnia konrolę położenia. W jej miejscu można dołączyć przykładowo serownik predykcyjny czy serownik PID badając jakość regulacji we współpracy ze serownikiem H. Odpowiedzi skokowe dla każdej pęli (quad.mdl, quad22.mdl, quad33.mdl) przedsawiono na rysunkach 6.22-6.24. Czas symulacji wynosił 2 sekund, skok o warości był zadawany w 5 sekundzie symulacji na wszyskie wejścia danej pęli. W przypadku pęli quad22.mdl i quad33.mdl konieczne okazało się zadanie skoku na ką myszkowania osobno w sosunku do innych skoków. Podczas symulacji, w kórych wszyskie skoki były zadawane jednocześnie, układ desabilizował się położenie x i y zaczynało oscylować w okolicach zadanej warości. Jednak esy z mniejszymi skokami lub lekko opóźnionym skokiem dla kąa myszkowania nie wykazywały podobnych zjawisk. Z uzyskanych odpowiedzi wynika, że quadroor sabilizuje się maksymalnie w ciągu 7 sekund od zadanego wymuszenia na pożądanej warości. W przypadku wcześniejszych pęli quadroor sabilizuje się nawe dwukronie szybciej po około 3 sekundach. Jednocześnie odpowiedzi skokowe nie przekraczają zadanej warości odpowiedź łagodnie dochodzi do warości wymuszenia.

6.5. Implemenacja algorymu H dla quadroora 75.2 W. zadana W. rzeczywisa.2 W. zadana W. rzeczywisa.8.8 z [m].6.4 ẋ [m/s].6.4.2.2.2 5 5 2 czas [s].2 5 5 2 czas [s].2 W. zadana W. rzeczywisa.2 W. zadana W. rzeczywisa.8.8 ẏ [m/s].6.4 ψ [rad].6.4.2.2.2 5 5 2 czas [s].2 5 5 2 czas [s] Rysunek 6.23. Odpowiedzi skokowe dla quad22.mdl.2 W. zadana W. rzeczywisa.2 W. zadana W. rzeczywisa.8.8.6.6 x [m].4 y [m].4.2.2.2 5 5 2 czas [s].2 5 5 2 czas [s].2 W. zadana W. rzeczywisa.2 W. zadana W. rzeczywisa.8.8 z [m].6.4 ψ [rad].6.4.2.2.2 5 5 2 czas [s].2 5 5 2 czas [s] Rysunek 6.24. Odpowiedzi skokowe dla quad33.mdl

76 6. Realizacja w środowisku MATLAB 6.6. Inerfejs użykownika 6.6.. Wprowadzenie Nowoczesny użykownik wymaga od współpracy z dowolnym środowiskiem prosoy i inuicyjności w jego obsłudze, ak aby w najszybszy możliwy sposób mógł uzyskać wymagane przez siebie wyniki. Serowanie procesami zachodzącymi w środowisku MATLAB poprzez linię poleceń ma wiele zale: jes o elasyczny sposób inerakcji z programem, dający użykownikowi największe możliwości. Wymaga jednak nauczenia się synakyki i semanyki języka skrypowego, przy czym ich znajomość nie gwaranuje uniknięcia błędów wynikających chociażby z niepoprawnego wprowadzenia danych czy eż nazwy polecenia. Nauralnie rzuuje o na powarzalność orzymywanych wyników. W celu wyeliminowania podobnych problemów zdecydowano się sworzyć graficzny inerfejs użykownika (Graphical User Ineface - GUI) pozwalający na obsługę opisanych w niniejszej pracy funkcjonalności w sposób całkowicie powarzalny i niezawodny. 6.6.2. Srukura GUI Przy zaproponowanym podejściu całe GUI mieści się w jednym m-pliku. Można go podzielić na rzy wyraźne części:. Kod wykonywany podczas uruchomienia GUI 2. Deklaracje wszyskich obieków widniejących w GUI 3. Wywołania zwrone Obecnie funkcjonalności GUI pozwalają na przeglądanie i modyfikację paramerów modelu oraz serowań. Możliwe jes eż wyświelenie rajekorii quadroora. Kod wykonywany podczas uruchomienia GUI Głównym celem ej części kodu jes wsępne przygoowanie danych w aki sposób, by mogły zosać wyświelone. W ym celu niezbędne jes przejście ze srukur używanych przez grupy projekowe do ablic komórek. Proces en, dla srukury QP zawierającej paramery modelu, realizuje poniższy fragmen kodu: cqp = sruc2cell (QP ); 2 x= size ( cqp ); N=x (); 4 for i =: N 6 fqp {i }= sruc2cell ( cqp {i }); val (i )= fqp {i }(); 8 min_val (i)= fqp {i }(2); max_val (i)= fqp {i }(3); uni_sym (i )= fqp {i }(4); desc (i)= fqp {i }(5); 2 end Wydruk 6.7. Konwersja srukury QP W celu konwersji srukur do ablic komórek użyo polecenia sruc2cell. Proces realizowany jes w pęli zależnej od ilości elemenów srukury. Dzięki emu dodanie nowego parameru w kórejkolwiek ze srukur nie pociąga za sobą porzeby zmian w GUI. Poszczególne elemeny srukury przypisywane są do ablic, kóre saną się kolumnami ablicy. Po konwersji, kolumny są łączone w jedną całość za pomocą polecenia ca. Nasępną czynnością jes usalenie nazw kolumn, kóre wyświelane będą w GUI, usawienie formau danych, kóre znajdować się będą w poszczególnych kolumnach oraz zaznaczenie kóre z nich będą mogły być edyowalne. Wszyskie e funkcje realizuje się przez sworzenie odpowiednich wekorów, kóre zosaną nasępnie przekazane do odpowiedniego obieku GUI. W przypadku srukury QP wekory e przedsawiają się nasępująco:

6.6. Inerfejs użykownika 77 Wydruk 6.8. Formaowanie ablic columnname = { Val, Min_Val, Max_Val, Uni_sym, Opis }; 2 columnforma = { numeric, numeric, numeric, char, char }; columnediable = [ rue false false false false ]; Proces en powarzany jes dla wszyskich srukur, kóre mają zosać wyświelone w abelach. Deklaracje obieków Deklaracje obieków rozpoczynamy od sworzenia ła dla pozosałych obieków. Jes nim zwykły obiek figure. Przy worzeniu możemy usalić wymiary oraz własności ego obieku, w szczególności możemy wyłączyć możliwość zmiany jego rozmiaru przez użykownika. Wszyskie obieky GUI worzone są jako elemeny jednej srukury, kóra jes poem wykorzysywana w wywołaniach zwronych. Deklaracja ła odbywa się w nasępujący sposób: S.fh = figure ( posiion,[ 7],... menubar, none,... 3 name, GUI,... numberile, off,... 5 resize, off ); Wydruk 6.9. Deklaracja ła Na ym eapie usalamy wszyskie paramery worzonego obieku, akie jak jego pozycja, nazwa, ip. Na sworzonej w en sposób palecie umieszczane są nasępnie wszyskie pozosałe obieky. W ym przypadku są o uiable, worzący ablicę oraz uiconrol odpowiadający za wszelkiego rodzaju przyciski oraz pola eksowe. W ym miejscu obiekom uiable przypisywane są eż opisane w poprzedniej sekcji srukury danych w sposób przedsawiony poniżej. Wydruk 6.2. Deklaracja ablicy S. = uiable ( Unis, normalized, Posiion,... [.33.56.67.3], Daa, da,... 3 ColumnName, columnname,... ColumnForma, columnforma,... 5 ColumnEdiable, columnediable,... ColumnWidh, { auo auo auo auo 35},... 7 RowName,[]); Jedną z najważniejszych czynności dokonywanych w ej części kodu (z punku widzenia funkcjonalności aplikacji) jes przypisanie poszczególnym przyciskom funkcji, kóre zosaną wykonane po ich przyciśnięciu. Poniżej znajduje się przykład kodu realizującego o zadanie dla przycisku Symuluj. S. pb6 = uiconrol ( syle, push,... unis, pix,... 3 posiion,[85 33 8 4],... fonsize,4,... 5 sring, Symuluj,... callback,{ @symuluj,s }); Wydruk 6.2. Deklaracja przycisku W paramerach obieku usawiamy własność push (sworzony obiek będzie ypowym przyciskiem), nazwę przycisku i na końcu w sekcji callback podajemy nazwę funkcji, kóra ma zosać wywołana po jego naciśnięciu wraz z paramerami, kóre mają zosać do niej przekazane (w ym przypadku jedynym paramerem jes srukura S w kórej zapisane są wszyskie obieky graficzne przynależące do GUI, kóre zosały zadeklarowane do ego momenu).

78 6. Realizacja w środowisku MATLAB Wywołania zwrone Aby zapewnić poprawne działanie inerfejsu należy wszyskim elemenom GUI, kóre mają spełniać konkrene zadania (przyciski, checkbox y, ip.) przypisać odpowiednie funkcje. Taki mechanizm nazywany jes wywołaniem zwronym (callback). Poniżej przedsawiono przykładową funkcję dla przycisku Symuluj. funcion [] = symuluj ( varargin ) 2 simuuni ; Wydruk 6.22. Deklaracja funkcji Funkcja a uruchamia m-plik odpowiedzialny za poprawne przeprowadzenie symulacji w Simulinku. Należy zauważyć, że jedynym argumenem przyjmowanym przez funkcję jes varargin. Jes o srukura zawierająca wszyskie argumeny przekazane do funkcji podczas eapu worzenia obieku, kóremu jes przypisana. W szczególności srukura zawiera eż uchwy do ego obieku. Ważnym problemem podczas worzenia wywołań zwronych jes przekazywanie srukur danych. Niesey, jeśli podamy dane jako argumen wywołania o zosaną one przekazane przez warość i zmodyfikowane jedynie wewnąrz funkcji - oryginalne dane pozosaną bez zmian. Problem en zosał rozwiązany przez użycie funkcji seappdaa, kóra pozwala na wysłanie danych do przesrzeni powiązanej z konkrenym obiekem. Mogą one zosać nasępnie pobrane w dowolnym momencie za pomocą funkcji geappdaa. Rozwiązanie o pozwala na przekazywanie danych pomiędzy poszczególnymi elemenami GUI, jednak nie jes możliwe ich podglądanie co znacząco urudnia proces worzenia GUI. Planowane jes znalezienie rozwiązania ego problemu. Jednym z możliwych rozwiązań jes zasosowanie funkji assignin oraz evalin, kóre pozwalają na przesyłanie danych pomiędzy obszarami roboczymi. 6.6.3. Uruchamianie GUI Uruchomienie GUI sprowadza się do wpisania w linii komend Malaba polecenia GUI a co za ym idzie do do uruchomienia m-pliku GUI.m, zawierającego wsępną konfigurację porzebną do poprawnego funkcjonowania aplikacji. Po kolei wykonywane są nasępujące czynności:. Oczyszczenie obszaru roboczego oraz zamknięcie nieporzebnych okien 2. Uruchomienie m-plików odpowiedzialnych za wczyanie paramerów modelu 3. Uruchomienie m-plików odpowiedzialnych za wczyanie paramerów każdego z zaimplemenowanych algorymów 4. Uruchomienie symulacji ak, aby użykownik mógł bez wprowadzenia żadnych zmian obserwować zachowanie układu dla domyślnych usawień 5. Uruchomienie m-pliku GUI.m zawierającego właściwą aplikację 6.6.4. Srukura GUI z punku widzenia użykownika Po uruchomieniu oczom użykownika okazuje się okno przedsawione na rys. 6.25. Zgodnie z założeniami, najważniejsze paramery oraz funkcjonalności zgromadzone są w jednym oknie, co zwiększa przejrzysość oraz ławość korzysania z inerfejsu. Można wyróżnić kilka charakerysycznych obszarów okna. Zgromadzone w ych obszarach widżey spełniają podobne funkcje. Paramery modelu oraz algorymu Największą część głównego okna zajmują dwie abele zawierające paramery modelu (górna) oraz obecnie wybranego algorymu (dolna). Każda z abel zawiera nasępujące informacje o danym paramerze: Val - obecna warość parameru

6.6. Inerfejs użykownika 79 Rysunek 6.25. Osaeczny wygląd GUI Min Val, Max Val - warość minimalna oraz maksymalna parameru Uni sym - jednoska w jakiej wyrażony jes paramer Opis - króki opis danego parameru Jedyną modyfikowalną częścią abeli jes kolumna obecnych warości paramerów. Zmiana parameru w abeli nie pociąga za sobą bezpośrednio jego zmiany w przesrzeni roboczej Malaba - po dokonaniu zmian należy je zaakcepować za pomocą przycisku Akualizuj dane. Wygląd ej części inerfejsu dla algorymu całkowania wsecznego przedsawia rys. 6.26 Pasek wyboru algorymu Do wyboru algorymu służy rozwijalna lisa przedsawiona na rys. 6.27. Do wyboru mamy każdy z zaimplemenowanych algorymów (PID, backsepping, H inf ) oraz dodakowo brak algorymu. Zależnie od wybranego usawienia zmieniać się będzie abela z paramerami algorymu. W szczególności abela a zniknie całkowicie w przypadku wyboru osaniej pozycji. Usawienie o pozwala na esowanie samego modelu poprzez zadawanie sałych wejść. Zadajnik sałych wejść W lewym górnym rogu głównego okna GUI znajdują się dwie abele, przedsawione na rys. 6.28. W ych ablicach można wprowadzać sałe serowania w przypadku, gdy przeprowadzamy esy na samym modelu, bez wykorzysania algorymu. W innym przypadku nie są one wykorzysywane. W lewej ablicy znajdują się serowania zdefiniowane za pomocą równania 2.3, naomias w prawej ablicy serowania zdefiniowane za pomocą równania 2.3. Przyciski wyboru wizualizowanych elemenów Przyciski wyboru (checkbox y) znajdujące się w dolnej części okna służą do wyboru wizualizowanych danych. Przedsawia je rys. 6.29. Po zaznaczeniu konkrenego przycisku należy nacisnąć przycisk Wyświel w celu uzyskania zamierzonego efeku. Pięć przycisków powoduje wyświelenie wykresów (dwu- lub rójwymiaro-

8 6. Realizacja w środowisku MATLAB Rysunek 6.26. Paramery modelu i algorymu Rysunek 6.27. Lisa wyboru algorymów Rysunek 6.28. Zadajnik wejść modelu Rysunek 6.29. Przyciski wyboru

6.6. Inerfejs użykownika 8 wych) zawierających kluczowe dla badanego informacje o przebiegu symulacji. Osani przycisk - Wskaźniki jakości - powoduje wyświelenie osobnego okna. Przyciski funkcyjne Najważniejszym elemenem GUI są przyciski funkcyjne przedsawione na rys. 6.3. Rysunek 6.3. Przyciski funkcyjne Przyciski e odpowiadają za poprawne wykonanie najważniejszych funkcji GUI. Są o kolejno: Wczyaj z pliku - powoduje zapisanie srukury GUIOu zawierającej najważniejsze informacje o symulacji do wybranego pliku Zapisz do pliku - powoduje wczyanie srukury GUIOu z wybranego pliku Generuj rajekorię - odpowiada za oworzenie osobnego okna z generaorem rajekorii Akualizuj dane - powoduje akualizacje danych w obszarze roboczym Malaba Symuluj - uruchamia symulację na podsawie danych zapisanych w obszarze roboczym Malaba Zapisz r. wynikową - zapisuje rajekorię wyjściową symulacji jako rajekorię wejściową do nasępnych symulacji Wyświel - powoduje wyświelenie zaznaczonych za pomocą przycisków wyboru danych Wskaźniki jakości Rozwiązaniem proponowanym w celu zbadania jakości regulacji jes wysarowanie symulowanego quadroora z ego samego punku i śledzenie ej samej rajekorii przy użyciu poszczególnych algorymów. Nasępnie możemy obserwować błędy śledzenia rajekorii (e x () = q x () q x d(), e y () = q y () q y d(), id.) i na ich podsawie wnioskować o jakości regulacji. W przypadku zadania śledzenia rajekorii nie jes możliwe zasosowanie wszyskich wskaźników używanych powszechnie w auomayce, akich jak np. czas narasania czy czas regulacji. Można jednak wykorzysać akie wskaźniki jak: Błąd minimalny (najmniejszy z błędów śledzenia rajekorii na przesrzeni całej symulacji) Błąd maksymalny (największy z błędów śledzenia rajekorii na przesrzeni całej symulacji) Błąd średni warości absolunej błędu (średni błąd z całego przebiegu symulacji) Do uzyskania całościowej informacji o procesie regulacji zosaną wykorzysane również poniższe wskaźniki całkowe: Całka z kwadrau uchybu (ISE - Inegral Square Error)

82 6. Realizacja w środowisku MATLAB e() 2 d Większą wagę mają duże błędy. Minimalizowanie ISE prowadzi do szybkiego eliminowania błędów ale olerowane będą małe, długo rwające błędy. Może o powodować powsawanie znaczących oscylacji o niskich częsoliwościach. Całka z modułu uchybu (IAE - Inegral Absolue Error) e() d IAE nie wnosi kar zależnych od rozmiaru błędu. Minimalizowanie IAE prowadzi do wolniejszych odpowiedzi od ISE ale zwykle z mniejszymi oscylacjami. Całka z modułu pomnożonego przez czas (ITAE - Inegral Time Absolue Error) e() d ITAE wprowadza kary za błędy pojawiające się później w czasie. Minimalizowanie ITAE prowadzi do szybkiego usalania odpowiedzi za cenę powolnej reakcji w począkowych eapach. Wskaźniki wyliczone dla przykładowej symulacji z wykorzysaniem algorymu całkowania wsecznego przedsawia rys. 6.3 Rysunek 6.3. Wskaźniki jakości 6.6.5. Generaor rajekorii Zadaniem posawionym przed generaorem rajekorii było poprawne generowanie dyskrenych rajekorii wprowadzanych przez użykownika w sposób nauralny z zachowaniem składni zrozumiałej dla środowiska Malab. W ym celu zaprojekowano okno przedsawione na rys. 6.32. Użykownik może wprowadzić dowolną akcepowaną przez algorym funkcję zmiennej w odpowiednim polu. Zmienne dt oraz Tk odpowiadają za uworzenie wekora czasu. Ma on posać = : dt : T k. W celu odpowiedniego przeworzenia podanej przez użykownika funkcji w środowisku Malab jes ona zapisywana w zmiennej ypu sring. Poprawne zinerpreowanie

6.6. Inerfejs użykownika 83 Rysunek 6.32. Generaor rajekorii ak zapisanej funkcji umożliwia polecenie eval. Pierwsze oraz drugie pochodne orzymywane są z użyciem polecenia diff, kóre realizuje różniczkowanie symboliczne. Po wygenerowaniu odpowiednich wekorów są one zapisywane w przesrzeni roboczej Malaba za pomocą polecenia assignin. dt =.; 2 Tk =2; Wydruk 6.23. Generowanie rajekorii 4 gx=.2* +.25 ; gy=.5* ; 6 gz=.5* +.2* sin ( /2) ; gxp = diff ( sym (gx )); 8 gyp = diff ( sym (gy )); gzp = diff ( sym (gz )); =: dt:tk ; 2 x= eval (gx ); 4 y= eval (gy ); z= eval (gz ); 6 xp= eval ( gxp ); yp= eval ( gyp ); 8 zp= eval ( gzp ); 2 assignin ( base, x,x); assignin ( base, y,y); 22 assignin ( base, z,z); assignin ( base, xp,xp ); 24 assignin ( base, yp,yp ); assignin ( base, zp,zp ); Największym problemem okazało się odpowiednie przeworzenie wygenerowanych rajekorii w Simulinku. Osaecznie zasosowano nasępujące rozwiązanie: Wekor zawierający rajekorię, obecny czas oraz krok czasu (dt ) podawane są w Simulinku na wejście bloczka MATLAB Funcion Na podsawie obecnego czasu i kroku czasu wyliczany indeks elemenu, kóry należy pobrać z wekora Pobrana z wekora warość podawana jes na wyjście Schema blokowy realizujący w Simulinku en proces przedsawia rys. 6.33.

84 6. Realizacja w środowisku MATLAB Rysunek 6.33. Generaor - schema w Simulinku dla rajekorii x Kod zawary w MATLAB Funcion odpowiadający za generowanie odpowiedniego indeksu przedsawia poniższy wydruk. funcion y = fcn (dt,ime,u) 3 n= size (u); if n (2) > 5 index = round ( ime /dt )+; y = u( index ); 7 else y=u; 9 end Wydruk 6.24. Obliczanie indeksu dla rajekorii x 6.6.6. Połączenie algorymu z GUI W celu połączenia konkrenego algorymu z GUI spełnione muszą być dwa warunki:. Algorym musi współpracować z opisanym w poprzednim rozdziale generaorem (lub z innym przyjmującym wygenerowane wekory) 2. Odpowiednie sygnały muszą zosać wyprowadzone do obszaru roboczego Malaba. Są o: wyjścia obieku sygnały błędu sygnały serujące Przykład modelu podłączonego do GUI przedsawia rys. 6.34 Rysunek 6.34. Podłączenie do GUI modelu algorymu całkowania wsecznego. Wszyskie dane uzyskane w en sposób są po zakończeniu symulacji zapisywane w jednej srukurze o nazwie GUIOu, kóra nasępnie zapisywana jes w obszarze roboczym malaba za pomocą polecenia assignin. Proces en obrazuje poniższy wydruk.

6.6. Inerfejs użykownika 85 Wydruk 6.25. Tworzenie srukury GUIOu GUIOu.x=w (:,); GUIOu.y=w (:,2); GUIOu.z=w (:,3); GUIOu. phi =w (:,4); GUIOu. hea =w (:,5); GUIOu. psi =w (:,6); 3 GUIOu.xp =wp (:,); GUIOu.yp=wp (:,2); GUIOu.zp=wp (:,3); GUIOu. phip =wp (:,4); GUIOu. heap =wp (:,5); GUIOu. psip = wp (:,6); 5 GUIOu.xb =wb (:,); GUIOu.yb=wb (:,2); GUIOu.zb=wb (:,3); GUIOu. phib =wb (:,4); GUIOu. heab =wb (:,5); GUIOu. psib = wb (:,6); 7 GUIOu.ex =e (:,); GUIOu. ey=e (:,2); GUIOu.ez=e (:,3); 9 GUIOu. ephi =e (:,4); GUIOu. ehea =e (:,5); GUIOu. epsi =e (:,6); GUIOu. exp =ep (:,); GUIOu. eyp =ep (:,2); GUIOu. ezp =ep (:,3); GUIOu. ephip =ep (:,4); GUIOu. eheap =ep (:,5); GUIOu. epsip =ep (:,6); GUIOu. exb =eb (:,); GUIOu. eyb = eb (:,2); GUIOu. ezb =eb (:,3); 3 GUIOu. ephib =eb (:,4); GUIOu. eheab =eb (:,5); GUIOu. epsib =eb (:,6); GUIOu. = ; 5 GUIOu.xd = rd (:,); GUIOu. yd=rd (:,2); GUIOu.zd=rd (:,3); 7 GUIOu. phid = rd (:,4); GUIOu. head = rd (:,5); GUIOu. psid = rd (:,6); 9 GUIOu.u = Uou (:,); GUIOu.u2= Uou (:,2); GUIOu.u3= Uou (:,3); GUIOu.u4 = Uou (:,4); 2 assignin ( base, GUIOu, GUIOu ); Elemeny ak przygoowanej srukury są nasępnie wyświelane za pomocą poleceń plo oraz plo3 w wywołaniu zwronym zdefiniowanym dla przycisku Wyświel.

86 6. Realizacja w środowisku MATLAB 6.7. Obsługa inerfejsu użykownika 6.7.. Uruchamianie GUI Aby rozpocząć pracę z graficznym inerfejsem użykownika należy w wierszu poleceń programu MATLAB wprowadzić polecenie: >> GUI W wyniku ej operacji powiiniśmy orzymać okno robocze zgodne z ym przedsawionym na rysunku 6.35. Rysunek 6.35. Graficzny inerfejs użykownika 6.7.2. Usawienia paramerów modelu quadroora Aby dososowac model do porzeb symulacji należy w abeli Paramery modelu, przedsawionej na rysunku 6.36, dobrać odpowiednio warości paramerów. Poszczególne warości wprowadza się w kolumnie Val oznaczonej na rysunku czerwoną elpisą. Z kolejnych kolumn można odczyać przedział warości w kórym powinien znajdować się dany paramer: Min Val oznacza warość minimalną, Max Val oznacza warość maksymalną. Nasępna kolumna zawiera jednoski w jakich wyrażają się paramery, a osania kolumna dosarcza słownego opisu danego parameru. Należy pamięać aby po wprowadzeniu wszyskich wymaganych zmian, nacisnąć przycisk Akualizuj dane (rysunek 6.37), aby zmiany sały się widoczne dla planowanej symulacji. 6.7.3. Wybór algorymu serowania Aby wybrać odpowiedni algorym serowania, należy w rozwijanym menu (rysunki 6.38 i 6.39) wybrać odpowiednią pozycję: backsepping, dla algorymu całkowania wsecznego,

6.7. Obsługa inerfejsu użykownika 87 Rysunek 6.36. Paramery modelu przedsawienie danych Rysunek 6.37. Przycisk akualizujący usawienia modelu i algorymów serowania Hinf, dla algorymu H inf, PID, dla algorymu PID, brak algorymu, aby umożliwić symulację na modelu quadroora pozbawionym algorymu serowania Rysunek 6.38. Rozwijane menu wyboru algorymu serowania zwinięe 6.7.4. Usawienia paramerów algorymu serowania Aby dososowac algorym serowania do porzeb symulacji należy w abeli Paramery algorymu, przedsawionej na rysunku 6.4, dobrać odpowiednio warości paramerów. Poszczególne warości wprowadza się w kolumnie Val oznaczonej na rysunku czerwoną elipsą. Z kolejnych kolumn można odczyać przedział warości w kórym powinien znajdować się dany paramer: Min Val oznacza warość minimalną, Max Val oznacza warość maksymalną. Nasępna kolumna zawiera jednoski w jakich wyrażają się paramery, a osania kolumna dosarcza słownego opisu danego parameru. W przypadku wyboru opcji brak algorymu w menu wyboru algorymu, abela Paramery algorymu zosaje ukrya, a zamias niej wyświelone zosają abele Wejścia i Prędkości silników (rysunek 6.4), pozwalające na zadanie sałych serowanań na modelu. Należy pamięać aby po wprowadzeniu wszyskich wymaganych zmian, nacisnąć przycisk Akualizuj dane (rysunek 6.37), aby zmiany sały się widoczne dla planowanej symulacji. 6.7.5. Generowanie rajekorii zadanej Jeżeli isnieje porzeba uruchomienia symulacji dla rajekorii domyślnej, można pominąć en krok. Aby wygenerować rajekorię zadaną dla modelu quadroora należy wcisnąć przycisk Generuj rajekorię (rysunek 6.42). Spowoduje e oworzenie nowego okna (rysunek 6.43) oraz

88 6. Realizacja w środowisku MATLAB Rysunek 6.39. Rozwijane menu wyboru algorymu serowania rozwinięe Rysunek 6.4. Paramery algorymu przedsawienie danych Rysunek 6.4. Usawienia wejść modelu, w przypadku wyboru braku algorymu serowania

6.7. Obsługa inerfejsu użykownika 89 zablokowanie paska wyboru algorymu i przycisku Generuj rajekorię. Odblokowane one zosaną dopiero po zamknięciu okna generaora. Okno generaora umożliwia wprowadzenie w polu dt Rysunek 6.42. Przycisk owierający okno generaora rajekorii kroku symulacji, w polu Tk czasu symulacji, a w pozosałych polach funkcji czasu opisujących rajekorię zadaną dla poszczególnych zmiennych sanu (x y z Phi Thea Psi). W oknie akywne poza Tk i dt pozosają jedynie e pola, kóre odpowiadają współrzędnym sanu serowanym bezpośrednio przez algorym serowania. Funkcje opisujące generowaną rajekorię należy wprowadzac w noacji programu MATLAB, w zależności od zmiennej opisującej czas. Przykład również na rysunku 6.43. Dokonane zmiany należy powierdzić przyciskiem Generuj rajekorię, aby generaor wyworzył wszyskie wymagane przez symulację wekory. Rysunek 6.43. okno generaora rajekorii 6.7.6. Uruchomienie symulacji Aby uruchomić symulację dla zadanych paramerów i rajekorii, należy wcisnąć przycisk Symuluj (rysunek 6.44). Na czas rwania symulacji zosanie on zablokowany, aby uchronić użykownika, przed uruchomieniem kilku symulacji jednocześnie oraz zmieni swój wygląd (rysunek

9 6. Realizacja w środowisku MATLAB 6.45). Przycisk powróci do swojego normalnego wygladu po zakończeniu symulacji. Po wykonaniu symulacji można zapisać rajekorię wynikową quadroora w celu wykorzysania jej do dalszych symulacji za pomocą przycisku Zapisz r. wynikową. Należy uważać aby nie uruchomić po ej czynności generaora rajekorii, gdyż wedy zapisana rajekoria wynikowa zosanie nadpisana. Rysunek 6.44. Przycisk uruchamiający symulację Rysunek 6.45. Przycisk uruchamiający symulację w rakcie pracy 6.7.7. Prezenacja wyników Wyniki symulacji można obejrzeć poprzez naciśnięcie przycisku Wyświel (rysunek 6.46), co spowoduje prezenację wybranych za pomocą przycisków wyboru (rysunek 6.47) wykresów (rysunek 6.48), bądź okna prezenującego wskaźniki jakości. Rysunek 6.46. Przycisk uruchamiający wizualizację wyników 6.7.8. Zapisywanie i odczyywanie wyników Aby móc zachować orzymane w rakcie symulacji wyniki do dalszej pracy należy użyć przycisku Zapisz do pliku (rysunek 6.49). Spowoduje o oworzenie okna menadżera (rysunek 6.5), kóry pozwoli zapisać plik w dowolnym miejscu. Aby oworzyć plik z danymi i wykorzysać je w pracy z programem należy użyć przycisku Wczyaj z pliku (rysunek 6.5). Spowoduje o oworzenie okna menadżera (rysunek 6.52), kóry pozwoli wczyać zapisany plik do pamięci programu.

6.7. Obsługa inerfejsu użykownika 9 Rysunek 6.47. Przyciski wyboru pozwalające na wybór wizualizowanych danych Rysunek 6.48. Zbiorcze przedsawienie generowanych wykresów Rysunek 6.49. Przycisk pozwalający na zapis uzyskanych danych Rysunek 6.5. Przycisk pozwalający na wczyanie zapisanych danych

92 6. Realizacja w środowisku MATLAB Rysunek 6.5. Menadżer pozwalający zapisać plik wynikowy symulacji Rysunek 6.52. Menadżer pozwalający wczyać zapisany plik wynikowy symulacji

7. Badania 7.. Regulaor PID Pierwszy eap przeprowadzonych badań doyczył usalenia wpływu poszczególnych paramerów na zachowanie się serownika w zadaniu śledzenia rajekorii sinusoidalnych. Poniżej auorzy sarali się przedsawić pewne zauważone zależności. Powierdzają się one w odniesieniu do badanego obieku roboa ypu quadroor nie ylko na zaprezenowanych rajekoriach, ale w szerszym konekście opisują jego zachowanie w realizacji zadań śledzenia rajekorii i sabilizacji do punku. W poniższych badaniach poszczególnych członów przyjęo akie same warości paramerów wzmocnienia proporconalnego, całkującego i różniczkującego we wszyskich czerech regulaorach. Dokonano ego, aby pokazać rend idenyczny dla wszyskich współrzędnych, kóry zosał zauważony w czasie wsępnych badań. W celu bardzo dokładnej realizacji zadania śledzenia rajekorii zalecą się odpowiednią modyfikację poszczególnych nasawy w zależności od orzymywanych wyników. 7... Człon proporcjonalny P Badano zachowanie modelu quadroora w zależności od zmiany wszyskich 4 paramerów K p dla współrzędnych z, φ, θ i ψ przy warościach paramerów K i =. i K d = (w legendzie rysunków paramery są oznaczone kolejno jako P, I oraz D). Nie obserwowano warości zadawanych serowań dopiero po dobraniu opymalnych nasaw swierdzono, iż są one fizycznie realizowalne. Przyjmowano warości różniące się rzędem wielkości; jako podsawę usalono współczynnik wzmocnienia równy (w oparciu o wsępne wyniki doświadczalne). Paramery zmieniano od warości. do. Nasawy porównywano dla ych samych rajekorii zadanych w celu zauważenia pewnego rendu. Rysunki przedsawiające wyniki doświadczeń dla paramerów K p =. (7. i 7.2) oraz k p = (7.3 i 7.4) pozwalają swierdzić, iż przy wzmocnieniu (K p =) lub jego obniżeniu obiek nie jes w sanie śledzić zadanej rajekorii. Rysunek 7.. Śledzenie rajekorii dla paramerów K p =.

94 7. Badania Rysunek 7.2. Uchyb śledzenia rajekorii dla paramerów K p =. Rysunek 7.3. Śledzenie rajekorii dla paramerów K p = Rysunek 7.4. Uchyb śledzenia rajekorii dla paramerów K p = Każdorazowe zwiększanie o rząd wielkości wzmocnienia członu proporcjonalnego powoduje dużo lepsze śledzenie rajekorii (szybsza zbieżność uchybu do ). Prezenują o rysunki 7.5 i 7.6, przedsawiające śledzenie rajekorii dla parameru K p =.

7.. Regulaor PID 95 Rysunek 7.5. Śledzenie rajekorii dla paramerów K p = Rysunek 7.6. Uchyb śledzenia rajekorii dla paramerów K p = Niesey, każdorazowe zwiększenie egoż współczynnika znacząco wydłuża czas symulacji z powodu coraz większych warości liczbowych, kóre są przemnażane przez siebie. Ponado, duże warości wzmocnień mogą nie być realizowane na fizycznym obiekcie. Należy zwrócić szczególną uwagę na pojawiające się oscylacje, kóre zmniejszają sabilność sysemu. Efekem zby dużych nasaw są widoczne w pierwszych sekundach przy nasawach K p = przeregulowania, pokazane na rysunkach 7.7 i 7.8. Rysunek 7.7. Śledzenie rajekorii dla paramerów K p =

96 7. Badania Rysunek 7.8. Uchyb śledzenia rajekorii dla paramerów K p = W przypadku pracy z rzeczywisym układem akie zjawiska mogą powodować jego nieprawidłowe działanie, o czym koniecznie należy pamięać. Dalsze zwiększanie paramerów do K p = powoduje większe oscylacje, co zosało przedsawione na rysunkach 7.9 i 7.. Rysunek 7.9. Śledzenie rajekorii dla paramerów K p = Rysunek 7.. Uchyb śledzenia rajekorii dla paramerów K p = 7..2. Człon całkujący I Wpływ zmian paramerów wzmocnień członów całkujących badano w analogiczny sposób. Zakres zmian parameru K i usalono na warość od. do na podsawie wsępnych badań obieku, przy paramerach K p = i K d = (bezpiecznymi nasawami również znalezionymi we wsępnych badaniach). Warości K i nie różnią się znacząco orzymanymi wynikami, dlaego eż pominięo ukazanie wykresów dla warości K i =.. Wyniki doświadczeń pokazują, iż dla ych warości współrzędna z i ką ψ są bardzo dobrze śledzone, zaś pewne problemy pojawiają się dla kąów φ i θ. Pokazują o rysunki 7. i 7.2 dla K i =. oraz rysunki 7.3 i 7.4 dla K i =.

7.. Regulaor PID 97 Rysunek 7.. Śledzenie rajekorii dla paramerów K i =. Rysunek 7.2. Uchyb śledzenia rajekorii dla paramerów K i =. Rysunek 7.3. Śledzenie rajekorii dla paramerów K i =

98 7. Badania Rysunek 7.4. Uchyb śledzenia rajekorii dla paramerów K i = Warość K i = desabilizuje układ, powodując ruch wirowy wokół osi φ oraz θ. Podobnie jak w przypadku parameru K p, duża warość powoduje wzbudzenie się układu ukazują o rysunki 7.5 i 7.6. Z ego powodu większe warości K i nie zosały sprawdzone. Rysunek 7.5. Śledzenie rajekorii dla paramerów K i = Rysunek 7.6. Uchyb śledzenia rajekorii dla paramerów K i =

7.. Regulaor PID 99 7..3. Człon różniczkujący D Paramery wzmocnień członów różniczkujących K d rozparywano w zakresie od. do dla K p = i K i =.. Przy warości K d =. układ nieprawidłowo śledził rajekorię dla kąów θ oraz φ, co przedsawiają rysunki 7.7 i 7.8. Rysunek 7.7. Śledzenie rajekorii dla paramerów K d =. Rysunek 7.8. Uchyb śledzenia rajekorii dla paramerów K d =. Zwiększenie parameru K d o rząd wielkości umożliwiło polepszenie serowania i śledzenia kąów. Niesey, dalsze zwiększanie parameru do warości K d = skukuje zby dużą warością odpowiedzi i w efekcie przeregulowaniem układu. Pokazują o rysunki 7.9 i 7.2 (dla K d =) i 7.2 i 7.22 (dla K d =). Rysunek 7.9. Śledzenie rajekorii dla paramerów K d =

7. Badania Rysunek 7.2. Uchyb śledzenia rajekorii dla paramerów K d = Rysunek 7.2. Śledzenie rajekorii dla paramerów K d = Rysunek 7.22. Uchyb śledzenia rajekorii dla paramerów K d = Dalsza inkremenacja K d jes bezcelowa, gdyż serownik przesaje wówczas regulować układ co ukazują rysunki 7.23 i 7.24.

7.. Regulaor PID Rysunek 7.23. Śledzenie rajekorii dla paramerów K d = Rysunek 7.24. Uchyb śledzenia rajekorii dla paramerów K d = 7..4. Badania serowanie do punku i śledzenie wybranych rajekorii Zbadano serowanie PID dla zadań serowania do punku oraz śledzenia rajekorii oparych o krzywe Béziera i łańcuchowe. Symulacje wykonano dla nasaw K p =, K i =., K d =. Dla każdego z badanych przykładów uzyskano zbliżoną endencję. Wyszczególniono dwie pary błędów o podobnej jakości regulacji. Pierwsza, składająca się z wysokości z i kąa myszkowania ψ, osiąga zbieżność znacznie szybciej, w przeciągu 2 sekund. Druga, złożona z kąa kiwania φ i kołysania θ, do osiągnięcia zbieżności porzebuje czasu 6 sekund, co zosało przedsawione na rysunku 7.3. Ponado zmienna z jako jedyna wykazuje zmiany w procesie regulacji w zależności od badanej rajekorii. Dla krzywych Béziera i łańcuchowych po osiągnięciu zbieżności uchyb wzrasa do poziomu.2. Pozosałe zmienne, oraz z przy serowaniu do punku uzyskują rząd wielkości błędów na poziomie 4 5. Zbadano, czy gorsza jakość regulacji kąów φ i θ może wynikać z wzajemnego wzbudzania, wpływu oscylacji jednego z kąów na drugi. W ym celu odłączono regulaor kąa θ i zadano serowanie do punku. Rezula umieszczono na rysunkach 7.27 i 7.28. Nie zauważono isonych różnic w jakości serowania kąem φ. Wykresy rajekorii jak i uchybu nie odbiegają od analogicznych uzyskancych przy serowaniu czerema współrzędnymi. Przyczyną gorszej regulacji kąów kiwania i kołysania nie jes ich wzajemny wpływ.

2 7. Badania Rysunek 7.25. Serowanie do punku Rysunek 7.26. Uchyb przy serowaniu do punku Rysunek 7.27. Serowanie do punku, przy odłączonej współrzędnej θ

7.. Regulaor PID 3 Rysunek 7.28. Uchyb przy serowaniu do punku, przy odłączonej współrzędnej θ Rysunek 7.29. Serowanie do punku, dla czasu symulacji = Rysunek 7.3. Uchyb przy serowaniu do punku, dla czasu symulacji =

4 7. Badania Rysunek 7.3. Śledzenie rajekorii dla zadanych krzywych Béziera Rysunek 7.32. Uchyb śledzenia rajekorii dla zadanych krzywych Béziera Rysunek 7.33. Śledzenie rajekorii dla krzywych łańcuchowych

7.. Regulaor PID 5 Rysunek 7.34. Uchyb śledzenia rajekorii dla krzywych łańcuchowych 7..5. Podsumowanie Po serii dodakowych badań dla opisywanego obieku, przy zadanych warościach sałych fizycznych swierdzono, że najlepiej radzi sobie regulaor o nasawach K p =8, K i =., K d =5 pokazują o rysunki 7.35 i 7.36 oraz wykres serowań 7.37. Rysunek 7.35. Śledzenie rajekorii dla paramerów K p =8, K i =., K d =5 Rysunek 7.36. Uchyb śledzenia rajekorii dla paramerów K p =8, K i =., K d =5

6 7. Badania Rysunek 7.37. Wykresy zadawanego serowania przy śledzeniu rajekorii dla paramerów K p =8, K i =., K d =5 Błędy śledzenia rajekorii, kóre się pojawiają przy serowaniu quadroorem dla usalonych paramerów w oparciu o rajekorię sin x (o ampliudzie m), wynoszą.5 m; dla kąów skala błędów jes analogicznia. Z uwagi na gorszą jakość regulacjii kąów φ i θ, zaleca się zasosować dla nich nasaw o wyższych warościach. Sprawdzono, iż zbliżone przebiegi serowania dla wszyskich zmiennych przy około czerokronym zwiększeniem K p dla kąów kiwania i kołysania. Rezula przedsawiono na rysunkach 7.38 i 7.39, wraz z wykresem serowań 7.4. Rysunek 7.38. Serowanie do punku dla paramerów K p =8, K i =., K d =5 oraz podwyższonym K p =32 dla kąów φ i θ

7.. Regulaor PID 7 Rysunek 7.39. Uchyb serowania do punku i dla paramerów K p =8, K i =., K d =5 oraz podwyższonym K p =32 dla kąów φ i θ Rysunek 7.4. Wykresy zadawanego serowania dla serowania do punku i dla paramerów K p =8, K i =., K d =5 oraz podwyższonym K p =32 dla kąów φ i θ

8 7. Badania 7.2. Algorym całkowania wsecznego 7.2.. Trajekorie W przypadku śledzenia rajekorii układu łańcuchowego konieczne jes jej odpowiednie dobranie. Zadawane równania powinny być możliwe do wygenerowania przez dynamikę układu. W przypadku quadroora opisanego równaniami (2.32) dopuszczalne są więc kombinacje funkcji sałych, liniowych oraz sinusa i kosinusa. Na nich eż skupiły się badania algorymu. Symulacje pokazały jednak, że niekóre spośród rajekorii niedopuszczalnych z powodzeniem można realizować za pomocą zaimplemenowanego serownika, co zosało pokazane w części 7.2.4. Zachowanie algorymu pokazane zosanie na przykładzie wybranej rajekorii zadanej równaniem (7.) pokazanej na rysunkach 7.4 i 7.42. x d =, 2 +, 25 y d =, 5 z d =, 5 + 4 sin( 2 ) (7.) ψ d =, 8 sin( 6 ) Rysunek 7.4. Trajekoria zadana w przesrzeni rójwymiarowej Wszyskie symulacje przeprowadzane były dla sałego kroku całkowania wynoszącego dt =.s, meody całkowania ode3 (Bogacki-Shampine) oraz paramerów modelu pokazanych na wydruku 6.4, chyba że zosało wyraźnie zaznaczone, że jes inaczej. Czas symulacji wynosił s.

7.2. Algorym całkowania wsecznego 9 Rysunek 7.42. Trajekoria zadana równaniem (7.) 7.2.2. Dobór paramerów serownika Nasawy algorymu dobierane były na podsawie kryerium minimalizującego całkę K b kwadraów błędów rajekorii K b = Tk ( (x() x d ()) 2 + (y() y d ()) 2 + (z() z d ()) 2 + (ψ() ψ d ()) 2) d. (7.2) Serowanie charakeryzuje się prosą zależnością im większa warość paramerów α, ym mniejsza warość kryerium K b, a błędy szybciej zbiegają do zera. Oczywiście spełnione jes o w pewnym zakresie i pod pewnymi warunkami. Pierwszy problem, jaki pojawił się przy wyborze nasaw o obliczenia. Przy niekórych zesawach paramerów całkowanie numeryczne zaczynało generować bardzo duże błędy. Sysem ulegał wzbudzeniu i współrzędna x zaczynała dążyć do nieskończoności. Zwiększenie zadawanej dokładności (przy całkowaniu o zmiennym kroku) niewiele dawało. MATLAB informował o problemach z doborem długości kroku wynikających z zw. wysokiej szywności sysemu albo przeprowadzał bardzo dokładne i jednocześnie wolne symulacje, po kórych nie był w sanie wyeksporować danych z Simulinka do przesrzeni roboczej ze względu na ich rozmiar. Wykorzysanie meod całkowania przeznaczonych specjalnie dla sysemów szywnych nie poprawiało syuacji, zwiększając jedynie czas obliczeń. W celu uniknięcia problemu należało skorygować paramery α 9 i α, kóre okazały się kluczowym czynnikiem powodującym błędy. Unikać należy pewnych konfiguracji ych dwóch nasaw i dziesięciu pozosałych zwykle wysarczające jes zadbanie, by co najmniej jeden z α 9 i α nie przekraczał 2. W przypadku pozosałych nasaw, zwiększanie ich warości poprawiało wskaźnik jakości, choć nie zawsze były o znaczące różnice. Większe warości, pomimo spełnienia ograniczeń na α 9 i α ponownie prowadziły do niepoprawnego całkowania numerycznego. Przebiegi dla wybranych nasaw pokazane zosały w rozdziale 7.2.3. Przy wyborze serowania wzięo pod uwagę możliwość implemenacji serownika na rzeczywisym układzie. W akim wypadku czas obliczeń pełni ważną rolę, a moc obliczeniowa mikroprocesorów jes znacznie mniejsza od ej, kórą dysponuje sandardowy procesor. Zwykle eż implemenuje się obliczenia o sałym kroku. Osaecznie wybrane nasawy o BSparam = [5 5 5 5 5 5 5 5 2 5 5 5]. Wyniki śledzenia rajekorii (7.) pokazują rysunki 7.43 do 7.46. Warość całki wyniosła K b =.429. Rysunek (7.47) pokazuje wyniki symulacji po zmianie paramerów modelu quadroora na QP.m = 5, QP.l =.25, QP.b =, QP.d =, QP.Ix =, QP.Iy =, QP.Iz =, QP.J =.

7. Badania Rysunek 7.43. Realizacja rajekorii (7.) w przesrzeni rójwymiarowej Rysunek 7.44. Realizacja rajekorii (7.).

7.2. Algorym całkowania wsecznego (a) położenie (b) ψ Rysunek 7.45. Błędy śledzenia rajekorii (7.) (a) pełny przebieg (b) przebieg w pierwszej sekundzie Rysunek 7.46. Zadawane serowanie podczas realizacji (7.)

2 7. Badania Trajekoria nadal była wiernie śledzona, a warość kryerium nie uległa zmianie. Ze względu na zmianę masy przeskalowane zosało serowanie. (7.3) (a) położenie (b) ψ Rysunek 7.47. Błędy śledzenia współrzędnych x,y i z oraz serowanie po zmianie paramerów modelu quadroora 7.2.3. Przegląd wyników symulacji dla różnych nasaw serownika Poniżej zamieszczony zosał przegląd wyników symulacji dla wybranych nasaw serownika. Przedsawione w nim wykresy błędów dla współrzędnyche x, y, z, wykres błędu dla kąa ψ nie zosał pokazany ze względu na niewielkie warości oraz nieznaczne różnice dla różnych nasaw. BSparam = [.8.8.8.8.8.8.8.8.8.8.8.8] Wykres błędu i rajekorię pokazano na rysunku 7.48, K b = 5.656, zdecydowanie zby małe warości nasaw powodują wysępowanie znacznych oscylacji w sysemie. (a) błędy (b) rajekoria w przesrzeni Rysunek 7.48. Wyniki symulacji dla BSparam = [.8.8.8.8.8.8.8.8.8.8.8.8]

7.2. Algorym całkowania wsecznego 3 BSparam = [5 5 5 5 5 5 5 5 5 5 5 5] Wykres błędu pokazano na rysunku 7.49, K b = 5.6875, przykład nasaw powodujących błędy w całkowaniu numerycznym. Rysunek 7.49. Wyniki symulacji dla BSparam = [5 5 5 5 5 5 5 5 5 5 5 5]. BSparam =[2 2 2 2 2 2 2 2 2 2 2 2] Wykres błędu pokazano na rysunku 7.5, K b =.758, nasawy pozwalają na dobrą realizację zadania śledzenia rajekorii, jednak dające gorszą warość całki niż uzyskana w częsci 7.2.2. Rysunek 7.5. Wyniki symulacji BSparam =[2 2 2 2 2 2 2 2 2 2 2 2] BSparam = [8 8 8 8 8 8 8 8.4.4 8 8] Wykres błędu pokazano na rysunku 7.5, K b =.374, paramery dają mniejszą warość kryerium, jednak generują serowania powyżej, ze względu na błędy numeryczne, badanie musiało zosać przeprowadzone przy zmiennym kroku całkowania (przyjęo meodę ode45 i dokładność ).

4 7. Badania (a) błędy (b) serowanie w pierwszej sekundzie Rysunek 7.5. Wyniki symulacji dla BSparam = [8 8 8 8 8 8 8 8.4.4 8 8] 7.2.4. Przegląd wyników symulacji dla różnych rajekorii Poniżej zamieszczony zosał przegląd wyników dla różnych rajekorii. Wszyskie symulacje przeprowadzone zosały dla warości serownika dobranych w części 7.2.2. Przemieszczanie się w poziomie Trajekoria zadana równaniem (7.4) wyznaczona jes jako swoisy parol quadroor ma za zadanie przemieszczać się na sałej wysokości, zmieniając orienację i wykonując ósemki. Zadanie o okazało się rudniejsze niż posawione w 7.2., pojawiają się większe błędy. Całka wynosi K b =.7789 Trajekorię w przesrzeni rójwymiarowej i błędy śledzenia współrzędnych x, y, z pokazuje rysunek 7.52. x d = sin( 2 ) y d = sin( ) (7.4) z d = ψ d =, 8 sin( 6 ) Funkcja kwadraowa Trajekoria zadana równaniem (7.5) różni się od rajekorii (7.) współrzędnymi x z i y z. Teoreycznie należy ona do grupy rajekorii niedopuszczalnych ze względu na wysępowanie funkcji kwadraowej. Przeprowadzone symulacje pokazały jednak, że układ serowania działa poprawnie, choć śledzenie obarczone jes większym błędem. Pojawiają się piki warości błędu. Częsość ich wysępowania określona jes przez pół okresu sinusa zadawanego dla kąa ψ, zaś warość błędu współrzędnej x z począkowym błędem rajekorii. Kryerium całkowej wyniosło K d =.9749. Realizację rajekorii w przesrzeni rójwymiarowej i błędy śledzenia współrzędnych x, y, z pokazuje rysunek 7.53. x d =, 2 2 +, 25 y d =, 3 2 z d =, 5 +, 2 sin( 2 ) (7.5) ψ d =, 8 sin( 6 ) Zawis Trajekoria zadana równaniem (7.6) o po prosu zadanie quadroorowi pozosania w zawisie i rozglądania się po okolicy. Również w ym wypadku serownik doskonale sobie radzi. Całka wyniosła K b = 4.2 6.

7.2. Algorym całkowania wsecznego 5 (a) rajekoria zadana (b) rajekoria zrealizowana (c) błędy Rysunek 7.52. Wyniki symulacji dla rajekorii zadanej równaniem (7.4) (a) rajekoria zrealizowana (b) błędy Rysunek 7.53. Wyniki symulacji dla rajekorii zadanej równaniem 7.5.

6 7. Badania Błędy śledzenia współrzędnych x, y, z pokazuje rysunek 7.54. x d = y d = z d = ψ d =, 8 sin( 6 ) (7.6) Rysunek 7.54. Wyniki symulacji dla rajekorii zadanej równaniem (7.6) 7.2.5. Podsumowanie Przeprowadzone symulacje pokazują, że serownik wykorzysujący algorym całkowania wsecznego bardzo dobrze wykonuje zadanie śledzenia rajekorii. Podsawowym problemem przy realizacji zadania są obliczenia numeryczne mogą powodować ak duże błędy, że sysem saje się niesabilny. Dlaego eż ważne jes odpowiednie dobranie paramerów. Same paramery można zmieniać w szerokiej skali warości (zależnej m.in. od paramerów modelu). Możliwe jes akże zadawanie rajekorii eoreycznie niedopuszczalnej choć w akim przypadku znacznie częściej pojawiają się problemy z obliczeniami. Algorym sprawdza się akże w zadaniu sabilizacji w punkcie.

e z e y e θ e x e φ z ψ y θ x φ 7.3. Badania jakości serowania H dla quadroora 7 7.3. Badania jakości serowania H dla quadroora Podobnie jak w przypadku wcześniej analizowanych algorymów serowania po zaimplemenowaniu serownika H przeprowadzono badania jego jakości. W lieraurze nie znaleziono żadnych warunków nakładających resrykcje na posać rajekorii wynikające z właściwości samego serownika. Na począku wybrano rajekorię opisaną równaniami (7.), ak aby móc porównać jakość serownika H ze serownikiem oparym o algorym całkowania wsecznego, gdyż w obu ych algorymach wekor serowań jes aki sam. Na wykresach z rysunku 7.55 przedsawiono uzyskane położenia rzeczywise obok położeń zadanych oraz zadany i rzeczywisy ką ψ (ką myszkowania). Kąy kiwania i kołysania nie wchodzą w skład wekora serowań, jednakże udosępnione użykownikowi GUI (parz sekcja 6.6, 6.7) rysuje ich przebiegi auomaycznie sąd ich obecność na wykresach 2.5 2 W. rzeczywisa W. zadana Wspolrzedna x.5 W. rzeczywisa W. zadana Ka φ.5.5.5 2 3 4 5 6 7 8 9 6 4 W. rzeczywisa W. zadana Wspolrzedna y 2 3 4 5 6 7 8 9.5 W. rzeczywisa W. zadana Ka θ 2.5 2 2 3 4 5 6 7 8 9 6 4 W. rzeczywisa W. zadana Wspolrzedna z 2 3 4 5 6 7 8 9.5 W. rzeczywisa W. zadana Ka ψ 2.5 2 2 3 4 5 6 7 8 9 2 3 4 5 6 7 8 9 Rysunek 7.55. Położenia i kąy zadane i rzeczywise (uzyskane symulacyjne) dla H podczas realizacji (7.) Niezależnie od analizowanej współrzędnej (x, y, z) można zaobserwować przesunięcie rajekorii rzeczywisej w sosunku do rajekorii zadanej. Podobnie dzieje się w przypadku kąa myszkowania. Gdyby w procesie serowania przesunięcie można było wyeliminować działanie serownika H można by określić jako idealne. Niesey algorym serowania będzie podążał za przebiegiem zadanej rajekorii z pewnym opóźnieniem w celu wyeliminowania szumów. Filry wysępujące w pierwszej pęli mają właściwości całkujące wprowadzające owe opóźnienie. Jes o jednak własność sysemu serowania wynikająca ze sposobu jego zaprojekowania i ego ypu opóźnień należy się spodziewać. Wysępujące przesunięcie wpływa zaem na posać i wymiar uzyskanych błędów. Wykresy z rysunku 7.56 pokazują błąd rajekorii i kąów..4 Blad sledzenia rajekorii x Blad sledzenia rajekorii φ.3.5.2..5 2 3 4 5 6 7 8 9 2 3 4 5 6 7 8 9.2 Blad sledzenia rajekorii y Blad sledzenia rajekorii θ.5.5..5.5 2 3 4 5 6 7 8 9 2 3 4 5 6 7 8 9.6 Blad sledzenia rajekorii z.2 Blad sledzenia rajekorii ψ.4..2.2 e ψ..4 2 3 4 5 6 7 8 9.2 2 3 4 5 6 7 8 9 Rysunek 7.56. Błędy śledzenia rajekorii i kąów dla H podczas realizacji (7.)

8 7. Badania Biorąc pod uwagę wysępujące opóźnienie kryerium oceny przyjęe do analizy jakości serowania algorymem całkowania wsecznego nie jes miarodajnym kryerium dla algorymu H. Pozosałe przyjęe przy wizualizacji kryeria błędu również nie są miarodajne. Rysunek 7.57. Wskaźniki jakości serowania dla H podczas realizacji (7.) Waro zwrócić uwagę na warości serowań z rysunku 7.58, kóre w przypadku rajekorii (7.) mają podobny przebieg do serowań uzyskanych dla algorymu całkowania wsecznego. Jednakże w innych przypadkach serowania o może być znacznie łagodniejsze (mniejsze piki) niż w przypadku dwóch pozosałych algorymów parz sekcja 7.5. Trajekorię zadaną i uzyskaną przedsawiono na rysunku 7.59. W związku z charakerem swojej implemenacji serownik opary o algorym H nie oferuje użykownikowi żadnych nasaw regulacji do samodzielnej manipulacji. Paramery lineryzacji wyznaczane są dla konkrenego modelu i jedynie zmiana jego paramerów wpływa na ich warości.