STEROWANIE RUCHEM MANIPULATORA magisterska - praca... · Web viewPrzy rozwiązywaniu tych...

STEROWANIE RUCHEM MANIPULATORA – Spis treści

Spis treści.1. WPROWADZENIE.............................................................................................3

1.1. Wstęp....................................................................................................................................31.2. Przegląd literatury i istniejących rozwiązań.........................................................................41.3. Cel i zakres pracy.................................................................................................................5

2. PODSTAWOWE POJĘCIA................................................................................62.1. Podstawowe pojęcia, struktury i systematyzacja manipulatorów........................................62.2. Układy sterowania manipulatorami....................................................................................10

3. KINEMATYKA I DYNAMIKA MANIPULATORA.....................................163.1. Kinematyka manipulatora..................................................................................................16

3.1.1. Wyprowadzenie zależności kinematycznych.............................................................................................163.1.2. Zadanie proste i odwrotne kinematyki.......................................................................................................193.1.3. Zadanie planowania trajektorii ruchu chwytaka.........................................................................................233.1.4. Weryfikacja numeryczna............................................................................................................................26

3.2. Dynamika manipulatora.....................................................................................................283.2.1. Przyjęcie metody rozwiązania....................................................................................................................293.2.2. Wyprowadzenie dynamicznych równań ruchu..........................................................................................31

1 / 106


3.2.3. Weryfikacja otrzymanych równań dynamicznych.....................................................................................37

4. KONWENCJONALNE STEROWANIE RUCHEM MANIPULATORA.. .414.1. Wybór algorytmu sterowania i stabilność układu..............................................................414.2. Weryfikacja numeryczna układu sterowania.....................................................................43

5. ADAPTACYJNY ALGORYTM STEROWANIA MANIPULATOREM....555.1. Wstęp i wybór prawa sterowania.......................................................................................555.2. Stabilność układu i dobór prawa adaptacji parametrów.....................................................565.3. Weryfikacja numeryczna układu sterowania.....................................................................58

6. BADANIA EKSPERYMENTALNE................................................................716.1. Opis manipulatora..............................................................................................................716.2. Realizacja techniczna układu sterowania...........................................................................736.3. Uzyskane wyniki i wnioski................................................................................................77

7. KIERUNKI DALSZYCH BADAŃ...................................................................898. LITERATURA...................................................................................................909. SPIS ZAŁĄCZONEGO OPROGRAMOWANIA...........................................9110. DODATEK - KODY ŹRÓDŁOWE PROGRAMÓW.................................92

10.1. Kinematyka........................................................................................................................9210.2. Planowanie trajektorii.........................................................................................................9410.3. Dynamika i sterowanie.......................................................................................................95


1. Wprowadzenie.

1.1. Wstęp.

Robotyka jest dziedziną nauki i techniki, która zajmuje się problematyką mechaniki, sterowania, projektowania, pomiarów, zastosowań oraz eksploatacji manipulatorów i robotów. Przedmiotem robotyki jest zastosowanie robotów w badaniach naukowych, szeroko pojętej technice, budownictwie, transporcie, rolnictwie, jak równieżw medycynie. Teoria manipulatorów i robotów jest interdyscyplinarną dziedziną badań wymagającą współdziałania specjalistów z różnych dziedzin [1].

Przez ostatnie dwadzieścia - trzydzieści lat nastąpił duży rozwój robotów przemysłowych, które znajdują zastosowanie szczególnie w przemyśle maszynowymdo prac spawalniczych, malarskich, montażowych oraz obsługi pras i obróbek wykańczających, jak szlifowanie i polerowanie. Głównym celem ich zastosowania jest podwyższenie jakości wykonywanych prac, skrócenie czasu wykonania oraz uwolnienie człowieka od ciężkiej i monotonnej pracy, a zwłaszcza od prac niebezpiecznychdla zdrowia.

Przedmiotem szczególnego zainteresowania są zagadnienia: dokładności pozycjonowania i orientacji, realizacji trajektorii w przestrzeni z przeszkodami, czynnego sterowania siłą, komunikacji głosowej i wizyjnej, modelowania elastycznych manipulatorów. Przy rozwiązywaniu tych zagadnień korzysta się z całej klasy metod matematyki, mechaniki, teorii maszyn, teorii sterowania, informatyki, teorii systemów, miernictwa, diagnostyki, teorii eksploatacji [1].

Zagadnienie sterowania manipulatorami należy do klasy sterowania nadążnego. Trajektorie wyznaczone są zazwyczaj w przestrzeni kartezjańskiej lub w przestrzeni zmiennych konfiguracyjnych. Do podstawowych zagadnień, które należy rozwiązaćze względu na sterowanie ruchem nadążnym to zapewnienie odpowiedniej dokładnościi stabilności ruchu. Trzeba zaznaczyć, że projektując algorytmy sterowania, koniecznie powinno się uwzględnić zmienne warunki pracy, które wynikają z realizacji różnych zadań [4]. Algorytmy sterowania ruchem nadążnym manipulatorów powinny zapewniać dużą dokładność i stabilność realizowanego ruchu.

3 / 106

STEROWANIE RUCHEM MANIPULATORA – Wprowadzenie

1.2. Przegląd literatury i istniejących rozwiązań.

Około 95% istniejących rozwiązań przemysłowych układów sterowania manipulatorów to układy oparte o regulatory PD lub PID [1]. Takie rozwiązania podyktowane były realizacją algorytmów sterowania w technice analogowej. Jeśli już pokuszono się o układy cyfrowe – to powyższe rozwiązanie narzucone było niewystarczającymi częstotliwościami próbkowania. Kolejne 3% to układy sterowania bez sprzężenia zwrotnego. Około 1% stanowią układy sterowania otwartego, w których rolę „regulatora” pełni operator. W końcu 1% to inne rozwijające się systemy sterowania, jak np. sterowanie rozmyte, adaptacyjne, neuronowe, ślizgowe i pozostałe.

Istnieje wiele technik i metod sterowania, które mogą być zastosowane do sterownia manipulatorami. Konkretnie, wybrana metoda i sposób jej realizacji mogą mieć istotny wpływ na osiągi manipulatora, a w konsekwencji na jego zakres zastosowań [3]. Obecnie wykorzystując szybkie sterowniki cyfrowe i komputery klasy PC, istnieje możliwość przeprowadzenia zarówno symulacji dowolnych algorytmów sterowania, jaki ich realizacji w czasie rzeczywistym.

Autorzy pracy [1] przedstawiają dogłębnie analizę manipulatorów z punktu mechaniki ruchu, jak również wprowadzają w wybrane zagadnienia robotyki. Brakjest natomiast rzetelnej i uporządkowanej wiedzy z obszaru sterowania manipulatorami.

W pracy [2] przedstawiono niezbędną wiedzę z zakresu mechaniki manipulatorów, poszerzając obszar pracy o sformułowanie sterowania głównie w oparciu o regulatorPD / PID.

Problem mechaniki ruchu i sterowania manipulatorami omówiono w pracy [3]. Przedstawiono zarówno podstawowe sterowanie: z regulatorem PD, jak i rozszerzono zasób wiedzy o sterowanie ślizgowe i adaptacyjne dla układów nieliniowych. Przedstawiono również wyniki symulacyjne dla prostych przypadków manipulatorów. Na bazie tej literatury i następnej wyprowadzano większość zależności matematycznych, jak również porównywano otrzymane wyniki.

Systematyczną wiedzę z dziedziny sterowania odpornego, ale robotami mobilnymi przedstawił autor pracy [4]. Swoje rozważania teoretyczne, autor poparł wynikami symulacyjnymi i badaniami laboratoryjnymi.

W publikacji [5] autorzy prowadzą teoretyczne rozważania o strukturze sterowania adaptacyjnego, poparte o wyniki symulacyjne dla dobrze znanego manipulatora dwuczłonowego.

Literatura [6] poświęcona jest prawie w całości zagadnieniom mechaniki ruchu manipulatorów i robotów.

W pozycji [7] autorzy solidnie i wyczerpująco przedstawiają problemy zarówno mechaniki ruchu jak i sterowania jednak w odniesieniu do robotów mobilnych.

Podsumowując: brakuje powszechnej literatury polskojęzycznej poświęconej problemom sterowania manipulatorami. W analizowanej literaturze obcojęzyczneji polskojęzycznej brakuje natomiast zgodności w przyjęciu oznaczeń pomiędzy wiodącymi ośrodkami badawczymi zarówno krajowymi jak i zagranicznymi, co z kolei znacząco utrudnia analizę przedstawianych prac.

STEROWANIE RUCHEM MANIPULATORA – Wprowadzenie

1.3. Cel i zakres pracy.

Niniejsza praca stanowi próbę sformułowania problemu mechaniki ruchudla przyjętego układu manipulatora i następnie jego sterowania w oparciu o jednąz dostępnych metod.

W pracy rozważa się sterowanie pozycją manipulatora o strukturze stawowej. Przyjęto, że sterowany manipulator jest obiektem silnie nieliniowym i wielowymiarowym.

W rozdziale drugim sklasyfikowano struktury manipulatorów i układów sterowania. Rozdział trzeci poświęcono mechanice ruchu – wprowadzając w problemy kinematykii dynamiki przyjętego układu wykorzystując oprogramowanie Maple® i Matlab®Simulink®. W oparciu o wyprowadzone zależności przeprowadzono symulację układów sterowania, zarówno: konwencjonalnego opartego o regulator PD – rozdział czwarty, jaki adaptacyjnego algorytmu sterowania – rozdział piąty; wykorzystując oprogramowanie Matlab®Simulink®. W rozdziale szóstym zawarto opis układu sterowania i wyniki badań laboratoryjnych nad układem sterowania przy wykorzystaniu manipulatora Scorbot Er4pc wraz z oprogramowaniem Matlab®Simulink®.

STEROWANIE RUCHEM MANIPULATORA – Podstawowe pojęcia

2. Podstawowe pojęcia.

2.1. Podstawowe pojęcia, strukturyi systematyzacja manipulatorów.

Manipulator - urządzenie techniczne przeznaczone do realizacji niektórych funkcji: manipulacyjnych (gr. manus - ręka) wykonywanych przez chwytaki wysięgnikowych realizowanych przez ramię manipulatora. Współczesne manipulatory składają się z pojedynczego łańcucha kinematycznego otwartego od pięciu do dziewięciu stopniach swobody lub zdwojonego łańcucha, zespołu siłowników (napędu), układu sterowania, czujników i układu zasilania.

Na rysunku 2.1 pokazano schemat blokowy manipulatora [1].

Rys. 2.1. Schemat blokowy manipulatora [1].

Obierając kryterium, manipulatory możemy podzielić ze względu na:1. Ruchliwość,2. Odmianę łańcucha kinematycznego,3. Przeznaczenie,4. Zastosowany napęd,5. Stopień specjalizacji,6. Własności geometryczne,7. Układ sterowania,8. Kolejne generacje,9. Inne.

W każdej z wymienionych grup, można wydzielić kolejne podgrupy i zaliczyćdo nich klasyfikowany obiekt. Poszczególne punkty zostano pokrótce omówione poniżej. Często spotkać można tzw. manipulatory hybrydowe, które powstają przez zastosowanie różnych konfiguracji sprzętowo-programowych, np. połączenie napędu hydraulicznego ramion z napędem pneumatycznym chwytaka i elektronicznym układem sterowania.

Ruchliwość manipulatora można wyznaczyć z zależności


n

iipinw

1

6 (2.1)

gdzie:w – ruchliwość jako liczba niezależnych ruchów członów ruchomych względem

podstawy,n – liczba członów ruchomych,p – liczba połączeń różnych rodzajów [1].

Ponieważ w przypadku otwartych łańcuchów liczba członów ruchomych równa jest liczbie par kinematycznych, to zależność (2.1) przyjmie postać

(2.2)

co oznacza, że ruchliwość łańcucha otwartego równa jest liczbie stopni swobody jego połączeń – par kinematycznych [1].

Rozważmy niektóre odmiany łańcuchów kinematycznych, złożonych z par połączeń obrotowych „O” i postępowych „P”. Liczbę możliwych wariacji utworzonych z dwóch elementów można określić

(2.3)

gdzie:k – liczba członów.

Zmieniając usytuowanie osi par można uzyskać dodatkowe odmiany [1]. W sumie dla k=3, możliwe jest uzyskanie aż 144 różnych konfiguracji łańcucha kinematycznego.

Odmiana POP OOO PPP OOP POO% 47 25 14 13 1

Tabl. 2.1. Procentowy udział występowania poszczególnych odmian łańcuchów manipulatorów [1].

Roboty i manipulatory rozwinęły się dzięki swojemu wszechstronnemu zastosowaniu. Znalazły zastosowanie we wszystkich rozwijających się (jak również istniejących) gałęziach przemysłu. Dla przykładu można wymienić przemysł samochody przy zgrzewaniu blach i pracach malarskich, przemysł hutniczy przy wytopie metalii obsłudze pras, ogólnie we wszystkich gałęziach przemysłu przy transporcie i paletyzacji elementów. Do tego dochodzą prace podwodne i kosmiczne, manipulatory rehabilitacyjnei medyczne, maszyny kroczące i cały dział związany z mikrorobotyką.

We współczesnych robotach stosowane są w zasadzie trzy rodzaje napędów: pneumatyczne, hydrauliczne i elektryczne. Każdy robot jest wyposażony w układ silnikówi siłowników rozmieszczonych na jego ramionach lub w jego połączeniach ruchowych, tworząc napęd robota. Według literatury [1] przewagę w zastosowaniu znalazł napęd elektryczny i hydrauliczny przed pneumatycznym.


Rys. 2.2. Procentowy udział różnego rodzaju napędów stosowanych w robotach na rok 1990 według literatury [1]

Do zalet napędów hydraulicznych możemy wymienić:• łatwość uzyskiwania dużych sił przy małych rozmiarach i ciężarach urządzeń

wykonawczych,• łatwość precyzyjnego sterowania,• dobre własności dynamiczne,• możliwość uzyskiwania małych prędkości ruchu bez konieczności stosowania

przekładni,• łatwość uzyskiwania ruchów jednostajnych,• mała wrażliwość na zmianę obciążeń,• łatwość konserwacji,• duża pewność ruchowa.

Wady stosowania napędów hydraulicznych to dużej mierze:• duży hałas przy wytwarzaniu ciśnienia (pompy),• zanieczyszczenie środowiska pracy.

Napęd pneumatyczny rozwinął się dzięki korzystnym parametrom:• duża pewność ruchowa,• większa prostota konstrukcji, aniżeli napędu hydraulicznego,• niska cena urządzeń,• mała masa własna urządzeń i mała masa czynnika roboczego,• powolne narastanie sił,• duża przeciążalność,• iskrobezpieczeństwo.

Wady stosowania napędu pneumatycznego wynikają głownie z:• duża wrażliwość ruchu na zmiany obciążenia,• gwałtowny rozruch przy małym obciążeniu lub źle dobranych elementach,• znacznie mniejsze siły i momenty, aniżeli w napędzie hydraulicznym,• korozja elementów,• trudność w sterowaniu elementu wykonawczego.

Duży wzrost stosowania napędu elektrycznego podyktowany jest:• małe rozmiary,• odporność na krótkotrwałe przeciążenia,• krótkie czasy rozruchu i sterowania,• łatwość w płynnym sterowaniu,


• małe koszty produkcji i utrzymania,• duża niezawodność,• praca bez hałasu,• brak dodatkowego oprzyrządowania, jak to ma miejsce w hydraulice

i pneumatyce,

Wady ze stosowania napędu elektrycznego:• niekorzystny stosunek mocy do masy,• właściwości dynamiczne mniejsze niż hydrauliki,• wrażliwość na długotrwałe przeciążenia,• przy dużych prędkościach obrotowych wymagają zastosowania przekładni.

Większość manipulatorów jest zaprojektowanych tak, że ostatnie n-3 pary obrotowe łańcucha kinematycznego, orientujące człon roboczy, mają osie przecinające się w jednym punkcie nazywanym środkiem kiści. Pierwsze trzy pary kinematyczne określają pozycję środka kiści. Dlatego rozróżnia się dwie części struktury manipulatora: strukturę pozycjonowania zwaną regionalną (ramieniem) oraz strukturę orientowania zwaną lokalną (kiścią).

Struktura pozycjonowania robota - określa kształt przestrzeni roboczeji jej objętość. Można wykazać, że dla manipulatora z sześcioma parami obrotowymi objętość przestrzeni roboczej jest maksymalna, gdy osie pierwszych dwóch par obrotowych przecinają się pod kątem prostym, a oś trzeciej pary obrotowej jest równoległa do osi drugiej pary obrotowej (np. analizowany dalej manipulator Scorbot Er4pc).

Struktura orientowania - określa zdolność ustalenia lub zmiany orientacji członu roboczego. Jak wykazano, optymalną zdolność orientowania uzyskuje się wówczas,gdy osie ostatnich trzech par obrotowych przecinają się kolejno pod kątem prostym. Jeżeli osie trzech par obrotowych są dodatkowo ortogonalne i przecinają się w jednym punkcie,to są one równoważne przegubowi kulistemu. Zatem idealna struktura manipulatora może być typu: 3R - 3S (R - para obrotowa, S - para kulista) lub 3P - 3S (P - para przesuwna) [6].

Przestrzeń robocza manipulatora – jest całkowitym obszarem, do którego sięga jego końcówka robocza przy pełnych zakresach wszystkich możliwych ruchów manipulatora [3]. Granice przestrzeni roboczej n - członowego robota jest trudno opisać przez ogólne równania przemieszczeń. Łatwiej jest to zrobić za pomocą przekrojów tej przestrzeni i wyznaczenia granicznego konturu na zadanej płaszczyźnie przekroju. Obracając lub przesuwając tę płaszczyznę, otrzymuje się trójwymiarową przestrzeń roboczą.

Zadania, jakie może wykonywać manipulator, są zależne od jego konstrukcjii ogólnych wskaźników, takich jak np. udźwig, szybkobieżność, wymiary przestrzeni roboczej, dokładność i powtarzalność. W pewnych zastosowaniach bierze się pod uwagę wymiary manipulatora, zużycie mocy i koszt eksploatacji.

Dokładność jest miarą zdolności manipulatora do osiągnięcia zaprogramowanego położenia członu roboczego.

Powtarzalność jest pojęciem najczęściej stosowanym przy ilościowej ocenie zdolności manipulatora do przemieszczania członu roboczego w to samo położenieprzy kolejnych próbach.

Większość współczesnych robotów przemysłowych ma powtarzalność znacznie lepszą od dokładności. Powtarzalność jest ważna w przypadku „nauczania" manipulatora przez przemieszczanie do pożądanego położenia za pomocą programatora przenośnego


(klawiszowego). Obecnie występuje tendencja do programowania ruchu manipulatora metodą „off-line". W takim przypadku dokładność manipulatora staje się ważniejsza,a zatem występuje potrzeba analizy błędów pozycjonowania i orientacji.

Rozróżnia się maksymalną prędkość członu roboczego i całkowity czas cyklu poszczególnego zadania. Często fazy przyśpieszania i opóźniania trwają przez większą część cyklu i dlatego zdolność przyśpieszania może być ważniejsza od maksymalnej prędkości.

Klasyfikując rozpatrywany w tej pracy manipulator Scorbot Er4pc, można powiedzieć, że jest on otwartym łańcuchem kinematycznym o pięciu stopniach swobody. Trzy pierwsze odpowiadają za pozycjonowanie i są typu RRR, kolejne dwa to orientacja, również typu RR, (przy czym R to para obrotowa). Zastosowano w nim silniki elektryczne prądy stałego z magnesami stałymi i wbudowaną przekładnią mechaniczną. Sterowanie odbywa się bezpośrednio z komputera PC poprzez kartę rozszerzeń ISA. Elementami podającymi położenie ramion są encoder’y optyczne. Według specyfikacji przeznaczony jest on do celów naukowych, jak również możliwe jest zastosowanie w przemyśle lekkim przy spawaniu lub szlifowaniu.

2.2. Układy sterowania manipulatorami.

Zagadnienie sterowania manipulatorami sprowadza się do problemu określenia przebiegu czasowego na wejściach przegubów, niezbędnego do wykonania zadanego ruchu przez końcówkę manipulatora. Wejściami przegubów mogą być siły i momenty podanena te przeguby lub też wejścia na ich napędy, np. napięcia wejściowe na silniki,w zależności od rodzaju konstrukcji sterownika.

Istnieje wiele technik i metod sterowania, które mogą być zastosowanedo sterowania manipulatorami. Konkretna - wybrana metoda i sposób jej zastosowania mogą mieć istotny wpływ na osiągi manipulatora, a w konsekwencji - na zakres jego możliwych zastosowań. Przykładowo sterowanie wzdłuż trajektorii ciągłej wymaga innych rozwiązań w zakresie sprzętu i oprogramowania sterującego niż sterowanie z punktudo punktu. Ponadto sama konstrukcja mechaniczna manipulatora ma wpływ na typ koniecznego układu sterowania. Dla przykładu zadania sterowania manipulatorem kartezjańskim są zdecydowanie inne niż sterowania manipulatorem z łokciem. Stwarzato potrzebę tzw. kompromisu sprzętowo-programowego między strukturą mechaniczną układu a architekturą czy oprogramowaniem sterownika [3].

