Spis treści.

1. Wprowadzenie.

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 maszynowym
do 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 niebezpiecznych
dla 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ści
i 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.

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, jak
i 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. Brak
jest 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 regulator
PD / 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ęzycznej
i 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.

3. Cel i zakres pracy.

Niniejsza praca stanowi próbę sformułowania problemu mechaniki ruchu
dla 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 kinematyki
i 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, jak
i 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^®.

2. Podstawowe pojęcia.

1. Podstawowe pojęcia, struktury
   i systematyzacja manipulatorów.

Manipulator - urządzenie techniczne przeznaczone do realizacji niektórych funkcji: manipulacyjnych (gr. manus - ręka) wykonywanych przez chwytak
i 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

(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 metali
i 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 rehabilitacyjne
i 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ów
i 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 roboczej
i 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 konstrukcji
i 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żenie
przy 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. 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 podane
na 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ć zastosowane
do 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 punktu
do 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. Stwarza
to 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ów
w 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,

K_P, K_d - 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 zawartym
w 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ść modelu
za 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ącego
się 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 zapoznania
się 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 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 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 sterowania
w warunkach niepewności coraz większym zainteresowaniem cieszą się techniki stosowane
w 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 opisowymi
i 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 x₁ jest małe I x₂ 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.

3. Kinematyka i dynamika manipulatora.

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 dane
są zmienne konfiguracyjne (jak np. kąty obrotu ramion, przemieszczenia liniowe członów
i 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 manipulatora
w 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.

1. Wyprowadzenie zależności kinematycznych.

Obecnie w literaturze często spotyka się rozwiązanie kinematyki prostej
lub 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].

2. Zadanie proste i odwrotne kinematyki.

Rozwiązanie zadania prostego kinematyki otrzymujemy bezpośrednio
z rozwiązania równań od (3.1) do (3.9) znając wartości kątów obrotu, prędkości
i przyspieszeń kątowych. Na rysunku 3.5 pokazano otrzymany tor ruchu punktu D
dla 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)).

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 znane
są 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

(3.13)

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 punktu
we 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.

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. 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 obiektu
z 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)

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 t_k=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 D
w postaci.

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

Możemy zauważyć, że pomimo zdefiniowania punktów początkowych
i 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 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 obrotu
i 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ą wraz
z manipulatorem i nie jest możliwa jakakolwiek ingerencja osób trzecich.

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.

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 merytorycznych
i zgadza się z równaniami analitycznymi, co tym samym umożliwia wykorzystanie
go w dalszych pracach symulacyjnych.

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 dana
jest trajektoria ruchu manipulatora, np. w postaci zmian współrzędnych konfiguracyjnych
i ich pochodnych jako funkcji czasu, a trzeba wyznaczyć wektor sił i momentów napędowych M=[M₁, ..., M_n]^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'a
jak 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.

1. Przyjęcie metody rozwiązania.

Dla bryły sztywnej, wykonującej ruch obrotowy wokół stałej osi, określa
się moment bezwładności. Natomiast dla bryły wykonującej ruch kulisty, wprowadza
się 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ązanego
z bryłą i wyraża w postaci macierzy o wymiarach 3×3

(3.21)

gdzie:

Ponadto przyjmuje się, że ciało jest złożone z różniczkowych elementów
o objętości dv i gęstości ρ. Pozycję każdego elementu określa wektor ρ=[x y z]^T. Elementy I_xx, I_yy i I_zz 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 (I_xy, I_yz, I_xz)
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ęcie
tak układów współrzędnych, aby osie były głównymi osiami bezwładności.

Niech q₁,..., q_n będą współrzędnymi uogólnionymi, które w pełni określają położenie układu, natomiast E_k i E_p 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:

Q_i - uogólniona siła odpowiadająca uogólnionemu przemieszczeniu q_i. Siła ta może być określona metodą prac przygotowanych, które wykonują siły czynne działające
na układ.

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

(3.24)

gdzie:

m_i - masa członu i-tego,

I_Si - macierz tensora bezwładności,

