STEROWANIE OPARTE NA MODELU DLA NIEHOLONOMICZNYCH MANIPULATORÓW MOBILNYCH

Prace Naukowe Instytutu Informatyki, Automatyki i Robotyki Nr 17 Politechniki Wroc lawskiej Nr 17 Monografie Nr 31 29 manipulator mobilny, ograniczenia nieholonomiczne, sterowanie kinematyczne, sterowanie dynamiczne Alicja MAZUR STEROWANIE OPARTE NA MODELU DLA NIEHOLONOMICZNYCH MANIPULATORÓW MOBILNYCH W monografii przedstawiono jednolite podejście pozwalajace na znalezienie sterowania dla nieholonomicznych manipulatorów mobilnych w przypadku, gdy różne zadania podlegaja dekompozycji na podzadania definiowane osobno dla każdego z systemów sk ladowych, jakimi sa ko lowa platforma mobilna oraz ramie manipulacyjne zamontowane na tej platformie. Spośród czterech typów manipulatorów mobilnych rozważono tylko takie, w których ko lowa platforma porusza sie w sposób ściśle toczny, bez poślizgu i buksowania kó l, a wiec jest nieholonomiczna. Natomiast w przypadku manipulatora dopuszczono zarówno bezpośrednie napedy, jak i napedy nieholonomiczne. Równania ruchu uk ladów nieholonomicznych zawieraja równania ograniczeń, które musza być spe lnione w każdej chwili, oraz równania dynamiki, po l aczone w strukture kaskadowa. Z tego wzgledu do projektowania sterowania dla różnych zadań zastosowano podejście, które wymaga jednoczesnego rozwiazywania równania ograniczeń i użycia otrzymanych rozwiazań do sterowania na poziomie dynamicznym. Jednym z czestych braków spotykanych w wielu pracach jest nieuwzglednianie b l edów pochodzacych z poziomu dynamiki i zak lócajacych dzia lanie sterownika kinematycznego (uk ladu rozwiazuj acego równania ograniczeń, który jest projektowany w przypadku idealnym, a wiec bez wziecia pod uwage efektów dynamicznych, takich np. jak duża masa, czy bezw ladność uk ladu. W monografii przedstawiono takie roz- Instytut Informatyki, Automatyki i Robotyki Politechniki Wroc lawskiej, Wybrzeże Wyspiańskiego 27, 5-37 Wroc law.

4 wiazania dla wszystkich rozważanych zadań, w których wspomniane b l edy zosta ly sprowadzone do zera. W przeciwnym razie nie można zagwarantować poprawnego dzia lania uk ladów sterowania podczas procesu regulacji. Proponowane w pracy algorytmy sterowania dzia laja poprawnie, co potwierdzaja dowody zbieżności i badania symulacyjne. Przedstawione algorytmy sterowania obejmuja wiekszość zadań, jakie można sformu lować dla każdego z podsystemów sk ladowych nieholonomicznego manipulatora mobilnego: sterowanie do punktu, śledzenie trajektorii oraz śledzenie ścieżki. Metoda postepowania w każdym przypadku jest podobna: należy znaleźć algorytm kinematyczny zapewniajacy realizacje zadania dla danego poduk ladu nieholonomicznego, a nastepnie wykorzystać otrzymane rozwiazanie do zaprojektowania sterowania na poziomie dynamicznym. Wybór jednego spośród zaprezentowanych algorytmów dynamicznych jest dowolny, jednak algorytm dysypatywny i uniwersalny moga w prosty sposób zostać zmodyfikowane do postaci adaptacyjnej, stosowanej w przypadku parametrycznej niepewności co do modelu dynamiki. Sformu lowanie problemu sterowania w tak ogólnej postaci pozwala również na realizacje zadań mieszanych dla poszczególnych podsystemów, np. platforma może podażać do ustalonej konfiguracji, natomiast manipulator w tym samym czasie może śledzić zadana trajektorie przegubowa. Jedynym warunkiem wymaganym do poprawnej realizacji zadań jest użycie algorytmu kinematycznego posiadajacego odpowiednie w laściwości, na przyk lad funkcje Lapunowa gwarantujac a globalna lub pó lglobalna asymptotyczna stabilność dla uk ladu z zamkniet a petl a sprzeżenia zwrotnego. Prezentowane wyniki moga być zastosowane w procesie sterowania nieholonomicznymi manipulatorami mobilnymi podczas realizacji wielu zadań, takich jak pobranie ladunku z ustalonego punktu przestrzeni roboczej, podażanie wzd luż zadanej trajektorii w przestrzeni wewnetrznej lub zewnetrznej, roz ladowywanie cześci ladunku podczas operacji transportowych itp.

1. Wprowadzenie 1.1. Nieholonomiczne manipulatory mobilne Ograniczenia w ruchu uk ladów mechanicznych można zaobserwować wówczas, gdy nie wszystkie trajektorie moga być przez system zrealizowane. Niektóre ograniczenia wynikaja z konstrukcji uk ladu, a wiec sa pochodzenia wewnetrznego (np. przegub manipulatora nie może osiagn ać po lożenia poza ogranicznikami mechanicznymi, inne zaś wynikaja ze sposobu realizacji ruchu, czyli pochodza od otoczenia, w którym uk lad sie przemieszcza. Przyk ladem ograniczenia drugiego typu jest ruch samochodu po zboczu góry, czyli po pewnej powierzchni definiujacej grunt. Ograniczenia takie sa przyk ladami tzw. ograniczeń holonomicznych, w których ruch uk ladu jest ograniczony do g ladkiej podrozmaitości w przestrzeni stanu Q. Ograniczenia holonomiczne można przedstawić lokalnie jako ograniczenia postaci φ 1 (q φ(q =. φ k (q =. (1.1 Każde przekszta lcenie φ i ogranicza ruch uk ladu. W klasycznej literaturze z dziedziny mechaniki ograniczenia postaci (1.1 sa czasem nazywane ograniczeniami skleronomicznymi. Zarówno s lowo,,holonomiczne, jak i,,skleronomiczne pochodzi z greki i oznacza odpowiednio,,razem s luszne, poprawne oraz,,sztywne, przy czym ograniczeniami skleronomicznymi nazywane sa ograniczenia, które nie zawieraja czasu w jawnej postaci. Jednak w dziedzinie robotyki przyje lo sie pierwsze określenie i dlatego w dalszej cześci bedziemy używać nazwy,,ograniczenia holonomiczne. Ograniczenia (1.1 sa sta le w czasie, dlatego można je wyrazić jako φ = φ q =, q to jest w tzw. postaci Pfaffa A(q q =, (1.2

6 1. Wprowadzenie gdzie A(q = φ 1 q 1...... φ k q 1... jest nazywana macierza ograniczeń Pfaffa i jest pe lnego rzedu. Innym rodzajem ograniczeń, które moga być obserwowane w ruchu uk ladów, sa ograniczenia nieholonomiczne. Ograniczenia tego typu pojawiaja sie wówczas, gdy predkości uk ladu sa ograniczone do (n k-wymiarowej podprzestrzeni, przy jednoczesnym braku ograniczeń na dopuszczalne konfiguracje uk ladu. Wyste- powanie ograniczeń nieholonomicznych można zaobserwować podczas próby zaparkowania samochodu w zatoczce. Samochód wykonuje wówczas szereg manewrów majacych przemieścić go w zaplanowane miejsce, gdyż nie jest możliwe wygenerowanie sk ladowej predkości w kierunku prostopad lym do powierzchni kó l. Najcześciej powodem pojawienia sie ograniczeń nieholonomicznych jest brak poślizgów w ruchu tocznym w punkcie styku dwóch cia l lub zasada zachowania momentu pedu. Ograniczenia nieholonomiczne pojawiaja sie w zachowaniu wielu obiektów robotycznych. W przypadku ko lowych robotów mobilnych obecność ograniczeń nieholonomicznych wyrażonych w postaci Pfaffa (1.2 wynika z przyjecia za lożenia o braku poślizgu w punkcie styczności każdego ko la z pod lożem. Z kolei, manipulatory moga mieć ograniczenia nieholonomiczne, gdy zostana wyposażone w palczasty chwytak lub gdy zostana skonstruowane z wykorzystaniem specjalnych sprzegie l Nakamury, Chunga i Sørdalena [68]. Również wiele robotów projektowanych do specjalnych zastosowań, takich jak roboty latajace (samoloty, sterowce, szybowce, helikoptery, roboty morskie (roboty podwodne, statki nawodne, batyskafy, roboty do zastosowań kosmicznych maja czesto ograniczenia nieholonomiczne na lożone na ruch, wynikajace na przyk lad ze sposobu przekazywania napedu. W ostatnich latach można zauważyć rosnace zainteresowanie uk ladami nieholonomicznymi. Przyczyna tego stanu rzeczy jest fakt, że wprawdzie sterowanie takimi systemami jest znacznie trudniejsze, gdyż sa to uk lady z deficytem sterowań, jednak wymagaja zastosowania mniejszej liczby uk ladów napedowych. Prezentowana praca dotyczy zagadnień sterowania dla specjalnych obiektów robotycznych, jakimi sa nieholonomiczne manipulatory mobilne. Manipulator mobilny sk lada sie ze sztywnego manipulatora zamontowanego na platformie mobilnej, zwanej w literaturze ko lowym robotem mobilnym, wyposażonej w niepodlegajace deformacji ko la. Umieszczenie manipulatora (ramienia robota na platformie powoduje znaczne powiekszenie jego przestrzeni roboczej (dzieki mobilności platformy przy jednoczesnym zachowaniu zreczności i możliwości mani- φ 1 q n φ k q n

1.2. Przeglad zawartości rozprawy 7 pulacyjnych ramienia, jednak jest to okupione istotnymi utrudnieniami w procesie sterowania. Po pierwsze, po l aczenie dwóch podsystemów o różnej strukturze powoduje powstanie znacznych sprzeżeń dynamicznych miedzy obydwoma cz lonami, co może na przyk lad wywo lać ruch platformy nawet wówczas, gdy w l aczone sa napedy tylko dla przegubów manipulatora. Po drugie, może wystapić niejednoznaczność rozwiazań projektowanych zadań dla obu podsystemów, na przyk lad śledzenie ścieżki lub trajektorii może być realizowane tylko przez jeden podsystem (manipulator lub platforme lub poprzez skoordynowane wspó ldzia lanie obu podsystemów. W zwiazku z tym niezbedna jest precyzyjna definicja śledzonej ścieżki lub trajektorii dla manipulatora mobilnego. Po trzecie, pojawienie sie ograniczeń na lożonych na ruch tylko jednego podsystemu musi zostać uwzglednione w sterowaniu dla ca lego uk ladu z lożonego. Ze wzgledu na rodzaj ograniczeń na lożonych na poszczególne podsystemy, manipulatory mobilne można podzielić na nastepuj ace typy: typ (h, h holonomiczny manipulator na holonomicznej platformie, typ (h, nh holonomiczna platforma z nieholonomicznym manipulatorem, typ (nh, h nieholonomiczna platforma z holonomicznym manipulatorem, typ (nh, nh zarówno platforma, jak i manipulator nieholonomiczne. Ten typ manipulatora mobilnego nosi nazw e podwójnie nieholonomicznego [86]. 1.2. Przeglad zawartości rozprawy Niniejsza praca dotyczy problematyki sterowania w środowisku wolnym od przeszkód dla nieholonomicznych manipulatorów mobilnych dwóch ostatnich typów, a wiec zawierajacych nieholonomiczna platforme mobilna oraz zamontowany na niej manipulator o holonomicznym lub nieholonomicznym sposobie przekazywania napedów. Poniżej zostanie pokrótce omówiona zawartość poszczególnych rozdzia lów monografii. W rozdziale 2 przedstawiono ograniczenia nieholonomiczne, jakie moga pojawić sie w ruchu manipulatorów mobilnych. Ograniczenia te sa typu kinematycznego, ponieważ wiaż a predkości rozważanych obiektów. Pokazano, w jaki sposób wyprowadza sie ograniczenia wynikajace z braku poślizgów dla ko lowych platform mobilnych wyposażonych w ko la konwencjonalne oraz jak otrzymuje sie ograniczenia predkościowe dla napedu skonstruowanego przez Nakamure, Chunga i Sørdalena. Ponieważ rozważane ograniczenia moga być przedstawione w postaci Pfaffa, omówiono sposoby sprawdzania nieholonomiczności ograniczeń kinematycznych z wykorzystaniem tzw. ma lej lub dużej flagi systemu. Ponadto podano

8 1. Wprowadzenie metode transformacji otrzymanych ograniczeń do jednej z tzw. postaci normalnych, jaka jest postać lańcuchowa. Przekszta lcenie równań kinematyki do tej postaci umożliwia wykorzystanie znanych algorytmów sterowania dla uk ladów lańcuchowych. Rozdzia l 3 dotyczy sposobów modelowania zachowania nieholonomicznych manipulatorów mobilnych. Ponieważ w pracy skupiono sie na manipulatorach mobilnych typu (nh, h i (nh, nh, omówiono zarówno równania kinematyki (równania ograniczeń, jak i równania dynamiki dla obu wspomnianych typów manipulatorów mobilnych wyrażone w różnych wspó lrzednych. Wybór wspó lrzednych, a wiec postaci modelu, jest zdeterminowany przez rodzaj zadań, jakie manipulator mobilny ma realizować podczas swojej pracy. Problem sterowania zosta l sformu lowany w rozdziale 4. Ze wzgledu na kaskadowa strukture równań ruchu, do celów sterowania manipulatorem mobilnym stosowany jest algorytm ca lkowania wstecznego. Wymaga on równoczesnego rozwiazania równań opisujacych ograniczenia nieholonomiczne (jest to tzw. sterowanie kinematyczne i sterowania na poziomie dynamicznym, zarówno przy pe lnej znajomości modelu dynamiki manipulatora mobilnego, jak i w przypadku parametrycznej nieznajomości tego modelu (podano algorytm sterowania adaptacyjnego oraz uniwersalnego. Sterowanie kinematyczne ma na celu rozwiazanie różnych zadań dla podsystemów nieholonomicznych, tj. ko lowych platform mobilnych z ograniczona mobilnościa oraz manipulatorów z nieholonomicznym przeniesieniem napedów. W rozdziale 5 przedstawiono wybrane algorytmy kinematyczne, zarówno pracuja- ce w petli otwartej, jak i w petli zamknietej. Rozważono algorytmy zapewniajace śledzenie trajektorii, śledzenie ścieżki oraz sterowanie do punktu. W rozdziale 6 przedstawiono algorytmy sterowania do punktu gwarantujace osiagni ecie ustalonej konfiguracji dla manipulatorów typu (nh, h i (nh, nh. Na przyk ladzie algorytmu wielomianowego dla zadania sterowania do punktu dla uk ladów lańcuchowych pokazano, jak dynamika wp lywa na trajektorie uk ladu i dlaczego dla dok ladnej realizacji wybranego celu niezbedne jest zastosowanie algorytmów ze sprzeżeniem zwrotnym. W przypadku kaskadowego po l aczenia sterowania na poziomie kinematycznym i dynamicznym należy rozważyć dodatkowe b l edy pochodzace z poziomu dynamicznego i zak lócajace rozwiazania teoretyczne, jakimi sa algorytmy kinematyczne. Przy odpowiednim doborze parametrów regulacji można pokazać asymptotyczna zbieżność do zera b l edów pochodzacych z poziomu dynamicznego. W ten sposób można udowodnić poprawna koordynacje dzia lań obu podsystemów, tj. platformy i manipulatora, sprzeżonych dynamicznie. Kolejnym zadaniem dla manipulatora mobilnego, przedstawionym w rozdziale 7, jest śledzenie zadanej trajektorii definiowanej w przestrzeni wewnetrznej

1.2. Przeglad zawartości rozprawy 9 manipulatora. Pokazano algorytmy sterowania dla obu rozpatrywanych typów nieholonomicznych manipulatorów mobilnych oraz dowody ich poprawnego dzia- lania. W obu typach obiektów w roli sterowania kinematycznego zastosowano algorytmy śledzenia trajektorii ogólnego przeznaczenia, jakimi sa linearyzacja dynamiczna oraz algorytm Jianga i Nijmeijera dla uk ladów lańcuchowych. W rozdziale 8 omówiono problematyke śledzenia trajektorii zdefiniowanej w przestrzeni zewnetrznej. Podstawowa metoda wykorzystywana w tak sformu- lowanym zadaniu jest odsprzeganie transformacji wejściowo-wyjściowej, a nastepnie sterowanie uk ladem odsprzeżonym w celu realizacji zadania postawionego przed manipulatorem mobilnym. Punktem wyjścia do uzyskania uk ladu odsprzeżonego jest podejście zaproponowane przez Yamamoto i Yuna, polegajace na odpowiednim wyborze wspó lrzednych efektora w zależności od liczby wejść w poduk ladzie nieholonomicznym. Ponieważ metoda Yamamoto i Yuna cechuje sie pewnymi ograniczeniami, zaproponowano rozszerzenie wyboru funkcji wyjściowych koniecznych do przeprowadzenia procedury odsprzegania do postaci, która umożliwi laby jednoczesne przemieszczanie platformy i poruszanie manipulatorem wzgledem podstawy. Taki sposób definiowania wydaje sie przydatny w procesie roz ladowywania przewożonych towarów podczas operacji transportowych. Rozszerzone funkcje wyjściowe znajduja także zastosowanie podczas śledzenia trajektorii zadaniowej dla podwójnie nieholonomicznego manipulatora mobilnego. Ostatnie zadanie dla nieholonomicznych manipulatorów mobilnych, czyli śledzenie ścieżki, przedstawiono w rozdziale 9. Dla każdego z dwóch typów obiektów zadanie jest definiowane w odmienny sposób. Asymptotyczne śledzenie ścieżki dla poduk ladu nieholonomicznego (ko lowej platformy mobilnej lub manipulatora z nieholonomicznym napedem może być przekszta lcone za pomoca parametryzacji Freneta do zadania sterowania do punktu dla bezdryfowego uk ladu sterowania. Z kolei dla holonomicznego manipulatora, bed acego cześci a manipulatora mobilnego typu (nh, h, śledzenie ścieżki jest rozumiane jako przemieszczanie sie wzd luż ograniczonej krzywej sparametryzowanej odleg lościa krzywoliniowa, z zatrzymaniem na jej końcu. Rozdzia l 1 zawiera podsumowanie zawartości rozprawy i uwagi końcowe. W szczególności podkreślono oryginalne rozwiazania przedstawione w monografii w dziedzinie sterowania nieholonomicznych manipulatorów mobilnych. W rozdziale tym omówiono również otwarte kierunki badań i tematyke przysz lych prac. Podstawowe definicje i twierdzenia o stabilności dla różnych typów uk ladów sterowania przedstawiono w rozdziale 11. Twierdzenia te sa podstawowym narzedziem do badania stabilności nieholonomicznych manipulatorów mobilnych. W tabeli 1.1 zestawiono kinematyczne i dynamiczne algorytmy sterowania użyte do realizacji różnych zadań formu lowanych dla nieholonomicznych manipulatorów mobilnych, których dowody zamieszczono w monografii.

1 1. Wprowadzenie Tabela 1.1. Algorytmy sterowania realizujace różne zadania dla nieholonomicznych manipulatorów mobilnych, których dowody zamieszczono w pracy Table 1.1. Control algorithms solving different tasks for nonholonomic mobile manipulators which proofs of convergence are included in the monography Zadanie Obiekt Algorytm kinematyczny dla platformy: (nh, h algorytm Pometa z parametryzacja Freneta sterowanie dynamiczny: dysypatywny do punktu kinematyczny dla platformy: (rozdz. 6 algorytm Pometa z parametryzacja Freneta (nh, nh kinematyczny dla manipulatora: algorytm Nakamury, Chunga i Sørdalena dynamiczny: dysypatywny kinematyczny dla platformy: (nh, h linearyzacja dynamiczna dynamiczny: adaptacyjny dysypatywny śledzenie z kompensacja b l edów na poziomie dynamicznym trajektorii kinematyczny dla platformy: wewnetrznej linearyzacja dynamiczna (rozdz. 7 (nh, nh kinematyczny dla manipulatora: algorytm Jianga i Nijmeijera dynamiczny: dysypatywny z kompensacja b l edów na poziomie dynamicznym śledzenie wyjścia Yamamoto i Yuna trajektorii (nh, h wyjścia rozszerzone zewnetrznej odsprzeganie i linearyzacja we wy (rozdz. 8 (nh, nh wyjścia rozszerzone odsprzeganie i linearyzacja we wy kinematyczny dla platformy: (nh, h algorytm Samsona z parametryzacja Freneta śledzenie dynamiczny: dysypatywny z modyfikacja Galickiego ścieżki kinematyczny dla platformy: (rozdz. 9 algorytm Samsona z parametryzacja Freneta (nh, nh kinematyczny dla manipulatora: odsprzeganie dla parametryzacji Freneta dynamiczny: dysypatywny

2. Kinematyka uk ladów nieholonomicznych Niniejszy rozdzia l prezentuje kinematyke uk ladów nieholonomicznych. Szczególna uwage zwrócono na równania kinematyki nieholonomicznych ko lowych platform mobilnych, a także manipulatorów o nieholonomicznym sposobie przenoszenia napedów, ponieważ takie systemy robotyczne stanowia cześci sk ladowe nieholonomicznych manipulatorów mobilnych typu (nh, h oraz (nh, nh, rozważanych w tej pracy. Istnieje kilka powodów do specjalnego potraktowania obu wspomnianych typów obiektów robotycznych. Pierwszym z nich jest fakt, że zarówno ko lowe platformy mobilne, jak i manipulatory nieholonomiczne maja ograniczenia nieholonomiczne wynikajace z za lożenia o braku poślizgów stykajacych sie powierzchni podczas realizacji ruchu. Po drugie, warto przedstawić metode wyprowadzania ograniczeń kinematycznych w systematyczny sposób, ponieważ artyku ly publikowane w czasopismach czesto prezentuja je w sposób uproszczony. Po trzecie, nieholonomiczne manipulatory mobilne by ly obiektem badań autorki przez ostatnie lata, co znalaz lo wyraz w szeregu publikacji [47] [56]. Zwrot,,uk lady nieholonomiczne oznacza uk lady z pewnym typem ograniczeń na lożonych na realizowane przez nie trajektorie. Podstawowe źród la ograniczeń nieholonomicznych to za lożenie o braku poślizgu (zerowym wektorze predkości chwilowych w punkcie kontaktu pomiedzy stykajacymi sie powierzchniami elementów mechanicznych, a także zasada zachowania pedu. Ograniczenia pochodzace z pierwszego źród la sa to ograniczenia kinematyczne, natomiast drugi rodzaj ograniczeń musi być rozważany na poziomie dynamicznym. W tej pracy omawiane bed a wy l acznie ograniczenia nieholonomiczne pochodzenia kinematycznego, czyli ograniczenia predkościowe. Rozważania dotyczace nieholonomiczności ograniczeń należy poprzedzić pewnymi uwagami terminologicznymi. W wielu podrecznikach do robotyki [84], [15] kinematyka nazywa sie przekszta lcenie miedzy dwiema przestrzeniami po lożeń. Przestrzeń zawierajaca sterowane zmienne stanu jest nazywana przestrzenia wew-

12 2. Kinematyka uk ladów nieholonomicznych netrzn a (przegubowa, konfiguracyjna, natomiast przestrzeń, w której definiuje sie ruch, nosi nazwe przestrzeni zewnetrznej (zadaniowej, roboczej. W klasycznej robotyce kinematyka prosta to transformacja z przestrzeni przegubowej do roboczej, transformacja zaś z przestrzeni roboczej do przegubowej to kinematyka odwrotna. Jeśli rozważane sa nie tylko po lożenia, ale również pochodne po czasie (predkości zdefiniowane w obu przestrzeniach, to transformacja l acz ac a je jest macierz Jacobiego. Z kolei, kinematyka uk ladu nieholonomicznego jest nazywane równanie ograniczeń na lożonych na ruch takiego uk ladu, wyrażone nastepuj ac a zależnościa miedzy predkości a q a po lożeniem q uk ladu A(q q =. Jak wspomniano we wstepie, taka postać kinematyki nazywa sie postacia Pfaffa. Należy zwrócić szczególna uwage na fakt, że w postaci Pfaffa można wyrazić zarówno ograniczenia nieholonomiczne, jak i holonomiczne. Z tego wzgledu istnieje konieczność wprowadzenia kryterium pozwalajacego zweryfikować nieholonomiczność ograniczeń na lożonych na ruch uk ladu. 2.1. Nieholonomiczność ograniczeń Rozważmy uk lad mechaniczny, którego zachowanie jest opisane przez n uogólnionych wspó lrzednych q R n i predkości q R n, spe lniajacych l (l < n niezależnych ograniczeń fazowych o postaci Pfaffa gdzie: A(q macierz (l n pe lnego rz edu, q(t trajektoria uk ladu. A(q q =, (2.1 Obecność ograniczeń (2.1 może, ale nie musi, wp lywać na ograniczenie dopuszczalnych stanów uk ladu. Zależy to od w laściwości ograniczeń nazywanej holonomicznościa. Ograniczenia holonomiczne oznaczaja, że macierz Pfaffa pomnożona z lewej strony przez pewna nieosobliwa macierz M(q bedzie macierza Jacobiego pewnego odwzorowania φ : R n R l, tzn. M(qA(q = φ q (q. Wówczas (2.1 oznacza, że d dtφ(q(t =, czyli φ(q = const, a zatem trajektorie uk ladu sa zawarte w (n l-wymiarowej rozmaitości. Innymi s lowy, ograniczenia holonomiczne powoduja zmniejszenie przestrzeni konfiguracyjnej uk ladu.

2.1. Nieholonomiczność ograniczeń 13 Inaczej sie przedstawia sytuacja w przypadku ograniczeń nieholonomicznych. Jeśli ograniczenia spe lniaja w lasność nieholonomiczności, to oznacza, że nie jest możliwe sca lkowanie równań (2.1, a co za tym idzie, nie ulega zmniejszeniu przestrzeń stanu uk ladu, natomiast sposób uzyskiwania pewnych konfiguracji może ulec istotnemu utrudnieniu. W dalszych rozważaniach ograniczenia be- dziemy nazywali nieholonomicznymi, jeżeli wszystkie spośród l ograniczeń bed a nieholonomiczne (sa to tzw. ograniczenia ca lkowicie nieholonomiczne. Za lóżmy, że wszystkie ograniczenia sa nieholonomiczne. Z równania (2.1 wynika, że dopuszczalne predkości q uk ladu w konfiguracji q należa do jadra (przestrzeni zerowej macierzy Pfaffa q Ker A(q. Oznacza to, że predkości dopuszczalne można wyrazić jako kombinacje pewnych wektorów rozpinajacych jadro macierzy A(q, jak nastepuje m q = g i (qu i = G(qu, (2.2 i=1 przy czym macierz G(q oblicza sie z równania A(qG(q =. Z niezależności ograniczeń fazowych wynika, że w każdym punkcie przestrzeni stanu rzad macierzy G(q jest pe lny i równy q rank G(q = n l = m. Pola wektorowe g 1, g 2,..., g m (kolumny macierzy G tworza w przestrzeni stanu obiekt geometryczny G = span C {g 1, g 2,..., g m } noszacy nazwe dystrybucji stowarzyszonej z uk ladem (2.2. Ograniczenia fazowe (2.1 bed a spe lnione wówczas, gdy w każdym punkcie należacym do przestrzeni stanu predkość uk ladu bedzie należa la do dystrybucji, czyli bedzie kombinacja pól wektorowych zdefiniowanych przez kolumny macierzy G(q. Pojecie dystrybucji G stanowi punkt wyjścia do sformu lowania kryterium nieholonomiczności ograniczeń [65]. Twierdzenie 1. Niech G = span C {g 1, g 2,..., g m }. Zdefiniujmy ciag dystrybucji G = G, G 1 = G + [G, G ],..., G i = G i 1 + [G i 1, G ],..., i = 1, 2,...,

14 2. Kinematyka uk ladów nieholonomicznych w którym operacja [, ] oznacza nawias Liego dwóch pól wektorowych, zdefiniowany w nastepuj acy sposób Jeżeli dla pewnego i = r zachodzi [f, g](q = g f f(q q q g(q. dim G r (q = n, to ograniczenia (2.1 sa nieholonomiczne. Najmniejsza liczba r o tej w laściwości nosi nazwe stopnia nieholonomiczności dystrybucji G. Ciag dystrybucji G G 1...G i... nazywamy filtracja dystrybucji G (ma l a flaga systemu (2.2. Pojecie flagi dystrybucji pojawi lo sie np. w pracy [63], natomiast termin ma la flaga wprowadzono za Mormulem. Jeżeli każda z dystrybucji G i ma sta ly wymiar w każdym punkcie przestrzeni stanu, tzn. q R n dim G i (q = r i = const, to filtracja jest regularna. Dla filtracji regularnej wymiary kolejnych wzrastajacych dystrybucji sa niemalejace r r 1 r i i po pewnej liczbie iteracji p n osiagaj a wartość ustalona r p. Ciag utworzony z wymiarów kolejnych dystrybucji (r, r 1,..., r p nazywamy wektorem wzrostu dystrybucji G. Jeżeli spe lniony jest warunek r p (q = n q, to ograniczenia sa w pe lni nieholonomiczne i uk lad (2.2 jest sterowalny. Jeżeli natomiast r p < n, to oznacza, że wśród ograniczeń Pfaffa wystepuj a ograniczenia holonomiczne, a wiec warunek ca lkowitej nieholonomiczności nie jest spe lniony. 2.2. Ograniczenia nieholonomiczne dla ko lowych platform mobilnych Ko lowe platformy mobilne (roboty mobilne sa jednymi z najcześciej rozważanych systemów nieholonomicznych [16], [17], [77]. Platformy mobilne sa to pojazdy wyposażone w niedeformowalne ko la, które poruszaja sie po p laszczyźnie ekwipotencjalnej (poziomej. Za lożenie o ruchu p laskim zosta lo wprowadzone jedynie dla uproszczenia opisu i nie ogranicza w lasności strukturalnych robota mobilnego.

2.2. Ograniczenia nieholonomiczne dla ko lowych platform mobilnych 15 Ograniczenia nieholonomiczne w opisie ko lowych platform mobilnych pojawiaja sie w przypadku przyjecia za lożenia o tocznym, bezpoślizgowym charakterze ruchu kó l. W dalszych rozważaniach przyjeto za lożenie, że ko la, w jakie jest wyposażona platforma, poruszaja sie bez poślizgów. Takie wyidealizowane ko la a nazywane ko lami konwencjonalnymi. s Ko la konwencjonalne, czyli bezpoślizgowe, można podzielić na nastepuj ace rodzaje [8]: ko lo ustalone (umocowane do sztywnej osi jest to ko lo, którego orientacja wzgledem uk ladu lokalnego stowarzyszonego z platforma jest sta la, ko lo sterowane (kierownica jest to ko lo, którego ruch wzgledem uk ladu lokalnego stowarzyszonego z platforma jest obrotem wokó l osi pionowej przechodzacej przez środek ko la, ko lo Castora jest to ko lo, którego ruch wzgledem uk ladu lokalnego stowarzyszonego z platforma jest obrotem wokó l osi pionowej nie przechodzacej przez środek ko la. Każdy typ ko la konwencjonalnego może poruszać sie bez poślizgu, co jest równoważne z przyjeciem za lożenia, że predkość chwilowa w punkcie kontaktu ko la z pod lożem może przyjmować wartość zerowa, co pokazano na rys. 2.1. y V L y x R a x V P b R Rys. 2.1. Predkość ko la w punkcie kontaktu z pod lożem: a sk ladowa poprzeczna, b sk ladowa wzd lużna Fig. 2.1. Velocity of a conventional wheel at the contact point with a surface: a orthogonal to the wheel plane, b parallel to the wheel plane V W pracy [28] pokazano, że po lożenie uk ladu lokalnego X i Y i Z i (stowarzyszonego z i-tym ko lem wzgledem uk ladu podstawowego można wyrazić za pomoca nastepuj acej transformacji

16 2. Kinematyka uk ladów nieholonomicznych A ki = Trans(X, xtrans(y, yrot(z, θrot(z, α i Trans(X, l i Rot(Z, β i Trans(X, d i Rot(Z, γ i Rot(X, φ i. (2.3 Parametry α i, β i, γ i sa katami opisujacymi geometrie i-tego ko la, l i i d i sa to parametry opisujace umiejscowienie i-tego ko la wzgledem środka masy nadwozia, a φ i jest katem obrotu i-tego ko la. Znajac parametry macierzy A ki, można wyprowadzić warunki na brak poślizgu dla i-tego ko la: brak poślizgu bocznego [ ] ẋ cos(α i + β i + γ i sin(α i + β i + γ i d i sin γ i + l i sin(γ i + β i R(θ ẏ θ brak poślizgu wzd lużnego (buksowania i blokowania [ + d i sin γ i βi =, (2.4 sin(α i + β i + γ i cos(α i + β i + γ i d i cos γ i + l i cos(γ i + β i ] R(θ ẋ ẏ θ + d i cos γ i βi + r i φi =, (2.5 gdzie macierz R(θ = Rot(Z, θ opisuje orientacje uk ladu podstawowego wzgle- dem uk ladu lokalnego robota, zaś r i jest promieniem i-tego ko la. Ograniczenia dla wszystkich kó l, w jakie jest wyposażona platforma, można przedstawić w postaci ogólnej C 1 (β s, β c R(θ J 1 (β s, β c R(θ ẋ ẏ θ ẋ ẏ θ + C 2 β c =, (2.6 + J 2 φ =, (2.7 przy czym symbolem β s oznaczono wektor katów orientacji dla wszystkich kó l sterowanych, zaś β c oznacza wektor katów orientacji dla wszystkich kó l Castora. Rozważmy teraz ograniczenia dane wzorem (2.6. W pracy [8] pokazano, że możliwość wykonania ruchu przez ko lowa platforme mobilna jest zwiazana z rzedem macierzy C 1 (β s, β c. Aby podzielić ko lowe platformy mobilne ze wzgledu na możliwości poruszania sie, Campion, Bastin i d Andrea-Novel w pracy [6] wprowadzili nastepuj ace pojecia:

2.2. Ograniczenia nieholonomiczne dla ko lowych platform mobilnych 17 stopień mobilności σ m wymiar dost epnej przestrzeni pr edkości w punkcie kontaktu kó l z pod lożem σ m = 3 rank C 1, stopień sterowalności σ s liczba kó l sterowanych (kierownic w platformie, które moga niezależnie zmieniać orientacje. Badajac wartości, jakie moga przyjmować parametry σ m i σ s, widać, że: a stopień mobilności spe lnia nierówność 1 σ m 3, co oznacza, że rozpatrujemy tylko przypadki, w których ruch w przestrzeni trójwymiarowej jest możliwy do wykonania, b stopień sterowalności spe lnia nierówność c zachodzi nastepuj acy zwiazek σ s 2, 2 σ m + σ s 3. Spe lnienie tej nierówności oznacza, że robot jest niezdegenerowany, czyli w przypadku zbyt dużej liczby kó l, ich ruch jest odpowiednio skoordynowany (może być zastapiony jednym ko lem. Z powyższych nierówności wynika, że jest 5 klas ko lowych robotów mobilnych [8], co przedstawiono w tabeli 2.1. Definiujac typ platformy, należy podać jej parametry (σ m, σ s. Spośród platform wymienionych w tabeli 2.1, wszystkie platformy, oprócz klasy (3,, sa nazywane platformami z ograniczona mobilnościa. Tabela 2.1. Podzia l na klasy ko lowych platform mobilnych Table 2.1. Possible classes of wheeled mobile platforms σ m 3 2 2 1 1 σ s 1 2 1 Warto zauważyć, że dla kó l ustalonych i sterowanych warunki na brak poślizgu bocznego i wzd lużnego, przeliczone wed lug oznaczeń z rys. 2.1, maja postać:

18 2. Kinematyka uk ladów nieholonomicznych brak poślizgu wzd lużnego brak poślizgu bocznego ẋ cos θ + ẏ sin θ R φ =, (2.8 ẋ sin θ ẏ cos θ =, (2.9 gdzie zmienne (x, y oznaczaja wspó lrzedne punktu kontaktu ko la z pod lożem, θ jest orientacja ko la, kat φ jest katem obrotu ko la, a R oznacza promień ko la. 2.3. Ograniczenia nieholonomiczne dla manipulatorów W przypadku robotów manipulacyjnych pojawienie sie ograniczeń nieholonomicznych zależy od konstrukcji robota. Do po lowy lat 9. XX wieku terminem,,manipulator nieholonomiczny określano manipulator z chwytakiem wielopalczastym o specjalnej budowie palców, w którym ograniczenia nieholonomiczne pojawia ly sie przy za lożeniu punktowego bezpoślizgowego kontaktu pomiedzy palcami a manipulowanym obiektem [65]. W po lowie lat 9. ubieg lego wieku Nakamura, Chung i Sørdalen przedstawili specjalny naped, w którym za pomoca dwóch niezależnych silników można napedzać dowolna liczbe obrotowych stopni swobody manipulatora. W pracy [68] zaprezentowano prototyp planarnego manipulatora z takim napedem. Sposób przekazywania napedu jest w nim nieholonomiczny, oparty na za lożeniu o bezpoślizgowym kontakcie pomiedzy zespo lem kó l i kul tworzacych specjalne sprzeg la umieszczone w przegubach robota. Schemat budowy nieholonomicznego sprzeg la pokazano na rys. 2.2. Nieholonomiczne sprzeg lo sk lada sie z ko la wejściowego IW, kuli oraz dwóch kó l wyjściowych OW 1 i OW 2. Ko lo wejściowe o promieniu r I jest umieszczone na biegunie kuli o promieniu R i styka sie z nia w jednym punkcie, przy czym zak lada sie, że wszystkie kontakty w sprzegle sa bezpoślizgowe. Ko lo wejściowe jest zamocowane w pierwszym przegubie, a ko la wyjściowe w przegubie nastepnym. Ko lo IW obraca sie wokó l nieruchomej osi α I z predkości a katow a u 2, która spe lnia role sterowania. Obracajace sie ko lo IW powoduje obrót kuli, przy czym w punkcie kontaktu predkość liniowa kuli ma taka sama wartość jak predkość liniowa ko la IW, lecz z odwrotnym znakiem r I u 2 = RΩ 1. Kolejnym elementem sprzeg la sa dwa ko la wyjściowe o promieniach r O1 i r O2 umieszczone w p laszczyźnie równika kuli. Pierwsze ko lo OW 1 obraca sie wokó l osi

2.3. Ograniczenia nieholonomiczne dla manipulatorów 19 IW OW 1 r O1 OW 1 O I 1 1,1 2,1 R IW OW 2 r I r O2 a OW 2 b KULA 1 Rys. 2.2. Nieholonomiczny naped: a widok ogólny, b rzut z góry Fig. 2.2. Nonholonomic gear: a schematic of the gear, b view from above α O, która tworzy z osia ko la wejściowego zmieniajacy sie kat θ 1, bed acy zmienna przegubowa manipulatora. Predkość katowa θ 1 = u 1 jest drugim sterowaniem dla manipulatora. Oś obrotu drugiego ko la wyjściowego OW 2 jest umieszczona pod katem prostym w stosunku do osi α O. Równania kinematyki sa sformu lowane w sposób rekurencyjny, poczawszy od przegubu pierwszego, dlatego wprowadźmy oznaczenie ω i,j dla predkości katowej i-tego ko la wyjściowego w j-tym przegubie. W pierwszym przegubie predkość katowa ω 1,1 ko la OW 1 oraz predkość katowa ω 2,1 ko la OW 2 sa równe ( π 1 2 θ. ω 1,1 = Ω 1 R r O1 cos θ 1, ω 2,1 = Ω 1 R r O2 cos Po wyeliminowaniu predkości katowej kuli Ω 1 równania te przyjmuja postać ω 1,1 = r I r O1 cos θ 1 u 2, ω 2,1 = r I r O2 sin θ 1 u 2. Jak widać, sterowanie i-tym przegubem wymaga zmiany dwóch wielkości: predkości katowej ko la wejściowego IW w danym przegubie oraz predkości katowej zmiennej przegubowej θ i. Transmisja predkości z przegubu o numerze (i 1 do ko la wejściowego w przegubie i odbywa sie poprzez ko lo OW 1 przegubu o numerze (i 1 z pewnym wspó lczynnikiem prze lożenia η 1,i ρ i = η 1,i ω 1,i 1. Natomiast transmisja pr edkości z przegubu o numerze (i 1 do zmiennej przegubowej θ i odbywa si e poprzez ko lo OW 2 w przegubie o numerze (i 1 ze wspó lczynnikiem prze lożenia równym η 2,i

2 2. Kinematyka uk ladów nieholonomicznych θ i = η 2,i ω 2,i 1. Majac podana regu l e rekurencyjna, można otrzymać nastepuj ace równania kinematyki dla nieholonomicznego manipulatora o n przegubach (planarnego n-wahad la θ 1 = u 1, (2.1 i 2 θ i = a i sin θ i 1 j=1 cos θ j u 2, i {2,..., n}. (2.11 Jak wynika z podanych równań, za pomoca dwóch sterowań u 1 i u 2 można sterować wieloma przegubami nieholonomicznego manipulatora o ogniwach rotacyjnych ze sprzeg lami konstrukcji Nakamury, Chunga i Sørdalena. 2.4. Uk lady lańcuchowe Ograniczenia nieholonomiczne wystepuj a w ruchu wielu uk ladów mechanicznych. W dotychczas rozważanych obiektach, tj. ko lowych platformach mobilnych i manipulatorach z nieholonomicznymi sprzeg lami, postaci kinematyki różnia sie. Możliwe jest jednak sprowadzenie wielu ograniczeń do jednej z tzw. postaci normalnych. Spośród postaci normalnych najcześciej stosowana jest postać lańcuchowa. Postać lańcuchowa jest to postać normalna, do której można sprowadzić nieholonomiczne ograniczenia kinematyczne wielu uk ladów mechanicznych, zw laszcza uk ladów mobilnych. Możliwość modelowania równań kinematyki ko lowych robotów mobilnych w tzw. postaci jedno lańcuchowej pokazano w pracy [66], natomiast dla samochodu z wieloma przyczepami konwersje do postaci lańcuchowej przedstawiono w [83]. Postać jedno lańcuchowa uk ladu nieholonomicznego wydaje sie atrakcyjna, ponieważ wiele uk ladów mobilnych, w tym dwie klasy ko lowych platform mobilnych, mianowicie klasa (2, i (1, 1, a także kinematyka manipulatora z nieholonomicznym napedem, daja sie sprowadzić do takiej postaci. Uk lad lańcuchowy można zapisać w nastepuj acej postaci ẋ 1 = u 1, ẋ 2 = u 2, ẋ 3 = x 2 u 1,. ẋ n = x n 1 u 1 (2.12

2.4. Uk lady lańcuchowe 21 lub w równoważnej postaci bezdryfowego uk ladu sterowania jako gdzie g 1 = ẋ = g 1 (xu 1 + g 2 u 2, (2.13 1 x 2 x 3. x n 1, g 2 = Dla uk ladu (2.13 można obliczyć ma l a flage systemu G = {g 1, g 2 }, 1. G 1 = {g 1, g 2, [g 2, g 1 ] = e 3 }, = e 2. G 2 = {g 1, g 2, e 3, [g 2, e 3 ] = e 4 },. G n 2 = {g 1, g 2,..., [g 2, e n 1 ] = e n }. Wektor wzrostu uk ladu lańcuchowego wynosi (2, 3, 4,..., n, a stopień nieholonomiczności jest równy n 2. Widać, że ma la flaga uk ladu lańcuchowego rozpina ca l a przestrzeń stanu, a wiec uk lad lańcuchowy jest sterowalny. 2.4.1. Postać lańcuchowa dla nieholonomicznych ko lowych platform mobilnych Jak wspomniano poprzednio, spośród nieholonomicznych ko lowych platform mobilnych jedynie dwie klasy można przekszta lcić do postaci lańcuchowej. Sa to klasy, których równania kinematyki maja dwa wejścia sterujace: klasa (2, i klasa (1, 1. Klasa (2,, do której zalicza sie najprostsza platforme mobilna, czyli tzw. monocykl, ma nastepuj ace równania kinematyki ẋ cos θ ( v q m = ẏ = sin θ, (2.14 ω θ 1 gdzie v jest predkości a liniowa, zaś ω predkości a katow a platformy. Aby powyższe równania przekszta lcić do postaci lańcuchowej, należy zastosować globalny dyfeomorfizm

22 2. Kinematyka uk ladów nieholonomicznych z 1 = θ, oraz statyczne sprz eżenie zwrotne z 2 = x cos θ y sin θ, z 3 = x sin θ + y cos θ u 1 = ω, u 2 = v ωz 3. Przekszta lcenie to pozwala na globalna transformacje równań kinematyki (2.14 do postaci lańcuchowej ż 1 = u 1, ż 2 = u 2, ż 3 = z 2 u 1. Natomiast klasa (1, 1 o kinematyce równej ẋ = v, ẏ = tan θv, θ = tan β v, (2.15 l β = ω, po zastosowaniu lokalnej ( θ < π/2, β < π/2 zmiany wspó lrz ednych [65] oraz zdefiniowaniu nowych wejść u 1 = v, przyjmuje postać lańcuchowa z 1 = x, z 2 = tan β l cos 2 θ, z 3 = tan θ, z 4 = y u 2 = 2 tan2 β tan θ l 2 cos 2 θ v + 1 l cos 2 β cos 2 θ ω ż 1 = u 1, ż 2 = u 2, ż 3 = z 2 u 1, ż 4 = z 3 u 1.

2.4. Uk lady lańcuchowe 23 2.4.2. Postać lańcuchowa dla manipulatorów nieholonomicznych Aby sterować manipulatorem nieholonomicznym, należy przekszta lcić jego kinematyke do postaci lańcuchowej. Taki zabieg pozwala na użycie wszystkich istniejacych algorytmów sterowania dedykowanych uk ladom lańcuchowym, np. algorytmu Jianga i Nijmeijera [33] gwarantujacego śledzenie trajektorii, czy też algorytmu sterowania sinusoidalnego [66] przeprowadzajacego uk lad do zadanego stanu końcowego w określonym horyzoncie czasowym. W dalszych rozważaniach ograniczymy sie do nieholonomicznego manipulatora typu trójwahad lo, choć prezentowana procedura, podana przez Sørdalena w pracy [68], może być uogólniona do manipulatora o n przegubach. Kinematyke nieholonomicznego trójwahad la opisana równaniem (2.1 można przedstawić nastepuj aco θ 1 = u 1, θ 2 = a 2 sin θ 1 u 2, θ 3 = a 3 sin θ 2 cos θ 1 u 2, φ = cos θ 1 cos θ 2 u 2, (2.16 gdzie φ jest orientacja ko la OW 2 w drugim przegubie trójwahad la. W pracy [68] pokazano, że nie jest możliwe przekszta lcenie kinematyki nieholonomicznego manipulatora do postaci lańcuchowej, jeżeli zmienna φ nie jest dodana do przestrzeni stanu trójwahad la, z lożonej z katów q r = (θ 1, θ 2, θ 3. Wspó lrzedne transformacji z = h(φ, q r i sprzeżenie zwrotne ν = F (φ, q r zaproponowane w [68] sa lokalne (obowiazuj a jedynie dla θ i ( π 2, π 2, i = 1, 2 i mog a być zdefiniowane jako z nowymi wejściami z 1 = φ, z 2 = tan θ a 2 a 1 3 cos 3 θ 1, z 3 = a 3 tan θ 2, z 4 = θ 3, (2.17 ν 1 = cos θ 1 ( cos θ 2 u 2, u ν 2 = a 2 a 1 sin 3 cos 2 θ 1 cos 3 θ 2 + 3a 2 θ 1 sin θ 2 u 2 2 cos θ 1 cos 4 θ 2 Kinematyka nieholonomicznego trójwahad la przedstawiona w postaci lańcuchowej przyjmuje wówczas postać. ż 1 = ν 1, ż 2 = ν 2, ż 3 = z 2 ν 1, ż 4 = z 3 ν 1. (2.18

24 2. Kinematyka uk ladów nieholonomicznych Jak widać, zarówno kinematyka manipulatora z nieholonomicznymi sprzeg lami, jak i kinematyka wybranych klas ko lowych platform mobilnych, dadza sie przekszta lcić lokalnie lub globalnie do postaci lańcuchowej.

3. Modelowanie nieholonomicznych manipulatorów mobilnych Kinematyka nieholonomicznych uk ladów robotycznych przedstawiona w poprzednim rozdziale opisuje sposób, w jaki zmienne stanu zależa wzajemnie od siebie; innymi s lowy, przez kinematyke rozumie sie równania ograniczeń. W sterowaniu kinematyka zak lada sie, że możliwa jest bezpośrednia zmiana predkości, tak aby realizowana by la pewna trajektoria uk ladu. W rzeczywistości jednak zmiane stanu uk ladu można uzyskać jedynie poprzez sterowanie silnikami (elektrycznymi, pneumatycznymi lub hydraulicznymi, które przenosza momenty si l lub si ly w elementach mechanicznych robota. Równania opisujace zachowanie (ruch uk ladu w odpowiedzi na sygna ly pochodzace z napedów bedziemy nazywali dynamika uk ladu. Istnieje wiele metod otrzymywania równań dynamiki uk ladu mechanicznego, spośród których formalizm Hamiltona i formalizm Lagrange a znalaz ly najwieksze zastosowanie w robotyce. Równania Hamiltona ciesza sie duża popularnościa [4], [22], gdyż wychodzac z roważań energetycznych, pozwalaja w prosty sposób uzyskiwać nowe algorytmy sterowania, przede wszystkim algorytmy dysypatywne wykorzystujace pojecie bierności uk ladu [81], [87]. Z kolei formalizm Lagrange a ma pewne zalety, dzieki którym jest czesto spotykany w zastosowaniach robotycznych [36], [48]. Wynika to z kilku przes lanek. Po pierwsze, wspó lrzednymi uk ladu sa po lożenia i predkości uogólnione, które sa bardziej intuicyjne, niż pojecie pedów uogólnionych stosowane w formalizmie Hamiltona. Po drugie, równania Lagrange a pozwalaja na wykorzystanie pewnych strukturalnych w laściwości modelu dynamiki. Do wyprowadzenia równań dynamiki uk ladu z ograniczeniami nieholonomicznymi należy zastosować zasade d Alemberta. Należy również wspomnieć o jeszcze jednym podejściu do otrzymywania równań ruchu uk ladów z ograniczeniami, zwanym mechanika wakonomiczna. Takie

26 3. Modelowanie nieholonomicznych manipulatorów mobilnych podejście zosta lo zaproponowane przez Kozlova w pracy [35]. Różni sie ono od klasycznej mechaniki nieholonomicznej sposobem uwzglednienia ograniczeń w ruchu. W podejściu wakonomicznym ruch uk ladu mechanicznego z ograniczeniami jest rozważany jako standardowy problem wariacyjny, a równania ruchu otrzymuje sie z rachunku wariacyjnego przy za lożeniu, że w funkcjonale należy uwzglednić ograniczenia na lożone na system. Porównujac metode wakonomiczna i zasade d Alemberta, można zauważyć, że dla uk ladów nieholonomicznych obie metody daja różne równania ruchu. Równania te pokrywaja sie dla uk ladów holonomicznych. Warto podkreślić, że weryfikacja eksperymentalna równań ruchu otrzymywanych obiema metodami wykazuje zgodność trajektorii ruchu otrzymanych z zasady d Alemberta z wynikami praktycznie przeprowadzonych eksperymentów [41], co przesadza o zastosowaniu tej w laśnie metody do celów modelowania ruchu uk ladów nieholonomicznych. Znajomość modelu dynamiki nieliniowego uk ladu sterowania, jakim jest nieholonomiczny manipulator mobilny, ma decydujacy wp lyw na jakość realizowanych zadań. Przy nieznanym modelu opisujacym zachowanie sterowanego uk ladu nieliniowego nie jest możliwa stabilizacja systemu, a realizowane trajektorie lub ścieżki sa obarczone dużym b l edem w stosunku do trajektorii lub ścieżek zadanych. Jeśli wiec chcemy zastosować algorytm sterowania zapewniajacy poprawne dzia lanie systemu i wysoka jakość realizowanych zadań, np. poprzez osiagni ecie dużej dok ladności pozycjonowania, niezbedne staje sie poznanie modelu dynamiki rozważanego nieholonomicznego uk ladu sterowania. 3.1. Zasada d Alemberta Rozważmy uk lad mechaniczny, którego zachowanie jest opisane przez uogólnione wspó lrzedne q R n i predkości q R n, spe lniajace l (l < n niezależnych ograniczeń fazowych, majacych postać Pfaffa A(q q =. (3.1 Ograniczenia oddzia luja na system poprzez si ly wiezów o takiej postaci, aby równania by ly zawsze spe lnione. D Alembert przedstawi l swoja zasade w postaci formu ly: Si ly wiezów F, wymuszajace spe lnienie ograniczeń nieholonomicznych, nie wykonuja pracy na dopuszczalnych trajektoriach uk ladu. W myśl powyższej zasady zachodzi nastepuj aca zależność F T dq =. (3.2

3.2. Model dynamiki we wspó lrz ednych uogólnionych 27 Z kolei równanie ograniczeń Pfaffa można przedstawić w równoważnej postaci jako A(qdq =. (3.3 Z równań (3.3 i (3.2 wynika, że F T macierzy A(q, a wiec zachodzi zwiazek musi być kombinacja liniowa kolumn F T = λ T A(q F = A T (qλ, gdzie λ R l jest wektorem mnożników Lagrange a. Aby otrzymać równania dynamiki uk ladu z ograniczeniami (3.1, należy najpierw zdefiniować lagranżian dla uk ladu swobodnego, a wi ec bez ograniczeń fazowych L(q, q = K(q, q V (q, (3.4 przy czym poszczególne symbole maja nastepuj acy sens fizyczny: K(q, q = 1 2 qt Q(q q energia kinetyczna uk ladu swobodnego, V (q energia potencjalna uk ladu swobodnego. Równania ruchu dla uk ladu z ograniczeniami nieholonomicznymi otrzymuje si e z zasady d Alemberta, czyli z uwzgl ednieniem si l przyczepności F, co prowadzi do równań postaci d L dt q L q = AT (qλ. (3.5 Jeżeli natomiast na pewne wspó lrzedne uk ladu oddzia luja uogólnione si ly zewne- trzne τ, to równania ruchu (3.5 przyjmuja postać zmodyfikowana gdzie: d L dt q L q = AT (qλ + B(qτ, (3.6 τ R m wektor uogólnionych si l zewn etrznych, m = n l, B(q macierz wejściowa n m. 3.2. Model dynamiki we wspó lrz ednych uogólnionych Przed przystapieniem do wyprowadzenia równań dynamiki nieholonomicznego manipulatora mobilnego przyjmijmy nastepuj ace za lożenia:

28 3. Modelowanie nieholonomicznych manipulatorów mobilnych 1. Holonomiczny manipulator ma bezpośrednie nap edy dla wszystkich stopni swobody. 2. Liczba nap edów dla nieholonomicznej platformy jest równa n l. 3. Równania ruchu manipulatora mobilnego obejmuja model dynamiki oraz równanie ograniczeń nieholonomicznych, natomiast nie wymagaja równań wyjścia opisujacych wspó lrzedne chwytaka. Wynika to z faktu, że zak lada sie, iż zadanie sformu lowane w przestrzeni zewnetrznej może być przekszta lcone do przestrzeni wewnetrznej (przegubowej ramienia manipulatora. Przy takich za lożeniach, korzystajac z postaci funkcji Lagrange a (3.4 i postaci ograniczeń (3.1, można wyrazić równania dynamiki uk ladu nieholonomicznego (3.6 we wspó lrzednych uogólnionych jako Q(q q + C(q, q q + D(q = A T (qλ + B(qτ, (3.7 gdzie poszczególne elementy modelu oznaczaja: Q(q symetryczna, dodatnio określona macierz inercji rozmiaru n n, C(q, q macierz si l Coriolisa i si l odśrodkowych C(q, q = d dt Q(q 1 (Q(q q, 2 q D(q wektor oddzia lywań potencjalnych, najcz eściej wektor si l grawitacji. Wektor λ R l, nazywany wektorem mnożników Lagrange a, jest najtrudniejsza do określenia cześci a modelu, przy czym model we wspó lrzednych uogólnionych wymaga znalezienia jawnej postaci tych wyrażeń. Mnożniki Lagrange a λ można obliczyć rozwiazuj ac równania (3.1, (3.7 i wiedzac, że w kierunkach zabronionych przez ograniczenia nie może pojawić sie ruch, czyli że równanie (3.1 obowiazuje w każdej chwili czasu. Używajac równania (3.7 i różniczkujac po czasie równanie ograniczeń (3.1, otrzymujemy ( q = Q(q 1 C(q, q q D(q + A T (qλ + Bτ, A(q q + Ȧ(q q =. Po wstawieniu pierwszego równania do drugiego i pogrupowaniu wyrażeń dostajemy formu l e określajac a postać mnożników Lagrange a jako

3.2. Model dynamiki we wspó lrz ednych uogólnionych 29 ( AQ 1 A T λ = AQ 1 (C q + D Bτ + Ȧ q, przy czym macierz stojaca przed wektorem mnożników Lagrange a jest macierza pe lnego rzedu, jeśli ograniczenia sa niezależne. Widać, że mnożniki Lagrange a sa funkcjami uogólnionych po lożeń q i pred- kości q, a także użytych sterowań τ, co dodatkowo utrudnia ich obliczenie [36]. Ponadto model dynamiki we wspó lrzednych uogólnionych ma znacznie mniej sterowań, niż wynosi wymiar przestrzeni stanu q. 3.2.1. Model manipulatora mobilnego (nh, h we wspó lrz ednych uogólnionych Niech wektor wspó lrzednych uogólnionych manipulatora mobilnego bedzie oznaczony jako q = (qm, T qr T T, gdzie q m oznacza wektor wspó lrzednych platformy mobilnej, a q r to wektor wspó lrzednych przegubowych manipulatora. Przy za lożeniu jednorodności kó l, energia potencjalna platformy mobilnej poruszajacej sie po p laszczyźnie ekwipotencjalnej jest sta la, nie wp lywa zatem na równania ruchu uk ladu. Przy takim za lożeniu funkcja Lagrange a dla manipulatora mobilnego jest równa L(q, q = K m (q m, q m + K r (q, q V r (q, (3.8 gdzie: K m (q m, q m = 1 2 qt mq m (q m q m energia kinetyczna platformy mobilnej, K r (q, q = 1 2 qt Q r (q q energia kinetyczna manipulatora, V r (q energia potencjalna manipulatora. Energia kinetyczna manipulatora zależy zarówno od wspó lrzednych przegubowych q r ramienia manipulacyjnego, jak i od wspó lrzednych platformy mobilnej, na której zamontowano manipulator. Wynika to z faktu, że energia kinetyczna manipulatora jest wyrażana wzgledem nieruchomego uk ladu globalnego, a wiec manipulator może być rozważany jako uk lad do l aczony do cześci mobilnej. Z kolei równania ograniczeń (3.1 w manipulatorze mobilnym (nh, h odnosza sie jedynie do wspó lrzednych platformy mobilnej, jednak wp lywaja na zachowanie ca lego uk ladu. Wstawmy wyrażenie na funkcje Lagrange a (3.8 do równań dynamiki (3.7 otrzymanych z zasady d Alemberta. Równania dynamiki manipulatora mobilnego typu (nh, h przyjma wówczas postać

3 3. Modelowanie nieholonomicznych manipulatorów mobilnych [ Q11 Q 12 + gdzie Q 21 Q 22 [ Cm11 ] ( qm q r ] ( qm q r + + [ Qm11 ( D = ] ( qm q r [ A T λ ] + + [ C11 C 12 C 21 C 22 [ B I ] ( τm ] ( qm τ r q r, (3.9 [ ] Q11 Q Q r = 12 macierz bezw ladności manipulatora, Q 21 Q 22 [ ] Qm11 Q m = macierz bezw ladności platformy, [ ] C11 C C r = 12 macierz si l Coriolisa i si l odśrodkowych manipulatora, C 21 C 22 [ ] Cm11 C m = macierz si l Coriolisa i si l odśrodkowych platformy, D = V r q wektor grawitacji manipulatora, τ m wektor sterowań dla platformy, τ r wektor sterowań dla manipulatora. 3.2.2. Model manipulatora mobilnego (nh, nh we wspó lrz ednych uogólnionych Ograniczenia nieholonomiczne na lożone na ruch manipulatora mobilnego (nh, nh sa nieca lkowalne, dlatego do wyprowadzenia równań dynamiki należy zastosować zasade d Alemberta. Takie postepowanie prowadzi do równań postaci Q(q q + C(q, q q + D(q = A 11 (q m λ 1 + A 21 (q r λ 2 + B(qτ, (3.1 gdzie: Q(q = Q r (q + Q m (q m macierz bezw ladności manipulatora mobilnego, C(q, q = C r (q, q + C m (q m, q m macierz si l Coriolisa i si l odśrodkowych manipulatora mobilnego, D(q wektor grawitacji, A i1 macierz ograniczeń Pfaffa dla i-tego podsystemu, λ i wektor mnożników Lagrange a dla i-tego podsystemu, B(q macierz wejściowa, τ wektor sterowań.