Stanowisko badawcze do modelowania, identyfikacji i regulacji napędów manipulatorów

Transkrypt

1 Rok akademicki 15/16 POLITECHNIKA WARSZAWSKA WYDZIAŁ ELEKTRONIKI I TECHNIK INFORMACYJNYCH INSTYTUT AUTOMATYKI I INFORMATYKI STOSOWANEJ PRACA DYPLOMOWA MAGISTERSKA inż. Bartosz Wojciech Świstak Stanowisko badawcze do modelowania, identyfikacji i regulacji napędów manipulatorów Opiekun pracy: dr inż. Tomasz Winiarski Ocena pracy: Data i podpis Przewodniczącego Komisji Egzaminu Dyplomowego

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

3 Streszczenie Celem pracy była modernizacja istniejącego stanowiska badawczego napędów oraz projekt i implementacja procedur do modelowania i identyfikacji napędów manipulatorów. Prace zostały przeprowadzone na robocie Sarkofag. W niniejszej pracy zawarte zostały: model matematyczny robota o pojedynczym stopniu swobody, identyfikacja parametrów napędów robotów, przykłady strojenia regulatorów, weryfikacje na sprzęcie i podsumowanie otrzymanych wyników. Słowa kluczowe: Sarkofag, manipulator IRp-6, modelowanie, identyfikacja, strojenie regulatorów, sterowanie Abstract Title: The test setup for modeling, identification and adjustment of manipulator s drives The aim of this thesis was to modernize of the existing drive test stand and the design and implementation of procedures for modeling and identification of manipulator s drives. Initial work has been caried out on the robot Sarkofag. The thesis contains mathematical model of single degree of freedom manipulator, identification of drives parameters, controls tuning examples, verification on hardware and summary. Keywords: Sarkofag, IRp-6 manipulator, modeling, identification, controls tuning, control

4 4

5 Spis treści 1 Wstęp Motywacja pracy Cel i założenia pracy Układ pracy Zasoby 11.1 Sprzęt Stanowisko badawcze Sarkofag Zespół robotów IRp Oprogramowanie MRROC ROS OROCOS IRPOS MATLAB Model pojedynczego stopnia swobody Kinematyka Dynamika Dynamika napędu Identyfikacja Identyfikacja parametrów modelu Identyfikacja stałej momentowej silnika i tarcia statycznego Identyfikacja tarcia dynamicznego Algorytmy Algorytm Gaussa-Newtona Algorytm Levenberga-Marquardta Algorytm Monte Carlo

6 6 SPIS TREŚCI 4..4 Algorytm PSO Układy regulacji Regulator PID Regulacja kaskadowa Regulacja z sprzężeniem wprzód Regulacja z uwzględnieniem momentów sił Komponenty i modele Komponenty OROCOS dla robota Sarkofag sark_kinematic sark_regulator sark_bringup sark_diagnostic sark_description ec_manager Komponenty OROCOS do identyfikacji modelu file_input_control motor_state_export cascade_regulator Modele Simulink Sarkofag Regulator Cascade Regulator Sarkofag/IRP Testy identyfikacyjne Testy identyfikacyjne parametrów modelu Testy identyfikacyjne stałej momentowej silnika i tarcia statycznego Testy identyfikacyjne tarcia dynamicznego Identyfikacja nowej funkcji tarcia Identyfikacja tarcia statycznego Dopasowanie bezwładności i funkcji tarcia Strojenie nastaw regulatora dla robota Sarkofag Identyfikacja parametrów robotów IRp Track Postument

7 SPIS TREŚCI Porównanie obu robotów IRp Weryfikacja Weryfikacja regulatorów na robocie Sarkofag Regulator Sarkofag Regulator Kaskadowy Sarkofag Regulator Kaskadowy z modułem FeedBack Regulator Kaskadowy z modułem FeedForward Porównanie regulatorów Sarkofag Weryfikacja regulatorów na robocie Track Regulator IRP Regulator Kaskadowy IRP Porównanie regulatorów IRP Podsumowanie 97 A Tabele 99 B Wykresy 19

8 8 SPIS TREŚCI

9 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. Taka konstrukcja 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 bardzo trudno uwzględnić w nim wszystkie zjawiska zachodzące w prawdziwym obiekcie i jego środowisku. 1. Cel i założenia pracy Praca ma na celu ułatwienie przyszłych badań algorytmów sterowania. W pracy można wyodrębnić modernizację i obsługę stanowiska badawczego, oraz projekt i implementacje systemu wspierającego identyfikacje parametrów napędów manipulatorów. Model symulacyjny oparty jest na modelu matematycznym robota typu wahadło, istniejącym stanowisku badawczym napędów Sarkofag, znajdującym się 9

10 1 ROZDZIAŁ 1. WSTĘP w pracowni robotyki Instytutu Automatyki i Informatyki Stosowanej Politechniki Warszawskiej. Dodatkowo zostaną zaprezentowane zaprojektowane i przetestowane przykładowe układy regulacji. 1.3 Układ pracy Rozdział. przedstawia opis sprzętu, oprogramowania i algorytmów wykorzystanych w trakcie realizacji pracy. Zaprezentowana została fizyczna budowa stanowiska badawczego, oprogramowanie wykorzystywane do jego sterowania i środowisko do identyfikacji modeli i analizy testów. Rozdział 3. prezentuje 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. opisuje procedury identyfikacyjne parametrów modelu wraz z wykorzystanymi algorytmami. Rozdział 5. zawiera opis układów regulacji i przykładowych regulatorów. Rozdział 6. prezentuje komponenty i modele stworzone na potrzeby wykonania niniejszej pracy. Rozdział 7. opisuje przeprowadzone testy identyfikacyjne. Rozdział 8. zawiera testy weryfikacyjne wykonane na robotach. Rozdział 9. przedstawia podsumowanie pracy i wnioski.

11 Rozdział Zasoby W tym rozdziale zawarty jest opis urządzeń (podrozdział.1) i programów (podrozdział.) zastosowanych w trakcie opracowywania tematu pracy. Badania oparte są na modelu matematycznym (rozdział 3.) istniejącego stanowiska badawczego napędów Sarkofag (sekcja.1.1). Do sterowania robotów, na różnych etapach pracy, wykorzystane zostało oprogramowanie MRROC++ (sekcja..1), ROS (sekcja..) i OROCOS (sekcja..3). Jednym z głównych narzędzi wykorzystanym w pracy jest program MATLAB (sekcja..5)..1 Sprzęt Poniżej znajduje się opis robotów znajdujących się w Laboratorium Robotyki Instytutu Automatyki i Informatyki Stosowanej Politechniki Warszawskiej [13]. Sekcja.1.1 charakteryzuje robota Sarkofag, sekcja.1. przedstawia zespół robotów IRp Stanowisko badawcze Sarkofag Częścią mechaniczną robota Sarkofag [13, 17] jest napęd elektryczny z przekładnią falową 1:158 oraz zamontowanym ramieniem o regulowanej długości i obciążeniu. Służy jako element laboratoryjnego stanowiska do badań sterowań i napędów mannipulatorów. Sarkofag (rys..1) został zbudowany przez Przemysłowy Instytut Automatyki i Pomiarów MERA-PIAP [1]. 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 11

12 1 ROZDZIAŁ. ZASOBY 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. Rysunek.1: Stanowisko badawcze Sarkofag Konstrukcja mechaniczna Głównym elementem robota (rys..) jest kolumna obrotu, znajdująca się w centrum konstrukcji nośnej. Wał napędowy napędzany jest silnikiem prądu stałego PZTK TRR z przekładnią harmoniczną HDUC 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. Do wału silnika przymocowany jest przetwornik obrotowo-impulsowy MOK4/5/BZ/N do pomiaru prędkości obrotowej silnika. Jest to enkoder optoelektroniczny z wyjściowym sygnałem kwadraturowym z pojedynczym impulsem synchronizacji na obrót wału. 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.

13 .1. SPRZĘT 13 (a) widok z przodu (b) widok z boku Rysunek.: 1. konstrukcja nośna,. obudowa, 3. silnik, 4. przekładnia harmoniczna, 5. przetwornik obrotowo-impulsowy, 6. uchwyt, 7. ramię, 8. obciążniki Dodatkowo na uchwycie ramienia zostały zamontowane dwa czujniki. Jeden służy do synchronizacji ramienia, a drugi to krańcówka odcinająca zasilanie. W przeciwieństwie do konstrukcji robotów IRp6, gdzie krańcówka uniemożliwia robotowi wykonanie obrotu przy którym może dojść do jego uszkodzenia (np. zerwanie kabli), ramieniem Sarkofag-u można wykonywać wiele obrotów. Krańcówkę można załączać i wyłączać w zależności od przeprowadzanego testu Silnik wykonawczy Jednostką napędową robota jest silnik szczotkowy prądu stałego typu PZTK TRR. Parametry silnika opisane zostały w tabeli.1. 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, [kg m ] Rezystancja R A =,56[Ω] Indukcyjność L A =,9[mH] Tablica.1: Dane silnika napędowego prądu stałego typu PZTK TRR

