Symulacja pojedynczego stopnia swobody robota manipulacyjnego

POLITECHNIKA WARSZAWSKA Rok akademicki 1/13 WYDZIAŁ ELEKTRONIKI I TECHNIK INFORMACYJNYCH INSTYTUT AUTOMATYKI I INFORMATYKI STOSOWANEJ PRACA DYPLOMOWA INŻYNIERSKA Bartosz Wojciech Świstak Symulacja pojedynczego stopnia swobody robota manipulacyjnego Opiekun pracy: dr inż. Tomasz Winiarski Ocena pracy:..................................................................... Data i podpis Przewodniczącego Komisji Egzaminu Dyplomowego

Podziękowanie Chciałbym podziękować wszystkim biorącym udział przy powstawaniu niniejszej pracy. Dziękuje rodzinie za wyrozumiałość i cierpliwość. Dziękuje osobom z laboratorium 1 i załodze KNR Bionik za wsparcie merytoryczne i moralne.

Streszczenie Celem pracy był projekt i implementacja symulacji komputerowej pojedynczego stopnia swobody manipulatora. Model symulacji oparty został na istniejącym już w laboratorium robocie Sarkofag. W niniejszej pracy zawarte zostały: model matematyczny robota o pojedynczym stopniu swobody typu wahadło, symulacja oparta na tym modelu, testy weryfikujące poprawność implementacji, podsumowanie otrzymanych wyników i perspektywy rozwoju. Słowa kluczowe: Robot typu wahadło, symulator, Sarkofag, sterowanie Abstract Title: Simulation of a single degree of freedom of a manipulating robot The aim of this thesis was to design and implement computer simulation of manipulator s single degree of freedom. The simulation model was based on an existing robot Sarkofag. The thesis contains mathematical model of single degree of freedom of pendulum type manipulator, simulation based on this model, tests verifying the correctness of the implementation, summary and future work. Keywords: Pendulum type robor, simulator, Sarkofag, control

4

Spis treści 1 Wstęp 7 1.1 Motywacja pracy............................. 7 1. Cel i założenia pracy........................... 7 1.3 Układ pracy................................ 8 Użyty sprzęt i oprogramowanie 9.1 Stanowisko badawcze Sarkofag.................... 9.1.1 Konstrukcja mechaniczna.................... 9.1. Silnik wykonawczy........................ 1. Matlab................................... 11 3 Model pojedynczego stopnia swobody 13 3.1 Kinematyka................................ 14 3. Dynamika................................. 15 3..1 Macierz inercji.......................... 16 3.. Prędkość kątowa......................... 16 3..3 Prędkość kątowa środka masy.................. 17 3..4 Przyspieszenie kątowe...................... 17 3..5 Przyspieszenie kątowe środka masy............... 17 3..6 Przyspieszenie liniowe...................... 17 3..7 Przyspieszenia liniowe środka masy............... 18 3..8 Siły bezwładności......................... 19 3..9 Momenty sił bezwładności.................... 19 5

6 SPIS TREŚCI 3..1 Suma sił oddziałujących na ramię................ 3..11 Suma momentów sił oddziałujących na ramię......... 3..1 Moment siły wzdłuż osi Z.................... 1 3.3 Dynamika napędu............................. 3.3.1 Moment elektromagnetyczny.................. 3.3. Moment sił bezwładności silnika................ 3.3.3 Moment tarcia widziany na wale silnika............ 3 3.3.4 Moment sił ogniwa widziany na wale silnika.......... 3 3.3.5 Równanie elektryczne....................... 3 4 Symulacja 5 4.1 Funkcje symulacyjne........................... 5 4.1.1 Ruch bez zadanego sterowania.................. 7 4.1. Sterowanie prędkością ramienia................. 3 4.1.3 Sterowanie przyspieszeniem ramienia.............. 33 4.1.4 Sterowanie natężeniem prądu silnika.............. 35 4.1.5 Sterowanie napięciem elektrycznym na silniku......... 38 4. Bezpośrednie korzystanie z funkcji modelu............... 41 4.3 Symulacja czasu rzeczywistego z GUI.................. 45 4.3.1 Główna pętla programu..................... 45 4.3. Panele symulacji......................... 47 4.3.3 Funkcje pomocnicze....................... 51 5 Testy poprawności symulacji 53 5.1 Sprawdzenie modelu symulacyjnego.................. 53 5.1.1 Testy podstawowe........................ 53 5.1. Testy zaawansowane....................... 56 5. Sprawdzenie symulacji czasu rzeczywistego.............. 59 6 Podsumowanie 61

Rozdział 1 Wstęp 1.1 Motywacja pracy Prace badawcze związane ze sterowaniem robotów wymagają sprawdzenia zaprojektowanych algorytmów. Testowanie pozwala na wychwycenie błędów mogących doprowadzić do usterek lub uszkodzeniu docelowego robota. Jedną z metod sprawdzających opracowane algorytmy jest ich uruchomienie na odpowiednio przygotowanym stanowisku badawczym. Odpowiednio przygotowane stanowisko badawcze oddaje fizyczny charakter robota, dzięki czemu otrzymujemy bardzo wiarygodne wyniki działania algorytmu. Drugą metodą jest zaprojektowanie odpowiedniego modelu symulacyjnego. Umożliwia on przeprowadzenie powtarzalnych i zależnych tylko od danych wejściowych testów. Symulacja umożliwia przetestowanie opracowanego algorytmu bez angażowania większych zasobów co zmniejsza koszt projektu. Pozwala też na wykonanie znacznie większej liczby testów niż na stanowisku badawczym dzięki szybszemu działaniu niż rzeczywisty obiekt. Model symulacji stara się odtworzyć przybliżone zachowanie obiektu badanego lecz nie uwzględnia wszystkich zjawisk zachodzących w prawdziwym obiekcie i jego środowisku. 1. Cel i założenia pracy Celem pracy jest zaprojektowanie i implementacja symulacji komputerowej pojedynczego stopnia swobody manipulatora. Model symulacji oparty jest na istniejącym stanowisku badawczym napędów Sarkofag, znajdującym się w pracowni robotyki Instytutu Automatyki i Informatyki Stosowanej Politechniki Warszawskiej. Głównym założeniem symulacji jest implementacja gotowego modelu matematycznego robota typu wahadło i jego modyfikacja. Zmienne parametry robota (takie 7

8 ROZDZIAŁ 1. WSTĘP jak cechy ramienia) muszą być łatwo modyfikowalne. Praca ma na celu ułatwienie badań algorytmów sterowania, w szczególności ich wstępny projekt. 1.3 Układ pracy Rozdział. zawiera opis robota, na bazie którego został napisany model, i środowiska, w których została napisana aplikacja symulacji. Przedstawiona została fizyczna budowa stanowiska badawczego i parametry jego silnika, potrzebne w implementowanej symulacji. Rozdział 3. przedstawia model matematyczny opisujący przedstawione wcześniej stanowisko badawcze. Zawarta jest w nim kinematyka i dynamika robota o pojedynczym stopniu swobody typu wahadło oraz model dynamiki napędu. Rozdział 4. zawiera symulację opracowaną na podstawie zaprezentowanego modelu. W jej skład wchodzą funkcje symulacyjne i symulacja czasu rzeczywistego z graficznym interfejsem użytkownika. Rozdział 5. prezentuje przeprowadzone testy sprawdzające poprawność implementacji modelu. Porównanie symulacji z działaniem rzeczywistego robota nie jest zagadnieniem tej pracy. Rozdział 6. przedstawia podsumowanie pracy, wyniki i wnioski.

Rozdział Użyty sprzęt i oprogramowanie W tym rozdziale jest zawarty opis urządzeń i programów zastosowanych w trakcie opracowywania tematu pracy. Symulacja oparta jest na modelu matematycznym istniejącego stanowiska badawczego napędów Sarkofag (sekcja.1). Głównym narzędziem wykorzystanym do stworzenia modelu symulacyjnego i interfejsu graficznego symulacji jest Matlab (sekcja.). Sekcja.1.1 opisuje konstrukcje robota widocznego na rysunku.1..1 Stanowisko badawcze Sarkofag Według projektu technicznego wyposażenia pracowni robotyki [4], Sarkofag jest to napęd elektryczny z przekładnią falową 1:158 oraz zamontowanym ramieniem o regulowanej długości i obciążeniu jako element laboratoryjnego stanowiska do badań napędów. Sarkofag został zbudowany przez MERA-PIAP. Jest to najprostszy typ robota o pojedynczym stopniu swobody typu wahadło, napędzanym silnikiem prądu stałego. Powstał jako stanowisko badawcze do rejestracji przebiegów współrzędnych i prędkości wewnętrznych oraz prądów w napędach robotów. Sarkofag służy do badania cyfrowych algorytmów regulacji napędów robotów z silnikami prądu stałego oraz algorytmów regulacji położenia w układach ze zmiennym obciążeniem..1.1 Konstrukcja mechaniczna Głównym elementem robota jest kolumna obrotu. Znajduję się ona w centrum konstrukcji nośnej. Wał napędowy napędzany jest silnikiem prądu stałego PZTK 88-35 TRR z przekładnią harmoniczną HDUC-3-158- o przełożeniu 1:158. Wał zakończony jest uchwytem do mocowania ramienia. Silnik znajduję się na zewnątrz obudowy, umożliwiając do niego dostęp w trakcie eksploatacji. Obudowę silnika można łatwo zdjąć, także w czasie działania robota. 9

1 ROZDZIAŁ. UŻYTY SPRZĘT I OPROGRAMOWANIE (a) widok z przodu (b) widok z boku Rysunek.1: Stanowisko badawcze Sarkofag, 1. konstrukcja nośna,. obudowa, 3. silnik, 4. przekładnia harmoniczna, 5. przetwornik obrotowo-impulsowy, 6. uchwyt, 7. ramię, 8. obciążniki Do wału silnika przymocowany jest przetwornik obrotowo-impulsowy do pomiaru prędkości obrotowej silnika. Uchwyt do mocowania ramienia zapewnia łatwe i pewne zaczepienie elementów o szerokości do 4mm. Ramię o większej szerokości nie będzie kompatybilne z uchwytem. W dowolnym miejscu ramienia istnieje możliwość montażu dodatkowych obciążników, zmieniających charakterystykę dynamiczną układu. Obudowa ma na celu osłonięcie poruszających się elementów robota mogących stanowić zagrożenie dla ludzi. Stanowi także element ochrony użytkowników na wypadek defektów mechanicznych ramienia lub nieprawidłowego montażu obciążników. Dodatkowo na uchwycie ramienia zostały zamontowane dwa czujniki służące do synchronizacji ramienia..1. Silnik wykonawczy Jednostką napędową robota jest silnik prądu stałego typu PZTK 88-35 TRR. Parametry silnika opisane zostały w tablicy.1.