Stały rozwój technologiczny widoczny w dziedzinie mechanicznej konstrukcji robotów poprawia ich potencjalne możliwości i rozszerza zakres zastosowań. Jednak zwiększające się możliwości wymagają bardziej złożonego podejścia do problemu sterowania.


Rys. 2.3. Podstawowa struktura sterowania ze sprzężeniem zwrotnym.

Podstawowa struktura systemu sterowania ze sprzężeniem zwrotnym z jednym wejściem i jednym wyjściem jest pokazana na rysunku 2.3.

Celem projektowania układu sterowania - jest wybór regulatora w taki sposób,aby wyjście obiektu „śledziło" lub nadążało za wyjściem żądanym, podanym przez sygnał wartości zadanej. Sygnał sterujący nie jest jednak jedynym wejściem, działającym na układ. Zakłócenia, które są w istocie wejściami i których nie kontrolujemy, także wpływająna zachowanie się wyjścia. Dlatego sterownik musi być tak zaprojektowany, aby wpływ zakłóceń na wyjście obiektu był zredukowany. Jeśli uda się to osiągnąć, to można powiedzieć, że obiekt „odrzuca" zakłócenia. Podwójny cel: śledzenie parametrów zadanych i odrzucanie zakłóceń jest podstawą każdej metody sterowania [3].

Ograniczymy się wstępnie do omówienia kilku najbardziej rozpowszechnionych metod sterowania, jak również przedstawimy wstępnie kilka najnowszych trendóww dziedzinie projektowania układów sterowania.

Jeżeli w miejsce regulatora wybierzemy strukturę regulatora PD o równaniu

(2.4)

gdzie:e=qd-q – jest uchybem nadążania,KP, Kd – są diagonalnymi macierzami wzmocnień regulatora.

to otrzymamy najprostszy układ sterowania w pętli zamkniętej. Wykazanie stabilności zaproponowanego regulatora pokażemy w rozdziale 3. Przedstawiony regulator opisany jest w każdej literaturze poświęconej układom sterowania i automatycznej regulacji. Jego popularność wynika z prostoty i funkcjonalności. Obecnie 95% urządzeńz automatyczną regulacją opartych jest właśnie na regulatorach P lub PD, rzadko PID.

Rys. 2.4. Struktura sterowania z regulatorem P-PD-PID plus kompensacja od sił grawitacji


Regulatory PD posiadają podstawową wadę – nie potrafią odrzucać zakłóceń.Z tego względu pojawiły się inne zmodyfikowane postacie regulacji opartej na regulatorach PD, jak chociażby: regulacja PD, plus kompensacja od sił grawitacji (rysunek 2.4),lub regulacja PD, plus kompensacja oporów ruchu. Wprowadzenie do układu sterowania dodatkowych zależności wymusza zastosowania szybszych sterowników cyfrowych, które w czasie rzeczywistym będą przeliczały zadane wartości.

Powyższe podejście umożliwiło poprawienie jakości sterowania. Nadal jednak układ sterowania zaprojektowany raz musiał taki pozostać. Nie mógł ulegać samoregulacji, dostosowując się do panujących warunków pracy.

Poszukiwano, więc takich rozwiązań, które będą na bieżąco korygować swoje współczynniki, aby minimalizować różnicę pomiędzy opisem matematycznym zawartymw układzie sterowania a samym układem rzeczywistym. Sterowanie takie nosi nazwę sterowania odpornego. Pod tym pojęciem kryją się dwie przodujące metody, mianowicie: sterowanie ze zmienną strukturą i sterowanie adaptacyjne.

Układ sterowania ze zmienną strukturą to taki układ, w którym struktura może być nagle zmieniana lub przełączana zgodnie z pewną logiką przełączeń, której celem jest zapewnienie pożądanego zachowania się układu [3] (rysunek 2.5.). Najprostszymi przykładami układów ze zmienną strukturą są układy przekaźnikowe lub układy typu„on-off” w układach ogrzewania lub klimatyzacji.

Rys. 2.5. Struktura sterowania z wykorzystaniem logiki przełączeń.

Na podstawie posiadanej wiedzy o obiekcie, określa się dokładność modeluza pomocą przewidywanego przedziału zmienności każdego z parametrów. Następnie projektuje się regulator, którego struktura składa się z kompensatora i stabilizatora,aby układ zamknięty pracował poprawnie dla każdego obiektu mieszczącegosię w założonych granicach dokładności. Otrzymany w ten sposób regulator nosi nazwę regulatora odpornego. Zaletą tej metody syntezy algorytmów sterowania jest duża pewność działania układu. Do wad opisanego podejścia należy zaliczyć fakt, że w praktyce najmniejsze wartości ograniczeń zakłóceń parametrycznych i nieparametrycznych są rzadko dostępne i stosuje się ich zawyżone wartości, co może powodować nadmierne amplitudy sygnału sterowania, a w konsekwencji występowanie poślizgów. W celu zapoznaniasię z układami sterowania ze zmienną strukturą odsyłamy do literatury[1, 2, 3, 4], jak również innych obcojęzycznych.

Podstawowa idea sterowania adaptacyjnego polega na tym, że zmieniasię wartość wzmocnienia lub innych parametrów prawa sterowania, zgodnie z pewnym


algorytmem działającym „on-line”. W ten sposób regulator może „nauczyć się” odpowiedniego zestawu parametrów podczas pracy. Ta idea jest szczególnie użyteczna dla manipulatorów realizujących zadanie powtarzania. Bez adaptacji parametrów jak się później okaże błędy są również powtarzane. Wprowadzając adaptację jakość nadążania może być skutecznie poprawiana przez kolejne powtórzenia [3].

Rys. 2.6. Idea sterowania adaptacyjnego.

Sterowanie adaptacyjne zostanie szczegółowo przedstawione w rozdziale 5.

Obok wspomnianych metod stosowanych do projektowania układów sterowaniaw warunkach niepewności coraz większym zainteresowaniem cieszą się techniki stosowanew sztucznej inteligencji - sieci neuronowe i układy z logiką rozmytą [7].

Sztuczne sieci neuronowe, ze względu na ich możliwości aproksymacji dowolnych odwzorowań nieliniowych oraz możliwość uczenia się i adaptacji stały się atrakcyjnym narzędziem stosowanym w teorii układów nieliniowych. Ze względu na posiadaną własność uczenia się nieliniowych charakterystyk, stosuje się je do modelowania złożonych nieliniowych układów. Sztuczne sieci neuronowe wykorzystują informacje numeryczne wiążące w sposób jednoznaczny (ostry) sygnały wejścia i wyjścia.

Rys. 2.7. Graficzny algorytm sterowania neuronowego.

Alternatywnym sposobem analizy i syntezy układów dynamicznych, których modele jedynie w przybliżeniu opisują własności obiektu są układy z logiką rozmytą. Układy te wykorzystują informację lingwistyczną, która operuje pojęciami opisowymii mogą być zastosowane między innymi do aproksymacji funkcji nieliniowych w procesie modelowania i sterowania. Przykładami takimi mogą być zmienne lingwistyczne „bardzo


mały”, „mały”, „duży”, „bardzo duży”. Każde z tych określeń wobec nieprecyzyjnie zdefiniowanych różnic stanowi pojęcie nieostre, rozmyte. Zmiennym lingwistycznym można przyporządkować zmienne numeryczne. Funkcje opisujące ich zależności tworzą reguły opisujące w sposób ścisły tego typu systemy. W układach z logiką rozmytą występują symboliczne reguły, „JEŻELI – TO” jakościowe zmienne opisane zmiennymi lingwistycznymi oraz operatory rozmyte „I”,

JEŻELI x1 jest małe I x2 jest duże TO y jest średnie ujemne

Ze względu na możliwość elementarnego opisu tego typu pojęć a w związku z tym szybkich realizacji w czasie rzeczywistym, układy z logiką rozmytą zostały zaimplementowane w środowisko numeryczne modelowania i sterowania złożonymi procesami.

Rys. 2.8. Graficzny algorytm sterowania z logiką rozmytą.

Układy z logiką rozmytą znajdują najszersze zastosowania w układach sterowania. Wynika to z faktu, że można je stosować do aproksymacji nieliniowych odwzorowańoraz do sterowania układów dynamicznych, których modele jedynie w przybliżeniu opisują własności sterowanego obiektu. Typowym przykładem tego typu obiektów są manipulatory i mobilne roboty kołowe [7].

Podsumowując: w każdym z przedstawionych algorytmów sterowania występuje regulator proporcjonalno – różniczkujący (PD). Jest on podstawą przy zamkniętych układach sterowania. Dodatkowe elementy takie jak kompensatory typu krzepkiego, adaptacyjnego, sieci neuronowe, logika rozmyta mają za zadanie zredukować wady regulatora PD, o których będzie mowa w rozdziale 4.

Im dokładniej odzwierciedlimy nieznane związki obiektu sterowania w układzie sterowania tym lepszy będzie sam układ sterowania.

STEROWANIE RUCHEM MANIPULATORA – Kinematyka i dynamika manipulatora

3. Kinematyka i dynamika manipulatora.

3.1. Kinematyka manipulatora.

Ogólnie termin kinematykę, można zdefiniować jako rozdział mechaniki teoretycznej zajmujący się geometrią ruchu bez uwzględniania przyczyn, które ten ruch wywołują.

Rozróżnia się dwa rodzaje zadań kinematyki manipulatorów. W pierwszym danesą zmienne konfiguracyjne (jak np. kąty obrotu ramion, przemieszczenia liniowe członówi odpowiednio pierwsze i drugie pochodne po czasie, czyli prędkości i przyspieszenia),a poszukujemy pozycji i orientacji członu roboczego względem układu odniesienia podstawy. Zadanie to można traktować jako odwzorowanie opisu położenia manipulatoraw przestrzeni współrzędnych konfiguracyjnych na opis w przestrzeni współrzędnych kartezjańskich. Drugi rodzaj zadania polega na wyznaczeniu zmiennych konfiguracyjnych, znając pozycję, orientację i odpowiednio pierwsze i drugie pochodne po czasie,czyli prędkość i przyśpieszenie członu roboczego.

Zadania te będziemy nazywali odpowiednio zadanie proste i zadanie odwrotne kinematyki manipulatora.

3.1.1. Wyprowadzenie zależności kinematycznych.

Obecnie w literaturze często spotyka się rozwiązanie kinematyki prostejlub odwrotnej wykorzystujące przy tym metody macierzowe opisu członów. Takie podejście umożliwia stosunkowo prosty i zwięzły zapis przeznaczony głównie dla inżynierów różnych specjalności. Istnieją również inne sposoby opisu zależności kinematycznych. Na przykład wykorzystanie równań klasycznej mechaniki teoretycznej.


Rys. 3.1. Przyjęty schemat manipulatora.

Wszystkie poniższe przekształcenia wykonywano w aplikacji Maple (kod źródłowy programu znajduje się w dodatku do pracy).

Pozycje punktu D należącego do członu 3 opisano wektorem w bazowym układzie współrzędnych

(3.1)

przy czym:

(3.2)

Ponieważ wektor prędkości punktu D jest równy


(3.3)

więc

(3.4)

gdzie:

(3.5)

Równanie (3.4) opisuje wektor prędkości punktu D jako pochodne wektora położenia po czasie. Chcąc otrzymać całkowitą wartość prędkości punktu należy wyznaczyć

(3.6)

Postępując analogicznie można wyznaczyć wektor przyśpieszenia

(3.7)

więc

(3.8)

Całkowita wartość przyśpieszenia będzie równa

(3.9)

Powyższe podejście do problemu kinematyki jest stosunkowo proste. Jedyną rzeczą, jaką należy wykonać - to poprawnie ułożyć równania wektora opisującego pozycję chwytaka. Następnie różniczkując po czasie wektor położenia, a następnie wektor prędkości otrzymujemy zestaw rozwiązań opisujących ruch chwytaka w przestrzeni kartezjańskiej.Jeżeli istnieje potrzeba można wyznaczyć np. przyśpieszenie normalne i styczne, tor ruchu, krzywiznę toru i inne [8].


3.1.2. Zadanie proste i odwrotne kinematyki.

Rozwiązanie zadania prostego kinematyki otrzymujemy bezpośrednioz rozwiązania równań od (3.1) do (3.9) znając wartości kątów obrotu, prędkościi przyspieszeń kątowych. Na rysunku 3.5 pokazano otrzymany tor ruchu punktu Ddla zadanych kątów obrotu jak na rysunkach 3.2, 3.3, 3.4.

Rys. 3.2. Kąt obrotu podstawy. Rys. 3.3. Kąt obrotu ramienia.

Rys. 3.4. Kąt obrotu łokcia.

Podsumowując: możemy stwierdzić, że rozwiązanie zadania kinematyki prostej nie sprawia specjalnych trudności (ogranicza się do rozwiązania równań od (3.1) do (3.9)).

0 10 20 30 40 50 600.75

0.8

0.85

0.9

0.95

1

0 10 20 30 40 50 601

1.05

1.1

1.15

1.2

1.25

0 10 20 30 40 50 60-1.1

-1

-0.9

-0.8

-0.7

-0.6

-0.5

-0.4


Rys. 3.5. Trajektoria punktu D we współrzędnych kartezjańskich.

Bardziej interesującym zagadnieniem jest kinematyka odwrotna. Przypomnijmy,że zadanie kinematyki odwrotnej polega na sformułowaniu i rozwiązaniu zależności, które pozwolą na otrzymanie parametrów konfiguracyjnych przy założeniu, że znanesą współrzędne kartezjańskie toru. Ogólnie problem jest typu: znając tor ruchu i prędkość dowolnego punktu manipulatora wyznaczyć parametry konfiguracyjne (np. kąty obrotu ramion), które pozwolą na realizację zadania.

W tym celu, należy wprowadzić pojęcie jakobianu (macierzy jakobianowej). Macierz jakobianową jest wielowymiarową postacią pochodnej funkcji wielu zmiennych [1]. Wykorzystując macierz jakobianową możliwe jest odwzorowanie przestrzeni kartezjańskiej w przestrzeń współrzędnych konfiguracyjnych.

(3.10)

Wszystkie przekształcenia analityczne dokonano w aplikacji Maple (kod źródłowy programu znajduje się w dodatku do pracy).

Wykorzystując (3.5) równanie (3.10) w naszym przypadku przyjmie formę


(3.11)

Jeżeli teraz pomnożymy równanie (3.10) lewostronnie przez odwrotność jakobianu otrzymamy

(3.12)

Rozwiązując powyższą zależność otrzymaliśmy

22332

313132

33220