14 14 ROZDZIAŁ. ZASOBY 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 prostokątne 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 Oprogramowanie i sterowniki W trakcie opracowywania tematu pracy dokonane zostały dwie poważne modernizacje stanowiska. Pierwszą była zmiana struktury programowej z MRROC++ (sekcja..1) na ROS (sekcja..). Wiązało się to z zapewnieniem odpowiedniego oprogramowania (podrozdział 6.1) do eksploatacji robota. Drugą modernizacją, która spowodowała kolejne zmiany oprogramowania była wymiana sterowników (rys..3). Poprzedni zintegrowany sterownik osi [7 9], zaprojektowany do obsługi robotów IRp-6, został zmieniony na sterownik firmy ELMO Motion Control Gold DC Whistle /1 [19]. Tabela. przedstawia wybrane parametry sterowników. (a) zintegrowany sterownik osi (b) sterownik ELMO Rysunek.3: Sterowniki użyte w robocie Sarkofag Fizyczna wymiana sterowników nie jest zagadnieniem tej pracy.

15 .1. SPRZĘT 15 Zintegrowany sterownik osi Gold DC Whistle /1 Napięcie zasilania 1..5[V ] 1..95[V ] Max. ciągły prąd 3[A] [A] Częst. przełączania [khz] [khz] Interfejs RS-4 EtherCAT Tablica.: Porównanie wybranych parametrów nowego i starego sterownika.1. Zespół robotów IRp-6 Robot IRp-6 [1, 13] jest robotem przemysłowym, powstałym na licencji robota ASEA IRB6. Powstały w 1975 roku manipulator był pierwszym w pełni napędzanym elektrycznie i sterowanym mikroprocesorami (Intel) robotem. Oryginalnie manipulator tego typu miał pięć stopni swobody i był w stanie udźwignąć obciążenie 6kg. W laboratorium automatyki i robotyki znajdują się dwa zmodyfikowane manipulatory IRp-6 (rys..4) o nazwach Track i Postument. (a) Manipulator IRp-6 Track (b) Manipulator IRp-6 Postument Rysunek.4: Manipulatory IRp-6 Oba roboty posiadają dodatkowy szósty stopień swobody zakończony ruchomym chwytakiem. Przy każdym chwytaku umieszczone zostały kamery, a końcówki manipulatorów dodatkowo posiadają czujniki sił i momentów sił będące w stanie rejestrować siły wywierane na chwytaki wzdłuż oraz wokół osi X, Y oraz Z. Robot Postument znajduje się na stacjonarnej platformie postumencie. Robot Track znajduje się na torze jezdnym umożliwiającym przesuwanie się manipulatora wzdłuż jednej osi. Dzięki temu Track ma w sumie aż siedem stopni swobody. W niniejszej pracy wykorzystane zostały tylko pierwsze stopnie swobody robotów Postument i Track kolumny obrotów.

16 16 ROZDZIAŁ. ZASOBY. Oprogramowanie Poniżej znajduje się opis oprogramowania użytego do sterowania robotów oraz symulacji i analizy wyników. W pierwszych etapach pracy wykorzystywana była programowa struktura ramowa MRROC++ (sekcja..1). Następnie w wyniku modernizacji pracowni prace kontynuowane były na platformie ROS (sekcja..). Program MATLAB (sekcja..5) został użyty przy identyfikacji parametrów modelu (rozdział 3.), oraz przy symulacji działania i analizie wyników z rzeczywistych przebiegów robotów Sarkofag (sekcja.1.1) i IRp-6 (sekcja.1.)..1 MRROC++ MRROC++ (Multi-Robot Research Oriented Controller) [31, 3] jest programową strukturą ramową wspomagającą tworzenie sterowników dla systemów wielorobotowych. Oprogramowanie zostało stworzone w Instytucie Automatyki i Informatyki Stosowanej Politechniki Warszawskiej. Złożone jest ze wzorców projektowych i bibliotek modułów. Ogólną strukturę MRROC++ przedstawia rysunek.5. Sterowniki tworzone w oparciu o MRROC++ mają strukturę hierarchiczną. Procesy nadrzędne, sterujące działaniem aplikacji zajmują pozycję szczytową w hierarchii. Procesy odpowiedzialne za sterowanie urządzeniami znajdują się u podstaw hierarchii. Proces EDP (Effector Driver Process) komunikacja ze sterownikami efektorów i kinematyka robota. Wykonuje niskopoziomowe polecenia związane ze sterowaniem robota na podstawie poleceń otrzymanych od ECP. Używając odwrotnej kinematyki dobiera układ stawów dla zadanego położenia i orientacji. Przesyła do ECP takie informacje jak aktualny obrót przegubów, a za pomocą kinematyki prostej wyznacza orientacje i położenie poszczególnych członów manipulatora. Proces VSP (Visual Sensor Process) odpowiada za uzyskiwanie informacji o otoczeniu za pośrednictwem pomiarów z sensorów robota. Informacja z VSP jest przesyłana do pojedynczego ECP. Proces ECP (Effector Control Process) realizuje algorytm sterowania pojedynczym manipulatorem. Pobiera dane o położeniu stawów z EDP, pobiera dane o otoczeniu z procesu VSP, wysyła odpowiednie polecenia do procesu EDP. Przykładowym poleceniem może być wektor opisujący żądany układ stawów manipulatora lub przesunięcie końcówki manipulatora względem osi końcówki.

17 .. OPROGRAMOWANIE 17 Proces MP (Master Process) jest odpowiedzialny za koordynowanie działań wszystkich robotów w środowisku wielorobotowym, które mają osiągnąć określony cel. Zadaje stan docelowy końcówki manipulatorów. Proces UI (User Interface Process) pozwala na komunikację między użytkownikiem a strukturą ramową MRROC++. Za jego pomocą można synchronizować roboty, ręcznie wydawać polecenia manipulatorom, śledzić przebieg stanu systemu, uruchamiać i zatrzymywać zadania. Operator Użytkownik UI User Interface Process Warstwa systemu MP Master Process Warstwa zadania ECP Effector Control Process VSP Virtual Sensor Process EDP Effector Driver Process Warstwa sprzętu Efektor robota Receptor robota Sprzęt Rysunek.5: Struktura systemu MRROC++ Proces ECP przyjmuje wymagane przemieszczenie manipulatora od MP. Następnie oblicza trajektorię zadaną, wybiera chwilową zadaną pozycję końcówki robota i wysyła ją do EDP. Polecenie skierowane do EDP w każdej chwili czasowej różni się zadanym przemieszczeniem w celu kontroli prędkości i przyspieszenia poruszania się stawów.

18 18 ROZDZIAŁ. ZASOBY.. ROS Ramowa struktura programowania ROS (Robot Operating System) [1, 3] jest platformą programistyczną zapewniającą zbiór narzędzi, bibliotek i konwencji wspomagających tworzenia wieloprocesowego oprogramowania dla robotów. ROS pozwala na tworzenie modułowych aplikacji o architekturze rozproszonej i na pełne rozdzielenie części algorytmicznej od sprzętowej. Strukturę ROS-a można podzielić na elementy takie jak: Zarządca (master) najważniejszy z procesów, zarządza przestrzenią nazw, rejestruje węzły, usługi i tematy Węzeł (node) poszczególny moduł aplikacji, odpowiedzialny za obliczenia oraz obsługę sensorów lub efektorów Wiadomość (message) służy do komunikacji między węzłami, każda wiadomość może zawierać typy proste (np. int) lub złożone (inne struktury lub tablice) Temat (topic) kanał komunikacji węzłów, publikowanie lub subskrybowanie wiadomości na określony temat nie wymaga posiadania informacji o sobie Usługa (service) komunikacja typu żądanie-odpowiedź, architektura klientserwer, gdzie klient wysyła żądanie poprzez nazwę serwisu i czeka na odpowiedź, nie wykonując żadnych czynności Każdy węzeł odpowiada za realizacje przeznaczonego dla siebie zadania: obliczenia, komunikację lub obsługę konkretnego elementu wykonawczego. Węzły są połączone w grafy miedzy którymi można przesyłać dowolne dane. Węzły komunikują się za pomocą wiadomości wysyłanych w trybie ciągłym przez tematy lub na żądanie przez usługi. W systemie plików podstawową jednostką systemu jest pakiet. Pakiety zawierają programy, skrypty, dane i deklaracje wykorzystywane przez. Każdy pakiet zawiera informacje o swojej zawartości w pliku package.xml. Wymienione są tam takie dane jak nazwa, wersja, opis i zależność od innych pakietów. Metapakiety są strukturami nadrzędnymi - kolekcjami pakietów. RVIZ RVIZ jest pakietem ROS-a, pozwalającym na trójwymiarową wizualizację informacji krążących w systemie ROS. Umożliwia obserwację parametrów robota takich jak nastawy stawów, położenia i orientację elementów łańcucha kinematycznego oraz wizualizację odczytów z czujników.

19 .. OPROGRAMOWANIE OROCOS OROCOS [4, ] jest strukturą programową wspomagającą tworzenie aplikacji działających w czasie rzeczywistym do sterowania robotami. Architektura komponentowa pozwala na wielokrotne wykorzystanie modułowych, konfigurowalnych komponentów. Nowe komponenty można w łatwy sposób dołaczać do gotowych rozwiązań. Wspiera wiele platform: Windows, Linux, Mac OS i zawiera rozszerzenia pozwalające na współpracę z innymi systemami i strukturami. Pozwala na spełnienie twardych ograniczeń czasowych dzięki czemu stanowi bardzo dobre uzupełnienie ROS-a (sekcja..). Udostępnia możliwość używania własnych typów danych, generowania kodu oraz logowania zdarzeń i komunikacji między komponentami...4 IRPOS IRPOS to system opracowany przez Zespół Programowania Robotów i Systemów Rozpoznających [13] służący do sterowania dwoma robotami IRp-6 (sekcja.1.) Trackiem i Postumentem. Zawiera wiele gotowych algorytmów ułatwiających realizację skomplikowanych trajektorii ruchu ramienia robota. IRPOS jest przystosowany do współpracy z systemem ROS (sekcja..). Ponadto, umożliwia wykonanie sterowania w czasie rzeczywistym, dzięki komponentom OROCOS (sekcja..3) wchodzącymi w skład systemu...5 MATLAB Matlab (ang. matrix laboratory) [7,1] jest interaktywnym środowiskiem obliczeniowym 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 Matlab-ie są wektory i macierze. Dzięki temu funkcje przystosowane są do operacji na zmiennych wieloelementowych.