ω_i - wypadkowa prędkość kątowa członu i-tego,

v_Si - wypadkowa prędkość liniowa środka masy członu i-tego,

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

(3.25)

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

(3.26)

gdzie:

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

Do sił uogólnionych Q_i 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=[M₁, ..., M_n]^T; jak również opory ruchu F_z=[F₁, ..., F_n]^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).

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 mas
i 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. Dla bryły drugiej.

3. Dla bryły trzeciej.

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.

Równanie różniczkowe 2.

Równanie różniczkowe 3.

gdzie:

,

m_i, Iz_i, Iy_i , Iz_i - masy i momenty bezwładności poszczególnych członów,

l_i, e₀ - geometria manipulatora,

g - wektor przyspieszenia ziemskiego,

MS_i - 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)

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

Wektor sił grawitacji

Wektor oporów ruchu

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.

1. 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 analiza
z 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.

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łównie
z 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 ramienia

zadany i otrzymany. zadany i otrzymany.

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

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.

Maple Simulink

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.

4. Konwencjonalne sterowanie ruchem manipulatora.

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ących
w 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:

K_p, K_d - 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 urojonych
na 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ąc
ten 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].

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=0

end_time=30

dt=0.001

Dla manipulatora przyjęto współczynniki

********** KINEMATYKA **********

%Warunki początkowe

teta1=pi/4; teta2=pi/6; teta3=-pi/6;

omega1=0; omega2=0; omega3=0;

%Długości i inne dane

l1=0.35; l2=0.22; l3=0.22; e0=0.02;

g=9.81; z=0; zt=z*0.2;

%********** DYNAMIKA **********

%Masy

m2=2.5;

m3=3.5; m3z=m3+z;

%Momenty bezwładności

Ix1=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 dynamicznego

T1d=6; T1dz=T1d+zt;

T2d=12; T2dz=T2d+zt;

T3d=10; T3dz=T3d+zt;

%Współczynniki tarcia statycznego

T1s=5; T2s=10; T3s=9;

%********** STEROWANIE Z REG. PD **********

%Współczynniki regulatora PD

Kp1=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 zadane
i 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).

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.

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ściami
i przyśpieszeniami. Jeżeli okazałoby się, że uzyskanie tak dużych momentów
jest 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.

Rys. 4.9. Błąd kąta obrotu podstawy. Rys. 4.10. Błąd kąta obrotu ramienia.

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

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ży
w tym przypadku zwiększyć współczynniki wzmocnień regulatora PD zgodnie
z 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.

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.

Rys. 4.13. Błąd kąta obrotu podstawy. Rys. 4.14. Błąd kąta obrotu ramienia.

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.

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

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.

Rys. 4.17. Błąd kąta obrotu podstawy. Rys. 4.18. Błąd kąta obrotu ramienia.

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

Maksymalny błąd kąta obrotu wynosi zaledwie 0.001[rad]. Uzyskując taki
wyniki, 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, luzy
w 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! 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 zmianie
w 15[s] ruchu.

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

Rys. 4.21. Błąd kąta obrotu podstawy. Rys. 4.22. Błąd kąta obrotu ramienia.

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

Zauważalny wzrost błędów obrotu ramienia i łokcia w 15[s] spowodowany
jest 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].

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 siebie
i tak pozostają do zakończenia ruchu.

Rys. 4.24. Trajektoria ruchu chwytaka zadana i otrzymana.

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 algorytmu
w 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.

5. Adaptacyjny algorytm sterowania manipulatorem.

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żyta
w wyprowadzeniu algorytmu sterowania adaptacyjnego. Cechą tą jest liniowość względem parametrów. Rozumie się pod tym fakt, że mimo równania dynamiki manipulatorów
są 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 n×r macierzą znanych funkcji,

p - jest r×1 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ą. Zastosowanie
tej 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)

K_s - 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.

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