.. MATLAB 11 Moment długotrwały -, 55[N m] Napięcie maksymalne - 35[V ] Prędkość maksymalna - 3[rpm] Moment maksymalny -, 8[N m] Prąd maksymalny - 7[A] Stała momentowa silnika - k M Ψ =, 15 ± 5%[N m Stała napięciowa silnika - k E Ψ = 11 1 [ V rpm A ] Moment bezwładności - J = 1, 45 1 4 [kg m ] Rezystancja - R A =, 56[Ω] Indukcyjność - L A =, 9[mH] Tablica.1: Dane silnika napędowego prądu stałego typu PZTK 88-35 TRR Silniki prądu stałego są zbudowane ze stojana i umieszczonego w nim wirnika. Stojan jest to nieruchoma obudowa z magnesami trwałymi, pomiędzy którymi powstaje pole magnetyczne. Wirnik składa się z wielu przewodów nawiniętych na ramki otaczające rdzeń ferromagnetyczny. Ramki tworzą uzwojenia wirnika i są połączone z komutatorem za pomocą szczotek. Komutator dostarcza napięcie na kolejne ramki uzwojenia wirnika, zmieniając kierunki przepływu prądu. Oddziaływanie pomiędzy polem magnetycznym powstałym wokół przewodnika i zewnętrznym polem magnetycznym stojana powoduje powstanie momentu obrotowego.. Matlab Matlab (ang. matrix laboratory) [1, 7] jest interaktywnym środowiskiem programistycznym stworzonym przez firmę The MathWorks Inc. Przeznaczony jest do przeprowadzania obliczeń numerycznych, wizualizacji i analizy danych. Matlab pozwala na opracowywanie algorytmów, tworzenie modeli, symulacji i aplikacji. Wysokopoziomowy język, narzędzia i wbudowane funkcje matematyczne pozwalają osiągać rozwiązanie szybciej niż w arkuszach kalkulacyjnych lub tradycyjnych językach programowania takich jak C++ lub Java. Głównym typem danych w matlabie są wektory i macierze. Dzięki temu funkcje przystosowane są do operacji na zmiennych wieloelementowych. Cechy środowiska programistycznego Matlab: język programowania wysokiego poziomu, integracja z językami: C, Java, Fortran, eksploracja i wizualizacja danych, wbudowane funkcje obliczeniowe,

1 ROZDZIAŁ. UŻYTY SPRZĘT I OPROGRAMOWANIE narzędzia do tworzenia GUI (graficznego interfejsu użytkownika). GUIDE (ang. Graphical User Interface Design Editor) jest narzędziem będącym elementem pakietu Matlab. Służy do tworzenia graficznych interfejsów użytkownika. Jest to interaktywny edytor generujący gotowy kod konstruujący UI, który może być później ręcznie modyfikowany. W tej pracy został wykorzystany do stworzenia GUI dla symulacji czasu rzeczywistego.

Rozdział 3 Model pojedynczego stopnia swobody W tym rozdziale przedstawiony jest model matematyczny stanowiska badawczego napędów Sarkofag. Model składa się z równań opisujących kinematykę (sekcja 3.1) i dynamikę (sekcja 3.) ramienia robota, oraz dynamikę napędu (sekcja 3.3). Model matematyczny jest opisowym modelem charakterystyki i zachowania układu. Właściwości układu opisywane są za pomocą zmiennych, a powiązania między zmiennymi w układzie opisywane są poprzez grupy odpowiednich funkcji. W zależności od ilości informacji o układzie przed modelowaniem można wyróżnić dwa typy modelów: model czarnej skrzynki (ang. black-box), model białej skrzynki (ang. white-box). Czarna skrzynka jest modelem projektowanym bez żadnej informacji o wewnętrznym działaniu układu. Funkcje wiążące wartości, użyte w tym modelowaniu, muszą być wyznaczone bez tej wiedzy. Ich wyznaczenie, razem z wartościami liczbowymi parametrów jest głównym zadaniem modelowania. Zwiększanie ilości parametrów utrudnia szacowanie ich wartości i połączeń miedzy nimi. Biała skrzynka jest modelem projektowanym z pełną wiedzą a priori. Wszystkie parametry modelu oraz wszystkie funkcje wiążące są znane. Wynikiem modelowania jest bardzo dokładny model zachowujący się zgodnie z rzeczywistym układem. Opisany model matematyczny wykorzystuje jak najwięcej znanej wiedzy bazując na gotowym robocie. Ze względu na złożoność otoczenia, w modelu wprowadzone są pewne przybliżenia i uproszczenia, nie jest to więc idealny model typu białej skrzynki. 13

14 ROZDZIAŁ 3. MODEL POJEDYNCZEGO STOPNIA SWOBODY Przedstawiony model jest uniwersalnym modelem dla manipulatorów o jednym stopniu swobody typu wahadło. Jednym z głównych założeń modelu jest poruszanie się tylko jednego członu, bez możliwości ruchu podstawy. 3.1 Kinematyka Kinematyka [] zajmuje się geometrycznymi i czasowymi własnościami ruchu, bez uwzględniania sił wywołujących ten ruch. Opisuje zależność pomiędzy strukturą manipulatora i nastawami ramion a zmianami położenia układów współrzędnych. Rysunek 3.1 przedstawia schemat kinamatyki robota Sarkofag. Osie z indeksem charakteryzują kolumnę obrotu manipulatora, osie z indeksem 1 - ruchomy człon manipulatora, osie z indeksem - końcówkę ramienia manipulatora (miejsce zaczepienia kolejnego członu). Rysunek 3.1: Schemat kinematyki manipulatora, a 1 - długość członu, θ 1 - kąt obrotu członu Kinematyka manipulatora opisana jest przez cztery parametry dla każdego członu. Dwa pierwsze charakteryzują aktualny człon manipulatora, dwa następne połączenie tego członu z następnym. w przedstawionym przypadku (para obrotowa) θ 1 (kąt obrotu członu) jest zmienną konfiguracyjną członu pierwszego, a a 1 (długość członu) jest ustalonym parametrem członu. Notacja Denavita-Hartenberga (tab. 3.1) pozwala szybko opisać i przedstawić konfigurację kinematyczną manipulatora. Macierze transformacji (3.1) opisują zależność pomiędzy współrzędnymi poszczególnych członów. Pierwsza macierz ( 1T ) opisuje rotację ramienia manipulatora względem kolumny obrotu manipulatora. druga macierz ( 1 T ) opisuje translację

3.. DYNAMIKA 15 i a i 1 α i 1 d i θ i 1 θ 1 a 1 Tablica 3.1: Parametry Denavita-Hartenberga opisujące manipulator końcówki ramienia względem jego początku (długość). Ostatnia macierz ( T ) jest złożeniem macierzy 1T z 1 T i opisuje położenie końcówki ramienia manipulatora względem kolumny obrotu manipulatora. 1T = 1 T = T = cos(θ 1 ) sin(θ 1 ) sin(θ 1 ) cos(θ 1 ) 1 1 1 a 1 1 1 1 cos(θ 1 ) sin(θ 1 ) a 1 cos(θ 1 ) sin(θ 1 ) cos(θ 1 ) a 1 sin(θ 1 ) 1 1 (3.1) 3. Dynamika Dynamika [, 3, 6] zajmuje się związkami miedzy ruchem mechanizmu a siłami i momentami sił wywołującymi ten ruch. Dynamiczne równania ruchu wiążą siły działające na manipulator z jego rozkładem masy i parametrami kinetycznymi. Z dynamiką związane są dwa zadania: 1. proste - mając dane wektory siły i momentu napędowego τ znaleźć wynikowy ruch manipulatora ( θ, θ, θ ).. odwrotne - mając dany punkt trajektorii ( θ, θ, θ ) znaleźć wymagane wektory siły i momentu napędowego τ, Efektywność sterowania manipulatorem robota zależy od dokładności jego modelu dynamicznego. W celu łatwiejszego zapoznania się z modelem, zmienne występujące w modelu zostały zebrane w tablicy 3..

16 ROZDZIAŁ 3. MODEL POJEDYNCZEGO STOPNIA SWOBODY M i - masa członu i C i I i - macierz inercji członu i j i R - macierz rotacji z układu { i } na układ { j } i jp - położenie punktu j na członie i i C i P - położenie punktu środka masy na członie i k ( i jω) - prędkość kątowa członu j względem członu i wyrażona w układzie { k } k ( i jɛ) - przyspieszenie kątowe członu j względem członu i wyrażona w układzie { k } k ( i ja) - przyspieszenie liniowe członu j względem członu i wyrażona w układzie { k } i if - siły oddziałujących na układ { i } i C F - siły bezwładności działające na człon i i in - momenty sił oddziałujące na układ { i } i C i N - momenty sił bezwładności działające na człon i Tablica 3.: Opisy wielkości fizycznych zastosowanych w modelu Założenia (3.): podstawa manipulatora i kolumna obrotu są nieruchome ω = ɛ = A = (3.) 3..1 Macierz inercji Macierz inercji członu 1 (3.3) jest zmienna w zależności od długości ramienia i położenia dodatkowego obciążnika. m k (yk + zk) m k x k y k m k z k x k k k k C i I 1 = m k x k y k m k (zk + x k) m k y k z k (3.3) k k k m k z k x k m k y k z k m k (x k + yk) k k k 3.. Prędkość kątowa Prędkość kątowa członu 1 (ramię) względem członu (kolumna) wyrażona w układzie {1} (początek ramienia) (3.4) jest równa sumie: prędkości kątowej członu wyrażonej w układzie {1}, prędkości kątowej pary obrotowej 1 (kolumna-ramię) w osi Z. 1 ( 1ω) = 1 R ( ω) + 1 1z θ 1 = 1 1z θ 1 = θ 1 (3.4) Ze względu na założenia (3.) pierwszy składnik jest zerowany.

3.. DYNAMIKA 17 3..3 Prędkość kątowa środka masy Prędkość kątowa środka masy członu 1 (ramię) względem członu (kolumna) wyrażona w układzie {1} (początek ramienia) (3.5) jest równa prędkości kątowej członu 1 względem członu wyrażonej w układzie {1} (3.4). 1 ( C 1 ω) = 1 ( 1ω) (3.5) 3..4 Przyspieszenie kątowe Przyspieszenie kątowe członu 1 (ramię) względem członu (kolumna) wyrażone w układzie {1} (początek ramienia) (3.6) jest równe sumie: przyspieszenia kątowego członu wyrażonej w układzie {1}, iloczynu wektorowego prędkość kątowej członu wyrażonej w układzie {1} i prędkości kątowej pary obrotowej 1 (kolumna-ramię), przyspieszenia kątowego pary obrotowej 1. 1 ( 1ɛ) = 1 R ( ɛ) + 1 R ( ω) 1 1z θ 1 + 1 1z θ 1 = 1 1z θ 1 = θ 1 (3.6) Ze względu na założenia (3.) pierwszy i drugi składnik są zerowane. 3..5 Przyspieszenie kątowe środka masy Przyspieszenie kątowe środka masy na członie 1 (ramię) względem członu (kolumna) wyrażone w układzie {1} (początek ramienia) (3.7) jest równe przyspieszeniu kątowemu członu 1 względem członu wyrażonemy w układzie {1} (3.6). 1 ( C 1 ɛ) = 1 ( 1ɛ) (3.7) 3..6 Przyspieszenie liniowe Przyspieszenie liniowe członu 1 (ramię) względem członu (kolumna) wyrażone w układu {1} (początek ramienia) (3.8) jest równe sumie: przyspieszenia liniowego członu wyrażonego w układzie {1}, iloczynu wektorowego przyspieszenia kątowego członu i położenia punktu przyłączenia członu 1 na członie, wyrażonego w układzie {1}, iloczynu wektorowego:

18 ROZDZIAŁ 3. MODEL POJEDYNCZEGO STOPNIA SWOBODY prędkości kątowej członu, iloczynu wektorowego prędkości kątowej członu i położenia punktu przyłączenia członu 1 na członie, wyrażonego w układzie {1}. 1 ( 1A) = 1 R [ ( A) + ( ɛ) 1P + ( ω) ( ( ω) 1P )] = (3.8) Ze względu na założenia (3.) przyspieszenie liniowe jest zerowane. 3..7 Przyspieszenia liniowe środka masy Przyspieszenie liniowe środka masy na członie 1 (ramię) względem członu (kolumna) wyrażone w układzie {1} (początek ramienia) (3.9) jest równe sumie: przyspieszenia liniowego członu 1 względem członu wyrażonego w układzie {1}, iloczynu wektorowego przyspieszenia kątowego członu 1 względem członu wyrażonego w układzie {1} i położenia punktu środka masy na członie 1, iloczynu wektorowego: prędkości kątowej członu 1 względem członu wyrażonego w układzie {1}, iloczynu wektorowego prędkości kątowej członu 1 względem członu wyrażonego w układzie {1} i położenia punktu środka masy na członie 1. 1 ( C 1 A) = 1 ( 1A) + 1 ( 1ɛ) 1 C 1 P + 1 ( 1ω) (1 ( 1ω) 1 C 1 P ) θ 1 1 C 1 P x θ 1 = + θ 1 C 1 P y + 1 1 θ 1 C 1 P x θ 1 1 θ 1 C 1 P y 1 1 C 1 P z C 1 P z 1 C 1 P y θ 1 θ 1 = C 1 P x θ 1 + 1 1 C 1 P y θ 1 θ 1 1 C 1 P x θ 1 1 C 1 P y θ 1 1 C 1 P x θ 1 = 1 C 1 P x θ 1 1 C 1 P yθ 1 (3.9)

3.. DYNAMIKA 19 3..8 Siły bezwładności Siły bezwładności przyłożone do punktu środka masy na członie 1 (ramię) (3.1) są równe iloczynowi masy członu 1 i przyspieszeń środka masy względem członu (kolumna) wyrażonym w układzie {1} (początek ramienia). 1 C 1 P y θ 1 1 C 1 P y θ 1 1 C 1 F = M 1 1 ( C 1 A) = M 1 1 C 1 P x θ 1 1 C 1 P yθ 1 Siły bezwładności obliczane są z równania Newtona. (3.1) 3..9 Momenty sił bezwładności Momenty sił bezwładności przyłożonych do punktu środka masy na członie 1 (ramię) (3.11) są równe sumie: iloczynu wektorowego: prędkości kątowej punktu środka masy na członie 1 względem członu (kolumna) wyrażonej w układzie {1} (początek ramienia), iloczynu macierzy inercji członu i prędkości kątowej punktu środka masy na członie 1 względem członu wyrażonej w układzie {1}, iloczynu macierzy inercji członu 1 i przyspieszenia kątowego punktu środka masy na członie 1 względem członu wyrażonej w układzie {1}. 1 C 1 N = 1 ( C 1 ω) [ C 1 I 1 1 ( C 1 ω) ] + C 1 I 1 1 ( C 1 ɛ) C θ 1 1 C I 1 C 1xx I 1 C 1xy I 1xz 1 C I 1 C = θ C 1 1 C I 1 C 1xy I 1 1yy I 1yz + 1xx I 1 1xy I 1xz C 1 C I 1 C 1xy I 1 1yy I 1yz C 1 C I 1 C 1xz I 1 1yz I 1zz θ C 1 C 1 I 1 C 1xz I 1 1yz I 1zz θ 1 C θ 1 1 I 1xz C θ 1 1 I = θ C 1 1 I 1yz θ 1 + 1xz θ1 C 1 I 1yz θ1 C 1 I 1zz θ C 1 1 I 1zz θ1 C 1 I 1yz θ 1 + C 1 I 1xz θ1 = C 1 I 1xz θ 1 + C 1 I 1yz θ1 C 1 I 1zz θ1 Momenty siły bezwładności obliczone są z równania Eulera. (3.11)

ROZDZIAŁ 3. MODEL POJEDYNCZEGO STOPNIA SWOBODY 3..1 Suma sił oddziałujących na ramię Siły oddziałujące na człon 1 (ramię) (3.1) są równe sumie: sił bezwładności przyłożonym do punktu środka masy na członie 1 (ramię), sił zewnętrznych przyłożonych do układu {} (końcówka ramienia) względem układu {1} (początek ramienia), przeciwieństw sił grawitacji członu 1. 1 1F = 1 C 1 F + 1 R F M 1 1 g 1 C 1 P y θ1 1 C 1 P x θ 1 1 F x g y sin(θ 1 ) = M 1 1 C 1 P x θ1 1 C 1 P y θ 1 + 1 F y M 1 g y cos(θ 1 ) 1 F z ] M 1 [ 1 C 1 P y θ1 1 C 1 P x θ 1 + g y sin(θ 1 ) + F x ] = M 1 [ 1 C 1 P x θ1 1 C 1 P y θ 1 + g y sin(θ 1 ) + F y F z (3.1) gdzie siły zewnętrzne przyłożone do układu (3.13): F = F x F y F z (3.13) 3..11 Suma momentów sił oddziałujących na ramię Momenty sił oddziałujące na człon 1 (ramię) (3.14) są równe sumie: momentów sił bezwładności przyłożonych do punktu środka masy na członie 1 (ramię), momentów zewnętrznych przyłożonych do układu {} (końcówka ramienia) względem układu {1} (początek ramienia), iloczynu wektorowego położenia punktu środka masy na członie 1 i sił oddziałujących na człon 1, iloczynu wektorowego: różnicy położenia punktu końcowego członu 1 i położenia punktu środka masy na członie 1, sił zewnętrznych przyłożonych do układu {} względem układu {1}.

3.. DYNAMIKA 1 1 1N = 1 C 1 N + 1 R N + 1 C 1 P 1 1F + ( 1 P 1 C 1 P ) ( 1 R F ) C 1 I 1yz θ 1 + C 1 I 1xz θ1 = C 1 I 1xz θ 1 + C 1 I 1yz θ1 + C 1 I 1zz θ1 ] 1 1 C 1 P z C 1 P y M 1 [ 1 C 1 P y θ1 1 C 1 P x θ 1 + g y sin(θ 1 ) + F x ] 1 + C 1 P z 1 C 1 P x M 1 [ 1 1 C 1 P x θ1 1 C 1 P y θ 1 + g y sin(θ 1 ) + F y + 1 C 1 P y C 1 P x F z a 1 1 C 1 P x 1 F x + 1 C 1 P y 1 F y 1 C 1 P z 1 F z (3.14) gdzie momenty zewnętrzne przyłożone do układu (3.15): N = (3.15) 3..1 Moment siły wzdłuż osi Z Moment siły wzdłuż osi Z (3.16) jest częścią sumy momentów sił oddziałujących na ramię (3.14), działających wzdłuż osi obrotu ramienia. [ N z = C 1 I 1zz θ1 1 C 1 P y [M1 + 1 C 1 P x [M1 1 C 1 P y θ1 1 C 1 P x [ 1 C 1 P x θ1 1 C 1 P y θ 1 + g y cos(θ 1 ) ] θ 1 + g y sin(θ 1 ) + F ] x + ] + F ] y + + 1 C 1 P y F x + ( a 1 1 C 1 P x ) F y = ( C 1 I 1zz + 1 C 1 P y M1 + 1 C 1 P x M1 ) θ1 + ( 1 C 1 P x 1 C 1 P y M 1 ) θ 1 + + ( 1 C 1 P y sin(θ 1 ) + 1 C 1 P x cos(θ 1 ) ) M 1 g y + (3.16) 1 C 1 P y F x + 1 C 1 P x F y + 1 C 1 P y F x + a 1 F y 1 C 1 P x F y = [ C 1 I 1zz + M 1 ( 1C1 P x + 1 C1 P y )] θ1 + + ( 1 C1 P x cos(θ 1 ) 1 C 1 P y sin(θ 1 ) ) M 1 g y + a 1 F y Po przekształceniach otrzymujemy wzory do zastosowania w obliczeniach:

ROZDZIAŁ 3. MODEL POJEDYNCZEGO STOPNIA SWOBODY moment siły bezwładności (3.17), [ C1 I 1zz + M 1 ( 1 C 1 P x + 1 C 1 P y )] θ1 (3.17) moment siły grawitacji (3.18), ( 1 C 1 P x cos(θ 1 ) 1 C 1 P y sin(θ 1 ) ) M 1 g y (3.18) moment sił zewnętrznych (3.19). a 1 F y (3.19) 3.3 Dynamika napędu Dynamika napędu [5] opisuje parametry silnika i momenty sił występujące po jego stronie, w szczególności zależność między dostarczanym napięciem elektrycznym na zaciski silnika a otrzymywanym momencie elektromotorycznym. 3.3.1 Moment elektromagnetyczny Moment elektromagnetyczny τ g1 (3.) jest to moment sił powstających przez oddziaływanie pola magnetycznego w silniku lub prądnicy. Z perspektywy silnika jest on równy sumie: momentu sił bezwładności silnika τ I1 (3.1), momentu sił tarcia widzianego na wale silnika τ f1 (3.), momentu sił ogniwa widzianego na wale silnika τ r1 (3.5). τ g1 = τ I1 + τ f1 + τ r1 (3.) 3.3. Moment sił bezwładności silnika Moment sił bezwładności silnika (3.1) zależy od przyspieszenia kątowego wału silnika θ m1 i bezwładności silnika I m1. θ m1 = θ 1 r 1 jeżeli w układzie nie ma luzu r 1 - przełożenie przekładni r 1 1 τ I1 = I m1 θm1 (3.1)

3.3. DYNAMIKA NAPĘDU 3 3.3.3 Moment tarcia widziany na wale silnika Moment tarcia widziany na wale silnika (3.) jest równy sumie tarcia suchego, lepkiego i statycznego. τ f1 = µsign θ m1 + f 1 ( θ m1 ) + f 1 ( θ m1 ) tarcie suche tarcie lepkie tarcie statyczne (3.) Tarcie suche - występujące pomiędzy powierzchniami bez żadnych ciał obcych. Tarcie lepkie (3.3) - występujące pomiędzy powierzchniami rozdzielonymi środkiem smarnym. f 1 ( θ m1 ) = b 1 θm1 lub (3.3) b 1 θ m1 Tarcie statyczne (3.4) - występujące pomiędzy powierzchniami nie przemieszczającymi się względem siebie. f1 = B + θ m1 < θ p B θp < θ m1 (3.4) dla pozostałych θ m1 3.3.4 Moment sił ogniwa widziany na wale silnika Moment sił przyłożonych do ogniwa widziany na wale silnika (3.5) jest równy momentowi siły oddziałujących na ramię wzdłuż osi Z (3.16) pomnożonej przez przełożenie przekładni. τ r1 = r 1 τ 1 (3.5) r 1 - przełożenie przekładni r 1 1 3.3.5 Równanie elektryczne Równanie elektryczne (3.6) silnika prądu stałego ma postać:

4 ROZDZIAŁ 3. MODEL POJEDYNCZEGO STOPNIA SWOBODY e 1 = di i 1 R 1 + L 1 1 dt b 1 τ g1 = K i1 i 1 (3.6) e b1 = K b1 θm1 siła elektromotoryczna przyłożona do silnika e 1 jest równa sumie: napięcia powstałego przez przepływ prądu w obwodzie i rezystancję uzwojenia silnika, napięcia powstałego przez zmianę prądu płynącego w obwodzie i indukcyjność uzwojenia silnika, siły elektromotorycznej indukowanej w obwodzie, moment elektromagnetyczny τ g1 jest wprost proporcjonalny do natężenia prądu przepływającego przez silnik i stałej momentowej silnika, siła elektromotoryczna indukowana w obwodzie e b1 jest wprost proporcjonalna do prędkości kątowej wirnika silnika i stałej napięciowej silnika. Rysunek 3.: Schemat zastępczy układu elektrycznego silnika e 1 - siła elektromotoryczna na zaciskach silnika i 1 - prąd płynący w obwodzie R 1 - rezystancja uzwojenia silnika L 1 - indukcyjność uzwojenia silnika e b1 - siła elektromotoryczna indukowana w obwodzie τ g1 - moment elektromagnetyczny K i1 - stała momentowa silnika K b1 - stała napięciowa silnika

Rozdział 4 Symulacja W tym rozdziale przedstawiony jest opis stworzonej symulacji robota o pojedynczym stopniu swobody typu wahadło. Cechy zaprojektowanej symulacji: deterministyczna - zapewnia powtarzalne wyniki zależne od danych wejściowych, dynamiczna - wynikiem jest proces przebiegający w czasie, dyskretna - stały przyrost czasu, lokalna - przetwarzanie odbywa się na pojedynczym komputerze. Implementację symulacji (rys. 4.1) oparto na pięciu funkcjach (sekcja 4.1) wyodrębnionych ze względu na sposób sterowania determinujący dane wejściowe i wyjściowe. Zaprojektowaną symulację można używać w dwóch trybach: bezpośrednio odwołując się do funkcji symulacyjnych w przypadkach testowych (sekcja 4.) lub w czasie rzeczywistym poprzez przygotowany graficzny interfejs użytkownika (sekcja 4.3). 4.1 Funkcje symulacyjne Model symulacyjny powstał w oparciu o przedstawiony wcześniej w rozdziale 3 model matematyczny. Dla potrzeb symulacji opracowano pięć funkcji: ruch bez zadanego sterowania (sekcja 4.1.1), sterowanie prędkością ramienia (sekcja 4.1.), sterowanie przyspieszeniem ramienia (sekcja 4.1.3), sterowanie natężeniem prądu elektrycznego (sekcja 4.1.4), 5

6 ROZDZIAŁ 4. SYMULACJA Symulacja Bezpośrednie testowanie Przypadek testowy Funkcje symulacyjne sterowanie prędkością ramienia sterowanie przyspieszeniem ramienia sterowanie natężeniem prądu w silniku sterowanie napięciem elektrycznym na silniku brak sterowania Graficzny interfejs użytkownika Symulacja czasu rzeczywistego Rysunek 4.1: Schemat symulacji sterowanie napięciem elektrycznym (sekcja 4.1.5). Inicjalizacja parametrów silnika i ramienia jest wspólna dla wszystkich funkcji: 1 % ramie 3 % charakterystyka 4 arm_mass = arm(1); % [kg] 5 arm_length = arm(); % [m] 6 arm_point = arm(3); % [%] 7 8 % dlugosc ramienia 9 a_1 = arm_length; % [m] 1 11 % kierunek grawitacji 1 g_ = [; 9.8665; ]; % [m / s^] 13 14 % masa czlonu 15 M_1 = arm_mass; % [kg] 16 17 % macierz inercji 18 I =.66 + arm_mass*(arm_length*arm_length*arm_point/1*arm_point/1); % [kg * m^] 19 I_1 = [I I I; I I I; 1 I I I]; 3 % polozenie srodka masy dla czlonu 1 4 P1_c1 = [arm_length*arm_point/1; ; ]; % [m] 5 6 % odzialywanie zewnetrzne

4.1. FUNKCJE SYMULACYJNE 7 7 F_ = [; ext_force; ]; % [kg * m / s^] 8 9 % silnik 3 31 % przekladnia silnika 3 r_1 = 158; 33 34 % sprawnosc przekladni 35 r_spr = 75/1; 36 37 % stala momentowa silnika 38 K_i1 =.15; % [N * m / A] = [kg * m^ / s^ / A] 39 4 % stala napieciowa silnika 41 K_b1 = 11/1; % [V / (obr/min)] 4 K_b1 = K_b1*6; % [V / (obr/s)] 43 44 % bezwladnosc silnika 45 I_m1 =.145; % [kg * m^] 46 47 % rezystancja uzwojenia 48 R_1 =.56; % [ohm] 49 5 % indukcyjnosc uzwojenia 51 L_1 =.9; % [mh] Charakterystyki ramienia podawane są jako argumenty funkcji. Dane silnika wprowadzone są na podstawie tablicy.1. 4.1.1 Ruch bez zadanego sterowania Funkcja wyznacza następny stan układu przy niezadanym żadnym sterowaniu. Zasilanie jest odłączone, a układ elektryczny silnika jest przerwany. Wyniki wyjściowe podawane są dla danego przyrostu czasu. 1 function [dynamic, torque] = model_sarkofag(dynamic, arm, friction, dt, ext_force) %% 65 % obliczenia 66 67 % ramie 68 c1 = cos(fi_1); 69 s1 = sin(fi_1); 7 71 mom_bezw = (I_1(3,3) + M_1 * (P1_c1(1)) * P1_c1(1) + P1_c1() * P1_c1()) * fi_1kk / 4 / pi / pi; % [kg * m^ / s^] 7 mom_graw = (P1_c1(1) * c1 - P1_c1() * s1) * M_1 * g_(); % [kg * m^ / s^] 73 mom_wyp = mom_graw + mom_bezw + a_1 * F_(); % [kg * m^ / s^] 74 75 % tarcie 76 w_arm = fi_1k + (-mom_wyp * dt / I * * pi); 77 78 if abs(fi_1k) <.1 79 % statyczne 8 mom_fr = friction(1); 81 8 % ruszanie z miejsca 83 if abs(mom_wyp) > mom_fr

8 ROZDZIAŁ 4. SYMULACJA 84 moment = -mom_wyp - mom_fr; 85 w_arm = fi_1k + (moment * dt / I * * pi); 86 else 87 moment = ; 88 w_arm = ; 89 end 9 91 else 9 % dynamiczne 93 mom_fr = sign(w_arm)*(friction()*abs(w_arm) + friction(3)); 94 95 moment = -mom_wyp - mom_fr; 96 w_arm = fi_1k + (moment * dt / I * * pi); 97 98 if abs(mom_fr) > abs(mom_wyp) 99 if (w_arm * fi_1k) < 1 w_arm = ; 11 end 1 end 13 end 14 15 % motor 16 e_b1 = K_b1 * fi_1k / / pi * r_1; 17 i_1 = ; 18 mom_motor = ; 19 e_1 = e_b1; 11 111 % obrot 11 a_arm = fi_1 + (w_arm * dt); 113 114 % dynamika 115 fi_1kk = ((a_arm - fi_1)/dt - fi_1k)/dt; 116 fi_1k = (a_arm - fi_1)/dt; 117 fi_1 = a_arm; Opis funkcji z odwołaniami do modelu matematycznego. 71 - Moment bezwładności (3.17). 7 - Moment siły grawitacji (3.18). 73 - Suma momentów sił oddziałujących na ramię (3.16). 76-13 - Moment siły tarcia ramienia. Przedstawiony model opisywał tarcie widziane na wale silnika (3.). Przy ruszaniu obowiązuję tarcie statyczne, po przekroczeniu prędkości minimalnej - tarcie dynamiczne. 16-19 - Siła elektromotoryczna indukowana w obwodzie (3.6). Obwód jest przerwany więc prąd nie płynie. 11-117 - Obrót ramienia i nowe przyspieszenie, prędkość i przemieszczenie. Grawitacja jest główną siłą napędową przy braku zadanego sterowania, moment sił grawitacji jest pierwszym elementem obliczanym w funkcji. Następnie obliczane

4.1. FUNKCJE SYMULACYJNE 9 są siły bezwładności i uwzględniane ewentualne siły zewnętrzne. Obroty ramienia powodują wytworzenie napięcia na zaciskach silnika, które jest notowane. Po obliczeniu wynikowego momentu obrotowego, uwzględniane są siły tarcia i wyznaczane nowe przemieszczenie ramienia. dane wejściowe: dynamic - poprzedni stan robota (wektor) położenie ramienia [rad] prędkość kątowa ramienia [ ] rad s przyspieszenie kątowe ramienia [ ] rad s natężenie prądu elektrycznego przepływającego przez silnik [A] napięcie elektryczne na silniku [V ] arm - charakterystyka ramienia (wektor) masa ramienia [kg] długość ramienia [m] położenie punktu środka masy ramienia względem całkowitej długości ramienia friction - charakterystyka funkcji tarcia (wektor) współczynnik tarcia statycznego współczynnik A tarcia dynamicznego współczynnik B tarcia dynamicznego gdzie tarcie dynamiczne = A θ 1 + B sign( θ 1 ) dt - przyrost czasu pomiędzy kolejnymi krokami [s] ext_force - zewnętrzny moment siły przyłożony do ramienia [N m] dane wyjściowe: dynamic - następny stan robota (wektor) położenie ramienia [rad] prędkość kątowa ramienia [ ] rad s przyspieszenie kątowe ramienia [ ] rad s

3 ROZDZIAŁ 4. SYMULACJA natężenie prądu elektrycznego przepływającego przez silnik [A] napięcie elektryczne na silniku [V ] torque - aktualne momenty sił wpływające na ramię (wektor) moment siły grawitacji [N m] moment siły bezwładności [N m] moment siły motorycznej po przekładni [N m] moment siły tarcia [N m] moment siły wypadkowej [N m] 4.1. Sterowanie prędkością ramienia Funkcja wyznacza następny stan układu przy zadanej stałej prędkości kątowej ramienia manipulatora. 1 function [dynamic, torque] = model_sarkofag_speed(dynamic, arm, friction, constspeed, dt, ext_force) 3 %% 65 % obliczenia 66 67 % obrot 68 w_arm = constspeed; 69 moment = ; 7 a_arm = fi_1 + (w_arm * dt); 71 7 % ramie 73 c1 = cos(fi_1); 74 s1 = sin(fi_1); 75 76 mom_bezw = (I_1(3,3) + M_1 * (P1_c1(1)) * P1_c1(1) + P1_c1() * P1_c1()) * fi_1kk / 4 / pi / pi; % [kg * m^ / s^] 77 mom_graw = (P1_c1(1) * c1 - P1_c1() * s1) * M_1 * g_(); 78 mom_wyp = mom_graw + mom_bezw + a_1 * F_(); % [kg * m^ / s^] 79 8 % tarcie 81 mom_fr = sign(fi_1k)*(friction()*abs(fi_1k) + friction(3)); 8 83 % motor 84 mom_motor = mom_wyp + mom_fr; 85 i_ = i_1; 86 i_1 = mom_motor / K_i1 / r_1 / r_spr; 87 e_b1 = K_b1 * fi_1k / / pi * r_1; 88 e_1 = i_1 * R_1 + L_1 * (i_1 - i_) / dt + e_b1; 89 9 % dynamika 91 fi_1kk = ((a_arm - fi_1)/dt - fi_1k)/dt; 9 fi_1k = (a_arm - fi_1)/dt; 93 fi_1 = a_arm; Opis funkcji z odwołaniami do modelu matematycznego.

4.1. FUNKCJE SYMULACYJNE 31 68-7 - Prędkość ramienia jest stała, więc wypadkowa momentów sił musi się równać zero. Obrót ramienia zależy od prędkości zadanej. 76 - Moment bezwładności (3.17) powinien się zerować. 77 - Moment siły grawitacji (3.18). 78 - Suma momentów sił oddziałujących na ramię (3.16). 81 - Moment siły tarcia. 84-88 - Moment elektromagnetyczny (3.6) musi równoważyć sumę momentów sił oddziałujących na ramię (3.16) i moment siły tarcia. Wyliczane stąd jest potrzebne natężenie prądu i wymagane napięcie na zaciskach silnika. 91-93 - Nowe przyspieszenie, prędkość i przemieszczenie. Pierwszy wyznaczony przez funkcję jest moment siły grawitacji ramienia. Ze względu na stała prędkość, przyspieszenie ramienia jest równe zero, a więc moment bezwładności również jest zerowany. Moment siły tarcia ramienia obliczany jest bezpośrednio z zadanej prędkości i jest stały przy braku zmiany prędkości kątowej ramienia. Aby zachować stałą prędkość ramienia, moment elektromotoryczny musi być równy sumie momentów siły grawitacji ramienia i siły tarcia. Następnie obliczane są natężenia prądu w układzie silnika potrzebne do uzyskania odpowiedniego momentu elektromagnetycznego i napięcie siły elektromotorycznej indukowanej w obwodzie powstałej przez obroty silnika. Mając dane te dwie wartości wyznaczana jest wartość potrzebnej siły elektromotorycznej źródła przyłożonego do silnika. Ostatecznie wyznaczane jest nowe przemieszczenie ramienia. dane wejściowe: dynamic - poprzedni stan robota (wektor) położenie ramienia [rad] prędkość kątowa ramienia [ ] rad s przyspieszenie kątowe ramienia [ ] rad s natężenie prądu elektrycznego przepływającego przez silnik [A] napięcie elektryczne na silniku [V ] arm - charakterystyka ramienia (wektor) masa ramienia [kg]

3 ROZDZIAŁ 4. SYMULACJA długość ramienia [m] położenie punktu środka masy ramienia względem całkowitej długości ramienia friction - charakterystyka funkcji tarcia (wektor) współczynnik tarcia statycznego współczynnik A tarcia dynamicznego współczynnik B tarcia dynamicznego gdzie tarcie dynamiczne = A θ 1 + B sign( θ 1 ) const_speed - ustalona stała wartość prędkości ramienia [ ] rad s dt - przyrost czasu pomiędzy kolejnymi krokami [s] ext_force - zewnętrzny moment siły przyłożony do ramienia [N m] dane wyjściowe: dynamic - następny stan robota (wektor) położenie ramienia [rad] prędkość kątowa ramienia [ ] rad s przyspieszenie kątowe ramienia [ ] rad s natężenie prądu elektrycznego przepływającego przez silnik [A] napięcie elektryczne na silniku [V ] torque - aktualne momenty sił wpływające na ramię (wektor) moment siły grawitacji [N m] moment siły bezwładności [N m] moment siły motorycznej po przekładni [N m] moment siły tarcia [N m] moment siły wypadkowej [N m]

4.1. FUNKCJE SYMULACYJNE 33 4.1.3 Sterowanie przyspieszeniem ramienia Funkcja wyznacza następny stan mechanizmu przy zadanym stałym przyspieszeniu kątowym ramienia manipulatora bez względu na wychylenie. 1 function [dynamic, torque] = model_sarkofag_acceleration(dynamic, arm, friction, constacc, dt, ext_force) 3 %% 65 % obliczenia 66 67 % obrot 68 moment = constacc * I / / pi; 69 w_arm = fi_1k + constacc * dt; 7 a_arm = fi_1 + (w_arm * dt); 71 7 % ramie 73 c1 = cos(fi_1); 74 s1 = sin(fi_1); 75 76 mom_bezw = (I_1(3,3) + M_1 * (P1_c1(1)) * P1_c1(1) + P1_c1() * P1_c1()) * fi_1kk / 4 / pi / pi; % [kg * m^ / s^] 77 mom_graw = (P1_c1(1) * c1 - P1_c1() * s1) * M_1 * g_(); 78 mom_wyp = mom_graw + mom_bezw + a_1 * F_(); % [kg * m^ / s^] 79 8 % tarcie 81 if abs(fi_1k) <.1 8 % statyczne 83 mom_fr = friction(1); 84 else 85 % dynamiczne 86 mom_fr = sign(w_arm)*(friction()*abs(w_arm) + friction(3)); 87 end 88 89 % motor 9 mom_motor = moment + mom_fr + mom_wyp; 91 i_ = i_1; 9 i_1 = (mom_motor - I_m1 * fi_1kk / 4 / pi / pi * r_1) / K_i1 / r_1 / r_spr; 93 e_b1 = K_b1 * w_arm / / pi * r_1; 94 e_1 = i_1 * R_1 + L_1 * (i_1 - i_) / dt + e_b1; 95 96 % dynamika 97 fi_1kk = ((a_arm - fi_1)/dt - fi_1k)/dt; 98 fi_1k = (a_arm - fi_1)/dt; 99 fi_1 = a_arm; Opis funkcji z odwołaniami do modelu matematycznego. 68-7 - Przyspieszenie ramienia jest stałe, więc wypadkowa momentów sił musi mieć określoną wartość. Prędkość ramienia zależy od zadanego przyspieszenia a obrót od nowej prędkości. 76 - Moment bezwładności (3.17) stały dla stałego przyspieszenia. 77 - Moment siły grawitacji (3.18).

34 ROZDZIAŁ 4. SYMULACJA 78 - Suma momentów sił oddziałujących na ramię (3.16). 81-87 - Moment siły tarcia ramienia zmienia się w czasie przez zmianę prędkości. Przy ruszaniu obowiązuję tarcie statyczne, po przekroczeniu prędkości minimalnej - tarcie dynamiczne. 9-94 - Moment elektromagnetyczny (3.6) musi równoważyć potrzebny moment wypadkowy, momenty sił oddziałujących na ramię (3.16) i moment siły tarcia. Wyliczane stąd jest potrzebne natężenie prądu i wymagane napięcie na zaciskach silnika. 91-93 - Nowe przyspieszenie, prędkość i przemieszczenie. Pierwszy wyznaczony przez funkcję jest moment sił bezwładności oddziałujący na ramię, a następnie moment sił grawitacji ramienia. Moment sił tarcia zależy od prędkości ramienia i jest zmienny w czasie. Aby przyspieszenie było stałe w czasie potrzebny jest odpowiedni stały moment sił wypadkowych. Moment elektromotoryczny musi przeważać sumę wypadkową aby przyspieszenie było dodatnie lub być zaniżać aby zwalniać. Następnie obliczane są natężenia prądu w układzie silnika potrzebne do uzyskania odpowiedniego momentu elektromagnetycznego i napięcie siły elektromotorycznej indukowanej w obwodzie powstałej przez obroty silnika. Mając dane te dwie wartości wyznaczana jest wartość potrzebnej siły elektromotorycznej źródła przyłożonego do silnika. Ostatecznie wyznaczane jest nowe przemieszczenie ramienia. dane wejściowe: dynamic - poprzedni stan robota (wektor) położenie ramienia [rad] prędkość kątowa ramienia [ ] rad s przyspieszenie kątowe ramienia [ ] rad s natężenie prądu elektrycznego przepływającego przez silnik [A] napięcie elektryczne na silniku [V ] arm - charakterystyka ramienia (wektor) masa ramienia [kg] długość ramienia [m] położenie punktu środka masy ramienia względem całkowitej długości ramienia

4.1. FUNKCJE SYMULACYJNE 35 friction - charakterystyka funkcji tarcia (wektor) współczynnik tarcia statycznego współczynnik A tarcia dynamicznego współczynnik B tarcia dynamicznego gdzie tarcie dynamiczne = A θ 1 + B sign( θ 1 ) const_acc - ustalona stała wartość przyspieszenia ramienia [ ] rad s dt - przyrost czasu pomiędzy kolejnymi krokami [s] ext_force - zewnętrzny moment siły przyłożony do ramienia [N m] dane wyjściowe: dynamic - następny stan robota (wektor) położenie ramienia [rad] prędkość kątowa ramienia [ ] rad s przyspieszenie kątowe ramienia [ ] rad s natężenie prądu elektrycznego przepływającego przez silnik [A] napięcie elektryczne na silniku [V ] torque - aktualne momenty sił wpływające na ramię (wektor) moment siły grawitacji [N m] moment siły bezwładności [N m] moment siły motorycznej po przekładni [N m] moment siły tarcia [N m] moment siły wypadkowej [N m] 4.1.4 Sterowanie natężeniem prądu silnika Funkcja wyznacza następny stan mechanizmu przy zadanym stałym natężeniu prądu dostarczanego do układu. Ze względu na dynamikę elektryczną, wewnątrz funkcji znajdują się dodatkowe pętle zwiększające dokładność obliczeń. Wyniki wyjściowe podawane są dla danego przyrostu czasu.

36 ROZDZIAŁ 4. SYMULACJA 1 function [dynamic, torque] = model_sarkofag_electric(dynamic, arm, friction, constelectric, dt, ext_force) 3 %% 7 % obliczenia 71 7 % motor 73 i_ = i_1; 74 i_1 = constelectric; 75 e_b1 = K_b1 * fi_1k / / pi * r_1; 76 e_1 = i_1 * R_1 + L_1 * (i_1 - i_) / dt + e_b1; 77 i_1 = (i_ * L_1 / dt + e_1 - e_b1) / (R_1 + L_1/dt); 78 mom_motor = (K_i1 * i_1 * r_1 + I_m1 * fi_1kk / 4 / pi / pi * r_1) * r_spr; 79 8 % ramie 81 c1 = cos(fi_1); 8 s1 = sin(fi_1); 83 84 mom_bezw = (I_1(3,3) + M_1 * (P1_c1(1)) * P1_c1(1) + P1_c1() * P1_c1()) * fi_1kk / 4 / pi / pi; % [kg * m^ / s^] 85 mom_graw = (P1_c1(1) * c1 - P1_c1() * s1) * M_1 * g_(); 86 mom_wyp = mom_graw + mom_bezw + a_1 * F_(); % [kg * m^ / s^] 87 88 % tarcie 89 w_arm = fi_1k + ((mom_motor - mom_wyp) * dt / I * * pi); 9 91 if abs(fi_1k) <.1 9 % statyczne 93 mom_fr = friction(1); 94 95 % ruszanie z miejsca 96 if abs(mom_motor - mom_wyp) > mom_fr 97 moment = mom_motor - mom_wyp - mom_fr; 98 w_arm = fi_1k + (moment * dt / I * * pi); 99 else 1 moment = ; 11 w_arm = ; 1 end 13 14 else 15 % dynamiczne 16 mom_fr = sign(w_arm)*(friction()*abs(w_arm) + friction(3)); 17 moment = mom_motor - mom_wyp - mom_fr; 18 w_arm = fi_1k + (moment * dt / I * * pi); 19 11 if abs(mom_fr) > abs(mom_motor - mom_wyp) 111 if (w_arm * fi_1k) < 11 w_arm = ; 113 end 114 end 115 end 116 117 % obrot 118 a_arm = fi_1 + (w_arm * dt); 119 1 % dynamika 11 fi_1kk = ((a_arm - fi_1)/dt - fi_1k)/dt; 1 fi_1k = (a_arm - fi_1)/dt; 13 fi_1 = a_arm;

4.1. FUNKCJE SYMULACYJNE 37 Opis funkcji z odwołaniami do modelu matematycznego. 73-78 - Natężenie prądu dostarczanego do układu jest stałe. Moment elektromagnetyczny (3.6) wyznaczany jest z natężenia prądu w układzie. Potrzebne napięcie obliczane jest z dostarczanego natężenia i dodatkowej siły elektromotorycznej, indukowanej przez obroty ramienia. 84 - Moment bezwładności (3.17). 85 - Moment siły grawitacji (3.18). 86 - Suma momentów sił oddziałujących na ramię (3.16). 89-115 - Moment siły tarcia ramienia zmienia się w czasie przez zmianę prędkości. Przy ruszaniu obowiązuję tarcie statyczne, po przekroczeniu prędkości minimalnej - tarcie dynamiczne. 118-13 - Obrót ramienia i nowe przyspieszenie, prędkość i przemieszczenie. W pierwszej kolejności wyznaczana jest wartość siły elektromotorycznej na zaciskach silnika, potrzebnego do przepływu zadanego prądu. W tym celu obliczane jest napięcia elektryczne układu i siła elektromotoryczna indukowana w obwodzie przez obroty ramienia. Następnie wyznaczane jest natężenie prądu elektrycznego tworzącego pole magnetyczne i moment elektromagnetyczny. W dalszej kolejności obliczane są wartości momentów obrotowych związanych z dynamiką ramienia takie jak moment sił grawitacji i moment sił bezwładności. Na koniec wyznaczane są siły tarcia i nowe przemieszczenie ramienia. dane wejściowe: dynamic - poprzedni stan robota (wektor) położenie ramienia [rad] prędkość kątowa ramienia [ ] rad s przyspieszenie kątowe ramienia [ ] rad s natężenie prądu elektrycznego przepływającego przez silnik [A] napięcie elektryczne na silniku [V ] arm - charakterystyka ramienia (wektor) masa ramienia [kg] długość ramienia [m]

38 ROZDZIAŁ 4. SYMULACJA położenie punktu środka masy ramienia względem całkowitej długości ramienia friction - charakterystyka funkcji tarcia (wektor) współczynnik tarcia statycznego współczynnik A tarcia dynamicznego współczynnik B tarcia dynamicznego gdzie tarcie dynamiczne = A θ 1 + B sign( θ 1 ) const_electric - ustalona stała wartość natężenia prądu silnika [A] dt - przyrost czasu pomiędzy kolejnymi krokami [s] ext_force - zewnętrzny moment siły przyłożony do ramienia [N m] dane wyjściowe: dynamic - następny stan robota (wektor) położenie ramienia [rad] prędkość kątowa ramienia [ ] rad s przyspieszenie kątowe ramienia [ ] rad s natężenie prądu elektrycznego przepływającego przez silnik [A] napięcie elektryczne na silniku [V ] torque - aktualne momenty sił wpływające na ramię (wektor) moment siły grawitacji [N m] moment siły bezwładności [N m] moment siły motorycznej po przekładni [N m] moment siły tarcia [N m] moment siły wypadkowej [N m] 4.1.5 Sterowanie napięciem elektrycznym na silniku Funkcja wyznacza następny stan mechanizmu przy zadanym stałym napięciu elektrycznym dostarczanym do układu. Ze względu na dynamikę elektryczną, wewnątrz funkcji znajdują się dodatkowe pętle zwiększające dokładność obliczeń. Wyniki wyjściowe podawane są dla danego przyrostu czasu.

4.1. FUNKCJE SYMULACYJNE 39 1 function [dynamic, torque] = model_sarkofag_voltage(dynamic, arm, friction, constvoltage, dt, ext_force) 3 %% 7 % obliczenia 71 7 % motor 73 e_1 = constvoltage; 74 e_b1 = K_b1 * fi_1k / / pi * r_1; 75 i_ = i_1; 76 i_1 = (i_ * L_1 / dt + e_1 - e_b1) / (R_1 + L_1/dt); 77 mom_motor = (K_i1 * i_1 * r_1 + I_m1 * fi_1kk / 4 / pi / pi * r_1) * r_spr; 78 e_1 = i_1 * R_1 + L_1 * (i_1 - i_) / dt + e_b1; 79 8 % ramie 81 c1 = cos(fi_1); 8 s1 = sin(fi_1); 83 84 mom_bezw = (I_1(3,3) + M_1 * (P1_c1(1)) * P1_c1(1) + P1_c1() * P1_c1()) * fi_1kk / 4 / pi / pi; % [kg * m^ / s^] 85 mom_graw = (P1_c1(1) * c1 - P1_c1() * s1) * M_1 * g_(); 86 mom_wyp = mom_graw + mom_bezw + a_1 * F_(); % [kg * m^ / s^] 87 88 % tarcie 89 w_arm = fi_1k + ((mom_motor - mom_wyp) * dt / I * * pi); 9 91 if abs(fi_1k) <.1 9 % statyczne 93 mom_fr = friction(1); 94 95 % ruszanie z miejsca 96 if abs(mom_motor - mom_wyp) > mom_fr 97 moment = mom_motor - mom_wyp - mom_fr; 98 w_arm = fi_1k + (moment * dt / I * * pi); 99 else 1 moment = ; 11 w_arm = ; 1 end 13 14 else 15 % dynamiczne 16 mom_fr = sign(w_arm)*(friction()*abs(w_arm) + friction(3)); 17 18 moment = mom_motor - mom_wyp - mom_fr; 19 w_arm = fi_1k + (moment * dt / I * * pi); 11 111 if abs(mom_fr) > abs(mom_motor - mom_wyp) 11 if (w_arm * fi_1k) < 113 w_arm = ; 114 end 115 end 116 end 117 118 % obrot 119 a_arm = fi_1 + (w_arm * dt); 1 11 % dynamika 1 fi_1kk = ((a_arm - fi_1)/dt - fi_1k)/dt; 13 fi_1k = (a_arm - fi_1)/dt; 14 fi_1 = a_arm;

4 ROZDZIAŁ 4. SYMULACJA Opis funkcji z odwołaniami do modelu matematycznego. 73-78 - Napięcie elektryczne dostarczane do układu jest stałe. Moment elektromagnetyczny (3.6) wyznaczany jest z natężenia prądu w układzie. Natężenie obliczane jest z dostarczanego napięcia i dodatkowej siły elektromotorycznej, indukowanej przez obroty ramienia. 84 - Moment bezwładności (3.17). 85 - Moment siły grawitacji (3.18). 86 - Suma momentów sił oddziałujących na ramię (3.16). 89-116 - Moment siły tarcia ramienia zmienia się w czasie przez zmianę prędkości. Przy ruszaniu obowiązuję tarcie statyczne, po przekroczeniu prędkości minimalnej - tarcie dynamiczne. 119-14 - Obrót ramienia i nowe przyspieszenie, prędkość i przemieszczenie. W pierwszej kolejności wyznaczana jest wartość siły elektromotorycznej indukowanej w obwodzie przez obroty ramienia i działającej przeciw sile elektromotorycznej dostarczanej do układu na zaciski silnika. Następnie wyznaczane jest natężenie prądu elektrycznego tworzącego pole magnetyczne i moment elektromagnetyczny. W dalszej kolejności obliczane są wartości momentów obrotowych związanych z dynamiką ramienia takie jak moment sił grawitacji i moment sił bezwładności. Na koniec wyznaczane są siły tarcia i nowe przemieszczenie ramienia. dane wejściowe: dynamic - poprzedni stan robota (wektor) położenie ramienia [rad] prędkość kątowa ramienia [ ] rad s przyspieszenie kątowe ramienia [ ] rad s natężenie prądu elektrycznego przepływającego przez silnik [A] napięcie elektryczne na silniku [V ] arm - charakterystyka ramienia (wektor) masa ramienia [kg] długość ramienia [m]

4.. BEZPOŚREDNIE KORZYSTANIE Z FUNKCJI MODELU 41 położenie punktu środka masy ramienia względem całkowitej długości ramienia friction - charakterystyka funkcji tarcia (wektor) współczynnik tarcia statycznego współczynnik A tarcia dynamicznego współczynnik B tarcia dynamicznego gdzie tarcie dynamiczne = A θ 1 + B sign( θ 1 ) const_voltage - ustalona stała wartość napięcia elektrycznego na silniku [V ] dt - przyrost czasu pomiędzy kolejnymi krokami [s] ext_force - zewnętrzny moment siły przyłożony do ramienia [N m] dane wyjściowe: dynamic - następny stan robota (wektor) położenie ramienia [rad] prędkość kątowa ramienia [ ] rad s przyspieszenie kątowe ramienia [ ] rad s natężenie prądu elektrycznego przepływającego przez silnik [A] napięcie elektryczne na silniku [V ] torque - aktualne momenty sił wpływające na ramię (wektor) moment siły grawitacji [N m] moment siły bezwładności [N m] moment siły motorycznej po przekładni [N m] moment siły tarcia [N m] moment siły wypadkowej [N m] 4. Bezpośrednie korzystanie z funkcji modelu Używanie funkcji symulacyjnych bezpośrednio, ma za zadanie umożliwić wykonanie testów szybciej niż wersja z graficznym interfejsem użytkownika. Kontrola sterowania jest wówczas o wiele dokładniejsza i umożliwia automatyzację przeprowadzanych badań poprzez skrypty uruchomieniowe. Czas trwania testów zależy od

4 ROZDZIAŁ 4. SYMULACJA zasobów sprzętowych i zadanego przyrostu czasu pomiędzy kolejnymi krokami. Pozwala to na przeprowadzanie zarówno bardzo dokładnych lecz powolnych jak i bardzo szybko wykonywalnych pobieżnych testów. Aby korzystać z tego trybu symulacji trzeba zaprojektować przypadek testowy. Zmiany sterowania, notowanie wyników i rysowanie wykresów należy zaprogramować samodzielnie. Przykładowy przypadek testowy 1 function [ ] = test_example( ) 3 dynamic = [pi/ ]; 4 torque = [ ]; 5 6 arm = [.75 7]; 7 friction = [6 18 ]; 8 9 dt =.1; 1 i = ; 11 1 acceleration = 1 *pi/18; 13 speedmax = 4 **pi/6; 14 timer = 3 /dt; 15 16 time = tic(); 17 18 while dynamic() < speedmax 19 i=i+1; [dynamic, torque] = model_sarkofag_acceleration(dynamic, arm, friction, acceleration, dt, ); 1 dynamic_result(:,i) = dynamic; 3 torque_result(:,i) = torque; 4 end 5 6 time = toc(time); 7 display(time); 8 time = tic(); 9 3 change = (i) * dt; 31 3 while timer>-1 33 i=i+1; 34 [dynamic, torque] = model_sarkofag_speed(dynamic, arm, friction, speedmax, dt, ); 35 36 dynamic_result(:,i) = dynamic; 37 torque_result(:,i) = torque; 38 timer=timer-1; 39 end 4 41 time = toc(time); 4 display(time); 43 time = tic(); 44 45 while abs(dynamic()) > 46 i=i+1; 47 [dynamic, torque] = model_sarkofag(dynamic, arm, friction, dt, );

4.3. SYMULACJA CZASU RZECZYWISTEGO Z GUI 43 48 49 dynamic_result(:,i) = dynamic; 5 torque_result(:,i) = torque; 51 end 5 53 time = toc(time); 54 display(time); 55 56 timef = :i-1; 57 58 figure() 59 cla(); 6 out=dynamic_result(1,:); 61 hold ( on ); 6 xlabel( czas [s] ); 63 ylabel( pozycja [m] ); 64 axis([-inf Inf -Inf Inf]) 65 plot(timef*dt, sin(out)*arm(), k, Linewidth, ); 66 plot([,i*dt], [,], -.k, Linewidth,.5); 67 plot([change, change], [-arm(), arm()], :k, Linewidth,.5); 68 plot([change+3, change+3], [-arm(), arm()], :k, Linewidth,.5); 69 hold ( off ); 7 71 figure(3) 7 cla(); 73 out=dynamic_result(,:)//pi*6; 74 hold ( on ); 75 xlabel( czas [s] ); 76 ylabel( predkosc [obr/min] ); 77 axis([-inf Inf -Inf Inf]) 78 plot(timef*dt, (out), k, Linewidth, ); 79 plot([,i*dt], [,], -.k, Linewidth,.5); 8 plot([change, change], [min(out), max(out)], :k, Linewidth,.5); 81 plot([change+3, change+3], [min(out), max(out)], :k, Linewidth,.5); 8 hold ( off ); 3-7 - inicjalizacja parametrów ramienia 9-14 - inicjalizacja zmiennych symulacji 18-4 - rozpędzanie ramienia z przyspieszeniem 1 na s do uzyskania prędkości 4 obrotów na minutę 3-39 - utrzymanie osiągniętej prędkość przez 3 sekund 45-51 - oczekiwanie na zatrzymanie mechanizmu po odłączeniu zasilania 58-8 - wyświetlenie wyników (rys. 4.)

44 ROZDZIAŁ 4. SYMULACJA 4.6 3.4 pozycja [m]...4.6 1 3 4 5 6 7 czas [s] (a) wychylenie predkosc [obr/min] 1 1 3 4 1 3 4 5 6 7 czas [s] (b) prędkość Rysunek 4.: Wyniki przykładowego przypadku testowego Rysunek 4.3: Symulacja: okno główne

4.3. SYMULACJA CZASU RZECZYWISTEGO Z GUI 45 4.3 Symulacja czasu rzeczywistego z GUI Graficzny interfejs użytkownika (rys. 4.3) zapewnia łatwy podgląd działania modelu symulacyjnego. Pierwotnie powstał jako platforma testowa podstawowych parametrów statycznych i dynamicznych ramienia. Następnie aplikacja została rozwinięta w celach prezentacji wyników ze względu na łatwość obsługi i możliwość szybkiego przedstawienia działania mechanizmu. Częstotliwość zegara wyświetlającego wyniki symulacji została ustawiona na 1Hz. W tym czasie aplikacja dokonuje stu kolejnych kroków symulacji z przyrostem czasu.1 sekundy. Zapewnia to wystarczającą dokładność obliczeń bez nachodzenia na siebie kolejnych cyklów (sekcja 4.3.1). W dalszej części pracy przedstawione zostały poszczególne panele GUI okna głównego (sekcja 4.3.) i funkcje pomocnicze wykorzystywane przez symulację (sekcja 4.3.3). 4.3.1 Główna pętla programu Jej zadaniem jest wykonanie szeregu instrukcji w stałym odcinku czasowym. Częstotliwość wykonania wynosi 1Hz. W celu poprawy dokładności obliczeń, nowe wartości są wyznaczane stukrotnie w każdej pętli, z przyrostem czasu.1 sekundy. Stanowi to pewne przekłamanie w zasadach prowadzenia symulacji czasu rzeczywistego, lecz wyniki i tak są notowane tylko dla ostatnich kroków w każdym cyklu. Otrzymywane są dokładniejsze wartości wynikowe bez konieczności tworzenia dwóch różnych timerów i obsługi sekcji krytycznej. Obliczanie nowych wartości dynamicznych i momentów obrotowych 13 if inputs.sets == 133 [output.dynamic, output.torque] = model_sarkofag(output.dynamic, [arm.mass, arm.len, arm.point], arm.friction, st.dt, ); 134 end 135 136 if inputs.sets == 1 137 [output.dynamic, output.torque] = model_sarkofag_speed(output.dynamic, [arm.mass, arm.len, arm.point], arm.friction, inputs.speed, st.dt, ); 138 end 139 14 if inputs.sets == 141 [output.dynamic, output.torque] = model_sarkofag_acceleration(output.dynamic, [arm.mass, arm.len, arm.point], arm.friction, inputs.acceleration, st.dt, ); 14 end 143 144 if inputs.sets == 3

46 ROZDZIAŁ 4. SYMULACJA 145 [output.dynamic, output.torque] = model_sarkofag_electric(output.dynamic, [arm.mass, arm.len, arm.point], arm.friction, inputs.electric, st.dt, ); 146 end 147 148 if inputs.sets == 4 149 [output.dynamic, output.torque] = model_sarkofag_voltage(output.dynamic, [arm.mass, arm.len, arm.point], arm.friction, inputs.voltage, st.dt, ); 15 end W zależności od wybranego sterowania w panelu sterowania (rys. 4.4) wykonywana jest właściwa funkcja symulacyjna. Dodanie do historii otrzymanych wartości 153 results.dynamic = addtoresults5(results.dynamic, output.dynamic, 6); 154 results.torque = addtoresults5(results.torque, output.torque, 6); Historia składa się z 6 sekund przebiegu symulacji. Przy cząstotliwości 1Hz przechowywane jest 6 ostatnich wyników. Aktualizacja wizualizacji położenia ramienia manipulatora 163 h = handles.resultsaxes; 164 cla(h); 165 166 len = arm.len; 167 alfa = output.dynamic(1); 168 pos = [cos(alfa)*len ; sin(alfa)*len]; 169 17 axis(h, [-len*1.1, len*1.1, -len*1.1, len*1.1]); 171 axis(h, square ); 17 hold (h, on ); 173 plot(h, [, ], [-len, len], --k, Linewidth, 1); 174 plot(h, [-len, len], [, ], --k, Linewidth, 1); 175 plot(h, pos(1,1), pos(,1), om, LineWidth, 1); 176 plot(h, [, pos(1,1)], [, pos(,1)], r, LineWidth, ); 177 hold (h, off ); W panelu wyniki (rys. 4.7) rysowany jest nowy wykres położenia ramienia. Wyświetlenie nowych wartości 179 if st.posunit == 18 set(handles.resultsangleoutput, String,output.dynamic(1) * 18 / pi); 181 else 18 set(handles.resultsangleoutput, String,output.dynamic(1)); 183 end 184 185 if st.speunit == 1

4.3. SYMULACJA CZASU RZECZYWISTEGO Z GUI 47 186 187 set(handles.resultsspeedoutput, String,output.dynamic()); set(handles.resultsaccelerationoutput, String,output.dynamic(3)); 188 end 189 19 if st.speunit == 191 19 set(handles.resultsspeedoutput, String,output.dynamic() * 18 / pi); set(handles.resultsaccelerationoutput, String,output.dynamic(3) * 18 / pi); 193 end 194 195 if st.speunit == 3 196 197 set(handles.resultsspeedoutput, String,output.dynamic()//pi); set(handles.resultsaccelerationoutput, String,output.dynamic(3) / / pi); 198 end 199 if st.speunit == 4 1 set(handles.resultsspeedoutput, String,output.dynamic()//pi*6); set(handles.resultsaccelerationoutput, String,output.dynamic(3) * 18 / pi); 3 end 4 5 6 7 8 9 set(handles.resultselectricoutput, String,output.dynamic(4)); set(handles.resultsvoltageoutput, String,output.dynamic(5)); set(handles.resultsgravityoutput, String,output.torque(1)); set(handles.resultsinertiaoutput, String,output.torque()); 1 set(handles.resultsmotoroutput, String,output.torque(3)); 11 set(handles.resultsfrictionoutput, String,output.torque(4)); 1 set(handles.resultstorqueoutput, String,output.torque(5)); W panelu wyniki (rys. 4.7) aktualizowane są wartości parametrów dynamiki ramienia i napędu oraz momentów sił odziałujących na ramię. 4.3. Panele symulacji Ta sekcja zawiera informacje o elementach graficznych symulacji. Opisywane są komponenty paneli i zadania, które one spełniają. Panel Sterowanie Panel (rys. 4.4) służy do zarządzania sterowaniami obrotów ramienia. Umożliwia wybranie jakim trybem chcemy sterować manipulatorem i z jaką wartością. Przyciski Start i Stop pozwalają w każdej chwili wstrzymać symulację, a potem ją wznowić.

48 ROZDZIAŁ 4. SYMULACJA Dodatkową opcją jest możliwość ustawienia ramienia w konkretnej pozycji przed testem. Komponenty: ustawianie trybu i zadanej wartości sterowania, włączanie i pauzowanie symulacji, ustawianie stałej pozycji. Rysunek 4.4: Symulacja: panel sterowanie Panel Ramię Panel (rys. 4.5) służy do zarządzania parametrami ramienia manipulatora. Masa ramienia liczona jest razem z założonymi obciążnikami. Punkt środka masy należy podać z uwzględnieniem obciążników. Ramię bez żadnego obciążnika powinno mieć wyznaczony punkt środka masy w połowie swojej długości. Komponenty: ustawianie masy ramienia wraz z obciążnikami, ustawianie długości ramienia, ustawianie punktu środka masy ramienia.

4.3. SYMULACJA CZASU RZECZYWISTEGO Z GUI 49 Rysunek 4.5: Symulacja: panel ramię Panel Tarcie Panel (rys. 4.6) służy do zarządzania parametrami tarcia. Moment tarcia statycznego występuje tylko gdy obiekt porusza się z prędkością bliską [ ] rad s. Moment tarcia dynamicznego ma postać aω + b. Dodatkowym elementem jest wykres obrazujący przybliżony przebieg funkcji momentu tarcia. Komponenty: ustawianie współczynnika momentu tarcia statycznego, ustawianie współczynników momentu tarcia dynamicznego, wizualizacja przebiegu funkcji momentu tarcia. Rysunek 4.6: Symulacja: panel tarcie Panel Wyniki Panel (rys. 4.7) służy do przedstawienia aktualnych wartości dynamic (stan ramienia i napędu) i torque (momenty sił). Wizualizacja położenia ramienia przedstawia aktualny stan obrotów ramienia. Komponenty:

5 ROZDZIAŁ 4. SYMULACJA aktualne wartości parametrów dynamiki ramienia i napędu, aktualne wartości momentów sił oddziałujących na ramię, wizualizacja położenia ramienia. Rysunek 4.7: Symulacja: panel wyniki Panel Wykresy Panel (rys. 4.8) służy do wyboru potrzebnych wykresów i umożliwia ich rysowanie. Wykresy wychylenia, prędkości, przyspieszenia, natężenia prądu i napięcia elektrycznego mogą być rysowane tylko osobno ze względu na różne jednostki zadanych parametrów. Wykresy związane z momentami sił można połączyć na jednym wykresie dzięki wspólnej jednostce. Wykresy przedstawiają ostatnie 6 sekund do stanu aktualnego. Komponenty: opcje wykresów parametrów dynamiki ramienia i silnika,

4.3. SYMULACJA CZASU RZECZYWISTEGO Z GUI 51 opcje wykresów parametrów momentów sił oddziałujących na ramię, rysowanie wykresów. Rysunek 4.8: Symulacja: panel wykresy Panel Ustawienia Panel (rys. 4.9) służy do zmiany jednostek pozycji, prędkości i przyspieszenia. Jednostkami domyślnymi są radiany i radiany na sekundę. W tej formię są dostarczane dane do modelu symulacyjnego i w takiej formię są następnie odbierane. Zmiana jednostek służy ułatwieniu użytkownikom korzystania z symulacji: sterowaniu i prezentacji wyników. Zmiana jednostek wpływa także na rysowane wykresy. Komponenty: zmiana jednostki pozycji, zmiana jednostki prędkości i przyspieszenia. Rysunek 4.9: Symulacja: panel ustawienia 4.3.3 Funkcje pomocnicze W tej sekcji znajdują się opisy funkcji pomocniczych, wykorzystywanych w symulacji czasu rzeczywistego z graficznym interfejsem użytkownika.