20 ROZDZIAŁ. ZASOBY Cechy środowiska programistycznego Matlab: język programowania wysokiego poziomu, integracja z językami: C, Java, Fortran, eksploracja i wizualizacja danych, wbudowane funkcje obliczeniowe Simulink Simulink [5, 6] jest częścią oprogramowania Matlab. Jest graficznym środowiskiem programistycznym i stanowi uzupełnienie Matlab-a. Służy do modelowania, symulacji i analizy ciągłych i dyskretnych systemów dynamicznych zdefiniowanych w dziedzinie czasu. Simulink pozwala niskim nakładem czasu tworzyć i symulować graficznie przejrzyste i łatwe w parametryzacji systemy. Podstawową jednostką w Symulink-u jest blok. Bloki podzielone są na typy takie jak: źródła, wyniki, operacje matematyczne, operacje logiczne i bitowe, bloki modeli ciągłych, nieciągłych i dyskretnych. Bloki łączy się ze sobą metodą przeciągnij-iupuść zapewniając między nimi komunikacje. Dodatkowo większość bloków można konfigurować. Simulink pozwala zorganizować model w hierarchiczne podsystemy. Podsystem jest to grupa bloków i sygnałów zamknięta w pojedynczym bloku. Ułatwia to zarządzanie i prezentacje nawet skomplikowanego modelu. Cechy Simulink-a: edytor graficzny do tworzenia i zarządzania hierarchicznymi schematami blokowymi, biblioteki predefiniowanych bloków do modelowania systemów dyskretnych oraz ciągłych, silnik symulacji ze stało i zmiennokrokowymi solverami ODE, możliwość komunikacji z innymi programami symulacyjnymi i możliwość dołączania ręcznie pisanego kodu, bloki do wizualizacji wyników symulacji...5. CurveFittingToolbox CFTOOLS [6] jest pakietem narzędzi i funkcji do oprogramowania Matlab. Służy do znajdowania i dopasowywania krzywych do danego zbioru wartości funkcji. Moduł zawiera zintegrowane narzędzia (rys..6) do podglądu i wstępnego przetwarzania danych, budowy i porównywania modeli oraz analizy dopasowań.

21 .. OPROGRAMOWANIE 1 Cechy CFTOOLS: interaktywny graficzny interfejs użytkownika, liniowe i nieliniowe równania regresji z możliwością stosowania równań niestandardowych, biblioteka modeli regresji ze zoptymalizowanymi punktami startowymi i parametrami solvera, procedury wstępnego przetwarzania danych: skalowanie danych, podział na podgrupy, wygładzanie, usuwanie błędnych punktów, procedury po przetworzeniu: interpolacja, ekstrapolacja, przedziały ufności, całki i pochodne. (a) okno funkcji dopasowania (b) okno poglądowe Rysunek.6: CFTOOLS

22 ROZDZIAŁ. ZASOBY

23 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ę (podrozdział 3.1) i dynamikę (podrozdział 3.) ramienia robota, oraz dynamikę napędu (podrozdział 3.3). Model matematyczny [16, 18] jest opisem matematycznym wielkości fizycznych obiektu i zachodzącymi miedzy nimi zależnościami przyczynowo-skutkowymi. W szczególności opisuje 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ć model czarnej skrzynki (ang. black-box) i model białej skrzynki (ang. white-box). Model czarnej skrzynki jest projektowany 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. Postać modelu wynika z przeprowadzanej analizy badanego obiektu. Model białej skrzynki jest projektowany z pełną wiedzą a priori. Wszystkie parametry modelu oraz wszystkie funkcje wiążące są znane. Posiadana informacja o układzie charakteryzuje rodzaje funkcji wiążących zmienne układu. Wynikiem modelowania jest bardzo dokładny model zachowujący się zgodnie z rzeczywistym układem. 3

24 4 ROZDZIAŁ 3. MODEL POJEDYNCZEGO STOPNIA SWOBODY 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. W tabeli 3.1 umieszczony został wykaz ważniejszych oznaczeń w opisywanym modelu. M 1 masa ramienia C 1 I 1 macierz inercji członu 1 (ramienia) 1 R macierz rotacji z układu {} (kolumny obrotu) do układu {1} (ramienia manipulatora) 1P położenie punktu 1 (ramienia manipulatora) na członie (kolumnie obrotu) 1 C 1 P położenie punktu środka masy na członie 1 (ramienia manipulatora) 1 ( 1ω) prędkość kątowa członu 1 (ramienia manipulatora) względem członu (kolumny obrotu), wyrażona w układzie {1} (ramienia) 1 ( 1ɛ) przyspieszenie kątowe członu 1 (ramienia manipulatora) względem członu (kolumny obrotu), wyrażona w układzie {1} (ramienia) 1 ( 1A) - przyspieszenie liniowe członu 1 (ramienia manipulatora) względem członu (kolumny obrotu), wyrażona w układzie {1} (ramienia) 1 1F siły oddziałujące na człon 1 (ramię manipulatora) 1 C 1 F siły bezwładności działające na człon 1 (ramię manipulatora) 1 1N momenty sił oddziałujące na człon 1 (ramię manipulatora) 1 C 1 N momenty sił bezwładności działające na człon 1 (ramię manipulatora) Tablica 3.1: Opisy wielkości fizycznych zastosowanych w modelu 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 [8,3] zajmuje się geometrycznymi i czasowymi własnościami ruchu, bez uwzględniania sił wywołujących ten ruch. Głównymi parametrami kinematyki są odległości i kąty pomiędzy poszczególnymi ogniwami (bryły sztywne) manipulatora. Opisuje także zależność pomiędzy strukturą manipulatora i nastawami ramion, a zmianami położenia układów współrzędnych. W przypadku stanowiska badawczego Sarkofag istnieje tylko jedno ogniwo i tylko jeden przegub. Podstawa do której przymocowano ramię jest układem oznaczonym numerem (ogniwo zerowe). Tabela 3. przedstawia układy modelu robota Sarkofag, a rysunek 3.1 przedstawia schemat kinematyki.

25 3.1. KINEMATYKA 5 układ {} kolumna obrotu manipulatora układ {1} ramię manipulatora układ {c1} punkt środka masy ramienia manipulatora układ {} końcówka ramienia manipulatora (miejsce zaczepienia kolejnego członu). Tablica 3.: Definicje układów modelu Rysunek 3.1: Schemat kinematyki manipulatora, a 1 - długość członu, θ 1 - kąt obrotu członu Notacja Denavita-Hartenberga (tab. 3.3) pozwala szybko opisać i przedstawić konfigurację kinematyczną manipulatora. 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 θ 1 (kąt obrotu członu) jest zmienną konfiguracyjną członu pierwszego, a a 1 (długość członu) jest ustalonym parametrem członu. i a i 1 α i 1 d i θ i 1 θ 1 a 1 Tablica 3.3: Parametry Denavita-Hartenberga opisujące Sarkofag 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ę 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.

26 6 ROZDZIAŁ 3. MODEL POJEDYNCZEGO STOPNIA SWOBODY 1T = cos(θ 1 ) sin(θ 1 ) sin(θ 1 ) cos(θ 1 ) T = 1 a (3.1) T = cos(θ 1 ) sin(θ 1 ) a 1 cos(θ 1 ) sin(θ 1 ) cos(θ 1 ) a 1 sin(θ 1 ) 1 1 Założenia (3.): podstawa manipulatora i kolumna obrotu są nieruchome ω = ɛ = A = (3.) Prędkość kątowa i prędkość kątowa środka masy 1 ( 1ω) = 1 R ( ω) + 1 1z θ 1 = 1 1z θ 1 = 1 ( C 1 ω) = 1 ( 1ω) θ 1 (3.3) Ze względu na założenia (3.) pierwszy składnik jest zerowany. Przyspieszenie kątowe i przyspieszenie kątowe środka masy 1 ( 1ɛ) = 1 R ( ɛ) + 1 R ( ω) 1 1z θ z θ 1 = 1 1z θ 1 = 1 ( C 1 ɛ) = 1 ( 1ɛ) θ 1 (3.4) Ze względu na założenia (3.) pierwszy i drugi składnik są zerowane.

27 3.. DYNAMIKA 7 Przyspieszenie liniowe i przyspieszenie liniowe środka masy 1 ( 1A) = 1 R [ ( A) + ( ɛ) 1P + ( ω) ( ( ω) 1P )] = 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 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 C 1 P y θ θ 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.5) Ze względu na założenia (3.) przyspieszenie liniowe jest zerowane. 3. Dynamika Dynamika [8, 14, 5] 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, określają szybkość zmian w układzie. Efektywność sterowania manipulatorem robota zależy od dokładności jego modelu dynamicznego. 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 τ,