(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ład
od 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 Λ, K_s, i Γ. Elementy tych macierzy powinny być dobierane eksperymentalnie na obiekcie rzeczywistym z uwzględnieniem błędów pomiarowych.

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ń

Wektor parametrów adaptowanych

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 symulacji
jak i współczynniki manipulatora nie uległy zmianie. Dodano jedynie współczynniki wynikające ze zmiany układu sterowania.

%********** STEROWANIE ADAPTACYJNE **********

%Regulator PD

Ks1=8; Ks2=12; Ks3=10;

%Współczynniki błędu uogólnionego lambda

la1=2.5; la2=2.5; la3=2.5;

%Współczynniki adaptacji gamma

ga1=9e3; %Iz1

ga2=50; %m2

ga3=20; %m3

ga4=5e8; %Td1

ga5=3e6; %Td2

ga6=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.

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

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ścia
od 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.

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].

Rys. 5.7. Trajektoria ruchu chwytaka zadana i otrzymana.

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.

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 manipulatora
i 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 m₂ i m₃.

Rys. 5.11. Masa m₂ adaptowana i rzeczywista. Rys. 5.12. Masa m₃ 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.

Rys. 5.15. Błąd kąta obrotu podstawy. Rys. 5.16. Błąd kąta obrotu ramienia.

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.

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

Rys. 5.19. Trajektoria chwytaka zadana i otrzymana.

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ędowe
na silnikach, oraz udział sterowania od regulatora PD i algorytmu adaptacyjnego.

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

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.

Rys. 5.23. Masa m₂ adaptowana i rzeczywista. Rys. 5.24. Masa m₃ adaptowana i rzeczywista.

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 parametry
są 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. T₁d=10.8 do wartości T₁d=7,
przy czym jego rzeczywista wartość wynosi T₁d=12.

Rys. 5.25. Adaptowany i rzeczywisty współczynnik Rys. 5.26. Adaptowany i rzeczywisty współczynnik

tarcia 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.

Rys. 5.27. Błąd kąta obrotu podstawy. Rys. 5.28. Błąd kąta obrotu ramienia.

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

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ścia
do wartości 0.009[m].

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

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

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.

Rys. 5.32. Masa m₂ adaptowana i rzeczywista. Rys. 5.33. Masa m₃ adaptowana i rzeczywista.

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.

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 jak
i 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.

6. Badania eksperymentalne.

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 silnika
1	Podstawa	Obrót podstawy	1
2	Ramię	Podnoszenie / opuszczanie ramienia	2
3	Łokieć	Podnoszenie / opuszczanie łokcia	3
4	Chwytak	Przechylenie chwytaka	4+5
5	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ą wraz
z dokumentacją do manipulatora.

Struktura	Konfiguracja stawowa
Liczba stopni swobody	Pięć i chwytak
Zakres przemieszczeń w przegubach: Obrót podstawy Obrót ramienia Obrót łokcia Przechylenie chwytaka Pochylenie chwytaka	310º +130º / -35º ±130 ±130 Nieograniczony
Maksymalny zakres strefy roboczej	610[mm]
Chwytak	Serwomechanizm 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żenie	Silnik 1,2,3 - 127,7:1 Silnik 4,5 - 65,5:1 Silnik 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.

1. 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: PWM
i 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 komputerem
w celu nadążania z wypracowaniem sygnału sterowania w czasie rzeczywistym.

2. 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:

K_P1=500; K_D1=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:

K_P1=800; K_D1=400;

Otrzymano błędy jak na rysunkach 6.17 i 6.18.

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

K_P1=800; K_D1=400;

Otrzymano błędy jak na rysunkach 6.19 i 6.20.

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 przebiegi
dla przypadku trzeciego (rysunki 6.21 i 6.22).

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ści
z 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ówione
i 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.

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

K_P2=1500; K_D2=500;

Otrzymano błędy jak na rysunkach 6.23 i 6.24.

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:

K_P1=2000; K_D1=600;

Otrzymano błędy jak na rysunkach 6.25 i 6.26.

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

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.

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

K_P3=1800; K_D3=600;

Otrzymano błędy jak na rysunkach 6.29 i 6.30.

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

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.

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