111

)))(cos())(sin())(cos())((sin()())(sin()())(sin())(cos()())(cos())(cos(

))(cos())(cos()())(cos()())(sin(

ltttttzttytttxtt

tltletyttxt

DDD

DD

(3.13)

32332

212213 )))(cos())(sin())(cos())((sin(

)())(sin()())(sin())(cos()())(cos())(cos(ltttt

tzttytttxtt DDD

Przyjęto, że punkt D ma przemieszczać się po torze przedstawionym na rysunku 3.6, ze stałą prędkością. Odpowiada to wyznaczeniu wektora prędkości punktuwe współrzędnych kartezjańskich w postaci

(3.14)


Rys. 3.6. Zadany tor ruchu punktu D.

Rozwiązując zadanie kinematyki odwrotnej pozyskamy jak powinny zmieniaćsię kąty obrotu ramion, aby zrealizować postawione zadanie. Przebiegi kątów przedstawiono na rysunkach 3.7, 3.8, 3.9.

Rys. 3.7. Kąt obrotu podstawy. Rys. 3.8. Kąt obrotu ramienia.

0 10 20 30 40 50 600.3

0.4

0.5

0.6

0.7

0.8

0.9

1

0 10 20 30 40 50 600.8

0.9

1

1.1

1.2

1.3

1.4

1.5

1.6

1.7

1.8


Rys. 3.9. Kąt obrotu łokcia.

Z punktu widzenia planowania trajektorii ruchu, takie rozwiązanie budzi szereg wątpliwości. Rozwiązując zadanie kinematyki odwrotnej nie jesteśmy w stanie przewidzieć niebezpiecznych położeń lub prędkości członów manipulatora, ponieważ jedynym warunkiem jest rozwiązanie równania (3.12). Jeżeli nieświadomie zadalibyśmy położenie punktu D w pobliżu punktów osobliwych (np. na granicy strefy roboczej, lub poza nią), wartości prędkości uogólnionych bardzo szybko osiągają wartości krytyczne, a teoretycznie dążą do nieskończoności. Projektując ruch punktu z zastosowaniem kinematyki odwrotnej należy mieć świadomość istniejących zagrożeń.

3.1.3. Zadanie planowania trajektorii ruchu chwytaka.

Zadanie planowania trajektorii, można zrealizować we współrzędnych kartezjańskich lub we współrzędnych konfiguracyjnych. Jakkolwiek planowanie trajektorii we współrzędnych kartezjańskich zostało po części omówione przy okazji kinematyki odwrotnej, tak planowanie trajektorii we współrzędnych konfiguracyjnych polega na obliczeniu przebiegów czasowych przemieszczeń, prędkości i przyśpieszeńw połączeniach ruchowych, które spełniają warunki zadania, np. przemieszczenie obiektuz położenia początkowego w położenie końcowe.

Planowanie takie rozpoczyna się od rozwiązania zadania odwrotnego kinematyki,ale tylko dla położenia początkowego i położenia końcowego. Spośród kilku rozwiązań wybiera się takie, które spełnia dodatkowe warunki, np. najkrótszy czas realizacji, najmniejsze wartości przyśpieszeń, minimum zużycia energii, omijanie położeń osobliwych, omijanie przeszkód, itd.

W najprostszym zadaniu planowania ruchu manipulatora, dotyczącym operacji wziąć i położyć (ang. Pick and Place) dane są dwa położenia członu roboczego,tj. początkowe i końcowe, oraz odpowiednio prędkości w tych punktach [1].

Jednym ze sposobów generowania gładkiej krzywej, jest zastosowanie wielomianowej funkcji zmiennej czasu t [3]. Ponieważ mamy nałożone cztery więzy,a mianowicie

Start Koniec

(3.15)

0 10 20 30 40 50 60-1.2

-1

-0.8

-0.6

-0.4

-0.2

0

0.2

0.4


potrzebujemy wielomianu z czterema niezależnymi współczynnikami, które mogą być tak dobrane, aby spełnić warunki zadania. Rozważmy trajektorię trzeciego stopnia wyrażoną równaniem

(3.16)

wówczas zadana prędkość jest określona jako

(3.17)

Połączenie równań (3.16) i (3.17) z czterema równaniami więzów daje cztery równania z czterema niewiadomymi

(3.18)

Rozwiązując powyższy układ równań otrzymujemy wartości współczynników wielomianu (3.16) lub (3.17).

Wszystkie przekształcenia analityczne dokonano w aplikacji Maple (kod źródłowy programu znajduje się w dodatku do pracy).

Przyjęto, że punkt D musi się przemieścić z położenia początkowego

START (0,1697; 0,1697; 0,5)

do położenia końcowego

KONIEC (0,1924; 0,1197; 0,8)

w czasie tk=10[s].

Wyznaczono wartości kątów obrotu odpowiednio w punkcie start i koniec

(3.19)

Wykorzystując równania (3.17), (3.18), (3.19) wyznaczono


(3.20)

Na rysunku 3.10 przedstawiono przebieg kąta obrotu, a na rysunku 3.11 przebieg prędkości kątowej dla przykładowych zależności według (3.20)

Rys.3.10. Kąt obrotu. Rys. 3.11. Prędkość kątowa.

W wyniku zadania przebiegów według (3.20) otrzymano trajektorię punktu Dw postaci.

Rys. 3.12. Tor ruchu punktu D przy planowaniu trajektorii.

Możemy zauważyć, że pomimo zdefiniowania punktów początkowychi końcowych takich samych jak przy zadaniu kinematyki odwrotnej – uzyskany przebieg trajektorii jest zdecydowanie inny. Projektując ruch manipulatora na podstawie algorytmu planowania trajektorii, nie mamy możliwości wpływu na przebieg trajektorii pomiędzy zadanymi punktami! Opisany problem nie występuje w przypadku planowania przy

0 10 20 30 40 50 600.55

0.6

0.65

0.7

0.75

0.8

0 10 20 30 40 50 60-6

-5

-4

-3

-2

-1

0

1x 10

-3


wykorzystaniu kinematyki odwrotnej, gdzie „z góry” narzucamy trajektorię i „on-line” wyznaczamy parametry konfiguracyjne.

Niewątpliwie obydwa sposoby planowania trajektorii ruchu chwytaka posiadają wady i zalety a to, z której metody skorzysta projektant układu sterowania będzie zależało w dużej mierze od postawionego problemu.

Analizując przebiegi ruchu członów manipulatora Scorbot ER4pc,z oprogramowaniem ScorBase Pro, można zauważyć zbliżoną postać kątów obrotui prędkości kątowych do tych otrzymanych z równania (3.18). Stwierdzenie to zostało wysunięte tylko na podstawie wielu obserwacji autora i nie ma pokrycia doświadczalnego, ponieważ oprogramowanie ScorBase Pro jest integralną częścią dostarczaną wrazz manipulatorem i nie jest możliwa jakakolwiek ingerencja osób trzecich.

3.1.4. Weryfikacja numeryczna.

Analizując postać otrzymanych kątów obrotu z rozwiązania kinematyki odwrotnej trudno jest stwierdzić, czy są one poprawne, tzn. na przykład czy zadanie otrzymanych przebiegów kątów spowoduje osiągnięcie zadanego toru punktu D i jego prędkości.Aby to sprawdzić przeprowadzono test weryfikacji programu do symulacji.

Na rysunku 3.12 przedstawiono schemat do weryfikacji zależności kinematycznych.

Rys. 3.13. Schemat do weryfikacji zależności kinematycznych.

Na rysunkach 3.14, 3.15, 3.16 przedstawiono zadane i otrzymane wartości kątów obrotu po rozwiązaniu kinematyki prostej i odwrotnej.


Rys. 3.14. Kąt obrotu podstawy zadany i otrzymany. Rys. 3.15. Kąt obrotu ramienia zadany i otrzymany.

Rys. 3.16. Kąt obrotu łokcia zadany i otrzymany.

Na rysunkach 3.17, 3.18, 3.19 przedstawiono błędy pomiędzy wartościami zadanymi a otrzymanymi z rozwiązania po sobie kinematyki prostej i odwrotnej.

Rys. 3.17. Błąd kąta obrotu podstawy. Rys. 3.18. Błąd kąta obrotu ramienia.

0 10 20 30 40 50 600.75

0.8

0.85

0.9

0.95

1

0 10 20 30 40 50 601

1.05

1.1

1.15

1.2

1.25

0 10 20 30 40 50 60-1.1

-1

-0.9

-0.8

-0.7

-0.6

-0.5

-0.4

0 10 20 30 40 50 60-1.5

-1

-0.5

0

0.5

1

1.5x 10

-17

0 10 20 30 40 50 60-2

-1

0

1

2x 10

-17


Rys. 3.19. Błąd kąta obrotu łokcia.

Błędy pomiędzy wartościami zadanymi a otrzymanymi z rozwiązania zadania prostego i odwrotnego kinematyki wynikają z numerycznego rozwiązywania równańi nie świadczą o błędnych zależnościach.

Ponieważ po wykonaniu testu otrzymano prawidłowe rozwiązanie uzyskano gwarancję, że zbudowany model symulacyjny jest pozbawiony błędów merytorycznychi zgadza się z równaniami analitycznymi, co tym samym umożliwia wykorzystaniego w dalszych pracach symulacyjnych.

3.2. Dynamika manipulatora.

Ogólnie termin dynamikę można zdefiniować, jako dział mechaniki teoretycznej badającej ruch, równowagę ciał i układów mechanicznych pod działaniem sił.

Rozróżnia się dwa rodzaje zadań dynamiki manipulatorów. W pierwszym danajest trajektoria ruchu manipulatora, np. w postaci zmian współrzędnych konfiguracyjnychi ich pochodnych jako funkcji czasu, a trzeba wyznaczyć wektor sił i momentów napędowych M=[M1, ..., Mn]T – wykorzystywane przy projektowaniu układu sterowania. Drugi rodzaj zadań polega na wyznaczeniu ruchu manipulatora wywołanego przez dane siły i momenty napędowe – wykorzystywane przy symulacji ruchu manipulatora. Odpowiednio zadania te będziemy nazywali zadaniem odwrotnym i zadaniem prostym dynamiki manipulatora.

Aby mówić o symulacji dynamiki i układu sterowania manipulatora konieczne staje się wyprowadzenie odpowiednich zależności matematycznych, które będą opisywać analizowany układ – mowa tu o różniczkowych równaniach ruchu. W tym celu stosowane są najczęściej dwie metody: Newtoana-Eulera i Lagrange’a. Równania Lagrange’ajak zobaczymy później wyprowadza się z zależności energii kinetycznej i potencjalnej manipulatora od jego współrzędnych uogólnionych i ich pochodnych względem czasu.

0 10 20 30 40 50 60-2.5

-2

-1.5

-1

-0.5

0

0.5

1

1.5

2

2.5x 10

-17


3.2.1. Przyjęcie metody rozwiązania.

Dla bryły sztywnej, wykonującej ruch obrotowy wokół stałej osi, określasię moment bezwładności. Natomiast dla bryły wykonującej ruch kulisty, wprowadzasię tensor bezwładności, rozumiany jako uogólnienie skalarnego momentu bezwładności. Tensor bezwładności określa się zazwyczaj względem układu odniesienia związanegoz bryłą i wyraża w postaci macierzy o wymiarach 33

(3.21)

gdzie:

Ponadto przyjmuje się, że ciało jest złożone z różniczkowych elementówo objętości dv i gęstości . Pozycję każdego elementu określa wektor =[x y z]T. Elementy Ixx, Iyy i Izz nazwano masowymi momentami bezwładności. Każdy z sześciu elementów macierzy tensora bezwładności zależy od pozycji i orientacji układu, w którym jest określony. Jeśli układ odniesienia jest tak usytuowany, że momenty dewiacji (Ixy, Iyz, Ixz)są równe zero, to osie układu odniesienia nazwano osiami głównymi, a odpowiednie momenty bezwładności – głównymi momentami bezwładności.

Większość manipulatorów ma człony o tak skomplikowanym kształcie,że zastosowanie wzoru (3.21) jest trudne w praktyce. Stąd wynika, że w tych przypadkach jest uzasadnione doświadczalne wyznaczenie momentów bezwładności, lub przyjęcietak układów współrzędnych, aby osie były głównymi osiami bezwładności.

Niech q1,..., qn będą współrzędnymi uogólnionymi, które w pełni określają położenie układu, natomiast Ek i Ep przedstawia całkowitą energię kinetyczną i potencjalną układu. Wprowadzając pojęcie funkcji Lagrange’a (potencjału kinetycznego) w postaci

(3.22)

równania ruchu otrzymamy w postaci

i=1, ..., n (3.23)

gdzie:


Qi – uogólniona siła odpowiadająca uogólnionemu przemieszczeniu qi. Siła ta może być określona metodą prac przygotowanych, które wykonują siły czynne działającena układ.

Energia kinetyczna członu i-tego opisana jest przez wyrażenie

(3.24)

gdzie:mi – masa członu i-tego,ISi – macierz tensora bezwładności,i – wypadkowa prędkość kątowa członu i-tego,vSi – wypadkowa prędkość liniowa środka masy członu i-tego,

Całkowita energia kinetyczna manipulatora w skład, którego wchodzi n-członówjest sumą energii kinetycznych poszczególnych członów

(3.25)

Całkowita energia potencjalna manipulatora może być zapisana wzorem

(3.26)

gdzie:pSi – wektor pozycji środka masy członu i-tego, zależny od położenia manipulatora.

Do sił uogólnionych Qi zalicza się wszystkie siły i momenty działające na człony,z wyjątkiem sił ciężkości i bezwładności. Przyjęto, że w połączeniach ruchowych działają siły i momenty napędowe M=[M1, ..., Mn]T; jak również opory ruchu Fz=[F1, ..., Fn]T. Siłę uogólnioną można wyznaczyć metodą obliczenia pracy przygotowanej, wykonanej przez siły zewnętrzne według

(3.27)

Stąd otrzymamy

(3.28)


Zależność (3.28) jest zapisem wektorowym określającym wszystkie siły i momenty potrzebne do wyprowadzenia prawych stron równania (3.23).

3.2.2. Wyprowadzenie dynamicznych równań ruchu.

Do wyprowadzenia dynamicznych równań ruchu manipulatora według rysunku 3.20 potrzebna jest znajomość:

1. Geometrii układu.

2. Tensorów bezwładności członów w układach związanych z członami; ponieważ tak dobrano układy współrzędnych, że osie układów są głównymi osiami bezwładności - stąd momenty dewiacji są równe zero - zatem macierz tensora bezwładności jest macierzą diagonalną o wartościach na przekątnej głównej odpowiednio dla członu 1... 3.

3. Mas poszczególnych członów.

4. Prędkości liniowej środka masy (w układzie globalnym) i prędkości kątowej wokół osi obrotu przechodzącej przez środek masy (w układzie członu) .

5. Wektora przyśpieszenia ziemskiego g (w układzie inercjalnym).


Rys. 3.20. Schemat manipulatora z przyjętymi układami odniesienia.

Znając powyższe wyrażenia można zapisać wektory współrzędnych środków masi wektory prędkości kątowych odpowiednio dla członów od 1... 3.

Dla przyjętego schematu jak na rysunku 3.20 będą one postaci

1. Dla bryły pierwszej.

2

00

11 l

pS

1

1 00


2. Dla bryły drugiej.

122

022

1

022

1

)sin(2

)cos(2

)sin(

)cos(2

)cos(

2

ll

el

el

pS

2

21

21

2 )cos()sin(

3. Dla bryły trzeciej.

133

22

033

221

033

221

)sin(2

)sin(

)cos(2

)cos()sin(

)cos(2

)cos()cos(

3

ll

l

el

l

el

l

pS

32

31

31

3 )cos()sin(

Wykorzystując fakt, że

(3.29)

i podstawiając wyprowadzone zależności do (3.24), wykorzystując przy tym(3.25) można zapisać całkowitą energię kinetyczną manipulatora

(3.30)

Energia potencjalna na podstawie (3.26) całego manipulatora dla przyjętego poziomu porównawczego jak na rysunku 1 będzie

(3.31)

gdzie energie poszczególnych członów można wyznaczyć z (3.26) pomijając operator sumowania; przy czym wektor przyśpieszenia ziemskiego g jest postaci:

(3.32)

Tak przygotowane równania na energię kinetyczną i potencjalną umożliwiają wyprowadzenie funkcji Lagrange’a według wzoru (3.22). Następnie jej różniczkowanie odpowiednio po współrzędnych uogólnionych, prędkościach uogólnionych i czasie umożliwia zapis lewych stron różniczkowych równań ruchu według wzoru (3.23).


Przeprowadzenie analizy sterowania obiektu na podstawie otrzymanych zależności jest kłopotliwe ze względu na złożony zapis. Dlatego dalsze rozważania prowadzić będziemy opierając się na zapisie w postaci macierzowej.

(3.33)

gdzie: - macierz bezwładności,

- macierz sił odśrodkowych i sił Coriolis’a, - wektor oporów ruchu, - wektor sił grawitacji,

- wektor zakłóceń, - wektor sterowań,

- odpowiednio: współrzędna, prędkość i przyśpieszenie uogólnione.

Przyjmując schemat manipulatora jak na rysunku 3.20, poniżej przedstawiono wyniki uzyskane z aplikacji Maple (kod źródłowy programu znajduje się w dodatku).

Równanie różniczkowe 1.14 1 ( ) 4 m3 l2

2m2 l2

24 Iy2 4 Ix2 ( )cos 2

2 14 4 1 m3 l2 l3 ( )cos 3(

2 1 2 ( ) 4 m3 l22

m2 l22

4 Iy2 4 Ix2 ( )sin 2

4 ( ) 1 m3 l3 ( )sin 3 3 e0 1 ( )2 m3 m2 l2 ) ( )cos 2

14 l3

2m3 Ix3 Iy3 1 ( )cos 3

2 14

4 1 m3 l2 ( )sin 2 2 l3 8 3 1

14 l3

2m3 Ix3 Iy3 ( )sin 3 4 1 m3 e0 l3

( )cos 3 e0 1 l2 2 ( )2 m3 m2 ( )sin 2 1 m3 e0 l3 ( )sin 3 3

1 ( ) Iz1 Ix2 m2 e02

m3 e02

Ix3 MS1 d11

3s

1( )tanh 250 1

Równanie różniczkowe 2.14 1

2( ) ( )m2 4 m3 l2

24 Ix2 4 Iy2 ( )cos 2(

2 ( ) m3 l3 ( )12

32

( )cos 3 m3 l3 ( )sin 3 3 e0 12

( )2 m3 m2 l2 ) ( )sin 2

12 ( ) m3 l3 ( )cos 3 3 m3 l3 ( )sin 3 3

2g ( )2 m3 m2 l2 ( )cos 2

14 2 ( )m2 4 m3 l2

2 14 ( )4 Iz2 4 Iz3 2 Iz3 3

MS2 d22

3s

2( )tanh 250 2


Równanie różniczkowe 3.14 1

2( ) l3

2m3 4 Ix3 4 Iy3 ( )cos 3(

2 m3 ( ) l2 ( )22

12

( )cos 2 l2 ( )sin 2 2 12

e0 l3 ) ( )sin 3

12 m3 l3 ( ) l2 ( )sin 2 2

2l2 ( )cos 2 2 g ( )cos 3

14 m3 l3

23 Iz3 ( )3 2

MS3 d33

3s

3( )tanh 250 3

gdzie:,

mi, Izi, Iyi , Izi - masy i momenty bezwładności poszczególnych członów,li, e0 – geometria manipulatora,g – wektor przyspieszenia ziemskiego,MSi – momenty napędowe na silnikach,τd, τs – współczynniki funkcji aproksymującej opory ruchu.

Zapis powyższych równań w postaci macierzowej według (3.32), (kod źródłowy programu znajduje się w dodatku do pracy).

Wektor przyśpieszenia uogólnionego ; prędkości uogólnionej ; współrzędnej uogólnionej

Macierz bezwładności M(q)

Ix3 m2 e02

m3 e02

Ix2 ( )cos 22

( )cos 22

Iy2 Ix3 ( )cos 32

( )cos 32

Iy3

14 m2 l2

2( )cos 2

2m3 l2

2( )cos 2

2 14 m3 l3

2( )cos 3

2Iz1 Ix2

m3 e0 l3 ( )cos 3 m3 l2 ( )cos 2 l3 ( )cos 3 2 m3 e0 l2 ( )cos 2 m2 e0 l2 ( )cos 2 ,

0 0,

0 Iz2 m3 l22 1

4 m2 l22

Iz3 , ,

12 m3 l2 ( )sin 2 l3 ( )sin 3

12 m3 l2 ( )cos 2 l3 ( )cos 3 Iz3

, ,

0

12 m3 l2 ( )sin 2 l3 ( )sin 3

12 m3 l2 ( )cos 2 l3 ( )cos 3 Iz3 Iz3

14 l3

2m3


Macierz sił odśrodkowych i sił Coriolis’a

12 2 Ix2 ( )cos 2 ( )sin 2 2 ( )cos 2 Iy2 ( )sin 2

12 m2 l2

2( )cos 2 ( )sin 2

2 m3 l22

( )cos 2 ( )sin 2 m3 l2 ( )sin 2 l3 ( )cos 3 2 m3 e0 l2 ( )sin 2

m2 e0 l2 ( )sin 2 2


12 m3 l3

2( )cos 3 ( )sin 3 m3 e0 l3 ( )sin 3 m3 l2 ( )cos 2 l3 ( )sin 3

3

12

,

2 Ix2 ( )cos 2 ( )sin 2 2 ( )cos 2 Iy2 ( )sin 212 m2 l2

2( )cos 2 ( )sin 2

2 m3 l22


m2 e0 l2 ( )sin 2 1


,

12 m3 l3

2( )cos 3 ( )sin 3 m3 e0 l3 ( )sin 3 m3 l2 ( )cos 2 l3 ( )sin 3

1


12 m2 l2

2( )cos 2 ( )sin 2

2 m3 l22


m2 e0 l2 ( )sin 2 1 0

12 ( )m3 l2 ( )sin 2 l3 ( )cos 3 m3 l2 ( )cos 2 l3 ( )sin 3 3, ,


12 m3 l3

2( )cos 3 ( )sin 3

m3 e0 l3 ( )sin 3 m3 l2 ( )cos 2 l3 ( )sin 3 1 ,

12 ( )m3 l2 ( )cos 2 l3 ( )sin 3 m3 l2 ( )sin 2 l3 ( )cos 3 2 0,

Wektor sił grawitacji

0

12 m2 g l2 ( )cos 2 m3 g l2 ( )cos 2

12 m3 g l3 ( )cos 3


Wektor oporów ruchu

d11

3s

1( )tanh 250 1

d22

3s

2( )tanh 250 2

d33

3s

3( )tanh 250 3

Rys. 3.21. Założony moment oporowy tarcia w funkcji prędkości kątowej.

Przekształcając równanie (3.33) do postaci

(3.34)

i rozwiązując (3.34) przy znanych warunkach początkowych, po czym całkując dwukrotnie powyższą zależność - pozyskamy jak zmieniają się współrzędne uogólnione pod działaniem sił uogólnionych - czyli rozwiążemy zadanie dynamiki prostej manipulatora.

3.2.3. Weryfikacja otrzymanych równań dynamicznych.

Jakkolwiek wyprowadzenie analityczne równań różniczkowych wykorzystując przy tym aplikację Maple nie budzi zastrzeżeń, co do poprawności to jednak późniejsza analizaz tym oprogramowanie staje się kłopotliwa. Dlatego proponujemy prowadzenie dalszej syntezy z innym oprogramowaniem, jakim jest Matlab/Simulink. Jednak tutaj (podobnie jak w kinematyce) wymaga się pewnej weryfikacji numerycznej zbudowanych schematów.

-0.8 -0.6 -0.4 -0.2 0 0.2 0.4 0.6 0.8-15

-10

-5

0

5

10

15


Rys. 3.22. Schemat układu do weryfikacji poprawności przyjętych równań różniczkowych.

Pierwszym testem układu weryfikacji jest rozwiązanie po sobie zadania dynamiki prostej i odwrotnej. Postępowanie takie prowadzi do otrzymania jak się okaże bardzo zbliżonych form momentów napędowych, a różnice w ich postaci wynikają główniez numerycznego rozwiązywania równań różniczkowych.

Na rysunkach 3.23, 3.24, 3.25 przedstawiono postać momentów napędowych zadanych i otrzymanych z rozwiązania równań.

Rys. 3.23. Moment napędowy podstawy Rys. 3.24. Moment napędowy ramieniazadany i otrzymany. zadany i otrzymany.

Rys. 3.25. Moment napędowy łokcia zadany i otrzymany.

0 1 2 3 4 5 6 7 8 9 10-1

-0.8

-0.6

-0.4

-0.2

0

0.2

0.4

0.6

0.8

1

0 1 2 3 4 5 6 7 8 9 10-1.5

-1

-0.5

0

0.5

1

1.5

0 1 2 3 4 5 6 7 8 9 10-2

-1.5

-1

-0.5

0

0.5

1

1.5

2


Na rysunkach 3.26, 3.27, 3.28 przedstawiono błędy między wartością zadaną momentu a wyliczoną.

Rys. 3.26. Błąd momentu dla członu pierwszego. Rys.3.27. Błąd momentu dla członu drugiego.

Rys. 3.28. Błąd momentu dla członu pierwszego.

Maksymalna różnica pomiędzy przebiegami zadanymi a otrzymanymi wynosi i wynika głównie z niedokładności numerycznego całkowania równań.

Drugi test weryfikacji obejmuje porównanie rozwiązania dynamiki prostej bezpośrednio z wyprowadzonych równań różniczkowych w aplikacji Maple z rozwiązaniem otrzymanym z modelu symulacyjnego w Simulink’u.

Poniżej przedstawiono przebiegi zmian wartości kątów obrotu dla tych samych momentów napędowych i warunków początkowych obiektu.

0 1 2 3 4 5 6 7 8 9 10-1

-0.5

0

0.5

1

1.5x 10-14

0 1 2 3 4 5 6 7 8 9 10-1

-0.5

0

0.5

1

1.5x 10-15

0 1 2 3 4 5 6 7 8 9 10-2

-1.5

-1

-0.5

0

0.5

1

1.5

2

2.5x 10-14


Maple Simulink

0 1 2 3 4 5 6 7 8 9 100.7

0.8

0.9

1

1.1

1.2

1.3

1.4

0 1 2 3 4 5 6 7 8 9 10-10

0

10

20

30

40

50


Rys. 3.29. Porównanie wyników z aplikacji Maple i Simulink.

Ponieważ po wykonaniu obydwu z powyższych testów otrzymano prawidłowe rozwiązania uzyskano gwarancję, że zbudowany model symulacyjny jest pozbawiony błędów merytorycznych i zgadza się z równaniami analitycznymi, co tym samym umożliwia wykorzystanie go w dalszych pracach symulacyjnych.

0 1 2 3 4 5 6 7 8 9 100

10

20

30

40

50

60

70

80

90

STEROWANIE RUCHEM MANIPULATORA – Konwencjonalne sterowanie ruchem manipulatora

4. Konwencjonalne sterowanie ruchem manipulatora.

4.1. Wybór algorytmu sterowania i stabilność układu.

Wstępną analizę algorytmu sterowania z zastosowaniem regulatora PD przeprowadziliśmy w rozdziale 2.2.

Ogólne równania opisujące manipulator są postaci

(4.1)

Przyjmijmy, że znane są wszystkie wartości współczynników występującychw równaniu (4.1).

W procesie projektowania układów sterowania korzysta się z własności macierzy M(q), a mianowicie jej nieosobliwości. W takim przypadku z ogólnej teorii systemów [1] wiadomo, że dla układu (4.1) istnieje sprzężenie zwrotne

(4.2)

gdzie:v – nowy sygnał sterujący, który globalnie linearyzuje równania dynamiki robota

i sprowadza je do układu liniowego drugiego rzędu

(4.3)

(4.3) opisuje manipulator z nowym sterowaniem v.

Otrzymany schemat regulacji nosi nazwę sterowania opartego na równaniach dynamiki. Dla takiego układu możemy postawić zadanie doboru regulatora, który będzie minimalizował błąd regulacji

(4.4)

Do tego celu zastosujemy regulator PD o strukturze

(4.5)


gdzie:Kp, Kd – dodatnie macierze diagonalne wzmocnień regulatora PD – odpowiednio

wzmocnienie proporcjonalne i różniczkujące.

Z zależności (4.3) i (4.5) wynika, że równanie błędu regulacji jest jednorodnym równaniem różniczkowym drugiego rzędu

(4.6)

o równaniu charakterystycznym dla i-tej współrzędnej wektora błędu

(4.7)

Warunkiem koniecznym i dostatecznym stabilności liniowego układu sterowania według (4.5) jest to, aby wszystkie pierwiastki tj.

oraz (4.8)

miały ujemne części rzeczywiste (tzn. leżały na lewo od osi liczb urojonychna płaszczyźnie Re-Im)

(4.9)

Nierówności (4.9) umożliwiają dobór odpowiednich stałych [1].

Podsumowując: możemy stwierdzić, że przedstawiona metoda sterowania wymaga, aby procedury wymuszające sprzężenie zwrotne były realizowane w czasie rzeczywistym. Złożoność obliczeniowa silnie wpływa na częstotliwość sterowania, a w efekcie na jego jakość.

W większości układów sterowania (patrz rozdział 2) w równaniu (4.2) przyjmuje się

(4.10)

gdzie:I – oznacza jednostkową macierz n×n.

W efekcie uzyskuje się bardzo prosty i szybki układ regulacji. W takim przypadku każda para kinematyczna traktowana jest jako niezależny obiekt sterowania. Stosującten uproszczony rodzaj sterowania, trudno jest ocenić jego praktyczną dokładność, ponieważ w równaniu sterowania nie są uwzględnione wzajemne interakcje między poszczególnymi członami. Nieuwzględnienie tych interakcji powoduje powstawanie dodatkowych błędów, które muszą być wyeliminowane przez zastosowany regulator [1].


4.2. Weryfikacja numeryczna układu sterowania.

Na bazie zależności wyprowadzanych we wcześniejszych rozdziałach zbudowano układ do weryfikacji układu sterowania przedstawiony na rysunku 4.1.

Rys. 4.1. Schemat układu do prowadzenia weryfikacji układu sterowania z regulatorem PD.

W bloczkach zaimplementowano odpowiednio: „trajektoria zadana” – według rysunku 3.6, „błąd” - równanie (4.4), „regulator PD” – równanie (4.5), „manipulator” – równanie (3.34), G(q) – wektor grawitacji (rozdział 3.2.2).

Sygnał sterowania jest, więc opisany zależnością (4.2) przy warunkach (4.10).

Symulację prowadzono przy następujących parametrach jak na rysunku 4.2.


Rys. 4.2. Parametry symulacji.

Przy czym:start_time=0end_time=30dt=0.001

Dla manipulatora przyjęto współczynniki

********** KINEMATYKA **********%Warunki początkoweteta1=pi/4; teta2=pi/6; teta3=-pi/6;omega1=0; omega2=0; omega3=0;%Długości i inne danel1=0.35; l2=0.22; l3=0.22; e0=0.02;g=9.81; z=0; zt=z*0.2;

%********** DYNAMIKA **********%Masym2=2.5;m3=3.5; m3z=m3+z;%Momenty bezwładnościIx1=0; Iy1=0; Iz1=0.15;Ix2=0.001; Iy2=1/12*m2*l2^2; Iz2=1/12*m2*l2^2;Ix3=0.001; Ix3z=0.002; Iy3=1/12*m3*l3^2; Iy3z=1/12*m3z*l3^2; Iz3=1/12*m3*l3^2; Iz3z=1/12*m3z*l3^2;

%********** TARCIE **********%Współczynniki tarcia dynamicznegoT1d=6; T1dz=T1d+zt;T2d=12; T2dz=T2d+zt;T3d=10; T3dz=T3d+zt;%Współczynniki tarcia statycznegoT1s=5; T2s=10; T3s=9;

%********** STEROWANIE Z REG. PD **********%Współczynniki regulatora PDKp1=50; Kd1=20;Kp2=60; Kd2=30;


Kp3=60; Kd3=30;

TEST 1.Obejmuje symulację układu sterowania z regulatorem PD według zależności (4.2)

i (4.5) przy warunkach zdefiniowanych w (4.10).

Na rysunkach 4.3, 4.4, 4.5, pokazano jak kształtują się kąty obrotu zadanei otrzymane z manipulatora.

Rys. 4.3. Kąt obrotu podstawy zadany i otrzymany. Rys. 4.4. Kąt obrotu ramienia zadany i otrzymany.

Rys. 4.5. Kąt obrotu łokcia zadany i otrzymany.

Rozwiązując zadanie kinematyki prostej dla parametrów jak wyżej można wyznaczyć trajektorię chwytaka w przestrzeni kartezjańskiej (rysunek 4.6).

0 5 10 15 20 25 300.7

0.71

0.72

0.73

0.74

0.75

0.76

0.77

0.78

0.79

0.8

0 5 10 15 20 25 300.3

0.35

0.4

0.45

0.5

0.55

0.6

0.65

0.7

0 5 10 15 20 25 30-0.55

-0.5

-0.45

-0.4

-0.35

-0.3

-0.25

-0.2

-0.15


Rys. 4.6. Trajektoria ruchu chwytaka zadana i otrzymana.

Założono, że chwytak musi poruszać się po krzywej jak na rysunku 4.6. ze stałą prędkością. Jednak rzeczywista prędkość chwytaka na trajektorii jest postaci jak na rysunku 4.7.

Rys. 4.7. Prędkość chwytaka na trajektorii – zadana i otrzymana.

Aby zrealizować postawione zadanie postacie momentów napędowych na silnikach muszą byś jak na rysunku 4.8.

0 5 10 15 20 25 300

0.002

0.004

0.006

0.008

0.01

0.012


Rys. 4.8. Postacie momentów napędowych dla obrotu: podstawy, ramienia, łokcia.

Na podstawie przeprowadzonego testu można dobrać taki element napędowy, który sprosta postawionemu zadaniu. Mianowicie silnik musi wypracować dostatecznie duży moment obrotowy, który obróci części składowe manipulatora z założonymi prędkościamii przyśpieszeniami. Jeżeli okazałoby się, że uzyskanie tak dużych momentówjest niemożliwe wtedy należy zmniejszyć prędkości członów i/lub ich przyśpieszenia.

Błędy pomiędzy kątami obrotu zadanymi a otrzymanymi z manipulatora przedstawiają rysunki 4.9, 4.10, 4.11.


0 5 10 15 20 25 30-10

-5

0

5

10

15

20

0 5 10 15 20 25 30-0.02

-0.015

-0.01

-0.005

0

0.005

0.01

0.015

0.02

0 5 10 15 20 25 30-0.07

-0.06

-0.05

-0.04

-0.03

-0.02

-0.01

0

0.01



Maksymalny błąd kąta obrotu wynosi 0.06[rad] i jest to błąd obrotu ramienia.Jakkolwiek błędy kątów obrotu są istotne ze względu na problem sterowania

manipulatorem, to jednak dla samego zastosowania manipulatora w środowisku istotny jest błąd nadążania za zadaną trajektorią zdefiniowany jako

(4.11)

Oznacza to, że maksymalne „odejście” od zadanego toru wynosi δ=0,0112[m].W zależności od tego, jakie stawiamy wymagania od nośnie dokładności może się okazać, że takie wartości odchyleń są niedopuszczalne. Aby zminimalizować tą różnicę należyw tym przypadku zwiększyć współczynniki wzmocnień regulatora PD zgodniez zależnością (4.8) lub jak się okaże zmodyfikować algorytm sterowania.

TEST 2.Wpływ na wartość błędu ma również brak kompensacji: zarówno sił grawitacji jak

i oporów ruchu. Dlatego do układu wprowadzono kompensację sił grawitacji. Opory ruchu nadal pozostają nieuwzględnione w algorytmie sterowania.

0 5 10 15 20 25 30-0.05

-0.04

-0.03

-0.02

-0.01

0

0.01

0.02


Rys. 4.12. Tor ruchu chwytaka zadany i otrzymany.

Otrzymano błędy kątów obrotu członów jak na rysunkach 4.13, 4.14, 4.15.


Dzięki zastosowaniu kompensacji sił grawitacji udało się zmniejszyć maksymalny błąd obrotu ramienia do wartości 0,04[rad], a tym samym zmniejszyć błąd „odejścia”od toru do wartości δ=0,0070[m]. Widzimy, że korzyści płynące z wprowadzenia kompensacji są znaczące, lecz wprowadzają problem natury - nadążyć z wyznaczeniem wartości sygnałów sterujących w czasie rzeczywistym.

0 5 10 15 20 25 30-0.02

-0.015

-0.01

-0.005

0

0.005

0.01

0.015

0.02

0 5 10 15 20 25 30-0.04

-0.03

-0.02

-0.01

0

0.01

0.02

0.03

0.04



TEST 3.Zadawalające wyniki otrzymane z wprowadzenia kompensacji od sił grawitacji

nasunęły myśl, aby dodać do układu sterowania zależności związane z oporami ruchu.

Zmodyfikowano schemat do postaci jak na rysunku 4.16.

Rys. 4.16. Schemat układu sterowania z regulatorem PD i kompensacją od sił grawitacji i oporów ruchu.

Na rysunkach 4.17, 4.18, 4.19, przedstawiono błędy kątów obrotu członów.


0 5 10 15 20 25 30-0.03

-0.02

-0.01

0

0.01

0.02

0.03

0 5 10 15 20 25 30-10

-8

-6

-4

-2

0

2

4x 10

-5

0 5 10 15 20 25 30-8

-6

-4

-2

0

2

4

6

8

10x 10

-5



Maksymalny błąd kąta obrotu wynosi zaledwie 0.001[rad]. Uzyskując takiwyniki, udało się zminimalizować błąd odejścia od trajektorii do wartości 0.0000135[m]=0.013[mm]. Uzyskany wynik jest czysto teoretyczny, gdyż często struktura manipulatora nie zapewnia aż tak dużej dokładności. Podatność przegubów, luzyw przekładni, rozdzielczość encoder’ów itd. wprowadzają znacznie większe błędy, aniżeli błędy samego układu sterowania.

Na rysunku 4.20 przedstawiono trajektorię chwytaka zadaną i otrzymaną.

Rys. 4.20. Trajektoria chwytaka zadana i otrzymana.

Brak zauważalnej różnicy w przebiegu pomiędzy obiema trajektoriami na rysunku 4.20 świadczy o dużej dokładności ruchu manipulatora.

Przedstawione testy pokazują jak ważne w układach sterowania są elementy kompensacji zakłóceń. Wprowadzenie zaledwie dwóch elementów kompensacyjnych,tj. od sił grawitacji i oporów ruchu powoduje wzrost dokładności o ok. 1000 razy!

0 5 10 15 20 25 30-12

-10

-8

-6

-4

-2

0

2

4

6

8x 10

-5


Przedstawiona metoda sterowania zapewnia bardzo dobre nadążanie za zadaną trajektoriąw przypadku, gdy parametry manipulatora nie ulegają zmianie. Jest to przypadek czysto teoretyczny. W rzeczywistych warunkach pracy, gdy manipulator przemieszcza przedmioty - jego masa, momenty bezwładności, siły oporów ruchu i inne nieopisane zależności ulegają zmianie. W następnym teście spróbowano uwzględnić zmienne warunki pracy i ich wpływ na ruch po zadanej trajektorii.

TEST 4.Obejmuje symulację ruchu manipulatora po zadanej trajektorii w przestrzeni

kartezjańskiej według rysunku 3.6 przy czym masa i momenty ulegają skokowej zmianiew 15[s] ruchu.

Na rysunkach 4.21, 4.22, 4.23, przedstawiono błędy kątów obrotu poszczególnych członów.



Zauważalny wzrost błędów obrotu ramienia i łokcia w 15[s] spowodowanyjest skokową zmianą współczynników oporów ruchu i zmianą masy członu trzeciego.W rezultacie algorytm sterowania nie kompensuje w pełni wyżej wymienionych czynników, generując błędne sterowanie, które musi zostać poprawione przez regulator PD. Maksymalny błąd kąta obrotu wzrósł do wartości 0.01[rad]. Powstałe błędy kątów obrotu znalazły również odbicie w trajektorii chwytaka, powodując wzrost odejścia od trajektorii do wartości 0.0032[m].

0 5 10 15 20 25 30-5

-4

-3

-2

-1

0

1

2

3x 10

-4

0 5 10 15 20 25 30-10

-8

-6

-4

-2

0

2x 10

-3

0 5 10 15 20 25 30-6

-5

-4

-3

-2

-1

0

1x 10

-3


Na rysunku 4.24 przedstawiono trajektorię ruchu chwytaka. Można zauważyć,że chwytak przemieszcza się z dużą dokładnością na początku ruchu, a od momentu wystąpienia zakłócenia parametrycznego obie trajektorie oddalają się od siebiei tak pozostają do zakończenia ruchu.


Podsumowując: regulator PD zapewnia nadążanie za zadaną trajektorią. Problemem jest kwestia dokładności śledzenia trajektorii. Aby zwiększyć dokładność odzwierciedlenia zadanego ruchu manipulatora należy:

- zwiększyć współczynniki regulatora,- zmodyfikować algorytm sterowania.Modyfikację algorytmu można zrealizować poprzez wprowadzenie kompensacji np.

od oporów ruchu lub sił ciężkości.Jakkolwiek sposób pierwszy jest zdecydowanie prostszy gdyż nie wymaga zmiany

aparatu obliczeniowego. Może jednak powodować nadmierne nasycenie amplitudy sygnału sterującego, co z kolei pociąga za sobą przeregulowanie układu i/lub pracę w strefie rezonansu przegubu. Sposób drugi nie wprowadza zagrożeń jak poprzedni, lecz istnieje potrzeba zwiększenia aparatu obliczeniowego w celu zapewnienia realizacji algorytmuw czasie rzeczywistym.

Test 4 dowiódł, że regulatory PD posiadają podstawową wadę – nie potrafią odrzucać zakłóceń. Zaczęto, więc poszukiwać takich rozwiązań, które będą na bieżąco korygować swoje współczynniki, aby minimalizować błąd występujący przy zakłóceniach parametrycznych. Sterowanie takie nosi nazwę sterowania odpornego i jest tematem następnego rozdziału.

STEROWANIE RUCHEM MANIPULATORA – Adaptacyjny algorytm sterowania manipulatorem

5. Adaptacyjny algorytm sterowania manipulatorem.

5.1. Wstęp i wybór prawa sterowania.

Podstawowa idea sterowania adaptacyjnego polega na tym, że zmienia się wartość wzmocnienia lub innych parametrów prawa sterowania, zgodnie z pewnym algorytmem działającym „on-line”. W ten sposób regulator może „nauczyć się” odpowiedniego zestawu parametrów podczas pracy. Ta idea jest szczególnie użyteczna dla manipulatorów realizujących zadanie powtarzania. Bez adaptacji parametrów błędy są również powtarzane (regulator PD). Wprowadzając adaptację jakość nadążania może być skutecznie poprawiana przez kolejne powtórzenia [3].

Przeanalizujmy główną cechę dynamiki manipulatorów, która będzie użytaw wyprowadzeniu algorytmu sterowania adaptacyjnego. Cechą tą jest liniowość względem parametrów. Rozumie się pod tym fakt, że mimo równania dynamiki manipulatorówsą silnie nieliniowe, to decydujące parametry jak masy czy momenty bezwładności, występują jako współczynniki znanych funkcji zmiennych przegubowych. Przez określenie każdego współczynnika jako oddzielnego parametru otrzymamy związki liniowe,a więc będziemy mogli zapisać równania dynamiki manipulatora w postaci

(5.1)

gdzie: - jest nr macierzą znanych funkcji,

p - jest r1 wektorem nieznanych parametrów.

W literaturze spotyka się różne propozycje sterowania adaptacyjnego [1,3,4].Do podstawowych należy sterowanie adaptacyjne z dynamiką odwrotną. Zastosowanietej metody związane jest jednak z pomiarem przyśpieszenia, co w warunkach rzeczywistych może być trudne do zrealizowania. Dlatego do dalszej analizy wybrano algorytm sterowania adaptacyjnego według Slotine’a i Liego [3].

Dla manipulatora opisanego przez wyrażenie (5.1) zaproponowano adaptacyjny algorytm sterowania w postaci

(5.2)

gdzie:v – wektor sygnału pomocniczego zdefiniowany następująco


(5.3)

Ks – macierz najczęściej diagonalna dodatnich współczynników wzmocnienia regulatora PD,

s – wektor liniowych powierzchni ślizgania zdefiniowany następująco

(5.4)

przy czym:e – błąd nadążania zdefiniowany jako

(5.5)

- macierz diagonalna dodatnich wzmocnień.

Rys. 5.1. Przedstawienie graficzne zaproponowanego układu sterowania.

5.2. Stabilność układu i dobór prawa adaptacji parametrów.

Podstawienie (5.2) do układu (5.1) i pomijając zapis od współrzędnych i prędkości uogólnionych otrzymano

(5.6)

Przekształcając (5.4) do postaci

vsq (5.7)

i różniczkując po czasie

(5.8)

Można zapisać równanie (5.6) w postaci


(5.9)

gdzie:

Zauważmy, że nieliniowe prawo sterowania (5.2) różni się na przykładod sterowania z dynamiką odwrotną i nie prowadzi do układu liniowego z pętlą zamkniętą. Tak, więc badanie stabilności układu (5.9) jest trudniejsze niż w przypadku właściwego sterowania z dynamiką odwrotną.

W celu wykazania stabilności wybrano dopuszczalną funkcję Lapunow’a

(5.10)

Wynika z tego, że na trajektoriach rozwiązań mamy

(5.11)

Jeśli dobierze się prawo adaptacji parametrów tak, aby wyzerować drugi wyraz to uzyskamy

(5.12)

a więc

(5.13)

Ponieważ V jest dodatnio określona, a jest ujemnie półokreślona to na mocy teorii stabilności Lapunowa sygnały s i są ograniczone. Dodatkowo na podstawie lematu Barbalata co z kolei implikuje, że błąd nadążania e dąży do zera gdy .Przy takim doborze sygnału sterowania i prawa adaptacji parametrów algorytm sterowania zapewnia globalną stabilność układu [3].

Zaproponowany algorytm sterowania (5.2) zawiera regulator PD. Ponadto sterowanie to zależy od wektora współrzędnych stanu manipulatora, który musi być dostępny na podstawie pomiarów. Prawo adaptacji (5.12) w ogólności nie zapewnia wyznaczenia dokładnych wartości rzeczywistych parametrów manipulatora, ale umożliwia rozwiązanie postawionego zadania – nadążania za zadaną trajektorią.


Algorytm sterowania jest funkcją stałych macierzy diagonalnych , Ks, i . Elementy tych macierzy powinny być dobierane eksperymentalnie na obiekcie rzeczywistym z uwzględnieniem błędów pomiarowych.

5.3. Weryfikacja numeryczna układu sterowania.

Dla potrzeb symulacyjnych, na podstawie zależności (5.1) przekształcono zapis macierzowy (kod źródłowy programu znajduje się w dodatku do pracy) otrzymując.

Macierz sterowań

vp1 14 ( )( )cos 2 l2 2 e0 ( ) l2 vp1 ( )cos 2 l2 ( )1 v2 v1 2 ( )sin 2 2 vp1 e0

, ,

l2 vp1 ( )cos 212 vp1 l3 ( )cos 3

12 l3 ( )3 v1 1 v3 ( )sin 3

l2 ( )1 v2 v1 2 ( )sin 2 vp1 e0

( )cos 2 l2

12 l3 ( )cos 3 e0 1

30 0, , , ,

Ws1 ( )tanh 50 1 vp1 ( ) Ix2 Iy2 ( )cos 2

2

( )sin 2 ( )1 v2 v1 2 ( ) Ix2 Iy2 ( )cos 2 vp1 ( ) Iy3 Ix3 ( )cos 3

2

( )sin 3 ( )3 v1 1 v3 ( ) Iy3 Ix3 ( )cos 3 vp1 ( )Ix3 Ix2

014 l2 ( ) ( )( )sin 2 v1 l2 1 2 g ( )cos 2 2 ( )sin 2 1 v1 e0 vp2 l2

12 l2 (

, ,

( ) 2 v1 l2 1 ( )cos 2 l3 ( )1 v1 3 v3 ( )cos 3 2 1 v1 e0 ( )sin 3 l3 vp3 ( )sin 2

( ) ( )cos 3 l3 vp3 ( )sin 3 l3 v3 3 2 g ( )cos 2 2 vp2 l2 ) 0 2

30, , , ,

Ws2 ( )tanh 50 2 ( )sin 2 1 v1 ( ) Ix2 Iy2 ( )cos 2 ( )Iz3 Iz2 vp2 vp3 Iz3

0 014 (

, ,

( ) v1 1 l3 ( )cos 3 2 l2 ( )v2 2 1 v1 ( )cos 2 2 1 v1 e0 2 ( )sin 2 l2 vp2 ( )sin 3

( ) 2 ( )cos 2 l2 vp2 2 ( )sin 2 l2 v2 2 2 g ( )cos 3 vp3 l3 ) l3 0 0 3

3, , , ,

Ws3 ( )tanh 50 3 ( )cos 3 v1 1 ( ) Iy3 Ix3 ( )sin 3 Iz3 ( )vp3 vp2


Wektor parametrów adaptowanych

Iz1m2

m3

Wd1

Wd2

Wd3

1

W celu przeprowadzenia weryfikacji numerycznej posłużono się jak poprzednio oprogramowaniem Matlab®Simulink® budując schemat, który przedstawiono na rysunku 5.2.

Rys. 5.2. Schemat układu sterowania z adaptacją parametrów.

W bloczkach zaimplementowano odpowiednio: „trajektoria zadana” – według rysunku 3.6, „błąd” - równanie (5.5), „sygnały pomocnicze” – równania (5.3, 5.4), „manipulator” – równanie (3.34), „adaptacja” – równanie (5.2, 5.12) – bez regulatora PD, „regulator PD” – równanie (5.2) – bez adaptacji, „TV i Wizualizacja” – wyniki symulacji.

W celu obiektywnej oceny układu sterowania, zarówno parametry symulacjijak i współczynniki manipulatora nie uległy zmianie. Dodano jedynie współczynniki wynikające ze zmiany układu sterowania.

%********** STEROWANIE ADAPTACYJNE **********%Regulator PDKs1=8; Ks2=12; Ks3=10;%Współczynniki błędu uogólnionego lambdala1=2.5; la2=2.5; la3=2.5;


%Współczynniki adaptacji gammaga1=9e3; %Iz1ga2=50; %m2ga3=20; %m3ga4=5e8; %Td1ga5=3e6; %Td2ga6=1e6; %Td3

TEST 1.Obejmuje weryfikację układu sterowania dla nadążania za zadaną trajektorią.

Na rys. 5.3, 5.4, 5.5, przedstawiono błędy kątów obrotu poszczególnych członów.

Rys. 5.3. Błąd kąta obrotu podstawy Rys. 5.4. Błąd kąta obrotu ramienia.


Dzięki zastosowaniu adaptacyjnego algorytmu sterowania udało się uzyskać maksymalny błąd rzędu 0.003[rad]. W wyniku, czego maksymalny błąd odejściaod zadanej trajektorii wynosi zaledwie 0.00075[m]. Na rysunku 5.7 przedstawiono trajektorię ruchu chwytaka. Narzucono, że prędkość chwytaka na zadanej trajektorii musi byś stała, tymczasem prędkość chwytaka przebiega jak na rysunku 5.6.

0 5 10 15 20 25 30-10

-8

-6

-4

-2

0

2x 10

-4

0 5 10 15 20 25 30-8

-6

-4

-2

0

2

4

6

8x 10

-4

0 5 10 15 20 25 30-3.5

-3

-2.5

-2

-1.5

-1

-0.5

0

0.5x 10

-3


Rys. 5.6. Prędkość chwytaka na trajektorii: zadana i otrzymana.

Błąd prędkości chwytaka na początku symulacji wynika z tego, że manipulator zaczyna ruch z zerową prędkością początkową, tymczasem zadana prędkość początkowa jest 0.01[m/s].


Aby zrealizować ruch chwytaka po zadanej trajektorii i z zadaną prędkością należy uzyskać na wałach napędowych silników momenty jak na rysunku 5.7.

0 5 10 15 20 25 300

0.002

0.004

0.006

0.008

0.01

0.012


Rys. 5.8. Wypadkowe momenty napędowe dla: podstawy, ramienia, łokcia.

Rysunek 5.8 przedstawia wypadkowe momenty napędowe na silnikach. Powyższe przebiegi są sumą układu sterowania adaptacyjnego i regulatora PD. Odpowiednie przebiegi przedstawiono na rysunkach 5.9 i 5.10.

Rys. 5.9. Sterowanie układu adaptacyjnego dla: Rys. 5.10. Sterowanie z regulatora PD dla: podstawy, ramienia, łokcia. podstawy, ramienia, łokcia.

Analizując przebiegi momentów napędowych możemy zaobserwować, że regulator PD „działa” tylko na początku symulacji, gdy parametry adaptowane nie są dokładnie odzwierciedlone, co wiąże się z tym, że sygnał sterowania adaptacyjnego jest „niepełny”.W miarę realizacji ruchu algorytm adaptacji aktualizuje na bieżąco parametry manipulatorai wystawia uaktualniony sygnał sterowania, co powoduje „wyłączenie” regulatora.

Na rysunkach 5.11 i 5.12 przedstawiono przykładowe wartości adaptowanych parametrów – masy m2 i m3.

0 5 10 15 20 25 30-10

-5

0

5

10

15

20

0 5 10 15 20 25 30-10

-5

0

5

10

15

20

0 5 10 15 20 25 30-0.4

-0.3

-0.2

-0.1

0

0.1

0.2

0.3

0.4


Rys. 5.11. Masa m2 adaptowana i rzeczywista. Rys. 5.12. Masa m3 adaptowana i rzeczywista.

Pomimo, że przebiegi parametrów adaptowanych zbiegają do wartości rzeczywistych, nie można sądzić, że jest to oczywiste i będzie tak zawsze (jak na rysunkach 5.13 i 5.14). Aby estymaty parametrów zbiegały do ich prawdziwych wartości, zadana trajektoria musi być „bogata”, tzn. musi dostatecznie pobudzić odpowiedź dynamiki układu tak, aby działanie różnych parametrów mogło być wyraźne [3].

Na rysunkach 5.13 i 5.14 przedstawiono przykładowe wartości adaptowanych parametrów – współczynniki tarcia dla podstawy i ramienia.

Rys. 5.13. Adaptowany i rzeczywisty współczynnik Rys. 5.14. Adaptowany i rzeczywisty współczynnik tarcia dla podstawy tarcia dla ramienia.

W procesie adaptacji współczynnik tarcia dla podstawy, jak i dla ramienia nie osiąga wartości rzeczywistych. Pomimo tego udaje się uzyskać bardzo dobre rezultaty nadążania.

TEST 2.Obejmuje swoim zakresem symulacje układu sterowania adaptacyjnego, przy czym

w 15[s] ruchu zostaje zakłócony parametr – masa i momenty bezwładności członu trzeciego.

Na rysunkach 5.15, 5.16, 5.17 przedstawiono błędy kątów obrotu poszczególnych członów.

0 5 10 15 20 25 302.45

2.5

2.55

2.6

2.65

2.7

2.75

0 5 10 15 20 25 303.25

3.3

3.35

3.4

3.45

3.5

3.55

0 5 10 15 20 25 305

5.5

6

0 5 10 15 20 25 309.5

10

10.5

11

11.5

12



Rys. 5.17. Błąd kąta obrotu łokcia

Zauważalny uskok błędu w 15[s] symulacji wywołany jest wystąpieniem zakłócenia – skokowy przyrost masy członu trzeciego. Największy błąd kąta obrotu osiąga wartość 0.003[rad] dla obrotu ramienia.

Analiza samych błędów pozwala wysunąć wstępne spostrzeżenie, że adaptacyjny układ sterowania jest krzepki, czyli odporny na zakłócenia.


0 5 10 15 20 25 30-10

-8

-6

-4

-2

0

2x 10

-4

0 5 10 15 20 25 30-3

-2.5

-2

-1.5

-1

-0.5

0

0.5

1x 10

-3

0 5 10 15 20 25 30-3.5

-3

-2.5

-2

-1.5

-1

-0.5

0

0.5x 10

-3

0 5 10 15 20 25 300

0.005

0.01

0.015



Zakłócenie parametryczne występujące podczas ruchu chwytaka znajduje również odzwierciedlenie w prędkości chwytaka na trajektorii i błędzie odejścia od trajektorii osiągając największą wartość równą 0.0011[m]. Ważne jest, że po wystąpieniu zakłócenia chwytak powraca na zadaną trajektorię – inaczej jak z samym regulatorem PD.

Na rysunkach 5.20, 5.21, 5.22, przedstawiono wypadkowe momenty napędowena silnikach, oraz udział sterowania od regulatora PD i algorytmu adaptacyjnego.

Rys. 5.20. Wypadkowe momenty napędowe dla: podstawy, ramienia, łokcia.

0 5 10 15 20 25 30-10

-5

0

5

10

15

20


Rys. 5.21. Sterowanie adaptacyjne dla: Rys. 5.22. Sterowanie z regulatora PD dla: podstawy, ramienia, łokcia. podstawy, ramienia, łokcia.

Pojawienie się zakłócenia w 15[s] powoduje „zadziałanie” regulatora PD,który niweluje błąd powstały przez niepełną kompensację z układu adaptacyjnego.Gdy algorytm adaptacji „nauczy się” nowych wartości i wyznaczy prawidłową wartość sterowania – regulator ponownie „wyłącza się”.

Przykładowe wartości parametrów adaptowanych przedstawiają rysunki 5.23 i 5.24.


Na podstawie rysunków 5.23 i 5.24 możemy zaobserwować, że skokowy przyrost masy członu trzeciego pobudza również adaptację masy drugiej, jednak algorytm poprawnie aktualizuje wartość masy drugiej i powraca do wcześniejszej wartości. Oba parametrysą zbieżne do ich wartości rzeczywistych.

Również i w tym przypadku wartości współczynników tarcia nie zbiegają do ich rzeczywistych wartości (rysunki 5.25 i 5.26). Absurdalnie - współczynnik tarcia ramienia ulega skokowemu zmniejszeniu z ustalonej wartości ok. T1d=10.8 do wartości T1d=7,przy czym jego rzeczywista wartość wynosi T1d=12.

0 5 10 15 20 25 30-10

-5

0

5

10

15

20

0 5 10 15 20 25 30-0.5

-0.4

-0.3

-0.2

-0.1

0

0.1

0.2

0.3

0.4

0 5 10 15 20 25 302.45

2.5

2.55

2.6

2.65

2.7

2.75

0 5 10 15 20 25 303.2

3.3

3.4

3.5

3.6

3.7

3.8

3.9

4


Rys. 5.25. Adaptowany i rzeczywisty współczynnik Rys. 5.26. Adaptowany i rzeczywisty współczynniktarcia dla podstawy tarcia dla ramienia.

TEST 3.Obejmuje weryfikację adaptacyjnego algorytmu sterowania w przypadku

wystąpienia zakłóceń parametrycznych i nieparametrycznych.

Zakłócenie nieparametryczne wprowadzono poprzez usunięcie z algorytmu sterowania adaptacyjnego części zapisu matematycznego - dokładnie z elementów macierzy Y: Y[1,7]; Y[2,7]; Y[3,7] – pozostawiając jedynie współczynniki tarcia statycznego. Zakłócenie parametryczne wprowadzono poprzez zawyżenie współczynników tarcia statycznego w manipulatorze do wartości o 1 wyższej niż w algorytmie sterowania.

Na rysunkach 5.27, 5.28, 5.29, pokazano błędy kątów obrotu poszczególnych członów.


0 5 10 15 20 25 305

5.5

6

0 5 10 15 20 25 306

7

8

9

10

11

12

0 5 10 15 20 25 30-0.03

-0.02

-0.01

0

0.01

0.02

0.03

0 5 10 15 20 25 30-0.015

-0.01

-0.005

0

0.005

0.01

0.015

0.02



W przypadku wystąpienia zakłóceń parametrycznych i nieparametrycznych błędy kątów obrotu zdecydowanie zwiększyły się aż do wartości 0.04[rad]. Nie pozostało to bez znaczenia dla ruchu chwytaka po wyznaczonej trajektorii, zwiększając błąd odejściado wartości 0.009[m].

Na rysunku 5.30 przedstawiono prędkość chwytaka na trajektorii,a na rysunku 5.31 - trajektorię ruchu chwytaka.


0 5 10 15 20 25 30-0.05

-0.04

-0.03

-0.02

-0.01

0

0.01

0.02

0.03

0.04

0.05

0 5 10 15 20 25 300

0.002

0.004

0.006

0.008

0.01

0.012

0.014


Rys. 5.31. Trajektoria chwytaka zadana i otrzymana

Zwiększenie współczynnika tarcia statycznego w manipulatorze spowodowało,że algorytm adaptacji poszukiwał odpowiedniego zestawu innych parametrów starając się zminimalizować błąd niewłaściwej kompensacji.

Na rysunkach 5.32 i 5.33 przedstawiono aktualizację parametrów – masy członów drugiego i trzeciego.


Na rysunkach 5.34 i 5.35 przedstawiono przykładową aktualizację parametrów – współczynnik tarcia dynamicznego dla członu drugiego i moment bezwładności członu pierwszego.

0 5 10 15 20 25 300

0.5

1

1.5

2

2.5

3

3.5

4

4.5

5

0 5 10 15 20 25 30

3

3.5

4

4.5


Rys. 5.34. Adaptowany i rzeczywisty współczynnik Rys. 5.35. Adaptowany i rzeczywisty moment tarcia dynamicznego dla ramienia. bezwładności podstawy.

Zwiększając współczynniki regulatora PD, można zminimalizować błąd powstały przez niewłaściwą kompensację. Z przeprowadzenia symulacji numerycznej możemy wyciągnąć wnioski, że w przypadku wystąpienia zarówno zakłóceń parametryczny jaki nieparametrycznych algorytm sterowania nadal pozostawał stabilny.

Podsumowując: wyniki symulacji numerycznych pozwalają stwierdzić,że adaptacyjny algorytm sterowania ruchem manipulatora zapewnia globalną stabilność układu sterowania. Wynika również, że wektor błędu nadążania i jego pochodne dążądo zera, natomiast estymowane parametry przyjmują wartości ograniczone.

Z przeprowadzenia dotychczasowej dedukcji wyników sterowania konwencjonalnego i sterowania adaptacyjnego wynika, że uwzględnienie w algorytmie sterowania modelu matematycznego zdecydowanie zwiększa dokładność ruchu nadążnego manipulatora.

Dodatkowo wprowadzając element kompensacji o możliwościach adaptacyjnych - uzyskujemy algorytm odporny na ograniczone zakłócenia parametryczne.

0 5 10 15 20 25 300

20

40

60

80

100

120

0 5 10 15 20 25 30-4

-2

0

2

4

6

8

10

12

14

STEROWANIE RUCHEM MANIPULATORA – Badania eksperymentalne

6. Badania eksperymentalne.

6.1. Opis manipulatora.

Do badań eksperymentalnych wykorzystano manipulator Scorbot Er4pc [9], którego szkic wraz z opisem przedstawiono na rysunku 6.1.

Łokieć

Ramię

Chwytak

Podstawa

Ostoja

Rys. 6.1. Widok ogólny manipulatora.

W manipulatorze możemy wyróżnić: Strukturę pozycjonowania, Strukturę orientacji, Zacisk chwytaka.

W tabeli 6.2. zestawiono spis stopni swobody manipulatora, oraz ich realizację przez poszczególne silniki.


Numer osi Stopnie swobody Realizowany ruch Numer silnika1 Podstawa Obrót podstawy 1

2 Ramię Podnoszenie / opuszczanie ramienia 2

3 Łokieć Podnoszenie / opuszczanie łokcia 3

4 Chwytak Przechylenie chwytaka 4+55 Chwytak Pochylenie chwytaka 4-5

Tabl. 6.2. Spis stopni swobody manipulatora, oraz ich realizację przez silniki

Tabela 6.3 zawiera specyfikację techniczno - naukową dostarczoną wrazz dokumentacją do manipulatora.

Struktura Konfiguracja stawowaLiczba stopni swobody Pięć i chwytakZakres przemieszczeń w przegubach:

1. Obrót podstawy2. Obrót ramienia3. Obrót łokcia4. Przechylenie chwytaka5. Pochylenie chwytaka

310º+130º / -35º±130±130Nieograniczony

Maksymalny zakres strefy roboczej 610[mm]

ChwytakSerwomechanizm prądu stałego DC z encoder’em optycznym; przełożenie za pomocą śruby pociągowej

Maksymalne rozwarcie chwytaka 75[mm] bez podkładek gumowych.65[mm] z podkładkami gumowymi

Pozycjonowanie Za pomocą nieruchomych switch’y umieszczonych na każdym przegubie

Sprzężenie zwrotne Encoder’y optyczne podające pozycję na każdym przegubie

Moc silników 15[oz. in] moment szczytowy (zablokowany)70[W] dla momentu szczytowego

PrzełożenieSilnik 1,2,3 – 127,7:1Silnik 4,5 – 65,5:1Silnik 6 (chwytak) – 19,5:1

Przekazanie napędu Przekładnie mechaniczne, paski zębate, koła zębate stożkowe

Maksymalny udźwig 1[kg] (razem z chwytakiem)Dokładność ±0,2[mm]Waga 11,5[kg]Maksymalna prędkość na trajektorii 600[mm/s]Temperatura pracy 2ºC÷40ºC

Tabl. 6.3. Specyfikacja techniczno-naukowa manipulatora Scorbot Er4pc.

Firma Eshed Robotec INC nie zezwala na ingerencję osób trzecich zarówno w oprogramowanie jak i w konstrukcję mechaniczną lub elektryczną manipulatora.


6.2. Realizacja techniczna układu sterowania.

Ograniczymy się do omówienia zestawu wejść-wyjść dla jednego stopnia swobody.

Sterowanie manipulatorem Scorbot Er4pc ogranicza się do podania na wejścia wzmacniacza mocy odpowiednio przygotowanego sygnału elektrycznego i odczytanie położenia manipulatora z encoder’a optycznego i microswitch’a.

Poziom sygnału elektrycznego jest standardowy dla wszystkich układów we-wy typu TTL i wynosi 0÷5[V] - logicznie: „0”÷„1”. Układ posiada dwa wejścia, oznaczone jako „PWM” i „BREAK”.

Rys. 6.4. Schemat sterowania mocą silnika metoda cyfrową PWM.

Na pierwszy z nich zadajemy - zgodnie z metoda sterowania mocą przez wypełnienie - sygnał cyfrowy, którego okres jest zazwyczaj stały w czasie, natomiast zmienia się wypełnienie w czasie tego okresu (rysunek 6.4). Drugie wejście służy do całkowitego wyłączenia silnika. Zalecane jest, aby okres sygnału PWM był jak najkrótszy. W układach rzeczywistych optymalne okresy przy sterowaniu silnikami wynosząok. 10[ms]. W ten sposób zadając wypełnienie okresu 100% lub 0% uzyskujemy maksymalną moc silnika odpowiednio w jedną bądź drugą stronę. Oczywistym jest, że przy wypełnieniu 50% silnik nie będzie się obracał.


Rys. 6.5. Szkic silnika wraz z wbudowaną przekładnią i encoder’em optycznym [9].

Część druga układu sterowania, to odczyt encoder’a optycznego i microswitch’a. Podstawowa zasada wykorzystana przy odczycie encoder’a optycznego to: ustawić przerwanie na wejściu jednego kanału uaktywniające się zboczem narastającym i momencie wystąpienia przerwania sprawdzić poziom sygnału na kanale drugim, jeśli jest „1”to zwiększyć licznik o pewien przyrost, jeśli jest „0” to zmniejszyć licznik o pewien przyrost (rysunek 6.6).

Rys. 6.6. Szkic encoder’a optycznego [9] i przebieg impulsów w czasie dla dwóch kanałów, z zaznaczeniem zbocza narastającego na kanale pierwszym i odpowiadającej wartości kanału drugiego w zależności od kierunku obrotu

Znając liczbę impulsów z encoder’a na pełny obrót można wyznaczyć aktualny kąt obrotu encoder’a - to jest przyrost wartości kąta obrotu członu na jeden impuls. Jeżeli encoder nie znajduje się bezpośrednio na członie (jak to ma miejsce w manipulatorze Scorbot), uzyskaną wartość należy przeskalować uwzględniając przełożenia manipulatora.

Obydwie z opisanych powyżej czynności zaimplementowano bezpośrednio w aplikacji Matlab®Simulink® wykorzystując bibliotekę Real Time Windows Target (RTWinTgt). Na rysunku 6.7 przedstawiono schemat sterowania z regulatorem PD wykorzystujący podane wiadomości.


Rys. 6.7. Graficzna ilustracja sygnałów in-out dla rozpatrywanego manipulatora.

Natomiast na rysunkach 6.8 i 6.9 przedstawiono blok realizujący sterowanie mocą „PWM” i blok odczytu encoder’a optycznego „ENC”.

Rys. 6.8. Schemat funkcjonalny bloku ENC.

Rys. 6.9. Schemat funkcjonalny bloku PWM.

Bloki: „Digital Output” i „Digital Input” są częścią biblioteki Real Time Windows Target i odpowiadają za komunikację pomiędzy oprogramowaniem Matlab®Simulink®

a urządzeniami zewnętrznymi. W naszym przypadku obsługują port LPT komputera PC,


do którego podłączony jest jako wejście: encoder optyczny, oraz sygnały wyjścia: PWMi BREAK (według rysunku 6.10).

Rys. 6.10. Układ wyprowadzeń w porcie LPT komputera dla RTWinTgt.

Przypisanie cyfr na rysunku 6.10 odpowiada zdefiniowaniu takich samych w bibliotekach „Digital Output” i/lub „Digital Input”

Możliwość konfiguracji połączeń dla trzech stopni swobody:LPT 1.

PIN 2, 3 - encoder optyczny z podstawy, odpowiednio kanał 1 i 2,PIN 5,4 - encoder optyczny z ramienia, odpowiednio kanał 2 i 1,PIN 1, 2, 3, 4, 5, 6 – odpowiednio: PWM dla podstawy, ramienia, łokcia

BREAK dla podstawy, ramienia, łokcia,PIN 1... 8 - masa układu.

LPT 2.PIN 2, 3 - encoder optyczny z łokcia, odpowiednio kanał 1 i 2,PIN 1... 8 - masa układu.

Uwaga: w przypadku nieprawidłowej pracy układu sterowania można zamienić kanały encoder’ów (np. na pin’y 3, 2 zamiast 2, 3).

Tak, więc na jeden stopień swobody manipulatora potrzeba trzech wejść i dwóch wyjść. Powyższe biblioteki umożliwiają ustawienie portu LPT, na pięć wejść i dwanaście wyjść (patrz rysunek 6.10). Z tego też powodu dysponując dwoma portami LPT możliwe jest sterowanie maksymalnie trzema stopniami swobody manipulatora.

Jeżeli jednak pominiemy podłączenie microswitch’y (pojawia się problem pozycjonowania) to za pomocą dwóch portów LPT możemy sterować wszystkimi pięcioma stopniami swobody manipulatora ze sprzężeniem zwrotnym. Chwytak może być sterowany bez sprzężenia zwrotnego od położenia - wprowadzając np. sterowanie czasowe.Aby zrealizować postawione zadanie należy dysponować możliwie szybkim komputeremw celu nadążania z wypracowaniem sygnału sterowania w czasie rzeczywistym.

6.3. Uzyskane wyniki i wnioski.

Na rysunku 6.11 przedstawiono schemat, według którego przeprowadzano eksperymenty.


Rys. 6.11. Schemat prowadzenia weryfikacji na manipulatorze Scorbot Er4pc.

Eksperymenty przeprowadzano przy parametrach jak niżej

Rys. 6.12. Parametry „symulacji” przy sterowaniu manipulatora.

W bloczkach zaimplementowano odpowiednio: „trajektoria zadana” – zadane parametry ruchu – odpowiednio do testu, „regulator PD” – równanie (4.5), „Scorbot Er4pc” – rozdział 6.2, „Wizualizacja i zapisanie wyników” – wyniki i zapamiętanie do dalszej

analizy.

TEST 1.Obejmuje eksperyment, który ma na celu wyznaczenie optymalnych

współczynników regulatora PD dla manipulatora Scorbot Er4pc.

Zdefiniowano parametry ruchu zadane jak na rysunku 6.13 i 6.14.


Rys. 6.13. Zadany kąt obrotu. Rys. 6.14. Zadana prędkość kątowa.

1. Obrót podstawy.

Wstępnie przyjęto wartości współczynników regulatora:

KP1=500; KD1=200;

Otrzymano błędy jak na rysunkach 6.15 i 6.16.

Rys. 6.15. Błąd kąta obrotu podstawy. Rys. 6.16. Błąd prędkości kątowej podstawy.

Maksymalny błąd bezwzględny kąta obrotu wynosi 0.044[rad], dla prędkości kątowej możemy oszacować największy błąd w granicach 0.04[rad/s]. Uzyskany rezultat uznano za niewystarczający.

Zwiększono współczynniki regulatora do wartości:

KP1=800; KD1=400;


0 1 2 3 4 5 6 7 8 9 10-0.05

0

0.05

0.1

0.15

0.2

0.25

0.3

0.35

0 1 2 3 4 5 6 7 8 9 10-0.1

-0.08

-0.06

-0.04

-0.02

0

0.02

0.04

0.06

0.08

0.1

0 1 2 3 4 5 6 7 8 9 10-0.05

-0.04

-0.03

-0.02

-0.01

0

0.01

0.02

0.03

0.04

0.05

0 1 2 3 4 5 6 7 8 9 10-0.08

-0.06

-0.04

-0.02

0

0.02

0.04

0.06

0.08


Rys. 6.17. Błąd kąta obrotu podstawy. Rys.6.18. Błąd prędkości kątowej podstawy

Maksymalny błąd bezwzględny kąta obrotu wynosił teraz 0.016[rad], dla prędkości kątowej możemy oszacować największy błąd w granicach 0.02[rad/s]. Uzyskany rezultat uznano w dalszym ciągu za niewystarczający.

Zwiększono współczynniki regulatora do wartości

KP1=800; KD1=400;


Rys. 6.19. Błąd kąta obrotu podstawy. Rys. 6.20. Błąd prędkości kątowej podstawy.

Maksymalny błąd bezwzględny kąta obrotu wynosił teraz 0.009[rad], dla prędkości kątowej możemy oszacować największy błąd w granicach 0.013[rad/s]. Uzyskany rezultat uznano za wystarczający i do dalszych celów przyjęto współczynniki regulatora jak wyżej.

Wizualnie przedstawiono jak wyglądają zadane i otrzymane przebiegidla przypadku trzeciego (rysunki 6.21 i 6.22).

0 1 2 3 4 5 6 7 8 9 10-0.02

-0.015

-0.01

-0.005

0

0.005

0.01

0.015

0.02

0 1 2 3 4 5 6 7 8 9 10-0.04

-0.03

-0.02

-0.01

0

0.01

0.02

0.03

0 1 2 3 4 5 6 7 8 9 10-10

-8

-6

-4

-2

0

2

4

6

8x 10-3

0 1 2 3 4 5 6 7 8 9 10-0.03

-0.02

-0.01

0

0.01

0.02

0.03


Rys. 6.21. Kąt obrotu podstawy zadany i otrzymany. Rys. 6.22. Prędkość kątowa podstawy zadana i otrzymana

Eksperymentalnie zwiększano współczynniki regulatora ponad wartościz przypadku trzeciego. Uzyskane rezultaty były porównywalne z osiągniętymi wcześniej, jednak na przebiegach prędkości kątowej pojawiały się bardzo duże szumy jak również,co miało ważne znaczenie – pojawiły się drgania podstawy. Zjawisko to zostało omówionei wytłumaczone w rozdziale czwartym, przy okazji podsumowywania sterowania konwencjonalnego. Dla przypomnienia, krótko; związane jest to z rezonansem przegubu obrotowego poddanego zbyt dużym wzmocnieniom regulatora PD.

2. Obrót ramienia.


KP2=1500; KD2=500;


Rys. 6.23. Błąd kąta obrotu ramienia. Rys. 6.24. Błąd prędkości kątowej ramienia.

Maksymalny błąd bezwzględny kąta obrotu wynosi 0.008[rad], dla prędkości kątowej możemy oszacować największy błąd w granicach 0.03[rad/s]. Uzyskany rezultat uznano za niewystarczający. Zwiększono współczynniki regulatora do wartości:

KP1=2000; KD1=600;


0 1 2 3 4 5 6 7 8 9 100

0.05

0.1

0.15

0.2

0.25

0.3

0.35

0 1 2 3 4 5 6 7 8 9 10-0.2

-0.15

-0.1

-0.05

0

0.05

0.1

0.15

0 1 2 3 4 5 6 7 8 9 10-6

-4

-2

0

2

4

6

8x 10-3

0 1 2 3 4 5 6 7 8 9 10-0.06

-0.04

-0.02

0

0.02

0.04

0.06



Maksymalny błąd bezwzględny kąta obrotu wynosi 0.005[rad], dla prędkości kątowej możemy oszacować największy błąd w granicach 0.025[rad/s]. Uzyskany rezultat uznano za wystarczający i do dalszych prac przyjęto współczynniki jak wyżej. Wizualnie przedstawiono jak wyglądają zadane i otrzymane przebiegi dla tego przypadku.

Rys. 6.27. Kąt obrotu ramienia zadany i otrzymany. Rys. 6.28. Prędkość kątowa ramienia zadana i otrzymana.

3. Obrót łokcia.


KP3=1800; KD3=600;


0 1 2 3 4 5 6 7 8 9 10-6

-4

-2

0

2

4

6

8x 10-3

0 1 2 3 4 5 6 7 8 9 10-0.06

-0.04

-0.02

0

0.02

0.04

0.06

0 1 2 3 4 5 6 7 8 9 10-0.05

0

0.05

0.1

0.15

0.2

0.25

0.3

0.35

0 1 2 3 4 5 6 7 8 9 10-0.2

-0.15

-0.1

-0.05

0

0.05

0.1

0.15



Maksymalny błąd bezwzględny kąta obrotu wynosi 0.007[rad], dla prędkości kątowej możemy oszacować największy błąd w granicach 0.03[rad/s]. Uzyskany rezultat uznano za wystarczający i do dalszych prac przyjęto współczynniki jak wyżej. Wizualnie przedstawiono jak wyglądają zadane i otrzymane przebiegi dla tego przypadku.

Rys. 6.31. Kąt obrotu łokcia zadany i otrzymany. Rys. 6.32. Prędkość kątowa łokcia zadana i otrzymana.

TEST 2.Na podstawie rozumowania przeprowadzonego w rozdziale 3.1.3 przygotowano

weryfikację sterowania z regulatorem PD dla trajektorii zadanej jak na rysunku 6.33.

0 1 2 3 4 5 6 7 8 9 10-10

-8

-6

-4

-2

0

2

4x 10-3

0 1 2 3 4 5 6 7 8 9 10-0.06

-0.04

-0.02

0

0.02

0.04

0.06

0 1 2 3 4 5 6 7 8 9 100

0.05

0.1

0.15

0.2

0.25

0.3

0.35

0 1 2 3 4 5 6 7 8 9 10-0.2

-0.15

-0.1

-0.05

0

0.05

0.1

0.15


Rys. 6.33. Zadana trajektoria ruchu chwytaka.

Trajektoria jak na rysunku 6.33 jest wynikiem zadania na przegubach manipulatora przebiegów jak na rysunkach 6.34, 6.35, 6.36.

Rys. 6.34. Zadany kąt obrotu podstawy. Rys. 6.35. Zadany kąt obrotu ramienia.

Rys. 6.36. Zadany kąt obrotu łokcia.

Powyższe przebiegi opisane są funkcjami

0 1 2 3 4 5 6 7 8 9 100

0.1

0.2

0.3

0.4

0.5

0.6

0.7

0.8

0 1 2 3 4 5 6 7 8 9 100.5

0.6

0.7

0.8

0.9

1

1.1

1.2

0 1 2 3 4 5 6 7 8 9 10-1.1

-1

-0.9

-0.8

-0.7

-0.6

-0.5

-0.4


(6.1)

Dla konkretnego przypadku jak wyżej po wyznaczeniu współczynników zgodniez informacjami podanymi w rozdziale 3.1.3 wynoszą odpowiednio

- podstawa

- ramię

- łokieć

Kąty obrotów poszczególnych członów wynikają z scałkowania powyższych zależności przy warunkach początkowych jak wyżej.

Rys. 6.37. Zadana prędkość kątowa podstawy. Rys. 6.38. Zadana prędkość kątowa ramienia.

Rys. 6.39. Zadana prędkość kątowa łokcia.

Wyniki eksperymentu dla podstawy manipulatora przedstawiono na rysunkach 6.40i 6.41.

0 1 2 3 4 5 6 7 8 9 100

0.02

0.04

0.06

0.08

0.1

0.12

0 1 2 3 4 5 6 7 8 9 100

0.01

0.02

0.03

0.04

0.05

0.06

0.07

0.08

0 1 2 3 4 5 6 7 8 9 10-0.08

-0.07

-0.06

-0.05

-0.04

-0.03

-0.02

-0.01

0


Rys. 6.40. Kąt obrotu podstawy zadany i otrzymany. Rys. 6.41. Prędkość kątowa podstawy zadana i otrzymana

Ponieważ w układzie rejestrującym występują bardzo duże szumy pomiarowe zastosowano filtrację analogową dla sygnałów prędkości kątowych

Przedstawiono rezultat filtracji sygnału prędkości kątowej obrotu podstawy.

Rys. 6.42. Prędkość kątowa zadana i otrzymana po filtracji analogowej.

Na rysunkach 6.43 i 6.44 przedstawiono błędy pomiędzy kątami obrotu zadanymii otrzymanymi dla podstawy, oraz pomiędzy prędkościami kątowymi zadanymii otrzymanymi dla podstawy po operacji filtracji analogowej.

Rys. 6.43. Błąd kąta obrotu podstawy bez filtracji. Rys. 6.44. Błąd prędkości kątowej podstawy po filtracji.

Maksymalny błąd kąta obrotu podstawy wynosi 0.006[rad]. Dla prędkości kątowej błędy są większe - maksymalny osiąga wartość 0.05[rad/s]. Możemy przyjąć, że otrzymane

0 1 2 3 4 5 6 7 8 9 100

0.1

0.2

0.3

0.4

0.5

0.6

0.7

0.8

0 1 2 3 4 5 6 7 8 9 10-0.3

-0.2

-0.1

0

0.1

0.2

0.3

0.4

0 1 2 3 4 5 6 7 8 9 100

0.02

0.04

0.06

0.08

0.1

0.12

0.14

0.16

0.18

0 1 2 3 4 5 6 7 8 9 10-5

0

5

10x 10-3

0 1 2 3 4 5 6 7 8 9 10-0.06

-0.05

-0.04

-0.03

-0.02

-0.01

0

0.01

0.02

0.03

0.04


błędy mieszczą się w założonych przedziałach zmienności.

Na rysunkach 6.45 i 6.46 przedstawiono kąt obrotu ramienia zadany i otrzymanyz obiektu, oraz prędkość kątowa zadana i otrzymana.

Rys. 6.45. Kąt obrotu ramienia zadany i otrzymany. Rys. 6.46. Prędkość kątowa ramienia zadanai otrzymana po filtracji.

Błędy pomiędzy wyżej przedstawionymi zależnościami pokazano na rys. 6.47 i 6.48.

Rys. 6.47. Błąd kąta obrotu ramienia bez filtracji. Rys. 6.48. Błąd prędkości kątowej ramienia po filtracji.

Maksymalny błąd kąta obrotu ramienia wynosi 0.008[rad]. Dla prędkości kątowej błędy są zdecydowanie większe - maksymalny osiąga wartość 0.08[rad/s]. Możemy przyjąć, że otrzymane błędy mieszczą się w założonych przedziałach zmienności.

Na rysunkach 6.49 i 6.50 przedstawiono kąt obrotu łokcia zadany i otrzymanyz obiektu, oraz prędkość kątowa zadana i otrzymana. Natomiast na rysunkach 6.51 i 6.52 przedstawiono błędy pomiędzy tymi wartościami.

0 1 2 3 4 5 6 7 8 9 100.7

0.8

0.9

1

1.1

1.2

1.3

1.4

0 1 2 3 4 5 6 7 8 9 100

0.01

0.02

0.03

0.04

0.05

0.06

0.07

0.08

0.09

0.1

0 1 2 3 4 5 6 7 8 9 10-4

-2

0

2

4

6

8

10x 10

-3

0 1 2 3 4 5 6 7 8 9 10-0.1

-0.08

-0.06

-0.04

-0.02

0

0.02

0.04


Rys. 6.49. Kąt obrotu łokcia zadany i otrzymany. Rys. 6.50. Prędkość kątowa łokcia zadanai otrzymana po filtracji.

Rys. 6.51. Błąd kąta obrotu łokcia bez filtracji. Rys. 6.52. Błąd prędkości kątowej łokcia po filtracji.

Maksymalny błąd kąta obrotu łokcia wynosi 0.007[rad]. Dla prędkości kątowej błędy są zdecydowanie większe - maksymalny osiąga wartość 0.08[rad/s]. Również i w tym przypadku możemy przyjąć, że otrzymane błędy mieszczą się w założonych przedziałach zmienności.

Rozwiązując zadanie kinematyki prostej z otrzymanych kątów obrotu manipulatora, możemy wyznaczyć rzeczywistą trajektorię, po której porusza się chwytak w przestrzeni kartezjańskiej. Na rysunku 6.53 przedstawiono trajektorię rzeczywistą chwytaka i zadaną.

0 1 2 3 4 5 6 7 8 9 10-1.4

-1.3

-1.2

-1.1

-1

-0.9

-0.8

-0.7

0 1 2 3 4 5 6 7 8 9 10-0.1

-0.09

-0.08

-0.07

-0.06

-0.05

-0.04

-0.03

-0.02

-0.01

0

0 1 2 3 4 5 6 7 8 9 10-8

-7

-6

-5

-4

-3

-2

-1

0

1

2x 10-3

0 1 2 3 4 5 6 7 8 9 10-0.01

0

0.01

0.02

0.03

0.04

0.05

0.06

0.07

0.08

0.09



Znając trajektorię chwytaka możemy wyznaczyć maksymalny błąd odejściaod zadanej trajektorii zgodnie z zależnością (4.11). W rezultacie otrzymano wynik 0.0067[m]. Otrzymane rozwiązanie możemy różnie zinterpretować – w zależnościod zastosowania robota. Jeżeli np. manipulator wykonuje zadanie malowania natryskowego – otrzymana dokładność jest zadawalająca. Ale jeżeli manipulator ma za zadanie śledzić spoinę podczas spawania gazowego – uzyskany rezultat pozostawia wiele do życzenia.

Podsumowując: Wyniki eksperymentu numerycznego na obiekcie rzeczywistym pozwalają potwierdzać wstępne założenia teoretyczne.

Układ sterowania z regulatorem PD jest stabilny.Zapewnia nadążanie za zadaną trajektorią z założoną dokładnością.Podczas testu potwierdziły się wady sterowania w oparciu o ten regulator,

mianowicie: pojawia się rezonans przegubu przy dużych wzmocnieniach regulatora. Teoretycznie uchyb pozycyjny można dowolnie zmniejszyć przez zwiększenie współczynnika proporcjonalnego, jednak w rzeczywistości narzucone są ograniczenia na ten współczynnik, między innymi właśnie ograniczenie przed rezonansem przegubu.

Na podstawie przeprowadzenia części teoretycznej i doświadczalnej można domniemać się, że wprowadzenie sterowania adaptacyjnego mogłoby poprawić osiągi manipulatora.

Jakkolwiek badania doświadczalne prowadzone były na jednym manipulatorze - o strukturze RRR – wnioskujemy, że równie dobrze wypadły by na wszystkich innych (struktura RRR sprawia najwięcej trudności w sterowaniu).

STEROWANIE RUCHEM MANIPULATORA – Kierunki dalszych badań

7. Kierunki dalszych badań.

Jako kierunki dalszych badań, należy zaproponować przeprowadzenie wstępnej analizy układów sterowania typu adaptacyjnego i/lub krzepkiego dla rozpatrywanego manipulatora. W miarę nabierania praktyki nad sterowaniem manipulatorami należałoby wprowadzić do układów sterowania najnowsze rozwiązania w tej dziedzinie – sieci neuronowe i układy z logiką rozmytą.

Nie bez znaczenia pozostaje problem identyfikacji współczynników manipulatora, takich jak jego masy, momenty bezwładności członów, oporu ruchu i inne – równieżto pozostaje jako koncepcja dalszych prac.

Kolejne opracowania – wstępnie teoretycznie – powinny zawierać takie problemy jak podatność przegubów i luzy w przekładniach manipulatora. Uwzględnienie właśnie tych zjawisk zdecydowanie zwiększy dokładność realizowanego ruchu, a tym samyi funkcjonalność samego manipulatora.

Aby zrealizować wyżej przedstawione zadania istnieje potrzeba skonstruowania układu elektronicznego odpowiedzialnego za komunikację pomiędzy komputerem PC (konkretnie oprogramowaniem Matlab®Simulink®), a samym manipulatorem (konkretnie encoder’ami optycznymi i układem mocy PWM) bez udziału mocy obliczeniowej komputera.

Istnieje, więc jeszcze dużo do zrobienia, a to tylko wstępnie wybrane do realizacji koncepcje dalszych prac nad manipulatorem. Inne - jak zmiana konstrukcji mechanicznej manipulatora – tj. zastosowanie napędu bezpośredniego w celu wyeliminowania oporów ruchu i luzów w przekładniach, lub zastosowania sprzężenia zwrotnego od siły działającej na chwytak wykraczają chyba poza obszar prac „studenckich”.

STEROWANIE RUCHEM MANIPULATORA – Literatura

8. Literatura.

1. „Podstawy robotyki – teoria i elementy manipulatorów i robotów”,praca zbiorowa pod redakcją A. Moreckiego i J. Knapczyka – WNT 1993,1999.

2. „Wprowadzenie do robotyki – mechanika i sterowanie”, J. J. Craig WNT 1993, 1995.

3. „Dynamika i sterowanie robotów”, M. W. Spong WNT 1995.4. „Sterowanie ruchem nadążnym mobilnych robotów kołowych”,

Z. Hendzel OWPRz 1996.5. „Parameter estimation and upper bouding adaptation in adaptiv –

robust control approaches for trajectory control of robots”, Ibrahim Uzmay i Recept Burkan; Robotica vol. 20 part 6.

6. „Teoria mechanizmów i manipulatorów” A. Morecki, J. Knapczyk, K, Kędzior, WNT 2002.

7. „Modelowanie i sterowanie mobilnych robotów kołowych„ M. J. Giergiel, Z. Hendzel, W. Żylski. PWN 2002.

8. „Mechanika ogólna” część 1,2,3 – Z. Hendzel, W. Żylski OWPRz 2000.

9. „Dokumentacja manipulatora Scorbot Er4pc”. Eshed Robotec - Catalog #100118 Rev.A. 1999.

STEROWANIE RUCHEM MANIPULATORA – Spis załączonego oprogramowania

9. Spis załączonego oprogramowania.

Wszystkie zbiory z rozszerzeniami: *.mdl – są plikami aplikacji Matlab®Simulink® w wersji 6.5, *.m – są plikami aplikacji Matlab® w wersji 6.5, *.mws – są plikami aplikacji Maple w wersji 8, *.mat – są plikami aplikacji Matlab® w wersji 6.5 i zawierają zapisaną

przestrzeń roboczą podczas weryfikacji na manipulatorze Scorbot, *.doc – są plikami aplikacji MS Word XP.

Plik o nazwie zawiera:

DYNAMIKA i STEROWANIE.mws – wyprowadzenie różniczkowych równań ruchu dla przyjętego manipulatora, następnie rozwiązanie dynamiki odwrotnej, przejście na zapis macierzowy z różniczkowych równań ruchu, rozwiązanie dynamiki prostej, wyprowadzenie macierzy sterowań i wektora parametrów adaptowanych do algorytmu adaptacyjnego,

KINEMATYKA.mws – rozwiązanie kinematyki prostej dla przyjętego manipulatora, następnie zapis macierzy jakobianowej i rozwiązanie zadania kinematyki odwrotnej,

PLANOWANIE TRAJEKTORII.mws – wyznaczenie funkcji opisujących przebiegi prędkości kątowych dla problemu planowania trajektorii ruchu chwytaka,

DANE_SCORBOT.m – inicjalizujące dane o przyjętych współczynnikach manipulatora dla przeprowadzenia symulacji numerycznych,

INNE_POMOCNE.m - wyznaczenie błędu odejścia od zadanej trajektorii, DYNAMIKA_MANIP.mdl - symulacja zadania dynamiki prostej

i odwrotnej, KINEMATYKA_MANIP.mdl - symulacja zadania kinematyki prostej

i odwrotnej, ST_PID.mdl - symulacja sterowania manipulatora przy wykorzystaniu

regulatora PD, ST_ADAPT.mdl - symulacja sterowania manipulatora przy wykorzystaniu

algorytmu adaptacyjnego, ST_ADAPT_z.mdl - j.w. lecz przy zakłóconych parametrach manipulatora, PRACA.doc – dokument tekstowy pracy przygotowany do druku.

Katalog „Eksperyment” - zawiera wszystkie pliki wykorzystywane podczas weryfikacji układu sterowania na manipulatorze Scorbot Er4pc.

10. Dodatek - kody źródłowe programów.

10.1. Kinematyka.

Kod źródłowy programu KINEMATYKA.mws

> restart:> with(linalg):Warning, the protected names norm and trace have been redefined and unprotected

> V:=Vector(3,[Vx, Vy, Vz]):> q:=array(1..3,[theta[1], theta[2], theta[3]]):> qp:=array(1..3,[omega[1], omega[2], omega[3]]):> qt:=array(1..3,[theta[1](t), theta[2](t), theta[3](t)]):> qpt:=map(diff,qt,t):> podst1:=qpt[1]=qp[1], qpt[2]=qp[2], qpt[3]=qp[3], qt[1]=q[1], qt[2]=q[2], qt[3]=q[3]:> podst2:=qp[1]=qpt[1], qp[2]=qpt[2], qp[3]=qpt[3], q[1]=qt[1], q[2]=qt[2], q[3]=qt[3]:Wektor polożenia chwytaka.> Rt[1]:=cos(theta[1](t))*(e[0]+l[2]*cos(theta[2](t))+l[3]*cos(theta[3](t)));

:= Rt1 ( )cos ( )1 t ( ) e0 l2 ( )cos ( )2 t l3 ( )cos ( )3 t

> Rt[2]:=sin(theta[1](t))*(e[0]+l[2]*cos(theta[2](t))+l[3]*cos(theta[3](t)));

:= Rt2 ( )sin ( )1 t ( ) e0 l2 ( )cos ( )2 t l3 ( )cos ( )3 t

> Rt[3]:=l[1]+l[2]*sin(theta[2](t))+l[3]*sin(theta[3](t)); := Rt3 l1 l2 ( )sin ( )2 t l3 ( )sin ( )3 t

Wektor prędkości chwytaka.> for i from 1 to 3 do> Rtp[i]:=diff(Rt[i],t);> end do;

Rtp1 ( )sin ( )1 t

d

dt ( )1 t ( ) e0 l2 ( )cos ( )2 t l3 ( )cos ( )3 t :=

( )cos ( )1 t

l2 ( )sin ( )2 t

d

dt ( )2 t l3 ( )sin ( )3 t

d

dt ( )3 t

Rtp2 ( )cos ( )1 t

d

dt ( )1 t ( ) e0 l2 ( )cos ( )2 t l3 ( )cos ( )3 t :=

( )sin ( )1 t

l2 ( )sin ( )2 t

d

dt ( )2 t l3 ( )sin ( )3 t

d

dt ( )3 t

:= Rtp3 l2 ( )cos ( )2 t

d

dt ( )2 t l3 ( )cos ( )3 t

d

dt ( )3 t

> for i from 1 to 3 do> Rp[i]:=subs(podst1,Rtp[i]);> end do:Jakobian.> J:=array(1..3,1..3):> for i from 1 to 3 do> for j from 1 to 3 do> J[i,j]:=coeff(Rp[i],qp[j]);> end do;> end do;> J:=evalm(J);

:= J

( )sin 1 ( ) e0 l2 ( )cos 2 l3 ( )cos 3 ( )cos 1 l2 ( )sin 2 ( )cos 1 l3 ( )sin 3

( )cos 1 ( ) e0 l2 ( )cos 2 l3 ( )cos 3 ( )sin 1 l2 ( )sin 2 ( )sin 1 l3 ( )sin 3

0 l2 ( )cos 2 l3 ( )cos 3

> W:=simplify(det(J),’size’)=0:> J1:=inverse(J):Wektor prędkości uogólnionej.> K:=evalm(J1&*V):> for i from 1 to 3 do> qp[i]=simplify(K[i]);> end do;

1 ( )sin 1 Vx ( )cos 1 Vy

e0 l2 ( )cos 2 l3 ( )cos 3

2 ( )cos 3 ( )cos 1 Vx ( )cos 3 ( )sin 1 Vy ( )sin 3 Vz

( )( )sin 2 ( )cos 3 ( )sin 3 ( )cos 2 l2

3

( )sin 2 Vz ( )cos 1 Vx ( )cos 2 ( )cos 2 ( )sin 1 Vy

( )( )sin 2 ( )cos 3 ( )sin 3 ( )cos 2 l3

>

10.2. Planowanie trajektorii.

Kod źródłowy programu PLANOWANIE_TRAJEKTORII.mws

> restart:Zdefiniowanie poczatkowego i końcowego polożenia oraz poczatkowego i końcowego czasu.> t10:=0: tk1:=10: q10:=0: qp10:=0: qk1:=Pi/4: qpk1:=0:> t20:=0: tk2:=10: q20:=Pi/6: qp20:=0: qk2:=Pi/3: qpk2:=0:

> t30:=0: tk3:=10: q30:=-Pi/6: qp30:=0: qk3:=-Pi/3: qpk3:=0:Uklad równań z niewiadomymi "a".> R11:=a10+a11*t10+a12*t10^2+a13*t10^3=q10:> R12:=a11+2*a12*t10+3*a13*t10^2=qp10:> R13:=a10+a11*tk1+a12*tk1^2+a13*tk1^3=qk1:> R14:=a11+2*a12*tk1+3*a13*tk1^2=qpk1:Rozwiązanie.> ROZW1:=solve({R11,R12,R13,R14},{a10,a11,a12,a13});

:= ROZW1 { }, , ,a10 0 a11 0 a13

2000 a123 400

> R21:=a20+a21*t20+a22*t20^2+a23*t20^3=q20:> R22:=a21+2*a22*t20+3*a23*t20^2=qp20:> R23:=a20+a21*tk2+a22*tk2^2+a23*tk2^3=qk2:> R24:=a21+2*a22*tk2+3*a23*tk2^2=qpk2:> > ROZW2:=solve({R21,R22,R23,R24},{a20,a21,a22,a23});

:= ROZW2 { }, , ,a206 a21 0 a23

3000 a22

200

> R31:=a30+a31*t30+a32*t30^2+a33*t30^3=q30:> R32:=a31+2*a32*t30+3*a33*t30^2=qp30:> R33:=a30+a31*tk3+a32*tk3^2+a33*tk3^3=qk3:> R34:=a31+2*a32*tk3+3*a33*tk3^2=qpk3:> > ROZW3:=solve({R31,R32,R33,R34},{a30,a31,a32,a33});

:= ROZW3 { }, , ,a30 6 a31 0 a33

3000 a32

200

>

10.3. Dynamika i sterowanie.

Kod źródłowy programu Dynamika i Sterowanie.mws

> restart:> with(linalg):> with(LinearAlgebra):> with(plots):Warning, the protected names norm and trace have been redefined and unprotected

Warning, the assigned name GramSchmidt now has a global binding

Warning, the name changecoords has been redefined

Dane numeryczne.

Geometria> #e[0]:=0.02: l[1]:=0.35: l[2]:=0.22: l[3]:=0.22: g:=9.81:Masy i tensory bezwladnosci.> #m[1]:=5: m[2]:=2.5: m[3]:=3.5: Ix2:=0: Iy2:=0.01: Iz2:=0.01: Ix3:=0: Iy3:=0.02: Iz3:=0.02:> Ixx[1]:=Ix1: Iyy[1]:=Iy1: Izz[1]:=Iz1: Ixy[1]:=0: Ixz[1]:=0: Iyz[1]:=0:> Ixx[2]:=Ix2: Iyy[2]:=Iy2: Izz[2]:=Iz2: Ixy[2]:=0: Ixz[2]:=0: Iyz[2]:=0:> Ixx[3]:=Ix3: Iyy[3]:=Iy3: Izz[3]:=Iz3: Ixy[3]:=0: Ixz[3]:=0: Iyz[3]:=0:Wymuszenie.> #u[1]:=exp(-2*t)*sin(t): u[2]:=sin(4*t): u[3]:=0.2*cos(0.3*t):Wspólczynniki tarcia statycznego.> #Ws[1]:=3: Ws[2]:=6: Ws[3]:=5:Wspólczynniki tarcia dynamicznego> #Wd[1]:=8: Wd[2]:=12: Wd[3]:=10:CZĘŚĆ 1.Wyprowadzenie energii kinetycznej i potencjalnej dla zadanego manipulatora.DANE WEJŚCIOWE: wspólrzędne kartezjańskie środka masy bryly N wyrażone w ukadzie podstawy (globalnym) i rzyty wypadkowej prędkości kątowej na osie ukadu związanego z czlonem; obie wartości wyrażone przez wspórzędne i/lub prędkości uogólnione; Macierz tensora bezwladności bryly N wyrażona w ukadzie związanym z czlonem n-tym.Bryla nr. 1.Wspólrzędne środka masy w globalnym ukladzie wspólrzędnych.> x[S1]:=0:> y[S1]:=0:> z[S1]:=l[1]/2:Rzuty wektora prędkości kątowej w ukladzie związanym z czlonem pierwszym.> omega[x1]:=0:> omega[y1]:=0:> omega[z1]:=diff(theta[1](t),t):Tensor bezwladności w ukladzie zwiazanym z czlonem pierwszym.> I1:=Matrix(1..3,1..3,[[Ixx[1],-Ixy[1],-Ixz[1]],[-Ixy[1],Iyy[1],-Iyz[1]],[-Ixz[1],-Iyz[1],Izz[1]]]):Bryla nr. 2.Wspólrzędne środka masy w globalnym ukladzie wspólrzędnych.> x[S2]:=cos(theta[1](t))*(e[0]+l[2]/2*cos(theta[2](t))):> y[S2]:=sin(theta[1](t))*(e[0]+l[2]/2*cos(theta[2](t))):> z[S2]:=l[1]+l[2]/2*sin(theta[2](t)):Rzuty wektora prędkości kątowej w ukladzie związanym z czlonem drugim.> omega[x2]:=diff(theta[1](t),t)*sin(theta[2](t)):> omega[y2]:=diff(theta[1](t),t)*cos(theta[2](t)):> omega[z2]:=diff(theta[2](t),t):Tensor bezwladności w ukladzie związanym z czlonem drugim.> I2:=Matrix(1..3,1..3,[[Ixx[2],-Ixy[2],-Ixz[2]],[-Ixy[2],Iyy[2],-Iyz[2]],[-Ixz[2],-Iyz[2],Izz[2]]]):Bryla nr. 3.Wspólrzędne środka masy w globalnym ukladzie wspólrzędnych.

> x[S3]:=cos(theta[1](t))*(e[0]+l[2]*cos(theta[2](t))+l[3]/2*cos(theta[3](t))):> y[S3]:=sin(theta[1](t))*(e[0]+l[2]*cos(theta[2](t))+l[3]/2*cos(theta[3](t))):> z[S3]:=l[1]+l[2]*sin(theta[2](t))+l[3]/2*sin(theta[3](t)):Rzuty wektora prędkości kątowej w ukladzie związanym z czlonemtrzecim.> omega[x3]:=diff(theta[1](t),t)*sin(theta[3](t)):> omega[y3]:=diff(theta[1](t),t)*cos(theta[3](t)):> omega[z3]:=diff(theta[2](t),t)+diff(theta[3](t),t):Tensor bezwladności w ukladzie związanym z czlonem trzecim.> I3:=Matrix(1..3,1..3,[[Ixx[3],-Ixy[3],-Ixz[3]],[-Ixy[3],Iyy[3],-Iyz[3]],[-Ixz[3],-Iyz[3],Izz[3]]]):Zależności pośrednie.Wektor przyśpieszenia ziemskiego.> gg:=<0,0,g>:Wektory wspólrzędnych środków mas w globalnym ukladzie wspólrzędnych.> R1:=<x[S1]|y[S1]|z[S1]>:> R2:=<x[S2]|y[S2]|z[S2]>:> R3:=<x[S3]|y[S3]|z[S3]>:> R1T:=Transpose(R1):> R2T:=Transpose(R2):> R3T:=Transpose(R3):Wektory rzutów prędkości środków mas na osie globalnego ukadu wspólrzędnych.> V1:=map(diff,R1,t):> V2:=map(diff,R2,t):> V3:=map(diff,R3,t):> V1T:=Transpose(V1):> V2T:=Transpose(V2):> V3T:=Transpose(V3):Wektory prędkości kątowej w ukladach związanych z czlonami.> W1:=<omega[x1]|omega[y1]|omega[z1]>:> W2:=<omega[x2]|omega[y2]|omega[z2]>:> W3:=<omega[x3]|omega[y3]|omega[z3]>:> W1T:=Transpose(W1):> W2T:=Transpose(W2):> W3T:=Transpose(W3):Energia kinetyczna bryl.> Eb[1]:=1/2*m[1]*evalm(V1&*V1T)+1/2*evalm(W1&*I1&*W1T):> Eb[2]:=1/2*m[2]*evalm(V2&*V2T)+1/2*evalm(W2&*I2&*W2T):> Eb[3]:=1/2*m[3]*evalm(V3&*V3T)+1/2*evalm(W3&*I3&*W3T):Energia potencjalna bryl.> Vb[1]:=m[1]*evalm(gg&*R1T);

:= Vb1

12 m1 g l1

> Vb[2]:=m[2]*evalm(gg&*R2T); := Vb2 m2 g

l1

12 l2 ( )sin ( )2 t

> Vb[3]:=m[3]*evalm(gg&*R3T);

:= Vb3 m3 g

l1 l2 ( )sin ( )2 t

12 l3 ( )sin ( )3 t

CZĘŚĆ 2.Wyprowadzenie różniczkowych równań ruchu. Przeksztacenia ogólne dla N stopni swobody.DANE WEJŚCIOWE: Stopnie swobody (N), energia kinetyczna (Eb[N]) i energia potencjalna (Vb[N]) bryl od 1 do N.> N:=3:> E:=vector(N):> for i from 1 to N do> E[i]:=Eb[i]:> end do:> E:=evalm(E):> V:=vector(N):> for i from 1 to N do> V[i]:=Vb[i]:> end do:> V:=evalm(V):Równania pomocnicze.> P:=Vector(N,1):> q:=<theta[1]|theta[2]|theta[3]>:> qp:=<omega[1]|omega[2]|omega[3]>:> qpp:=<epsilon[1]|epsilon[2]|epsilon[3]>:> qt:=<theta[1](t)|theta[2](t)|theta[3](t)>:> qpt:=map(diff,qt,t):> qppt:=map(diff,qpt,t):Podstawienia.> podst1:=qppt[1]=qpp[1], qppt[2]=qpp[2], qppt[3]=qpp[3], qpt[1]=qp[1], qpt[2]=qp[2], qpt[3]=qp[3], qt[1]=q[1], qt[2]=q[2], qt[3]=q[3]:> podst2:=qpp[1]=qppt[1], qpp[2]=qppt[2], qpp[3]=qppt[3], qp[1]=qpt[1], qp[2]=qpt[2], qp[3]=qpt[3], q[1]=qt[1], q[2]=qt[2], q[3]=qt[3]:> EE:=evalm(E&*P):> VV:=evalm(V&*P);

:= VV 12 m1 g l1 m2 g

l1

12 l2 ( )sin ( )2 t m3 g

l1 l2 ( )sin ( )2 t

12 l3 ( )sin ( )3 t

Funkcja Lagrange'a.> Lt:=simplify(evalm(EE-VV)):Pochodne funkcji Lagrange'a po wspólrzędnych uogólnionych.> L:=subs({podst1},Lt):> for i from 1 to N do> rq[i]:=simplify(diff(L,q[i])):> end do;

:= rq1 0

rq2

12 m3 l2 ( )cos 2 2 l3 ( )sin 3 3 m3 1

2e0 l2 ( )sin 2 :=

12 m3 l2 ( )sin 2 2 l3 ( )cos 3 3

12 m3 1

2l2 ( )sin 2 l3 ( )cos 3

12 m2 1

2e0 l2 ( )sin 2

12 m2 g l2 ( )cos 2 m3 g l2 ( )cos 2

14 m2 1

2l2

2( )cos 2 ( )sin 2 m3 1

2l2

2( )cos 2 ( )sin 2 1

2( )cos 2 Iy2 ( )sin 2

1

2Ix2 ( )cos 2 ( )sin 2

rq312 m3 l2 ( )sin 2 2 l3 ( )cos 3 3

12 m3 l2 ( )cos 2 2 l3 ( )sin 3 3 :=

12 m3 1

2e0 l3 ( )sin 3

12 m3 1

2l2 ( )cos 2 l3 ( )sin 3

12 m3 g l3 ( )cos 3

14 m3 1

2l3

2( )cos 3 ( )sin 3 1

2Ix3 ( )cos 3 ( )sin 3 1

2( )cos 3 Iy3 ( )sin 3

Pochodne funkcji Lagrange'a po prędkościach uogólnionych.> for i from 1 to N do> rqp[i]:=simplify(diff(L,qp[i])):> end do:Powrót do zapisu od czasu.> for i from 1 to N do> rqt[i]:=subs({podst2},rq[i]);> end do:> for i from 1 to N do> rqpt[i]:=subs({podst2},rqp[i]);> end do:Wektor oporów ruchu> MT:=Vector(N):> for i from 1 to N do> MT[i]:=Wd[i]*omega[i]^3+Ws[i]*tanh(50*omega[i]);> end do:> MT:=MT;

:= MT

Wd1 1

3Ws1 ( )tanh 50 1

Wd2 2

3Ws2 ( )tanh 50 2

Wd3 3

3Ws3 ( )tanh 50 3

Równania różniczkowe.> for i from 1 to N do> Rt[i]:=simplify(diff(rqpt[i],t)-rqt[i]):> Rr[i]:=subs(podst1,Rt[i])=MS[i]-MT[i]:> end do:DYNAMIKA ODWROTNA.> simplify(Rr[1],'size'); simplify(Rr[2],'size'); simplify(Rr[3],'size');

1

14 m2 l2

2m3 l2

2Iy2 Ix2 ( )cos 2

2 14 4 1 m3 l2 l3 ( )cos 3

8 1

14 m2 l2

2m3 l2

2Iy2 Ix2 2 ( )sin 2

8 l2

12 1 m3 l3 ( )sin 3 3

12 m2 m3 e0 1

( )cos 2

14 l3

2m3 Iy3 Ix3 1 ( )cos 3

2 14

4 1 m3 l2 ( )sin 2 2 l3 8

14 l3

2m3 Iy3 Ix3 1 3 ( )sin 3 4 1 m3 e0 l3

( )cos 3 2 l2

12 m2 m3 e0 1 2 ( )sin 2 1 m3 e0 l3 ( )sin 3 3

1 ( ) Iz1 Ix3 Ix2 m2 e0

2m3 e0

2 MS1 Wd1 1

3Ws1 ( )tanh 50 1

14 4 1

2

14 m2 m3 l2

2Iy2 Ix2 ( )cos 2

2 l2 ( ) m3 l3 ( )3

21

2( )cos 3 m3 l3 ( )sin 3 3 1

2e0 ( )m2 2 m3

( )sin 2

12 ( ) 3 ( )cos 3 l3 m3 3

2( )sin 3 l3 m3 g ( )m2 2 m3 l2 ( )cos 2

14 2 ( )m2 4 m3 l2

2 14 ( )4 Iz2 4 Iz3 2 Iz3 3

MS2 Wd2 2

3Ws2 ( )tanh 50 2

14 4

14 l3

2m3 Iy3 Ix3 1

2( )cos 3

2 ( ) l2 ( )2

21

2( )cos 2 2 ( )sin 2 l2 1

2e0 m3 l3

( )sin 3

12 m3 l3 ( ) l2 ( )sin 2 2

2l2 ( )cos 2 2 g ( )cos 3

14 m3 l3

23 Iz3 ( )2 3

MS3 Wd3 3

3Ws3 ( )tanh 50 3

>

CZĘŚĆ 3.Przeksztacenie różniczkowych równań ruchu w zapis macierzowy.Wektor prędkości uogólnionej.> VT:=Vector(N):> for i from 1 to N do> VT[i]:=qp[i];> end do:> VT:=VT;

:= VT

1

2

3

Wektor przyśpieszenia uogólnionego.> QT:=Vector(N):> for i from 1 to N do> QT[i]:=qpp[i]:> end do:> QT:=QT;

:= QT

1

2

3

Wektor wymuszenia.> U:=Vector(N):> for i from 1 to N do> U[i]:=MS[i];> end do:> U:=U;

:= U

MS1

MS2

MS3

Wektor oporów ruchu.> T:=Vector(N):> for i from 1 to N do> T[i]:=MT[i];> end do:> T:=T;

:= T

Wd1 1

3Ws1 ( )tanh 50 1

Wd2 2

3Ws2 ( )tanh 50 2

Wd3 3

3Ws3 ( )tanh 50 3

Macierz bezwadności.> for i from 1 to N do> R[i]:=subs(podst1,Rt[i]):> end do:> M:=array(1..N,1..N):> for i from 1 to N do> for j from 1 to N do> M[i,j]:=coeff(R[i],qpp[j]);> end do;> end do;> M:=evalm(M);

M :=

Iz114 m2 l2

2( )cos 2

2m3 l2

2( )cos 2

2 14 m3 l3

2( )cos 3

2Ix2 Ix3 m3 e0

2

m2 e0

22 m3 e0 l2 ( )cos 2 m2 e0 l2 ( )cos 2 m3 l2 ( )cos 2 l3 ( )cos 3

m3 e0 l3 ( )cos 3 ( )cos 2

2Iy2 Ix2 ( )cos 2

2Ix3 ( )cos 3

2( )cos 3

2Iy3 0 0, ,

0 Iz2 Iz314 m2 l2

2m3 l2

2 , ,

12 m3 l2 ( )sin 2 l3 ( )sin 3

12 m3 l2 ( )cos 2 l3 ( )cos 3 Iz3

, ,

0

12 m3 l2 ( )sin 2 l3 ( )sin 3

12 m3 l2 ( )cos 2 l3 ( )cos 3 Iz3

14 l3

2m3 Iz3

> M1:=inverse(M):Macierz sil Coriolis'a i sil odśrodkowych.> C:=array(1..N,1..N):> for k from 1 to N do> for j from 1 to N do> C[k,j]:=0:> for i from 1 to N do> C[k,j]:=C[k,j]+1/2*(diff(M[k,j],q[i])+diff(M[k,i],q[j])-diff(M[i,j],q[k]))*qp[i]:> end do:> end do:> end do:> C:=evalm(C);C :=

12

12 m2 l2

2( )cos 2 ( )sin 2 2 m3 l2

2( )cos 2 ( )sin 2 2 m3 e0 l2 ( )sin 2

m2 e0 l2 ( )sin 2 m3 l2 ( )sin 2 l3 ( )cos 3 2 ( )cos 2 Iy2 ( )sin 2

2 Ix2 ( )cos 2 ( )sin 2 2

12

12 m3 l3

2( )cos 3 ( )sin 3 m3 l2 ( )cos 2 l3 ( )sin 3

m3 e0 l3 ( )sin 3 2 Ix3 ( )cos 3 ( )sin 3 2 ( )cos 3 Iy3 ( )sin 3 3

12

,

12 m2 l2

2( )cos 2 ( )sin 2 2 m3 l2

2( )cos 2 ( )sin 2 2 m3 e0 l2 ( )sin 2


2 Ix2 ( )cos 2 ( )sin 2 1

12

12 m3 l3

2( )cos 3 ( )sin 3 m3 l2 ( )cos 2 l3 ( )sin 3

,

m3 e0 l3 ( )sin 3 2 Ix3 ( )cos 3 ( )sin 3 2 ( )cos 3 Iy3 ( )sin 3 1

12

12 m2 l2

2( )cos 2 ( )sin 2 2 m3 l2

2( )cos 2 ( )sin 2 2 m3 e0 l2 ( )sin 2


2 Ix2 ( )cos 2 ( )sin 2 1 0, ,

12 ( )m3 l2 ( )sin 2 l3 ( )cos 3 m3 l2 ( )cos 2 l3 ( )sin 3 3

12

12 m3 l3

2( )cos 3 ( )sin 3 m3 l2 ( )cos 2 l3 ( )sin 3 m3 e0 l3 ( )sin 3

2 Ix3 ( )cos 3 ( )sin 3 2 ( )cos 3 Iy3 ( )sin 3 1 ,

12 ( )m3 l2 ( )cos 2 l3 ( )sin 3 m3 l2 ( )sin 2 l3 ( )cos 3 2 0,

Wektor sil grawitacji.> G:=Vector(1..N):> VV1:=subs(podst1,VV):> G[1]:=0:> for i from 1 to N do> G[i]:=diff(VV1,q[i]):> end do:> G:=G;

:= G

0

12 m2 g l2 ( )cos 2 m3 g l2 ( )cos 2

12 m3 g l3 ( )cos 3

Równanie macierzowe do dynamiki prostej.> E:=evalm(M1&*(U-T-C&*VT-G)):> for i from 1 to N do> RR[i]:=QT[i]=E[i]:> end do:DYNAMIKA PROSTA.> simplify(RR[1],'size'); RR[2]; RR[3];1 4 Ws1 ( )tanh 50 1

4

2

Iy2 Ix2

14 m2 l2

2m3 l2

22 ( )sin 2 m3 l2 l3 ( )sin 3 3 1 ( )cos 2

8

12 l3 2 ( )sin 2 l2 m3

Iy3

14 l3

2m3 Ix3 ( )sin 3 3 1 ( )cos 3

8 l2 e0

m3

12 m2 1 2 ( )sin 2 4 MS1 4 1 m3 e0 l3 ( )sin 3 3 4 Wd1 1

3

( ) 4 Iy2 4 Ix2 m2 l2

24 m3 l2

2( )cos 2

2

8 l2

12 m3 l3 ( )cos 3 e0

m3

12 m2 ( )cos 2 ( ) 4 Iy3 l3

2m3 4 Ix3 ( )cos 3

2

4 m3 e0 l3 ( )cos 3 4 Ix2 4 Ix3 4 m2 e0

24 m3 e0

24 Iz1

2 4 ( )l3

2m3 4 Iz3

12

12 m2 l2

2( )cos 2 ( )sin 2 2 m3 l2

2( )cos 2 ( )sin 2

2 m3 e0 l2 ( )sin 2 m2 e0 l2 ( )sin 2 m3 l2 ( )sin 2 l3 ( )cos 3

2 ( )cos 2 Iy2 ( )sin 2 2 Ix2 ( )cos 2 ( )sin 2 1

2


2MS2 Wd2 2

3

Ws2 ( )tanh 50 212 m2 g l2 ( )cos 2 m3 g l2 ( )cos 2

4 Iz2 l3

2m3(

16 Iz2 Iz3 4 Iz3 l3

2m3 m2 l2

2l3

2m3 4 m2 l2

2Iz3 4 m3

2l2

2l3

216 m3 l2

2Iz3

4 m3

2l2

2( )sin 2

2l3

2( )sin 3

28 m3

2l2

2( )sin 2 l3

2( )sin 3 ( )cos 2 ( )cos 3

16 m3 l2 ( )sin 2 l3 ( )sin 3 Iz3 4 m3

2l2

2( )cos 2

2l3

2( )cos 3

2

16 m3 l2 ( )cos 2 l3 ( )cos 3 Iz3 ) 8

( ) m3 l2 ( )sin 2 l3 ( )sin 3 m3 l2 ( )cos 2 l3 ( )cos 3 2 Iz312

12 m3 l3


2 Ix3 ( )cos 3 ( )sin 3 2 ( )cos 3 Iy3 ( )sin 3 1

2

12 ( )m3 l2 ( )cos 2 l3 ( )sin 3 m3 l2 ( )sin 2 l3 ( )cos 3 2

2MS3 Wd3 3

3

Ws3 ( )tanh 50 312 m3 g l3 ( )cos 3

4 Iz2 l3

2m3 16 Iz2 Iz3 4 Iz3 l3

2m3 (

m2 l2

2l3

2m3 4 m2 l2

2Iz3 4 m3

2l2

2l3

216 m3 l2

2Iz3

4 m3

2l2

2( )sin 2

2l3

2( )sin 3

28 m3

2l2

2( )sin 2 l3

2( )sin 3 ( )cos 2 ( )cos 3

16 m3 l2 ( )sin 2 l3 ( )sin 3 Iz3 4 m3

2l2

2( )cos 2

2l3

2( )cos 3

2

16 m3 l2 ( )cos 2 l3 ( )cos 3 Iz3 )

3 8 ( ) m3 l2 ( )sin 2 l3 ( )sin 3 m3 l2 ( )cos 2 l3 ( )cos 3 2 Iz312

12 m2 l2

2( )cos 2 ( )sin 2 2 m3 l2

2( )cos 2 ( )sin 2 2 m3 e0 l2 ( )sin 2


2 Ix2 ( )cos 2 ( )sin 2 1

2


2MS2 Wd2 2

3

Ws2 ( )tanh 50 2

12 m2 g l2 ( )cos 2 m3 g l2 ( )cos 2

4 Iz2 l3

2m3(

16 Iz2 Iz3 4 Iz3 l3

2m3 m2 l2

2l3

2m3 4 m2 l2

2Iz3 4 m3

2l2

2l3

216 m3 l2

2Iz3

4 m3

2l2

2( )sin 2

2l3

2( )sin 3

28 m3

2l2

2( )sin 2 l3

2( )sin 3 ( )cos 2 ( )cos 3

16 m3 l2 ( )sin 2 l3 ( )sin 3 Iz3 4 m3

2l2

2( )cos 2

2l3

2( )cos 3

2

16 m3 l2 ( )cos 2 l3 ( )cos 3 Iz3 ) 4 ( ) 4 Iz2 4 Iz3 m2 l2

24 m3 l2

2 12

12 m3 l3


2 Ix3 ( )cos 3 ( )sin 3 2 ( )cos 3 Iy3 ( )sin 3 1

2

12 ( )m3 l2 ( )cos 2 l3 ( )sin 3 m3 l2 ( )sin 2 l3 ( )cos 3 2

2MS3 Wd3 3

3

Ws3 ( )tanh 50 312 m3 g l3 ( )cos 3

4 Iz2 l3

2m3 16 Iz2 Iz3 4 Iz3 l3

2m3 (

m2 l2

2l3

2m3 4 m2 l2

2Iz3 4 m3

2l2

2l3

216 m3 l2

2Iz3

4 m3

2l2

2( )sin 2

2l3

2( )sin 3

28 m3

2l2

2( )sin 2 l3

2( )sin 3 ( )cos 2 ( )cos 3

16 m3 l2 ( )sin 2 l3 ( )sin 3 Iz3 4 m3

2l2

2( )cos 2

2l3

2( )cos 3

2

16 m3 l2 ( )cos 2 l3 ( )cos 3 Iz3 )

Pomocniczy wektor do sterowania i jego pochodna.> Vv:=Vector(N):> for i from 1 to N do> Vv[i]:=v[i];> end do:> Vv:=Vv:> Vvp:=Vector(N):> for i from 1 to N do> Vvp[i]:=vp[i]:> end do:> Vvp:=Vvp:>

CZĘŚĆ 4.STEROWANIE ADAPTACYJNE.Wyznaczenie macierzy sterowania adaptacyjnego i wektora parametrów adaptowanych (regulator PD osobno).> AD:=simplify(evalm(M&*Vvp+C&*Vv+G+T),'size');

AD Ws1 ( )tanh 50 1 vp1

14 m2 l2

2m3 l2

2Iy2 Ix2 ( )cos 2

2 14

:=

4

14 m2 l2

2m3 l2

2Iy2 Ix2 ( )v1 2 1 v2 ( )sin 2

8 l2

12 vp1 m3 l3 ( )cos 3

14 m3 l3 ( )1 v3 3 v1 ( )sin 3 vp1

m3

12 m2 e0

( )cos 2 l2 ( )v1 2 1 v2

12 m3 l3 ( )cos 3 e0

m3

12 m2 ( )sin 2

vp1

Iy3

14 l3

2m3 Ix3 ( )cos 3

2

14

4 ( )1 v3 3 v1

Iy3

14 l3

2m3 Ix3 ( )sin 3 4 vp1 m3 e0 l3 ( )cos 3

12 e0 m3 l3 ( )1 v3 3 v1 ( )sin 3 vp1 m3 e0

2

14 ( ) 4 Ix2 4 Ix3 4 m2 e0

24 Iz1 vp1 Wd1 1

3 Ws2 ( )tanh 50 2

14

,

4

14 m2 m3 l2

2Iy2 Ix2 1 v1 ( )cos 2

2 l2

l3 m3 ( )1 v1 3 v3 ( )cos 3 ( )sin 3 l3 m3 vp3 2

m3

12 m2 1 e0 v1

( )sin 2

12

vp3 m3 l3 ( )cos 3 3 v3 m3 l3 ( )sin 3 2

m3

12 m2 g l2 ( )cos 2

vp2

14 m2 m3 l2

2 14 ( )4 Iz2 4 Iz3 vp2 Wd2 2

3vp3 Iz3 Ws3 ( )tanh 50 3

14,

4 v1

Iy3

14 l3

2m3 Ix3 1 ( )cos 3

2 m3 l3 ( ) l2 ( )1 v1 2 v2 ( )cos 2 ( )sin 2 l2 vp2 1 v1 e0 ( )sin 3

12 m3 l3 ( ) vp2 l2 ( )cos 2 2 v2 l2 ( )sin 2 g ( )cos 3 Wd3 3

3vp2 Iz3

14 vp3 l3

2m3 vp3 Iz3

Wektor parametrów adaptowanych p^.> p:=<Iz1,m[2],m[3],Wd[1],Wd[2],Wd[3],1>;

:= p

Iz1m2

m3

Wd1

Wd2

Wd3

1

> pp:=<Iz1,m[2],m[3],Wd[1],Wd[2],Wd[3]>:> d:=Dimension(p):

Macierz sterowania Y(q,q',v,v') .> Yy:=array(1..N,1..d-1):> for i from 1 to N do> for j from 1 to d-1 do> Yy[i,j]:=coeff(AD[i],p[j]):> end do:> end do:> Yy:=evalm(Yy):> AD1:=evalm(Yy&*pp):> Ww:=evalm(AD-AD1):> Y:=array(1..N,1..d):> for i from 1 to N do> for j from 1 to d-1 do> Y[i,j]:=Yy[i,j]:> end do:> end do:> for i from 1 to N do> Y[i,d]:=Ww[i]:> end do:> Y:=simplify(evalm(Y),'size');Y :=

vp1

12 ( )cos 2 l2 e0

12 ( )cos 2 l2 vp1

12 l2 ( )v1 2 1 v2 ( )sin 2 vp1 e0

12

, ,

( ) 2 ( )cos 2 l2 2 e0 l3 ( )cos 3 ( )cos 2 l2 vp112 vp1 l3 ( )cos 3

12 l3 ( )1 v3 3 v1 ( )sin 3 l2 ( )v1 2 1 v2 ( )sin 2 vp1 e0

1

30 0, , , ,

Ws1 ( )tanh 50 1 vp1 ( ) Iy2 Ix2 ( )cos 2

2

( )sin 2 ( ) Iy2 Ix2 ( )v1 2 1 v2 ( )cos 2 vp1 ( ) Ix3 Iy3 ( )cos 3

2

( )sin 3 ( )1 v3 3 v1 ( ) Ix3 Iy3 ( )cos 3 vp1 ( )Ix3 Ix2

014 l2 ( ) ( )( )sin 2 l2 1 v1 2 g ( )cos 2 2 ( )sin 2 1 v1 e0 vp2 l2

12 (

, ,

( ) 2 l2 1 v1 ( )cos 2 l3 ( )1 v1 3 v3 ( )cos 3 2 1 v1 e0 ( )sin 3 l3 vp3 ( )sin 2

( ) vp3 l3 ( )cos 3 3 v3 l3 ( )sin 3 2 g ( )cos 2 2 vp2 l2 ) l2 0 2

30, , , ,

Ws2 ( )tanh 50 2 1 v1 ( )sin 2 ( )Iy2 Ix2 ( )cos 2 ( )Iz3 Iz2 vp2 vp3 Iz3

0 012

, ,

12 l3 ( )cos 3 1 v1 l2 ( )1 v1 2 v2 ( )cos 2 1 v1 e0 ( )sin 2 l2 vp2 ( )sin 3

( ) vp2 l2 ( )cos 2 2 v2 l2 ( )sin 2 g ( )cos 312 vp3 l3

l3 0 0 3

3, , , ,

Ws3 ( )tanh 50 3 v1 1 ( )sin 3 ( ) Ix3 Iy3 ( )cos 3 Iz3 ( )vp3 vp2

>

Autor:

E-mail

mailto:[email protected]

STEROWANIE RUCHEM MANIPULATORA magisterska - praca... · Web viewPrzy rozwiązywaniu tych...

Documents

Transcript of STEROWANIE RUCHEM MANIPULATORA magisterska - praca... · Web viewPrzy rozwiązywaniu tych...