28 8 ROZDZIAŁ 3. MODEL POJEDYNCZEGO STOPNIA SWOBODY Macierz inercji Macierz inercji członu 1 (3.6) jest zmienna w zależności od długości ramienia i położenia dodatkowego obciążnika. m k (y k + zk) k C 1 I 1 = m k x k y k k m k z k x k k m k x k y k k m k (zk + x k) k m k y k z k k m k z k x k k m k y k z k k m k (x k + yk) k (3.6) gdzie: x k, y k, z k składowe wektora wodzącego k-tego punktu m k masa k-tego punktu Siły bezwładności 1 C 1 P y θ 1 1 C 1 P y θ 1 1 C 1 F = M 1 1 ( 1 C 1 A) = M 1 C 1 P x θ 1 1 C 1 P yθ 1 (3.7) Siły bezwładności obliczane są z równania Newtona. Momenty sił bezwładności 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 1xz θ1 = θ C 1 1 I 1yz θ 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.8)

29 3.. DYNAMIKA 9 Suma sił oddziałujących na ramię 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 ) 1 = M 1 C 1 P x θ1 1 C 1 P y θ 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.9) gdzie siły zewnętrzne przyłożone do układu {} (3.1): F = F x F y F z (3.1) Suma momentów sił oddziałujących na ramię 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 C1 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 C 1 P x θ1 1 C 1 P y θ 1 + g y sin(θ 1 ) + F y C 1 P y C 1 P x F z a 1 1 C 1 P x C 1 P y F x 1 F y 1 C 1 P z 1 F z (3.11) gdzie momenty zewnętrzne przyłożone do układu {} (3.1): N = (3.1)

30 3 ROZDZIAŁ 3. MODEL POJEDYNCZEGO STOPNIA SWOBODY Moment siły wzdłuż osi Z [ 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 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 C1 P x 1 C1 P y M 1 ) θ ( 1 C 1 P y sin(θ 1 ) + 1 C 1 P x cos(θ 1 ) ) M 1 g y + (3.13) 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: moment siły bezwładności (3.14), [ C1 I 1zz + M 1 ( 1C1 P x + 1 C1 P y )] θ1 (3.14) moment siły grawitacji (3.15), ( 1C1 P x cos(θ 1 ) 1 C 1 P y sin(θ 1 ) ) M 1 g y (3.15) moment sił zewnętrznych (3.16). a 1 F y (3.16) 3.3 Dynamika napędu Dynamika napędu [8,3] 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 elektromagnetycznym. Równanie napędu τ e1 = τ I1 + τ f1 + τ r1 (3.17) τ e1 moment elektromagnetyczny (3.18) τ I1 moment sił bezwładności silnika (3.19) τ f1 moment sił tarcia widziane na wale silnika (3.) τ r1 moment sił ogniwa widziane na wale silnika (3.3)

31 3.3. DYNAMIKA NAPĘDU 31 Równanie elektryczne e 1 = di i 1 R 1 + L 1 1 dt b 1 τ 1 = K i1 i 1 (3.18) e b1 = K b1 θm1 Równanie elektryczne (3.18) opisuje zależności między siłami elektromotorycznymi, prądem płynącym w układzie i momentem elektromagnetycznym. Rysunek 3. przedstawia schemat zastępczy układu elektrycznego silnika, a tabela 3.4 opisuje wielkości fizyczne występujące w równaniu. Rysunek 3.: Zastępczy schemat układu elektrycznego e 1 siła elektromotoryczna na zaciskach i 1 prąd prąd twornika τ 1 moment elektromagnetyczny R 1 rezystancja uzwojenia twornika L 1 indukcyjność uzwojenia twornika e b1 siła elektromotoryczna indukowana w tworniku K i1 stała momentowa silnika K b1 stała napięciowa silnika Tablica 3.4: Opis wielkości fizycznych w równaniu elektrycznym Moment sił bezwładności silnika τ I1 = I m1 θm1 (3.19) θ m1 = θ 1 r 1 jeżeli w układzie nie ma luzu r 1 - przełożenie przekładni r 1 1

32 3 ROZDZIAŁ 3. MODEL POJEDYNCZEGO STOPNIA SWOBODY 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 - tarcie występujące pomiędzy powierzchniami bez żadnych substancji obcych. Tarcie lepkie (3.1) - tarcie występujące pomiędzy powierzchniami rozdzielonymi środkiem smarnym. f 1 ( θ m1 ) = b 1 θm1 lub (3.1) b 1 θ m1 Tarcie statyczne (3.) - tarcie występujące pomiędzy powierzchniami nie przemieszczającymi się względem siebie. f1 = B + θ m1 < θ p B θp < θ m1 (3.) dla pozostałych θ m1 Moment sił ogniwa widziany na wale silnika τ r1 = r 1 τ 1 (3.3) r 1 - przełożenie przekładni r 1 1

33 Rozdział 4 Identyfikacja W tym rozdziale przedstawione zostały zagadnienia związane z identyfikacją parametrów dynamicznych modelu robota (podrozdział 4.1) W podrozdziale 4. opisane zostały algorytmy wykorzystane przy identyfikacji modelu. Identyfikacja [16, 18] oznacza rozpoznanie właściwości statycznych i dynamicznych badanego obiektu. Głównym celem identyfikacji jest znalezienie zależności pomiędzy wejściem a wyjściem układu, sporządzenie opisu matematycznego, za pomocą przeprowadzonych doświadczeń. Parametrami dynamicznymi pojedynczego ogniwa są masa, momenty statyczne oraz elementy macierzy bezwładności, współczynniki tarcia suchego, lepkiego i statycznego. W przypadku robota Sarkofag (sekcja.1.1) masa ramienia jest łatwo mierzalna, z macierzy bezwładności interesujący jest tylko pojedynczy element, który można zsumować z bezwładnością silnika i przekładni. Pozwala to na redukcje szukanych paramentów do czterech przy czym tarcie statyczne działa tylko podczas ruszania i zatrzymywania mechanizmu. Ze względu na zużycie silników warto także dokonać próby oszacowania stałej momentowej K i. Podczas identyfikacji ważne jest jakie wielkości można zmierzyć na pojedynczym ogniwie manipulatora. W większości przypadków jest to natężenie prądu płynącego w układzie silnika, położenie i prędkość obrotowa ogniwa. Problemem jest mierzenie momentów sił oddziałujących na ramię. 33

34 34 ROZDZIAŁ 4. IDENTYFIKACJA 4.1 Identyfikacja parametrów modelu Poniżej przedstawiono procedurę identyfikacji parametrów modelu koniecznych do budowy sterownika z wejściem typu sprzężenie w przód kompensującym zakłócenie w postaci statyki robota oraz tarcia. W pierwszej kolejności opisana jest identyfikacja tarcia statycznego i stałej momentowej silnika (sekcja 4.1.1), a następnie parametrów funkcji aproksymującej tarcie dynamiczne (sekcja 4.1.). Opis zawiera analityczną procedurę identyfikacyjną. Wyniki testów zostały zawarte w rozdziale Identyfikacja stałej momentowej silnika i tarcia statycznego Moment elektromagnetyczny (3.18) jest wprost proporcjonalny do natężenia prądu przepływającego przez silnik i stałej momentowej silnika. W chwili przed poruszeniem moment elektromagnetyczny τ e jest równoważony (4.1) przez maksymalny moment tarcia statycznego τ fst, moment grawitacji ramienia τ grav i moment powstały przez dodatkową siłę przyłożoną do ramienia F l, widzianych na wale silnika. τ e = τ fst + r(τ grav + F l) (4.1) W uproszczeniu stosowane są równania prądu: i d = i fst + i grav i g = i fst + i grav i i s = d +i g = i grav i so = i d i d+i g = i d i g = i fst (4.) gdzie: i d zrywowe natężenie prądu przy obrocie do dołu i g zrywowe natężenie prądu przy obrocie do góry i fst prąd wykorzystywany do pokonania sił tarcia statycznego i grav prąd wykorzystywany do pokonania sił grawitacji i s średnie zrywowe natężenie prądu i so średnie odchylenie zrywowego natężenia prądu

35 4.1. IDENTYFIKACJA PARAMETRÓW MODELU Identyfikacja stałej momentowej silnika Przy wyznaczaniu stałej momentowej silnika wykorzystywane zostały wartości średnie natężenia prądu, eliminując wpływ momentu tarcia statycznego. Zestawianie ze sobą wyników dla różnych obciążeń pozwoliło na wyeliminowanie wpływu momentu grawitacyjnego samego ramienia zostawiając tylko wpływ zamontowanego ciężarka. Stała momentowa silnika wyznaczana jest ze wzoru (4.3). K i i n sz = r(τ grav + F n l) K i i m sz = r(τ grav + F m l) K i (i n sz i m sz) = rl(f n F m ) K i = rl(fn Fm) (i n s im s ) (4.3) gdzie: K i stała momentowa silnika i n sz średnie zrywowe natężenie prądu dla obciążenia n i m sz średnie zrywowe natężenie prądu dla obciążenia m r przekładnia (1:158) l odległość obciążenia od osi wału (.6m) F n siła wywierana przez obciążenie n na ramię F m siła wywierana przez obciążenie m na ramię Identyfikacja tarcia statycznego Przy wyznaczaniu momentu tarcia statycznego wykorzystywane zostały wartości średniego odchylenia natężenia prądu, eliminując wpływ momentu grawitacji. Mając daną stałą momentową silnika, graniczny moment tarcia statycznego wyznaczany jest ze wzoru (4.4) K i i d = τ fst + r(τ grav + F l) obrót w dół (i > ) K i i g = τ fst + r(τ grav + F l) obrót w górę (i < ) (4.4) K i (i d i g ) = τ fst gdzie: τ fst moment tarcia statycznego K i stała momentowa silnika i d zrywowe natężenie prądu przy obrocie w dół i g zrywowe natężenie prądu przy obrocie w górę