(6.1)

Dla konkretnego przypadku jak wyżej po wyznaczeniu współczynników zgodnie
z 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.40
i 6.41.

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 zadanymi
i otrzymanymi dla podstawy, oraz pomiędzy prędkościami kątowymi zadanymi
i 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 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 otrzymany
z 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 zadana

i 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 otrzymany
z 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.

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

i 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ą.

Rys. 6.53. Trajektoria chwytaka zadana i otrzymana.

Znając trajektorię chwytaka możemy wyznaczyć maksymalny błąd odejścia
od 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ści
od 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).

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 samy
i 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”.

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.

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.

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)));

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

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

Wektor prędkości chwytaka.

> for i from 1 to 3 do

> Rtp[i]:=diff(Rt[i],t);

> end do;

> 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);

> 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;

>

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});

> 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});

> 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});

>

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);

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

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

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);

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;

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;

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');

>

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;

Wektor przyśpieszenia uogólnionego.

> QT:=Vector(N):

> for i from 1 to N do

> QT[i]:=qpp[i]:

> end do:

> QT:=QT;

Wektor wymuszenia.

> U:=Vector(N):

> for i from 1 to N do

> U[i]:=MS[i];

> end do:

> U:=U;

Wektor oporów ruchu.

> T:=Vector(N):

> for i from 1 to N do

> T[i]:=MT[i];

> end do:

> T:=T;

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);

> 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);

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;

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];

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');

Wektor parametrów adaptowanych p^.

> p:=<Iz1,m[2],m[3],Wd[1],Wd[2],Wd[3],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');

>

Autor:

E-mail

STEROWANIE RUCHEM MANIPULATORA - Spis treści

STEROWANIE RUCHEM MANIPULATORA - Spis treści

1 / 103

STEROWANIE RUCHEM MANIPULATORA - Wprowadzenie

STEROWANIE RUCHEM MANIPULATORA - Podstawowe pojęcia

STEROWANIE RUCHEM MANIPULATORA - Kinematyka i dynamika manipulatora

STEROWANIE RUCHEM MANIPULATORA - Konwencjonalne sterowanie ruchem manipulatora

STEROWANIE RUCHEM MANIPULATORA - Adaptacyjny algorytm sterowania manipulatorem

STEROWANIE RUCHEM MANIPULATORA - Badania eksperymentalne

STEROWANIE RUCHEM MANIPULATORA - Kierunki dalszych badań

STEROWANIE RUCHEM MANIPULATORA - Literatura

STEROWANIE RUCHEM MANIPULATORA - Spis załączonego oprogramowania

0

1

0.9

0.8

0.7

0.6

0.5

0.4

0.3

60

50

40

30

20

10

0

0

1.8

1.7

1.6

1.5

1.4

1.3

1.2

1.1

1

0.9

0.8

60

50

40

30

20

10

10

20

30

40

50

60

-1.2

-1

-0.8

-0.6

-0.4

-0.2

0

0.2

0.4

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

0.75

0.8

0.85

0.9

0.95

1

0

10

20

30

40

50

60

1

1.05

1.1

1.15

1.2

1.25

0

10

20

30

40

50

60

0.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

1

x 10

-3

0

10

20

30

40

50

60

0.75

0.8

0.85

0.9

0.95

1

0

10

20

30

40

50

60

1

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.5

x 10

-17

0

10

20

30

40

50

60

-2

-1

0

1

2

x 10

-17

0

10

20

30

40

50

60

-2.5

-2

-1.5

-1

-0.5

0

0.5

1

1.5

2

2.5

x 10

-17

-0.8

-0.6

-0.4

-0.2

0

0.2

0.4

0.6

0.8

-15

-10

-5

0

5

10

15

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

0

1

2

3

4

5

6

7

8

9

10

-1

-0.5

0

0.5

1

1.5

x 10

-15

0

1

2

3

4

5

6

7

8

9

10

-1

-0.5

0

0.5

1

1.5

x 10

-14

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.5

x 10

-14

0

1

2

3

4

5

6

7