36 36 ROZDZIAŁ 4. IDENTYFIKACJA 4.1. Identyfikacja tarcia dynamicznego Tarcie dynamiczne występuje przy obrocie wału napędowego i zależy od prędkości obrotowej na wale silnika (4.5). W jego skład wchodzi tarcie lepkie (powierzchnie rozdzielone środkiem smarnym) i tarcie suche (bez żadnych substancji obcych). τ fd = b θ + µsign θ (4.5) Identyfikacja parametrów tarcia suchego i lepkiego Przy wyznaczaniu parametrów tarcia dynamicznego wykorzystywana została maksymalna prędkość osiągana przy zadanym natężeniu. Przy porównywanie obrotów w tym samym kierunku, tarcie suche ma taką samą wartość. K i i n = µsign θ n + b θ n K i i m = µsign θ m + b θ m K i (i n i m ) = b( θ n θ m ) (4.6) gdzie: K i stała momentowa silnika µ parametr tarcia suchego b parametr tarcia lepkiego i n zadane natężenie prądu w teście n θ n zmierzona prędkość obrotowa w teście n i m zadane natężenie prądu testu m θ m - zmierzona prędkość obrotowa w teście m 4. Algorytmy Poniżej znajduje się opis algorytmów wykorzystanych przy identyfikacji parametrów modeli i dobieraniu nastaw regulatorów. Algorytm Gaussa-Newtona (sekcja 4..1) i algorytm Levenberga-Marquardta (sekcja 4..) są wbudowanymi funkcjami narzędzia CFTOOLS, i służą jako metoda przybliżania równań nieliniowych. Algorytmy Monte Carlo (sekcja 4..3) i PSO (sekcja 4..4) zostały wykorzystane w trakcie realizacji procedur identyfikacyjnych.

37 4.. ALGORYTMY Algorytm Gaussa-Newtona Zadaniem algorytmu Gaussa-Newtona [9] jest estymacja parametrów funkcji nieliniowych. Metoda polega na zastępowaniu modelu w każdej kolejnej iteracji jego liniowym przybliżeniem. Do obliczania odchyleń i kolejnych przybliżeń wykorzystywane są pierwsze pochodne cząstkowe (jakobiany) względem aktualnych parametrów funkcji. Procedura trwa aż wartość wszystkich poprawek będzie poniżej zadanej dokładności. 4.. Algorytm Levenberga-Marquardta Algorytm Levenberga-Marquardta [4] jest procedurą minimalizacji łączącą szybkość obliczeń metody najmniejszych kwadratów z niezawodnością metody najszybszego spadku. Metoda opiera się na algorytmie rozwiązywania nieliniowego problemu najmniejszych kwadratów. W porównaniu z optymalizacją newtonowską, macierz hesjanu jest zastępowana przybliżeniami opartymi na gradientach. Na początku działania algorytm używa metody największego spadku, a w pobliżu rozwiązania jego działanie przypomina algorytm Gaussa-Newtona o kwadratowej zbieżności Algorytm Monte Carlo Metody Monte Carlo polegają na losowaniu wartości parametrów charakteryzujących funkcję przy pomocy generatora liczb losowych. Jakość użytego generatora i ilość sprawdzeń wpływa na dokładność wyniku do pewnego stopnia. Ze względu na działanie generatorów liczb pseudolosowych (ograniczenie liczb losowych w cyklu), zwiększenie liczby prób nie zawsze gwarantuje poprawny wynik. Algorytm został użyty do wstępnego doboru parametrów regulatorów Algorytm PSO Algorytm PSO (ang. particle swarm optimization) [3,15,4] rój cząsteczek. Jest algorytmem optymalizacyjnym naśladującym stado owadów. Cząsteczki poszukują wokół siebie najlepszego rozwiązania i wpływają na inne cząsteczki wokół siebie. W rezultacie stado przenosi się na obszar coraz lepszego rozwiązania. PSO wymaga jedynie wartości funkcji celu, bez kierunku zmian i gradientów. Wykorzystuje proste operatory matematyczne i nie jest wymagający obliczeniowo.

38 38 ROZDZIAŁ 4. IDENTYFIKACJA Dla D-wymiarowej przestrzeni przeszukiwań każda cząsteczka stada jest reprezentowana jako D-wymiarowy wektor. Istnieją dwa warianty algorytmu PSO: z globalnym sąsiedztwem i z lokalnym. W wersji globalnej cząsteczki przemieszcza się zależnie do najlepszej pozycji osiągniętej w całej grupie. W wersji lokalnej każda cząsteczka przemieszcza się według połączenia swojej poprzedniej najlepszej pozycji i najlepszej cząsteczki w jej sąsiedztwie. Stany wszystkich cząsteczek są uaktualniane jednocześnie. W niniejszej pracy Algorytm w wersji globalnej był wykorzystany do identyfikacji parametrów modelu (podrozdział 7.1) i dobierania parametrów regulatorów (podrozdział 7.).

39 Rozdział 5 Układy regulacji W tym rozdziale przedstawione zostały zagadnienia związane z układami regulacji. Podstawowym regulatorem opisanym w pracy jest regulator PID (podrozdział 5.1). Następnie opisane sa sposoby poprawy jakości sterowania takie jak regulacja kaskadowa (podrozdział 5.), sprzężenie wprzód (podrozdział 5.3) i uwzględnienie modelu w sprzężeniu zwrotnym (podrozdział 5.4). Podstawowym zadaniem układu regulacji [, 6] jest utrzymywanie wartości zmiennych regulowanych na zadanym poziomie. Regulator generuje sygnał sterujący dla urządzeń wykonawczych, wpływający na działanie obiektu, tak aby wartość zadana pokrywała się z wyjściem obiektu. Regulator Obiekt Rysunek 5.1: Schemat otwartego układu regulacji Sterowanie w pętli otwartej obiektem (rys. 5.1) wymaga zdefiniowania dokładnego modelu obiektu. Regulator na podstawie wartości zadanej y zad oblicza potrzebny sygnał sterujący u. Problemem układów regulacji w pętli otwartej jest występowanie zakłóceń zewnętrznych τ zew, które nie mogą być uwzględnione w modelu przez co regulator nie posiada wiedzy o aktualnym stanie obiektu y. Zakłóceniami mogą być zmiany napięcia linii zasilającej, dodatkowe obciążenia, zmiany tarcia i bezwładności. Częstym problemem jest także zmiana parametrów obiektu w trakcie pracy układu. 39

40 4 ROZDZIAŁ 5. UKŁADY REGULACJI Regulator Obiekt Rysunek 5.: Schemat zamkniętego układu regulacji y zad Zamknięty układ regulacji (rys. 5.)) mierzy różnicę pomiędzy wartością zadaną a wartością mierzoną y wykorzystując urządzenia pomiarowe. Zadaniem regulatora jest minimalizacja uchybu e = y zad y zgodnie z wybranym algorytmem. W wersji optymalnej układ sterowania ma zapewnić maksymalnie szybką odpowiedź na zmianę sygnału wejściowego nie powodując oscylacji i przeregulowania obiektu. 5.1 Regulator PID Większość regulatorów stosowanych w układach z zamkniętą pętlą regulacji to regulatory typu PID [, 6] (ang. Proportional Integral Derivative), szczególnie rozpowszechnione w przemyśle. Algorytm pozwala dość dokładnie dobrać potrzebne sterownia do charakterystyki obiektu. P I Obiekt D Rysunek 5.3: Schemat regulacji z algorytmem PID Regulator PID (rys. 5.) składa się z trzech oddzielnych bloków: proporcjonalnego P (5.1), całkującego I (5.) i różniczkującego D (5.3), których wyjścia są sumowane w jeden sygnał sterujący u. Regulator PID jest regulatorem reakcyjnym sygnał sterujący zależy tylko od wartości uchybu e.