8

9

10

0.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

0

1

2

3

4

5

6

7

8

9

10

0

10

20

30

40

50

60

70

80

90

0

5

10

15

20

25

30

0.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

30

0.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

0

5

10

15

20

25

30

0

0.002

0.004

0.006

0.008

0.01

0.012

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

0

5

10

15

20

25

30

-0.05

-0.04

-0.03

-0.02

-0.01

0

0.01

0.02

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

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

4

x 10

-5

0

5

10

15

20

25

30

-8

-6

-4

-2

0

2

4

6

8

10

x 10

-5

0

5

10

15

20

25

30

-12

-10

-8

-6

-4

-2

0

2

4

6

8

x 10

-5

0

5

10

15

20

25

30

-5

-4

-3

-2

-1

0

1

2

3

x 10

-4

0

5

10

15

20

25

30

-10

-8

-6

-4

-2

0

2

x 10

-3

0

5

10

15

20

25

30

-6

-5

-4

-3

-2

-1

0

1

x 10

-3

0

5

10

15

20

25

30

-10

-8

-6

-4

-2

0

2

x 10

-4

0

5

10

15

20

25

30

-8

-6

-4

-2

0

2

4

6

8

x 10

-4

0

5

10

15

20

25

30

-3.5

-3

-2.5

-2

-1.5

-1

-0.5

0

0.5

x 10

-3

0

5

10

15

20

25

30

0

0.002

0.004

0.006

0.008

0.01

0.012

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

0

5

10

15

20

25

30

2.45

2.5

2.55

2.6

2.65

2.7

2.75

0

5

10

15

20

25

30

3.25

3.3

3.35

3.4

3.45

3.5

3.55

0

5

10

15

20

25

30

9.5

10

10.5

11

11.5

12

0

5

10

15

20

25

30

5

5.5

6

0

5

10

15

20

25

30

-10

-8

-6

-4

-2

0

2

x 10

-4

0

5

10

15

20

25

30

-3

-2.5

-2

-1.5

-1

-0.5

0

0.5

1

x 10

-3

0

5

10

15

20

25

30

-3.5

-3

-2.5

-2

-1.5

-1

-0.5

0

0.5

x 10

-3

0

5

10

15

20

25

30

0

0.005

0.01

0.015

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.5

-0.4

-0.3

-0.2

-0.1

0

0.1

0.2

0.3

0.4

0

5

10

15

20

25

30

2.45

2.5

2.55

2.6

2.65

2.7

2.75

0

5

10

15

20

25

30

3.2

3.3

3.4

3.5

3.6

3.7

3.8

3.9

4

0

5

10

15

20

25

30

5

5.5

6

0

5

10

15

20

25

30

6

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

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

30

0

0.002

0.004

0.006

0.008

0.01

0.012

0.014

0

5

10

15

20

25

30

0

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

0

5

10

15

20

25

30

0

20

40

60

80

100

120

0

5

10

15

20

25

30

-4

-2

0

2

4

6

8

10

12

14

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

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

8

x 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

0

1

2

3

4

5

6

7

8

9

10

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

0

1

2

3

4

5

6

7

8

9

10

-6

-4

-2

0

2

4

6

8

x 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

-6

-4

-2

0

2

4

6

8

x 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

0

1

2

3

4

5

6

7

8

9

10

-10

-8

-6

-4

-2

0

2

4

x 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

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

0

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.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

0

1

2

3

4

5

6

7

8

9

10

0

0.02

0.04

0.06

0.08

0.1

0.12

0

1

2

3

4

5

6

7

8

9

10

0

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

0

1

2

3

4

5

6

7

8

9

10

0

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

10

0

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

10

x 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

0

1

2

3

4

5

6

7

8

9

10

0.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

0

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

10

x 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

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

-0.01

0

0.01

0.02

0.03

0.04

0.05

0.06

0.07

0.08

0.09

0

1

2

3

4

5

6

7

8

9

10

-8

-7

-6

-5

-4

-3

-2

-1

0

1

2

x 10

-3

---