41 5.1. REGULATOR PID 41 Historycznie regulatory tego typu zostały po raz pierwszy zastosowane w przemyśle w zamkniętych układach regulacji temperatury i były projektowane z wykorzystaniem komponentów analogowych. Blok Proporcjonalny Część proporcjonalna (ang. proportional) jest prostym wzmacniaczem uchybu kompensuje błąd bieżący. Blok proporcjonalny (5.1) działa zgodnie z prawem regulacji: u(t) = K p e(t) (5.1) gdzie e(t) jest sygnałem wejściowym regulatora (uchybem), u(t) oznacza sygnał wyjściowy, a K p współczynnik wzmocnienia W najprostszych obiektach może być stosowany samodzielnie, ale nie ma możliwości na całkowite zlikwidowanie uchybu w stanie ustalonym. Przy zbyt dużej wartości wzmocnienia układ traci stabilność i może wejść w oscylacje. Blok Całkujący Część całkująca (ang. integral) jest sumą uchybu kompensuje błąd regulacji z przeszłości. Składowa całkująca I ma za zadanie powolna zmianę sygnału wyjściowego w przypadku niezerowego uchybu, zapewnia dużą dokładność w długim okresie. W idealnym bloku całkującym (5.) sygnał wyjściowy u(t) jest proporcjonalny do całki sygnału wejściowego e(t) uchybu: u(t) = 1 t e(τ)dτ (5.) T i gdzie T i = 1/K i jest czasem całkowania czasem, po którym wartość u(t) jest równe e(t). Osiągnięcie zerowego błędu statycznego wymaga czasu i przy zbyt dużej nastawie mogą w układzie powstać oscylacje. Drugim utrudnieniem jest występowanie zjawiska wind-up, przy dużej zmianie wartości zadanej zbyt duża wartość sygnału gromadzi się w rejestrze całkującym powodując przeregulowanie. Blok Różniczkujący Część różniczkująca (ang. derivative) jest pochodną uchybu kompensuje przewidywane błędy w przyszłości. W idealnym bloku różniczkującym (5.3) sygnał wyjściowy u(t) jest proporcjonalny do różniczki sygnału wejściowego de(t)/dt: u(t) = T d de(t) dt (5.3)

42 4 ROZDZIAŁ 5. UKŁADY REGULACJI gdzie T d = K d jest czasem różniczkowania (wyprzedzenia) czasem potrzebnym na podwojenie się sygnału wyjściowego w stosunku do wartości początkowej wywołanej przez blok różniczkującym (przy stałym de(t)/dt). Składowa różniczkująca D ma za zadanie wytłumić zmiany błędu, sygnał jest proporcjonalny do szybkości zmiany uchybu. 5. Regulacja kaskadowa Regulacja kaskadowa (rys. 5.4) polega na rozdzieleniu obiektu sterowanego na dwie lub więcej części i zestawienia do pracy kilku regulatorów naraz. Regulator pętli zewnętrznej na wejściu otrzymuje uchyb od wartości zadanej (np. położenia). Następnie sygnał sterujący wysyłany jest do następnego regulatora pracującego w pętli wewnętrznej jako nastawa. Regulator pętli wewnętrznej steruje wartością, która podlega szybszym zmianom (np. położenie θ prędkość θ, prędkość θ przyspieszenie θ). Regulator pozycji Regulator prędkości Regulator prądu Obiekt Rysunek 5.4: Schemat regulacji kaskadowej 5.3 Regulacja z sprzężeniem wprzód Regulatory PID są regulatorami liniowymi, nie są odpowiednie w silnie nieliniowych układach regulacji. Wiedza o modelu (rozdział 3.) wprowadzona jest przez sprzężenie wprzód (ang. feedforward) i sumowana z wyjściem regulatora PID. Połączenie działania regulacji w pętli zamkniętej (regulator PID) z regulacją w pętli otwartej (sprzężenie wprzód) może usprawnić charakterystykę regulacji układu. W przypadku robota Sarkofag (sekcja.1.1) nieliniowości występują przy momencie sił grawitacji τ g (3.15) oraz momencie sił tarcia τ f (3.). Moment siły grawitacji jest zależny od położenia ramienia, a moment siły tarcia od prędkości na wale silnika. Zadane położenie i zadana prędkość mogą być nastawami regulatorów przy zastosowaniu regulacji kaskadowej (rys. 5.5).

43 5.4. REGULACJA Z UWZGLĘDNIENIEM MOMENTÓW SIŁ 43 Grawitacja Tarcie Regulator pozycji Regulator prędkości Regulator prądu Obiekt Rysunek 5.5: Schemat regulacji kaskadowej z sprzężeniem wprzód 5.4 Regulacja z uwzględnieniem momentów sił Kolejnym sposobem eliminacji nieliniowości z układu regulacji jest przewidywanie działań zachodzących w obiekcie na podstawie wiedzy o modelu. W porównaniu z regulacją ze sprzężeniem wprzód (podrozdział 5.3) uwzględniany jest stan wyjściowy obiektu (ang. feedback), a nie wartości zadane. Rysunek 5.6 przedstawia regulator kaskadowy z uwzględnieniem momentu sił tarcia τ f (3.) i momentu sił grawitacji τ g (3.15). Tarcie Grawitacja Regulator pozycji Regulator prędkości Regulator prądu Obiekt Rysunek 5.6: Schemat regulacji z uwzględnieniem momentów sił

44 44 ROZDZIAŁ 5. UKŁADY REGULACJI

45 Rozdział 6 Komponenty i modele W tym rozdziale przedstawione zostały komponenty stworzone dla systemu ROS do obsługi robota Sarkofag (podrozdział 6.1), do identyfikacji modelu (podrozdział 6.) oraz modele Simulink-owe (podrozdział 6.3) do przeprowadzania symulacji. 6.1 Komponenty OROCOS dla robota Sarkofag W wyniku modernizacji oprogramowania w Laboratorium Robotyki Instytutu Automatyki i Informatyki Stosowanej Politechniki Warszawskiej, system sterowania robotem MRROC++ (sekcja..1) został zastąpiony sprawniejszą strukturą programową ROS (sekcja..). Aby móc korzystać z robota Sarkofag (sekcja.1.1) powstały komponenty opisane poniżej sark_kinematic Główny komponent opisujący kinematykę (podrozdział 3.1) robota. Zawiera funkcje umożliwiające przeliczanie wartości obrotów silnika na obroty ogniwa i obroty ogniwa na obroty silnika. SarkofagJM Klasa odpowiada za przeliczanie wartości pozycji z układu ogniwa na wartości silnika. Dodatkowo uwzględnia przesunięcie pomiędzy zerową wartością silnika (synchro) i ogniwa. Port wejściowy: JointPosition pozycja w układzie ogniwa Port wyjściowy: MotorPosition pozycja w układzie silnika 45

46 46 ROZDZIAŁ 6. KOMPONENTY I MODELE ROS/OROCOS SarkofagRegulator Regulator prąd zadany HardwareInterface Komunikacja ze sprzętem zadana pozycja silnika pozycja silnika TrajectoryGeneratorMotor Generator trajektorii w układzie silnika zadana pozycja silnika LimitDetector Ograniczenie pozycji silnika SarkofagMJ Pozycja silnika na ogniwa pozycja ogniwa TrajectoryGeneratorJoint Generator trajektorii w układzie ogniwa zadana pozycja ogniwa SarkofagJM Pozycja ogniwa na silnika Rysunek 6.1: Uproszczony schemat połączenia komponentów SarkofagMJ Klasa odpowiada za przeliczanie wartości pozycji silnika na wartości z układu ogniwa. Dodatkowo uwzględnia przesunięcie pomiędzy zerową wartością silnika (synchro) i ogniwa. Port wejściowy: MotorPosition pozycja w układzie silnika Port wyjściowy: JointPosition pozycja w układzie ogniwa 6.1. sark_regulator Komponent zawierający regulator dla robota. W pierwszej wersji stanowił klon rozwiązania z robota IRp-6 []. W kolejnych iteracjach rozwijany był o regulacje kaskadową (podrozdział 5.) i feedback tarcia (podrozdział 5.4). Regulator kaskadowy został wydzielony jako osobny komponent (sekcja 6..3) SarkRegulator Klasa w której zaimplementowany jest regulator dla robota Sarkofag. Porty wejściowe: deltainc_in przyrost pozycji MeasuredPosition pozycja zmierzona DesiredPosition pozycja zadana SynchroStateIn stan synchronizacji

47 6.1. KOMPONENTY OROCOS DLA ROBOTA SARKOFAG 47 Porty wyjściowe: computedpwm_out wyliczony zadany prąd EmergencyStopOut stop awaryjny (zbyt duży zadany przyrost) Parametry: A nastawa przy poprzednim sterowaniu (algorytm PD + I) BB nastawa przy przyroście całki uchybu (algorytm PD + I) BB1 nastawa przy poprzednim przyroście całki uchybu (algorytm PD + I) eint_dif wygaszanie całki uchybu max_output_current limit zadanego prądu [ma] current_reg_kp przelicznik sterowania na wartość zadanego prądu enc_res rozdzielczość enkodera max_desired_increment maksymalny możliwy przyrost debug tryb testowy output_multiplicator przelicznik prądu dla sterownika ELMO sark_bringup Komponent sark_bringup zawiera pliki konfiguracyjne, pliki startowe i przykładowe skrypty do obsługi robota. Pliki startowe pozwalają na włączenie robota w dwóch trybach: testowym i sprzętowym. Przed modernizacją sterowników oba tryby korzystały z komponentu hardware_interface, który zawierał w sobie wirtualny model. Po zmianie sterowników regulator sark_regulator został bezpośrednio połączony ze sterownikami ec_hardware lub wirtualnym modelem ec_hw_model. sark-nohardware.launch Tryb testowy nie korzysta z prawdziwego robota, zamiast tego używa wirtualnego modelu. sark-hardware.launch Tryb sprzętowy działa na rzeczywistym robocie, komunikacja z robotem odbywa się za pomocą odpowiedniego interfejsu sark_diagnostic Komponent zapewniający informacje o stanie robota takie jak stan synchronizacji, błąd sprzętowy lub sprzęt zajęty. Kopia komponentu stosowanego w robocie IRp-6.

48 48 ROZDZIAŁ 6. KOMPONENTY I MODELE SarkDiagnostic Klasa w której zaimplementowany jest mechanizm zbierania danych i wysyłania ich na odpowiedni temat (topic). Porty wejściowe: SynchroStateIn stan synchronizacji HardwaPanicIn stop awaryjny HardwaBusyIn stan zajęty (np. w czasie synchronizacji) Port wyjściowy: Diagnostics temat wysyłający informacje diagnostyczne Parametr: hardware_label nazwa urządzenia (w wiadomości wychodzącej) sark_description Komponent sark_description zawiera opis robota w formacie urdf. W pliku tym zawarte są opisy ogniw, ich parametry wizualne, bryły kolizji i połączenia ze sobą. Pozwala to na wizualizacje działania robota w programie RVIZ (sekcja..). Wizualizacja jest szczególnie przydatna przy uruchamianiu robota w trybie testowym. Rysunek 6.: Wizualizacja robota Sarkofag w programie RVIZ ec_manager Celem komponentu ec_manager jest komunikacja z nowym sterownikiem firmy ELMO (podsekcja.1.1.3).

49 6.. KOMPONENTY OROCOS DO IDENTYFIKACJI MODELU 49 Parametry: hal_component_name nazwa komponentu sterownika scheme_component_name nazwa głównego wątku service_name nazwa usługi w komponencie sterownika regulator_name nazwa regulatora do wystartowania po synchronizacji debug tryb testowy Komponent włącza sterownik odpowiedzialny za Sarkofag komendą enable, następnie sprawdza w jakim jest stanie. Jeśli robot jest niezsynchronizowany to wysyła sterownikowi komendę beginhoming. Po zakończeniu synchronizacji komendą switch- Blocks załącza działanie regulatora sark_regulator. W przypadku wystąpienia błędu resetuje sterownik komendą resetfault. 6. Komponenty OROCOS do identyfikacji modelu W celu usprawnienia procesu identyfikacji parametrów modelu stworzone zostały komponenty i funkcje automatyzujące część pracy. Środowisko do identyfikacji modeli napędów robotów (rys. 6.3) składa się z funkcji generujących trajektorię, zadajnika prądu lub przyrostów pozycji, rejestratora i funkcji modelujących i identyfikujących parametry. Matlab ROS/OROCOS - generator trajektorii - modelowanie - identyfikacja parametrów FileInputControl Zadajnik prądu lub przyrostu pozycji prąd zadany lub przyrost zadany HardwareInterface Komunikacja ze sprzętem pozycja, przyrost, prąd zmierzony MotorStateExport Eksporter danych Rysunek 6.3: Schemat systemu identyfikacji

50 5 ROZDZIAŁ 6. KOMPONENTY I MODELE Funkcje modelujące, identyfikujące i generatory trojektorii zostały zrealizowane w oprogramowania wspomagającego obliczenia jakim jest Matlab (sekcja..5) oraz w jego pakietach Simulink (podsekcja..5.1) i CurveFittingToolbox (podsekcja..5.). Zadajnik file_input_control (sekcja 6..1) i rejestrator motor_state_export (sekcja 6..) zostały zrealizowane jako komponenty OROCOS (sekcja..3) uruchomione są w węzłach ROS-a (sekcja..) file_input_control Komponent file_input_control służy do zadawania prądu lub przyrostów pozycji w kolejnych cyklach działania systemu. Wektor wartości zadanych jest odczytywany z plik tekstowego. Zadany prąd jest wysyłany zamiast wyjścia z oryginalnego regulatora. Zadajnik przyrostów korzysta z wbudowanego regulatora przyrostowego i wyznacza potrzebny prąd. Po zadaniu wszystkich wartości lista powtarza się. FileInputControl Klasa w której zaimplementowany jest zadajnik prądu/przyrostów. Porty wejściowe: deltainc_in przyrost pozycji SynchroStateIn stan synchronizacji Porty wyjściowe: computedpwm_out wyliczony zadany prąd EmergencyStopOut stop awaryjny (zbyt duży zadany przyrost) Parametry: filename nazwa pliku wejściowego input_type typ wejścia (current/increment) max_output_current limit zadanego prądu [ma] debug tryb testowy Parametry wbudowanego regulatora przyrostowego: A nastawa przy poprzednim sterowaniu (algorytm PD + I) BB nastawa przy przyroście całki uchybu (algorytm PD + I) BB1 nastawa przy poprzednim przyroście całki uchybu (algorytm PD + I) eint_dif wygaszanie całki uchybu current_reg_kp przelicznik sterowania na wartość zadanego prądu

51 6.. KOMPONENTY OROCOS DO IDENTYFIKACJI MODELU motor_state_export Komponent motor_state_export służy do zbierania i zapisywania wielkości takich jak położenie, przyrost pozycji i natężenie prądu zmierzonego. Komponent działa w dwóch trybach: awaryjnym i ciągłym. Pierwszy tryb zbiera w buforze określona w parametrach startowych liczbę ostatnich wartości, i w przypadku wystąpienia błędu (hardware panic) zapisuje wyniki. Drugi tryb działa przez cały czas notując na bieżąco otrzymane dane. Charakterystyki każdego napędu robota zapisywane są do osobnych plików.csv. MotorStateExport Klasa w której zaimplementowany jest eksporter danych silnika. Porty wejściowe: DesiredPositionIn pozycja zadana (w układzie silnika) DesiredVelocityIn prędkość zadana (w układzie silnika) MotorPositionIn zmierzona pozycja (w układzie silnika) MotorIncrementIn zmierzona prędkość (w układzie silnika) MotorCurrentIn zmierzony prąd SynchroStateIn stan synchronizacji HardwaPanicIn stop awaryjny Parametry: reg_number numer silnika/regulatora pre_syn_export zapis stanu silnika także przed synchronizacją filename nazwa pliku wyjściowego max_log maksymalna ilość cykli pamiętanych przy zapisie awaryjnym full_log tryb ciągłego eksportu danych debug tryb testowy 6..3 cascade_regulator Regulator kaskadowy PID został wydzielony jako osobny komponent z sark_regulator, aby móc być wykorzystanym przy testowaniu robotów IRp-6 (sekcja.1.). CascadeRegulator Klasa w której zaimplementowany jest testowany regulator kaskadowy PID.

52 5 ROZDZIAŁ 6. KOMPONENTY I MODELE Porty wejściowe: deltainc_in przyrost pozycji MeasuredPosition pozycja zmierzona DesiredPosition pozycja zadana SynchroStateIn stan synchronizacji Porty wyjściowe: computedpwm_out wyliczony zadany prąd EmergencyStopOut stop awaryjny (zbyt duży zadany przyrost) Parametry: KP_POS wzmocnienie regulatora pozycji TI_POS całka regulatora pozycji TD_POS różniczka regulatora pozycji KP_INC wzmocnienie regulatora prędkości TI_INC całka regulatora prędkości TD_INC różniczka regulatora prędkości max_output_current limit zadanego prądu [ma] current_reg_kp przelicznik sterowania na wartość zadanego prądu max_desired_increment maksymalny możliwy przyrost debug tryb testowy output_multiplicator przelicznik prądu dla sterownika ELMO 6.3 Modele Simulink W celu weryfikacji oraz dopasowania parametrów modelu oraz parametrów regulatorów algorytmami optymalizującymi, powstały odpowiednie modele w środowisku Symulink. Wszystkie modele symulacyjne korzystają z modelu Sarkofag (podrozdział 6.3.1). Dodatkowo modele z regulatorami korzystają z różnych wersji modelu Regulator Cascade (podrozdział 6.3.) i Regulator Sarkofag/IRP (podrozdział 6.3.3) Sarkofag Główny model symulacyjny. Służył do dopasowania parametrów tarcia i bezwładności przy pomocy funkcji optymalizacyjnych (sekcja 7.1.5). Model wykorzystywany jest w modelach regulatorów do generacji odpowiedzi układu.

53 6.3. MODELE SIMULINK 53 (a) Sarkofag model główny (b) Tarcie (c) Bezwładność Rysunek 6.4: Model Sarkofag Sarkofag model główny Port wejściowy: current prąd dostarczony do układu [ma] Porty wyjściowe: increment wyliczony przyrost position wyliczona pozycja Tarcie Port wejściowy: current przyrost Port wyjściowy: fric moment siły tarcia Funkcje: fri_1 funkcja tarcia przy przyroście dodatnim fri_1_min funkcja tarcia przy przyroście dodatnim dla zakresu < ; 1) fri_ funkcja tarcia przy przyroście ujemnym fri min funkcja tarcia przy przyroście ujemnym dla zakresu ( 1; > Bezwładność Port wejściowy: current suma momentów sił Port wyjściowy: inc przyrost

54 54 ROZDZIAŁ 6. KOMPONENTY I MODELE 6.3. Regulator Cascade Model nowo powstałego regulatora kaskadowego. Za jego pomocą dobierane były nastawy poszczególnych regulatorów (podrozdział 7.) na podstawie odpowiedzi z modelu (podrozdział 6.3.1) (a) Regulator Cascade model główny (b) PID Rysunek 6.5: Model Regulator Cascade Regulator Cascade model główny Porty wejściowe: desired_position pozycja zadana measured_position pozycja zmierzona (dostarczona z modelu) measured_increment przyrost zmierzony (dostarczona z modelu) Porty wyjściowe: computedpwm_out wyliczona wartość prądu desired_increment przyrost zadany (w celu zapisu)

55 6.3. MODELE SIMULINK 55 PID Port wejściowy: e sygnał uchybu Port wyjściowy: u sygnał sterujący Funkcje: r Kp ( T d) T i r1 Kp ( 1 T d 1) T i r Kp T d Regulator Sarkofag/IRP Model weryfikujący, powstał na podstawie istniejącego regulatora wykorzystywanego do sterowania robotów IRp-6 oraz Sarkofag. Porty wejściowe: desired_position pozycja zadana deltainc_in zmiana przyrostu Port wyjściowy: computedpwm_out wyliczona wartość prądu Rysunek 6.6: Model Regulator IRP

56 56 ROZDZIAŁ 6. KOMPONENTY I MODELE

57 Rozdział 7 Testy identyfikacyjne W tym rozdziale zaprezentowane zostały przeprowadzone procedury testowe pozwalające na identyfikację parametrów modeli o pojedynczym stopniu swobody (podrozdział 7.1). Po znalezieniu poszukiwanych wielkości, zostały symulacyjnie dobrane nastawy regulatorów dla robota Sarkofag (podrozdział 7.). Procedury powtórzono dla kolumn obrotowych robotów IRp-6 (podrozdział 7.3) 7.1 Testy identyfikacyjne parametrów modelu Poniżej przedstawiono procedury testowe identyfikacji parametrów tarcia statycznego i stałej momentowej silnika (sekcja 7.1.1), oraz parametrów funkcji aproksymującej tarcie dynamiczne (sekcja 7.1.). Opis zawiera weryfikację eksperymentalną procedur identyfikacyjnych, opisanych w podrozdziale (podrozdział 4.1), w oparciu o sterownik zaimplementowany w programowej strukturze ramowej MRROC++ (sekcja..1), a następnie ROS (sekcja..) Testy identyfikacyjne stałej momentowej silnika i tarcia statycznego Ramię manipulatora ustawiane jest w położeniu poziomym (rys. 7.1). Zwiększając zadawane natężenie prądu w układzie badana jest chwila pierwszego poruszenia się wału silnika (z dokładnością enkodera). Wartość odpowiedniego prądu jest notowana. Następnie test powtarzany jest dla ruchu w przeciwną stronę. W pierwszych dwóch testach ramię nie jest dociążone żadnym ciężarkiem, w kolejnych seriach testów jest zmieniane obciążenie ramienia. W testach trzecim i czwartym na ramieniu 57

58 58 ROZDZIAŁ 7. TESTY IDENTYFIKACYJNE Rysunek 7.1: Stanowisko Sarkofag podczas eksperymentów identyfikacji tarcia statycznego bez dodatkowego obciążenia w odległości 6cm od kolumny obrotu zamontowany został ciężarek o masie 1kg, w piątym i szóstym teście zamontowany został ciężarek o masie kg. Test 1 Założenia: brak dodatkowej siły zewnętrznej, obrót ramienia przeciwnie do wskazówek zegara. Średnie natężenie przy zrywie wynosi 414,75[mA]. Rysunek 7. przedstawia wyniki testu obrot [rad] 4 obrot [rad] natezenie [ma] (a) < A;,5A > natezenie [ma] (b) < A; 1A > Rysunek 7.: Testy identyfikacyjne K i i τ st : wyniki testu 1

59 7.1. TESTY IDENTYFIKACYJNE PARAMETRÓW MODELU 59 Test Założenia: brak dodatkowej siły zewnętrznej, obrót ramienia zgodnie z wskazówkami zegara. Średnie natężenie przy zrywie wynosi 436,5[mA]. Rysunek 7.3 przedstawia wyniki testu. obrot [rad] natezenie [ma] (a) < A;,5A > obrot [rad] x natezenie [ma] (b) < A; 1A > Rysunek 7.3: Testy identyfikacyjne K i i τ st : wyniki testu Test 3 Założenia: dodatkowa siła zewnętrzna uzyskana przez ciężarek o masie 1[kg] założony na odległości,6[m], obrót ramienia przeciwnie do wskazówek zegara. Średnie natężenie przy zrywie wynosi 76,5[mA]. Rysunek 7.4 przedstawia wyniki testu. Test 4 Założenia: dodatkowa siła zewnętrzna uzyskana przez ciężarek o masie 1[kg] założony na odległości,6[m], obrót ramienia zgodnie z wskazówkami zegara. Średnie natężenie przy zrywie wynosi 136[mA]. Rysunek 7.5 przedstawia wyniki testu.

60 6 ROZDZIAŁ 7. TESTY IDENTYFIKACYJNE obrot [rad] obrot [rad] natezenie [ma] (a) < A;,5A > natezenie [ma] (b) < A; 1A > Rysunek 7.4: Testy identyfikacyjne K i i τ st : wyniki testu obrot [rad] natezenie [ma] Rysunek 7.5: Testy identyfikacyjne K i i τ st : wyniki testu 4 Test 5 Założenia: dodatkowa siła zewnętrzna uzyskana przez ciężarek o masie [kg] założony na odległości,6[m], obrót ramienia przeciwnie do wskazówek zegara. Średnie natężenie przy zrywie wynosi 55[mA]. Rysunek 7.6 przedstawia wyniki testu.

61 7.1. TESTY IDENTYFIKACYJNE PARAMETRÓW MODELU obrot [rad] 15 1 obrot [rad] natezenie [ma] (a) < A;,5A > natezenie [ma] (b) < A;,4A > Rysunek 7.6: Testy identyfikacyjne K i i τ st : wyniki testu 5 Test 6 Założenia: dodatkowa siła zewnętrzna uzyskana przez ciężarek o masie [kg] założony na odległości,6[m] obrót ramienia zgodnie z wskazówkami zegara Średnie natężenie przy zrywie wynosi 1374,5[mA]. Rysunek 7.7 przedstawia wyniki testu. obrot [rad] natezenie [ma] Rysunek 7.7: Testy identyfikacyjne K i i τ st : wyniki testu 6 Zestawienie wyników Tabela 7.1 prezentuje zestawienie wyników testów 1-6.

62 6 ROZDZIAŁ 7. TESTY IDENTYFIKACYJNE obciążenie [kg] - 1 prąd [ma], obrót zgodny 414,75 76,5 55 prąd [ma], obrót przeciwny 436, ,5 Tablica 7.1: Zestawienie wyników testów identyfikacyjnych stałej momentowej i tarcia statycznego Identyfikacja stałej momentowej silnika Korzystając ze wzorów na obliczenie stałej momentowej silnika (4.3), dla każdej pary testów wyznaczono stałą momentową K i. Wyniki zestaw 1 dla braku obciążenia i obciążenia 1[kg],19[ Nm] A zestaw dla braku obciążenia i obciążenia [kg],117[ Nm] A zestaw 3 dla obciążenia 1[kg] i obciążenia [kg],1357[ Nm] A Dla zestawu 1 i otrzymane wartości mieszczą się w granicach danych silnika (,15[ N m ] ± 5%) (tab..1). Zestaw 3 nie spełnia tej zależności. A Identyfikacja tarcia statycznego Korzystając ze wzorów na obliczenie tarcia statycznego silnika (4.4), dla każdej stałej momentowej silnika i każdego obciążenia wyznaczono moment tarcia statycznego τ fst. Wyniki przy stałej momentowej silnika =,19[ Nm] A obciążenie 1kg,768[N m] brak obciążenia,438[nm] obciążenie kg,836[n m] przy stałej momentowej silnika =,117[ Nm] A obciążenie 1[kg],768[N m] brak obciążenia,498[nm] obciążenie [kg],836[n m] przy stałej momentowej silnika =,1357[ Nm] A obciążenie 1[kg],891[N m] brak obciążenia,578[nm] obciążenie [kg],97[n m] przy stałej momentowej silnika =,15[ Nm] A obciążenie 1[kg],689[N m] brak obciążenia,447[nm] obciążenie [kg],75[n m]

63 7.1. TESTY IDENTYFIKACYJNE PARAMETRÓW MODELU Testy identyfikacyjne tarcia dynamicznego Ramię manipulatora zostało zdjęte do testów w celu eliminacji wpływu momentu grawitacyjnego na wyniki eksperymentu. Stan robota przygotowanego do testów przedstawia rysunek 7.8. Rysunek 7.8: Stanowisko Sarkofag ze zdemontowanym ramieniem podczas eksperymentów identyfikacji tarcia dynamicznego Ustalając natężeniem prądu zadajemy stały moment elektromagnetyczny, dzięki któremu mechanizm rozpędza się. Po osiągnięciu prędkości maksymalnej dla zadanego natężenia, moment elektromagnetyczny jest całkowicie redukowany przez moment tarcia dynamicznego. Tabela 7. prezentuje wyniki testów. Numer testu I II III IV V VI VII VIII IX Prąd [A], Przyrost enk RPM , , Prędkość [ rad ] s ,56 34,56,8 161,8 566,7 499,51 775,97 Tablica 7.: Zestawienie wyników testów identyfikacyjnych tarcia dynamicznego Wyniki przedstawiono dla wszystkich 6-ściu kombinacji prędkości ruchu w tym samym kierunku (dla drugiego kierunku wyniki te były zbliżone). Rezultaty wskazują na to, że standardowy model tarcia nie przystaje dobrze do badanego obiektu. 1. Dla stałej momentowej silnika =,19[ Nm A ] zestaw III-V III-VII III-IX V-VII V-IX VII-IX µ =,168,179,177,989,684,616 [N m] b = 1 3 1,416,7739,835,56,7148,981 [Nm s rad ]