Sterowanie oparte na modelu dla nieholonomicznych manipulatorów mobilnych

Prace Naukowe Instytutu Informatyki, Automatyki i Robotyki Nr 107 Politechniki WrocÃlawskiej Nr 107 Monografie Nr 31 2009 manipulator mobilny, ograniczenia nieholonomiczne, sterowanie kinematyczne, sterowanie dynamiczne Alicja MAZUR∗

STEROWANIE OPARTE NA MODELU

DLA NIEHOLONOMICZNYCH

MANIPULATOR ´

OW MOBILNYCH

W monografii przedstawiono jednolite podej´scie pozwalajace na znalezienie sterowa-_, nia dla nieholonomicznych manipulator´ow mobilnych w przypadku, gdy r´o˙zne zada-nia podlegaja dekompozycji na podzadania definiowane osobno dla ka˙zdego z sys-_, tem´ow skÃladowych, jakimi sa koÃlowa platforma mobilna oraz rami_, e manipulacyjne_, zamontowane na tej platformie. Spo´sr´od czterech typ´ow manipulator´ow mobilnych rozwa˙zono tylko takie, w kt´orych koÃlowa platforma porusza sie w spos´ob ´sci´sle toczny,_, bez po´slizgu i buksowania k´oÃl, a wiec jest nieholonomiczna. Natomiast w przy-_, padku manipulatora dopuszczono zar´owno bezpo´srednie napedy, jak i nap_, edy nieholo-_, nomiczne.

R´ownania ruchu ukÃlad´ow nieholonomicznych zawieraja r´ownania ogranicze´_, n, kt´ore musza by´c speÃlnione w ka˙zdej chwili, oraz r´ownania dynamiki, poÃl_, aczone_, w strukture kaskadow_, a. Z tego wzgl_, edu do projektowania sterowania dla r´o˙znych_, zada´n zastosowano podej´scie, kt´ore wymaga jednoczesnego rozwiazywania r´ownania_, ogranicze´n i u˙zycia otrzymanych rozwiaza´_, n do sterowania na poziomie dynamicznym. Jednym z czestych brak´ow spotykanych w wielu pracach jest nieuwzgl_, ednianie_, bÃled´ow pochodz_, acych z poziomu dynamiki i zakÃl´ocaj_, acych dziaÃlanie sterownika kine-_, matycznego (ukÃladu rozwiazuj_, acego r´ownania ogranicze´_, n), kt´ory jest projektowany w przypadku idealnym, a wiec bez wzi_, ecia pod uwag_, e efekt´ow dynamicznych, takich_, np. jak du˙za masa, czy bezwÃladno´s´c ukÃladu. W monografii przedstawiono takie

roz-∗_{Instytut Informatyki, Automatyki i Robotyki Politechniki WrocÃlawskiej, Wybrze˙ze}

wiazania dla wszystkich rozwa˙zanych zada´_, n, w kt´orych wspomniane bÃledy zostaÃly_, sprowadzone do zera. W przeciwnym razie nie mo˙zna zagwarantowa´c poprawnego dziaÃlania ukÃlad´ow sterowania podczas procesu regulacji. Proponowane w pracy algo-rytmy sterowania dziaÃlaja poprawnie, co potwierdzaj_, a dowody zbie˙zno´sci i badania_, symulacyjne.

Przedstawione algorytmy sterowania obejmuja wi_, ekszo´s´c zada´_, n, jakie mo˙zna sformuÃlowa´c dla ka˙zdego z podsystem´ow skÃladowych nieholonomicznego manipula-tora mobilnego: sterowanie do punktu, ´sledzenie trajektorii oraz ´sledzenie ´scie˙zki. Metoda postepowania w ka˙zdym przypadku jest podobna: nale˙zy znale´z´c algo-_, rytm kinematyczny zapewniajacy realizacj_, e zadania dla danego podukÃladu nieholo-_, nomicznego, a nastepnie wykorzysta´c otrzymane rozwi_, azanie do zaprojektowania_, sterowania na poziomie dynamicznym. Wyb´or jednego spo´sr´od zaprezentowanych algorytm´ow dynamicznych jest dowolny, jednak algorytm dysypatywny i uniwersalny moga w prosty spos´ob zosta´c zmodyfikowane do postaci adaptacyjnej, stosowanej_, w przypadku parametrycznej niepewno´sci co do modelu dynamiki.

SformuÃlowanie problemu sterowania w tak og´olnej postaci pozwala r´ownie˙z na realizacje zada´_, n mieszanych dla poszczeg´olnych podsystem´ow, np. platforma mo˙ze poda˙za´c do ustalonej konfiguracji, natomiast manipulator w tym samym czasie_, mo˙ze ´sledzi´c zadana trajektori_, e przegubow_, a. Jedynym warunkiem wymaganym do_, poprawnej realizacji zada´n jest u˙zycie algorytmu kinematycznego posiadajacego od-_, powiednie wÃla´sciwo´sci, na przykÃlad funkcje Lapunowa gwarantuj_, ac_, a globaln_, a lub_, p´oÃlglobalna asymptotyczn_, a stabilno´s´c dla ukÃladu z zamkni_, et_, a p_, etl_, a sprz_, e˙zenia zwrot-_, nego.

Prezentowane wyniki moga by´c zastosowane w procesie sterowania nieholonomicz-_, nymi manipulatorami mobilnymi podczas realizacji wielu zada´n, takich jak pobranie Ãladunku z ustalonego punktu przestrzeni roboczej, poda˙zanie wzdÃlu˙z zadanej tra-_, jektorii w przestrzeni wewnetrznej lub zewn_, etrznej, rozÃladowywanie cz_, e´sci Ãladunku_, podczas operacji transportowych itp.

1.

Wprowadzenie

1.1.

Nieholonomiczne manipulatory mobilne

Ograniczenia w ruchu ukÃlad´ow mechanicznych mo˙zna zaobserwowa´c w´owczas, gdy nie wszystkie trajektorie moga by´c przez system zrealizowane. Niekt´ore_, ograniczenia wynikaja z konstrukcji ukÃladu, a wi_, ec s_, a pochodzenia wewn_, etrznego_, (np. przegub manipulatora nie mo˙ze osiagn_, a´c poÃlo˙zenia poza ogranicznikami me-_, chanicznymi), inne za´s wynikaja ze sposobu realizacji ruchu, czyli pochodz_, a od_, otoczenia, w kt´orym ukÃlad sie przemieszcza. PrzykÃladem ograniczenia drugiego_, typu jest ruch samochodu po zboczu g´ory, czyli po pewnej powierzchni definiujacej_, grunt. Ograniczenia takie sa przykÃladami tzw. ogranicze´_, n holonomicznych, w kt´orych ruch ukÃladu jest ograniczony do gÃladkiej podrozmaito´sci w przestrzeni stanu Q. Ograniczenia holonomiczne mo˙zna przedstawi´c lokalnie jako ogranicze-nia postaci φ(q) =    φ1(q) .. . φ_k(q)   = 0. (1.1)

Ka˙zde przeksztaÃlcenie φ_i ogranicza ruch ukÃladu. W klasycznej literaturze z dzie-dziny mechaniki ograniczenia postaci (1.1) sa czasem nazywane ograniczenia-_, mi skleronomicznymi. Zar´owno sÃlowo ,,holonomiczne”, jak i ,,skleronomiczne” pochodzi z greki i oznacza odpowiednio ,,razem sÃluszne, poprawne” oraz ,,szty-wne”, przy czym ograniczeniami skleronomicznymi nazywane sa ograniczenia,_, kt´ore nie zawieraja czasu w jawnej postaci. Jednak w dziedzinie robotyki przyj_, eÃlo_, sie pierwsze okre´slenie i dlatego w dalszej cz_, e´sci b_, edziemy u˙zywa´c nazwy ,,ogra-_, niczenia holonomiczne”.

Ograniczenia (1.1) sa staÃle w czasie, dlatego mo˙zna je wyrazi´c jako_, ˙φ = ∂φ

∂q ˙q = 0,

to jest w tzw. postaci Pfaffa

gdzie A(q) =     ∂φ1 ∂q1 . . . ∂φ1 ∂qn . .. ∂φk ∂q1 . . . ∂φk ∂qn    

jest nazywana macierza ogranicze´_, n Pfaffa i jest peÃlnego rzedu._,

Innym rodzajem ogranicze´n, kt´ore moga by´c obserwowane w ruchu ukÃlad´ow,_, sa ograniczenia nieholonomiczne. Ograniczenia tego typu pojawiaj_, a si_, e w´owczas,_, gdy predko´sci ukÃladu s_, a ograniczone do (n − k)-wymiarowej podprzestrzeni, przy_, jednoczesnym braku ogranicze´n na dopuszczalne konfiguracje ukÃladu. Wyste-_, powanie ogranicze´n nieholonomicznych mo˙zna zaobserwowa´c podczas pr´oby za-parkowania samochodu w zatoczce. Samoch´od wykonuje w´owczas szereg ma-newr´ow majacych przemie´sci´c go w zaplanowane miejsce, gdy˙z nie jest mo˙zliwe_, wygenerowanie skÃladowej predko´sci w kierunku prostopadÃlym do powierzchni_, k´oÃl. Najcze´sciej powodem pojawienia si_, e ogranicze´_, n nieholonomicznych jest brak po´slizg´ow w ruchu tocznym w punkcie styku dw´och ciaÃl lub zasada zachowania momentu pedu._,

Ograniczenia nieholonomiczne pojawiaja si_, e w zachowaniu wielu obiekt´ow_, robotycznych. W przypadku koÃlowych robot´ow mobilnych obecno´s´c ogranicze´n nieholonomicznych wyra˙zonych w postaci Pfaffa (1.2) wynika z przyjecia zaÃlo˙zenia_, o braku po´slizgu w punkcie styczno´sci ka˙zdego koÃla z podÃlo˙zem. Z kolei, ma-nipulatory moga mie´c ograniczenia nieholonomiczne, gdy zostan_, a wyposa˙zone_, w palczasty chwytak lub gdy zostana skonstruowane z wykorzystaniem spec-_, jalnych sprzegieÃl Nakamury, Chunga i Sørdalena [68]. R´ownie˙z wiele robot´ow_, projektowanych do specjalnych zastosowa´n, takich jak roboty latajace (samoloty,_, sterowce, szybowce, helikoptery), roboty morskie (roboty podwodne, statki na-wodne, batyskafy), roboty do zastosowa´n kosmicznych maja cz_, esto ograniczenia_, nieholonomiczne naÃlo˙zone na ruch, wynikajace na przykÃlad ze sposobu przekazy-_, wania napedu. W ostatnich latach mo˙zna zauwa˙zy´c rosn_, ace zainteresowanie_, ukÃladami nieholonomicznymi. Przyczyna tego stanu rzeczy jest fakt, ˙ze wpraw-_, dzie sterowanie takimi systemami jest znacznie trudniejsze, gdy˙z sa to ukÃlady_, z deficytem sterowa´n, jednak wymagaja zastosowania mniejszej liczby ukÃlad´ow_, napedowych._,

Prezentowana praca dotyczy zagadnie´n sterowania dla specjalnych obiekt´ow robotycznych, jakimi sa nieholonomiczne manipulatory mobilne. Manipulator_, mobilny skÃlada sie ze sztywnego manipulatora zamontowanego na platformie mo-_, bilnej, zwanej w literaturze koÃlowym robotem mobilnym, wyposa˙zonej w niepodle-gajace deformacji koÃla. Umieszczenie manipulatora (ramienia robota) na plat-_, formie powoduje znaczne powiekszenie jego przestrzeni roboczej (dzi_, eki mobil-_, no´sci platformy) przy jednoczesnym zachowaniu zreczno´sci i mo˙zliwo´sci mani-_,

1.2. Przeglad zawarto´sci rozprawy_, 7 pulacyjnych ramienia, jednak jest to okupione istotnymi utrudnieniami w pro-cesie sterowania. Po pierwsze, poÃlaczenie dw´och podsystem´ow o r´o˙znej struk-_, turze powoduje powstanie znacznych sprze˙ze´_, n dynamicznych miedzy obydwo-_, ma czÃlonami, co mo˙ze na przykÃlad wywoÃla´c ruch platformy nawet w´owczas, gdy wÃlaczone s_, a nap_, edy tylko dla przegub´ow manipulatora. Po drugie, mo˙ze_, wystapi´c niejednoznaczno´s´c rozwi_, aza´_, n projektowanych zada´n dla obu podsys-tem´ow, na przykÃlad ´sledzenie ´scie˙zki lub trajektorii mo˙ze by´c realizowane tylko przez jeden podsystem (manipulator lub platforme) lub poprzez skoordynowane_, wsp´oÃldziaÃlanie obu podsystem´ow. W zwiazku z tym niezb_, edna jest precyzyjna_, definicja ´sledzonej ´scie˙zki lub trajektorii dla manipulatora mobilnego. Po trze-cie, pojawienie sie ogranicze´_, n naÃlo˙zonych na ruch tylko jednego podsystemu musi zosta´c uwzglednione w sterowaniu dla caÃlego ukÃladu zÃlo˙zonego._,

Ze wzgledu na rodzaj ogranicze´_, n naÃlo˙zonych na poszczeg´olne podsystemy, manipulatory mobilne mo˙zna podzieli´c 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´owno platforma, jak i manipulator nieholonomiczne. Ten

typ manipulatora mobilnego nosi nazwe podw´ojnie nieholonomicznego [86]._,

1.2.

Przegl

ad zawarto´sci rozprawy

_,

Niniejsza praca dotyczy problematyki sterowania w ´srodowisku wolnym od prze-szk´od dla nieholonomicznych manipulator´ow mobilnych dw´och ostatnich typ´ow, a wiec zawieraj_, acych nieholonomiczn_, a platform_, e mobiln_, a oraz zamontowany na_, niej manipulator o holonomicznym lub nieholonomicznym sposobie przekazywa-nia naped´ow. Poni˙zej zostanie pokr´otce om´owiona zawarto´s´c poszczeg´olnych roz-_, dziaÃl´ow monografii.

W rozdziale 2 przedstawiono ograniczenia nieholonomiczne, jakie moga po-_, jawi´c sie w ruchu manipulator´ow mobilnych. Ograniczenia te s_, a typu kine-_, matycznego, poniewa˙z wia˙z_, a pr_, edko´sci rozwa˙zanych obiekt´ow. Pokazano, w jaki_, spos´ob wyprowadza sie ograniczenia wynikaj_, ace z braku po´slizg´ow dla koÃlowych_, platform mobilnych wyposa˙zonych w koÃla konwencjonalne oraz jak otrzymuje sie_, ograniczenia predko´sciowe dla nap_, edu skonstruowanego przez Nakamur_, e, Chunga_, i Sørdalena. Poniewa˙z rozwa˙zane ograniczenia moga by´c przedstawione w postaci_, Pfaffa, om´owiono sposoby sprawdzania nieholonomiczno´sci ogranicze´n kinematy-cznych z wykorzystaniem tzw. maÃlej lub du˙zej flagi systemu. Ponadto podano

metode transformacji otrzymanych ogranicze´_, n do jednej z tzw. postaci normal-nych, jaka jest posta´c Ãla´_, ncuchowa. PrzeksztaÃlcenie r´owna´n kinematyki do tej postaci umo˙zliwia wykorzystanie znanych algorytm´ow sterowania dla ukÃlad´ow Ãla´ncuchowych.

RozdziaÃl 3 dotyczy sposob´ow modelowania zachowania nieholonomicznych manipulator´ow mobilnych. Poniewa˙z w pracy skupiono sie na manipulatorach_, mobilnych typu (nh, h) i (nh, nh), om´owiono zar´owno r´ownania kinematyki (r´ow-nania ogranicze´n), jak i r´ownania dynamiki dla obu wspomnianych typ´ow mani-pulator´ow mobilnych wyra˙zone w r´o˙znych wsp´oÃlrzednych. Wyb´or wsp´oÃlrz_, ednych,_, a wiec postaci modelu, jest zdeterminowany przez rodzaj zada´_, n, jakie manipula-tor mobilny ma realizowa´c podczas swojej pracy.

Problem sterowania zostaÃl sformuÃlowany w rozdziale 4. Ze wzgledu na kaska-_, dowa struktur_, e r´owna´_, n ruchu, do cel´ow sterowania manipulatorem mobilnym stosowany jest algorytm caÃlkowania wstecznego. Wymaga on r´ownoczesnego rozwiazania r´owna´_, n opisujacych ograniczenia nieholonomiczne (jest to tzw. ste-_, rowanie kinematyczne) i sterowania na poziomie dynamicznym, zar´owno przy peÃlnej znajomo´sci modelu dynamiki manipulatora mobilnego, jak i w przypadku parametrycznej nieznajomo´sci tego modelu (podano algorytm sterowania ada-ptacyjnego oraz uniwersalnego).

Sterowanie kinematyczne ma na celu rozwiazanie r´o˙znych zada´_, n dla pod-system´ow nieholonomicznych, tj. koÃlowych platform mobilnych z ograniczona_, mobilno´scia oraz manipulator´ow z nieholonomicznym przeniesieniem nap_, ed´ow._, W rozdziale 5 przedstawiono wybrane algorytmy kinematyczne, zar´owno pracuja-_, ce w petli otwartej, jak i w p_, etli zamkni_, etej. Rozwa˙zono algorytmy zapewniaj_, ace_, ´sledzenie trajektorii, ´sledzenie ´scie˙zki oraz sterowanie do punktu.

W rozdziale 6 przedstawiono algorytmy sterowania do punktu gwarantujace_, osiagni_, ecie ustalonej konfiguracji dla manipulator´ow typu (nh, h) i (nh, nh). Na_, przykÃladzie algorytmu wielomianowego dla zadania sterowania do punktu dla ukÃlad´ow Ãla´ncuchowych pokazano, jak dynamika wpÃlywa na trajektorie ukÃladu i dlaczego dla dokÃladnej realizacji wybranego celu niezbedne jest zastosowanie_, algorytm´ow ze sprze˙zeniem zwrotnym. W przypadku kaskadowego poÃl_, aczenia_, sterowania na poziomie kinematycznym i dynamicznym nale˙zy rozwa˙zy´c do-datkowe bÃledy pochodz_, ace z poziomu dynamicznego i zakÃl´ocaj_, ace rozwi_, azania_, teoretyczne, jakimi sa algorytmy kinematyczne._, Przy odpowiednim doborze parametr´ow regulacji mo˙zna pokaza´c asymptotyczna zbie˙zno´s´c do zera bÃl_, ed´ow_, pochodzacych z poziomu dynamicznego. W ten spos´ob mo˙zna udowodni´c po-_, prawna koordynacj_, e dziaÃla´_, n obu podsystem´ow, tj. platformy i manipulatora, sprze˙zonych dynamicznie._,

Kolejnym zadaniem dla manipulatora mobilnego, przedstawionym w rozdzia-le 7, jest ´srozdzia-ledzenie zadanej trajektorii definiowanej w przestrzeni wewnetrznej_,

1.2. Przeglad zawarto´sci rozprawy_, 9 manipulatora. Pokazano algorytmy sterowania dla obu rozpatrywanych typ´ow nieholonomicznych manipulator´ow mobilnych oraz dowody ich poprawnego dzia-Ãlania. W obu typach obiekt´ow w roli sterowania kinematycznego zastosowano algorytmy ´sledzenia trajektorii og´olnego przeznaczenia, jakimi sa linearyzacja_, dynamiczna oraz algorytm Jianga i Nijmeijera dla ukÃlad´ow Ãla´ncuchowych.

W rozdziale 8 om´owiono problematyke ´sledzenia trajektorii zdefiniowanej_, w przestrzeni zewnetrznej. Podstawow_, a metod_, a wykorzystywan_, a w tak sformu-_, Ãlowanym zadaniu jest odsprzeganie transformacji wej´sciowo-wyj´sciowej, a nas-_, tepnie sterowanie ukÃladem odsprz_, e˙zonym w celu realizacji zadania postawio-_, nego przed manipulatorem mobilnym. Punktem wyj´scia do uzyskania ukÃladu odsprze˙zonego jest podej´scie zaproponowane przez Yamamoto i Yuna, polegaj_, ace_, na odpowiednim wyborze wsp´oÃlrzednych efektora w zale˙zno´sci od liczby wej´s´c_, w podukÃladzie nieholonomicznym. Poniewa˙z metoda Yamamoto i Yuna cechuje sie pewnymi ograniczeniami, zaproponowano rozszerzenie wyboru funkcji wyj´scio-_, wych koniecznych do przeprowadzenia procedury odsprzegania do postaci, kt´ora_, umo˙zliwiÃlaby jednoczesne przemieszczanie platformy i poruszanie manipulatorem wzgledem podstawy. Taki spos´ob definiowania wydaje si_, e przydatny w procesie_, rozÃladowywania przewo˙zonych towar´ow podczas operacji transportowych. Roz-szerzone funkcje wyj´sciowe znajduja tak˙ze zastosowanie podczas ´sledzenia tra-_, jektorii zadaniowej dla podw´ojnie nieholonomicznego manipulatora mobilnego.

Ostatnie zadanie dla nieholonomicznych manipulator´ow mobilnych, czyli ´sle-dzenie ´scie˙zki, przedstawiono w rozdziale 9. Dla ka˙zdego z dw´och typ´ow obiekt´ow zadanie jest definiowane w odmienny spos´ob. Asymptotyczne ´sledzenie ´scie˙zki dla podukÃladu nieholonomicznego (koÃlowej platformy mobilnej lub manipulatora z nieholonomicznym napedem) mo˙ze by´c przeksztaÃlcone za pomoc_, a parametryza-_, cji Freneta do zadania sterowania do punktu dla bezdryfowego ukÃladu sterowa-nia. Z kolei dla holonomicznego manipulatora, bed_, acego cz_, e´sci_, a manipulatora_, mobilnego typu (nh, h), ´sledzenie ´scie˙zki jest rozumiane jako przemieszczanie sie_, wzdÃlu˙z ograniczonej krzywej sparametryzowanej odlegÃlo´scia krzywoliniow_, a, z za-_, trzymaniem na jej ko´ncu.

RozdziaÃl 10 zawiera podsumowanie zawarto´sci rozprawy i uwagi ko´ncowe. W szczeg´olno´sci podkre´slono oryginalne rozwiazania przedstawione w monografii_, w dziedzinie sterowania nieholonomicznych manipulator´ow mobilnych. W roz-dziale tym om´owiono r´ownie˙z otwarte kierunki bada´n i tematyke przyszÃlych prac._, Podstawowe definicje i twierdzenia o stabilno´sci dla r´o˙znych typ´ow ukÃlad´ow sterowania przedstawiono w rozdziale 11. Twierdzenia te sa podstawowym na-_, rzedziem do badania stabilno´sci nieholonomicznych manipulator´ow mobilnych._,

W tabeli 1.1 zestawiono kinematyczne i dynamiczne algorytmy sterowania u˙zyte do realizacji r´o˙znych zada´n formuÃlowanych dla nieholonomicznych mani-pulator´ow mobilnych, kt´orych dowody zamieszczono w monografii.

Tabela 1.1. Algorytmy sterowania realizujace r´o˙zne zadania_, dla nieholonomicznych manipulator´ow mobilnych, kt´orych 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 ´sledzenie z kompensacja bÃl_, ed´ow 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Ãled´ow na poziomie dynamicznym_,

´sledzenie wyj´scia Yamamoto i Yuna

trajektorii (nh, h) wyj´scia rozszerzone

zewnetrznej_, odsprzeganie i linearyzacja we–wy_, (rozdz. 8) (nh, nh) wyj´scia rozszerzone

odsprzeganie i linearyzacja we–wy_, kinematyczny dla platformy:

(nh, h) algorytm Samsona z parametryzacja Freneta_, ´sledzenie dynamiczny: dysypatywny z modyfikacja Galickiego_,

´scie˙zki 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´

ow

nieholonomicznych

Niniejszy rozdziaÃl prezentuje kinematyke ukÃlad´ow nieholonomicznych. Szczeg´ol-_, na uwag_, e zwr´ocono na r´ownania kinematyki nieholonomicznych koÃlowych plat-_, form mobilnych, a tak˙ze manipulator´ow o nieholonomicznym sposobie przeno-szenia naped´ow, poniewa˙z takie systemy robotyczne stanowi_, a cz_, e´sci skÃladowe_, nieholonomicznych manipulator´ow mobilnych typu (nh, h) oraz (nh, nh), rozwa-˙zanych w tej pracy.

Istnieje kilka powod´ow do specjalnego potraktowania obu wspomnianych ty-p´ow obiekt´ow robotycznych. Pierwszym z nich jest fakt, ˙ze zar´owno koÃlowe platformy mobilne, jak i manipulatory nieholonomiczne maja ograniczenia nieho-_, lonomiczne wynikajace z zaÃlo˙zenia o braku po´slizg´ow stykaj_, acych si_, e powierzchni_, podczas realizacji ruchu. Po drugie, warto przedstawi´c metode wyprowadza-_, nia ogranicze´n kinematycznych w systematyczny spos´ob, poniewa˙z artykuÃly pub-likowane w czasopismach czesto prezentuj_, a je w spos´ob uproszczony. Po trzecie,_, nieholonomiczne manipulatory mobilne byÃly obiektem bada´n autorki przez ostat-nie lata, co znalazÃlo wyraz w szeregu publikacji [47]–[56].

Zwrot ,,ukÃlady nieholonomiczne” oznacza ukÃlady z pewnym typem ogranicze´n naÃlo˙zonych na realizowane przez nie trajektorie. Podstawowe ´zr´odÃla ogranicze´n nieholonomicznych to zaÃlo˙zenie o braku po´slizgu (zerowym wektorze predko´sci_, chwilowych) w punkcie kontaktu pomiedzy stykaj_, acymi si_, e powierzchniami ele-_, ment´ow mechanicznych, a tak˙ze zasada zachowania pedu. Ograniczenia po-_, chodzace z pierwszego ´zr´odÃla s_, a to ograniczenia kinematyczne, natomiast drugi_, rodzaj ogranicze´n musi by´c rozwa˙zany na poziomie dynamicznym. W tej pracy omawiane bed_, a wyÃl_, acznie ograniczenia nieholonomiczne pochodzenia kinematy-_, cznego, czyli ograniczenia predko´sciowe._,

Rozwa˙zania dotyczace nieholonomiczno´sci ogranicze´_, n nale˙zy poprzedzi´c pew-nymi uwagami terminologiczpew-nymi. W wielu podrecznikach do robotyki [84], [15]_, kinematyka nazywa si_, e przeksztaÃlcenie mi_, edzy dwiema przestrzeniami poÃlo˙ze´_, n. Przestrze´n zawierajaca sterowane zmienne stanu jest nazywana przestrzeni_, a wew-_,

netrzn_, a (przegubow_, a, konfiguracyjn_, a), natomiast przestrze´_, n, w kt´orej definiuje sie ruch, nosi nazw_, e przestrzeni zewn_, etrznej (zadaniowej, roboczej). W klasy-_, cznej robotyce kinematyka prosta to transformacja z przestrzeni przegubowej do roboczej, transformacja za´s z przestrzeni roboczej do przegubowej to kinematyka odwrotna. Je´sli rozwa˙zane sa nie tylko poÃlo˙zenia, ale r´ownie˙z pochodne po czasie_, (predko´sci) zdefiniowane w obu przestrzeniach, to transformacj_, a Ãl_, acz_, ac_, a je jest_, macierz Jacobiego. Z kolei, kinematyka ukÃladu nieholonomicznego jest nazywane_, r´ownanie ogranicze´n naÃlo˙zonych na ruch takiego ukÃladu, wyra˙zone nastepuj_, ac_, a_, zale˙zno´scia mi_, edzy pr_, edko´sci_, a ˙q a poÃlo˙zeniem q ukÃladu_,

A(q) ˙q = 0.

Jak wspomniano we wstepie, taka posta´c kinematyki nazywa si_, e postaci_, a Pfaffa._, Nale˙zy zwr´oci´c szczeg´olna uwag_, e na fakt, ˙ze w postaci Pfaffa mo˙zna wyrazi´c_, zar´owno ograniczenia nieholonomiczne, jak i holonomiczne. Z tego wzgledu ist-_, nieje konieczno´s´c wprowadzenia kryterium pozwalajacego zweryfikowa´c nieholo-_, nomiczno´s´c ogranicze´n naÃlo˙zonych na ruch ukÃladu.

2.1.

Nieholonomiczno´s´

c ogranicze´

n

Rozwa˙zmy ukÃlad mechaniczny, kt´orego zachowanie jest opisane przez n uog´ol-nionych wsp´oÃlrzednych q ∈ R_, n _{i pr}

,

edko´sci ˙q ∈ Rn_{, speÃlniaj} ,

acych l (l < n) niezale˙znych ogranicze´n fazowych o postaci Pfaffa

A(q) ˙q = 0, (2.1)

gdzie:

A(q) – macierz (l × n) peÃlnego rzedu,_,

q(t) – trajektoria ukÃladu.

Obecno´s´c ogranicze´n (2.1) mo˙ze, ale nie musi, wpÃlywa´c na ograniczenie do-puszczalnych stan´ow ukÃladu. Zale˙zy to od wÃla´sciwo´sci ogranicze´n nazywanej

holonomiczno´scia._,

Ograniczenia holonomiczne oznaczaja, ˙ze macierz Pfaffa pomno˙zona z lewej_, strony przez pewna nieosobliw_, a macierz M (q) b_, edzie macierz_, a Jacobiego pewnego_, odwzorowania φ : Rn_{→ R}l_{, tzn.}

M (q)A(q) = ∂φ ∂q(q).

W´owczas (2.1) oznacza, ˙ze d

dtφ(q(t)) = 0, czyli φ(q) = const, a zatem trajektorie

ukÃladu sa zawarte w (n − l)-wymiarowej rozmaito´sci. Innymi sÃlowy, ograniczenia_, holonomiczne powoduja zmniejszenie przestrzeni konfiguracyjnej ukÃladu._,

2.1. Nieholonomiczno´s´c ogranicze´n 13 Inaczej sie przedstawia sytuacja w przypadku ogranicze´_, n nieholonomicznych. Je´sli ograniczenia speÃlniaja wÃlasno´s´c nieholonomiczno´sci, to oznacza, ˙ze nie jest_, mo˙zliwe scaÃlkowanie r´owna´n (2.1), a co za tym idzie, nie ulega zmniejszeniu przestrze´n stanu ukÃladu, natomiast spos´ob uzyskiwania pewnych konfiguracji mo˙ze ulec istotnemu utrudnieniu. W dalszych rozwa˙zaniach ograniczenia be-_, dziemy nazywali nieholonomicznymi, je˙zeli wszystkie spo´sr´od l ogranicze´n bed_, a_, nieholonomiczne (sa to tzw. ograniczenia caÃlkowicie nieholonomiczne)._,

ZaÃl´o˙zmy, ˙ze wszystkie ograniczenia sa nieholonomiczne. Z r´ownania (2.1)_, wynika, ˙ze dopuszczalne predko´sci ˙q ukÃladu w konfiguracji q nale˙z_, a do j_, adra_, (przestrzeni zerowej) macierzy Pfaffa

˙q ∈ Ker A(q).

Oznacza to, ˙ze predko´sci dopuszczalne mo˙zna wyrazi´c jako kombinacj_, e pewnych_, wektor´ow rozpinajacych j_, adro macierzy A(q), jak nast_, epuje_,

˙q =

m

X

i=1

gi(q)ui= G(q)u, (2.2)

przy czym macierz G(q) oblicza sie z r´ownania A(q)G(q) = 0. Z niezale˙zno´sci_, ogranicze´n fazowych wynika, ˙ze w ka˙zdym punkcie przestrzeni stanu rzad macierzy_,

G(q) jest peÃlny i r´owny

∀ q rank G(q) = n − l = m.

Pola wektorowe g1, g2, ..., gm (kolumny macierzy G) tworza w przestrzeni stanu_,

obiekt geometryczny

G = span _C∞{g₁, g₂, ..., g_m}

noszacy nazw_, e dystrybucji stowarzyszonej z ukÃladem (2.2). Ograniczenia fazowe_, (2.1) bed_, a speÃlnione w´owczas, gdy w ka˙zdym punkcie nale˙z_, acym do przestrzeni_, stanu predko´s´c ukÃladu b_, edzie nale˙zaÃla do dystrybucji, czyli b_, edzie kombinacj_, a_, p´ol wektorowych zdefiniowanych przez kolumny macierzy G(q). Pojecie dystry-_, bucji G stanowi punkt wyj´scia do sformuÃlowania kryterium nieholonomiczno´sci ogranicze´n [65].

Twierdzenie 1. Niech G = span_C∞{g₁, g₂, ..., g_m}. Zdefiniujmy ciag dystry-_, bucji

w kt´orym operacja [ , ] oznacza nawias Liego dw´och p´ol wektorowych, zdefinio-wany w nastepuj_, acy spos´ob_,

[f, g](q) = ∂g

∂qf (q) − ∂f ∂qg(q). Je˙zeli dla pewnego i = r zachodzi

dim Gr(q) = n,

to ograniczenia (2.1) sa nieholonomiczne. Najmniejsza liczba r o tej wÃla´sciwo´sci_, nosi nazwe stopnia nieholonomiczno´sci dystrybucji G._,

Ciag dystrybucji G_, 0 ⊂ G1 ⊂ ...Gi ⊂ ... nazywamy filtracja dystrybucji G (maÃl_, a_,

flaga systemu (2.2)). Poj_, ecie flagi dystrybucji pojawiÃlo si_, e np. w pracy [63],_, natomiast termin maÃla flaga wprowadzono za Mormulem.

Je˙zeli ka˙zda z dystrybucji Gi ma staÃly wymiar w ka˙zdym punkcie przestrzeni

stanu, tzn.

∀ q ∈ Rn dim Gi(q) = ri= const,

to filtracja jest regularna. Dla filtracji regularnej wymiary kolejnych wzrasta-jacych dystrybucji s_, a niemalej_, ace r_, 0 ≤ r1 ≤ · · · ≤ ri ≤ · · · i po pewnej liczbie

iteracji p ≤ n osiagaj_, a warto´s´c ustalon_, a r_, p. Ciag utworzony z wymiar´ow kolejnych_,

dystrybucji (r0, r1, ..., rp) nazywamy wektorem wzrostu dystrybucji G.

Je˙zeli speÃlniony jest warunek

r_p(q) = n ∀ q,

to ograniczenia sa w peÃlni nieholonomiczne i ukÃlad (2.2) jest sterowalny. Je˙zeli_, natomiast rp< n, to oznacza, ˙ze w´sr´od ogranicze´n Pfaffa wystepuj_, a ograniczenia_,

holonomiczne, a wiec warunek caÃlkowitej nieholonomiczno´sci nie jest speÃlniony._,

2.2.

Ograniczenia nieholonomiczne dla koÃlowych

platform mobilnych

KoÃlowe platformy mobilne (roboty mobilne) sa jednymi z najcz_, e´sciej rozwa˙zanych_, system´ow nieholonomicznych [16], [17], [77]. Platformy mobilne sa to pojazdy_, wyposa˙zone w niedeformowalne koÃla, kt´ore poruszaja si_, e po pÃlaszczy´znie ekwipo-_, tencjalnej (poziomej). ZaÃlo˙zenie o ruchu pÃlaskim zostaÃlo wprowadzone jedynie dla uproszczenia opisu i nie ogranicza wÃlasno´sci strukturalnych robota mobilnego.

2.2. Ograniczenia nieholonomiczne dla koÃlowych platform mobilnych 15 Ograniczenia nieholonomiczne w opisie koÃlowych platform mobilnych pojawia-ja si_, e w przypadku przyj_, ecia zaÃlo˙zenia o tocznym, bezpo´slizgowym charakterze_, ruchu k´oÃl. W dalszych rozwa˙zaniach przyjeto zaÃlo˙zenie, ˙ze koÃla, w jakie jest_, wyposa˙zona platforma, poruszaja si_, e bez po´slizg´ow. Takie wyidealizowane koÃla_, sa nazywane koÃlami konwencjonalnymi._,

KoÃla konwencjonalne, czyli bezpo´slizgowe, mo˙zna podzieli´c na nastepuj_, ace_, rodzaje [8]:

• koÃlo ustalone (umocowane do sztywnej osi) – jest to koÃlo, kt´orego orientacja

wzgledem ukÃladu lokalnego stowarzyszonego z platform_, a jest staÃla,_,

• koÃlo sterowane (kierownica) – jest to koÃlo, kt´orego ruch wzgledem ukÃladu_, lokalnego stowarzyszonego z platforma jest obrotem wok´oÃl osi pionowej_, przechodzacej przez ´srodek koÃla,_,

• koÃlo Castora – jest to koÃlo, kt´orego ruch wzgledem ukÃladu lokalnego sto-_, warzyszonego z platforma jest obrotem wok´oÃl osi pionowej nie przechodz_, acej_, przez ´srodek koÃla.

Ka˙zdy typ koÃla konwencjonalnego mo˙ze porusza´c sie bez po´slizgu, co jest r´ow-_, nowa˙zne z przyjeciem zaÃlo˙zenia, ˙ze pr_, edko´s´c chwilowa w punkcie kontaktu koÃla_, z podÃlo˙zem mo˙ze przyjmowa´c warto´s´c zerowa, co pokazano na rys. 2.1._,

a)

V

_P

V

_L

y

x

q

x

y

b)

R

R

V

f

f

Rys. 2.1. Predko´s´c koÃla w punkcie kontaktu z podÃlo˙zem: a) skÃladowa_, poprzeczna, b) skÃladowa wzdÃlu˙zna

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

W pracy [28] pokazano, ˙ze poÃlo˙zenie ukÃladu lokalnego X_iY_iZ_i (stowarzyszo-nego z i-tym koÃlem) wzgledem ukÃladu podstawowego mo˙zna wyrazi´c za pomoc_, a_, nastepuj_, acej transformacji_,

Aki₀ = Trans(X, x)Trans(Y, y)Rot(Z, θ)Rot(Z, αi)Trans(X, li)Rot(Z, βi)

·Trans(X, d_i)Rot(Z, γ_i)Rot(X, φ_i). (2.3)

Parametry αi, βi, γi sa k_, atami opisuj_, acymi geometri_, e i-tego koÃla, l_, i i di sa to_,

parametry opisujace umiejscowienie i-tego koÃla wzgl_, edem ´srodka masy nadwozia,_, a φ_i jest katem obrotu i-tego koÃla._,

Znajac parametry macierzy A_, ki₀ , mo˙zna wyprowadzi´c warunki na brak po´slizgu dla i-tego koÃla:

– brak po´slizgu bocznego

h

cos(αi+ βi+ γi) sin(αi+ βi+ γi) disin γi+ lisin(γi+ βi)

i R(θ)    ˙x ˙y ˙θ    + disin γiβ˙i= 0, (2.4)

– brak po´slizgu wzdÃlu˙znego (buksowania i blokowania)

h

− sin(αi+ βi+ γi) cos(αi+ βi+ γi) dicos γi+ licos(γi+ βi)

i R(θ)    ˙x ˙y ˙θ    + d_icos γ_iβ˙_i+ r_i˙φ_i = 0, (2.5) gdzie macierz R(θ) = Rot(Z, −θ) opisuje orientacje ukÃladu podstawowego wzgl_, e-_, dem ukÃladu lokalnego robota, za´s r_i jest promieniem i-tego koÃla.

Ograniczenia dla wszystkich k´oÃl, w jakie jest wyposa˙zona platforma, mo˙zna przedstawi´c w postaci og´olnej

C1(βs, βc)R(θ)    ˙x ˙y ˙θ   + C2β˙c= 0, (2.6) J1(βs, βc)R(θ)    ˙x ˙y ˙θ   + J2˙φ = 0, (2.7)

przy czym symbolem βs oznaczono wektor kat´ow orientacji dla wszystkich k´oÃl_,

sterowanych, za´s βcoznacza wektor kat´ow orientacji dla wszystkich k´oÃl Castora._,

Rozwa˙zmy teraz ograniczenia dane wzorem (2.6). W pracy [8] pokazano, ˙ze mo˙zliwo´s´c wykonania ruchu przez koÃlowa platform_, e mobiln_, a jest zwi_, azana_, z rzedem macierzy C_, 1(βs, βc). Aby podzieli´c koÃlowe platformy mobilne ze wzgledu_,

na mo˙zliwo´sci poruszania sie, Campion, Bastin i d’Andrea-Novel w pracy [6]_, wprowadzili nastepuj_, ace poj_, ecia:_,

2.2. Ograniczenia nieholonomiczne dla koÃlowych platform mobilnych 17

• stopie´n mobilno´sci σm – wymiar dostepnej przestrzeni pr_, edko´sci w punkcie_,

kontaktu k´oÃl z podÃlo˙zem

σm = 3 − rank C1,

• stopie´n sterowalno´sci σs – liczba k´oÃl sterowanych (kierownic) w platformie,

kt´ore moga niezale˙znie zmienia´c orientacj_, e._,

Badajac warto´sci, jakie mog_, a przyjmowa´c parametry σ_, m i σs, wida´c, ˙ze:

a) stopie´n mobilno´sci speÃlnia nier´owno´s´c 1 ≤ σm ≤ 3,

co oznacza, ˙ze rozpatrujemy tylko przypadki, w kt´orych ruch w przestrzeni tr´ojwymiarowej jest mo˙zliwy do wykonania,

b) stopie´n sterowalno´sci speÃlnia nier´owno´s´c 0 ≤ σ_s≤ 2,

c) zachodzi nastepuj_, acy zwi_, azek_,

2 ≤ σm+ σs≤ 3.

SpeÃlnienie tej nier´owno´sci oznacza, ˙ze robot jest niezdegenerowany, czyli w przypadku zbyt du˙zej liczby k´oÃl, ich ruch jest odpowiednio skoordynowany (mo˙ze by´c zastapiony jednym koÃlem)._,

Z powy˙zszych nier´owno´sci wynika, ˙ze jest 5 klas koÃlowych robot´ow mobilnych [8], co przedstawiono w tabeli 2.1. Definiujac typ platformy, nale˙zy poda´c jej parame-_, try (σ_m, σ_s). Spo´sr´od platform wymienionych w tabeli 2.1, wszystkie platformy, opr´ocz klasy (3, 0), sa nazywane platformami z ograniczon_, a mobilno´sci_, a._,

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 0 0 1 2 1

Warto zauwa˙zy´c, ˙ze dla k´oÃl ustalonych i sterowanych warunki na brak po´slizgu bocznego i wzdÃlu˙znego, przeliczone wedÃlug oznacze´n z rys. 2.1, maja posta´c:_,

• brak po´slizgu wzdÃlu˙znego

˙x cos θ + ˙y sin θ − R ˙φ = 0, (2.8)

• brak po´slizgu bocznego

˙x sin θ − ˙y cos θ = 0, (2.9) gdzie zmienne (x, y) oznaczaja wsp´oÃlrz_, edne punktu kontaktu koÃla z podÃlo˙zem,_,

θ jest orientacja koÃla, k_, at φ jest k_, atem obrotu koÃla, a R oznacza promie´_, n koÃla.

2.3.

Ograniczenia nieholonomiczne dla manipulator´

ow

W przypadku robot´ow manipulacyjnych pojawienie sie ogranicze´_, n nieholonomicz-nych zale˙zy od konstrukcji robota. Do poÃlowy lat 90. XX wieku terminem ,,manipulator nieholonomiczny” okre´slano manipulator z chwytakiem wielopal-czastym o specjalnej budowie palc´ow, w kt´orym ograniczenia nieholonomiczne pojawiaÃly sie przy zaÃlo˙zeniu punktowego bezpo´slizgowego kontaktu pomi_, edzy_, palcami a manipulowanym obiektem [65].

W poÃlowie lat 90. ubiegÃlego wieku Nakamura, Chung i Sørdalen przedsta-wili specjalny naped, w kt´orym za pomoc_, a dw´och niezale˙znych silnik´ow mo˙zna_, napedza´c dowoln_, a liczb_, e obrotowych stopni swobody manipulatora. W pracy [68]_, zaprezentowano prototyp planarnego manipulatora z takim napedem. Spos´ob_, przekazywania napedu jest w nim nieholonomiczny, oparty na zaÃlo˙zeniu o bez-_, po´slizgowym kontakcie pomiedzy zespoÃlem k´oÃl i kul tworz_, acych specjalne sprz_, egÃla_, umieszczone w przegubach robota. Schemat budowy nieholonomicznego sprzegÃla_, pokazano na rys. 2.2.

Nieholonomiczne sprzegÃlo skÃlada si_, e z koÃla wej´sciowego IW, kuli oraz dw´och_, k´oÃl wyj´sciowych – OW1 i OW2. KoÃlo wej´sciowe o promieniu rI jest

umieszczo-ne na biegunie kuli o promieniu R i styka sie z ni_, a w jednym punkcie, przy_, czym zakÃlada sie, ˙ze wszystkie kontakty w sprz_, egle s_, a bezpo´slizgowe. KoÃlo_, wej´sciowe jest zamocowane w pierwszym przegubie, a koÃla wyj´sciowe w przegubie nastepnym. KoÃlo IW obraca si_, e wok´oÃl nieruchomej osi α_, I z predko´sci_, a k_, atow_, a_, u2, kt´ora speÃlnia role sterowania. Obracaj_, ace si_, e koÃlo IW powoduje obr´ot kuli,_,

przy czym w punkcie kontaktu predko´s´c liniowa kuli ma tak_, a sam_, a warto´s´c jak_, predko´s´c liniowa koÃla IW, lecz z odwrotnym znakiem_,

rIu2= −RΩ1.

Kolejnym elementem sprzegÃla s_, a dwa koÃla wyj´sciowe o promieniach r_, O1 i rO2

2.3. Ograniczenia nieholonomiczne dla manipulator´ow 19 a)

OW

₁

OW

₂

IW

b) W 1 w 1,1 w 2,1 r_O 1 r_I OW₂ OW₁ R IW KULA r_O 2 a O a I q 1

Rys. 2.2. Nieholonomiczny naped: a) widok og´olny, b) rzut z g´ory_, Fig. 2.2. Nonholonomic gear: a) schematic of the gear, b) view from above

αO, kt´ora tworzy z osia koÃla wej´sciowego zmieniaj_, acy si_, e k_, at θ_, 1, bed_, acy zmienn_, a_,

przegubowa manipulatora. Pr_, edko´s´c k_, atowa ˙θ_, 1 = u1 jest drugim sterowaniem

dla manipulatora. O´s obrotu drugiego koÃla wyj´sciowego OW₂ jest umieszczona pod katem prostym w stosunku do osi α_, O.

R´ownania kinematyki sa sformuÃlowane w spos´ob rekurencyjny, pocz_, awszy od_, przegubu pierwszego, dlatego wprowad´zmy oznaczenie ω_i,j dla predko´sci k_, atowej_,

i-tego koÃla wyj´sciowego w j-tym przegubie. W pierwszym przegubie predko´s´c_, katowa ω_, 1,1 koÃla OW1 oraz predko´s´c k_, atowa ω_, 2,1 koÃla OW2 sa r´owne_,

ω1,1= −Ω1 R rO1 cos θ1, ω2,1= −Ω1 R rO2 cos µ π 2 − θ1 ¶ .

Po wyeliminowaniu predko´sci k_, atowej kuli Ω_, 1 r´ownania te przyjmuja posta´c_,

ω_1,1= rI

rO1

cos θ₁u₂, ω_2,1= rI

rO2

sin θ₁u₂.

Jak wida´c, sterowanie i-tym przegubem wymaga zmiany dw´och wielko´sci: predko´sci k_, atowej koÃla wej´sciowego IW w danym przegubie oraz pr_, edko´sci k_, atowej_, zmiennej przegubowej ˙θi. Transmisja predko´sci z przegubu o numerze (i − 1) do_,

koÃla wej´sciowego w przegubie i odbywa sie poprzez koÃlo OW_, 1przegubu o numerze

(i − 1) z pewnym wsp´oÃlczynnikiem przeÃlo˙zenia η1,i

˙ρi= η1,iω1,i−1.

Natomiast transmisja predko´sci z przegubu o numerze (i − 1) do zmiennej prze-_, gubowej θi odbywa sie poprzez koÃlo OW_, 2 w przegubie o numerze (i − 1) ze

˙θi = η2,iω2,i−1.

Majac podan_, a reguÃl_, e rekurencyjn_, a, mo˙zna otrzyma´c nast_, epuj_, ace r´ownania kine-_, matyki dla nieholonomicznego manipulatora o n przegubach (planarnego n-wa-hadÃla) ˙θ1 = u1, (2.10) ˙θi = aisin θi−1 i−2_Y j=1 cos θju2, i ∈ {2, ..., n}. (2.11)

Jak wynika z podanych r´owna´n, za pomoca dw´och sterowa´_, n u1 i u2 mo˙zna

sterowa´c wieloma przegubami nieholonomicznego manipulatora o ogniwach ro-tacyjnych ze sprzegÃlami konstrukcji Nakamury, Chunga i Sørdalena._,

2.4.

UkÃlady Ãla´

ncuchowe

Ograniczenia nieholonomiczne wystepuj_, a w ruchu wielu ukÃlad´ow mechanicznych._, W dotychczas rozwa˙zanych obiektach, tj. koÃlowych platformach mobilnych i ma-nipulatorach z nieholonomicznymi sprzegÃlami, postaci kinematyki r´o˙zni_, a si_, e. Mo-_, ˙zliwe jest jednak sprowadzenie wielu ogranicze´n do jednej z tzw. postaci normal-nych. Spo´sr´od postaci normalnych najcze´sciej stosowana jest posta´c Ãla´_, ncuchowa. Posta´c Ãla´ncuchowa jest to posta´c normalna, do kt´orej mo˙zna sprowadzi´c nieholonomiczne ograniczenia kinematyczne wielu ukÃlad´ow mechanicznych, zwÃlasz-cza ukÃlad´ow mobilnych. Mo˙zliwo´s´c modelowania r´owna´n kinematyki koÃlowych robot´ow mobilnych w tzw. postaci jednoÃla´ncuchowej pokazano w pracy [66], nato-miast dla samochodu z wieloma przyczepami konwersje do postaci Ãla´_, ncuchowej przedstawiono w [83].

Posta´c jednoÃla´ncuchowa ukÃladu nieholonomicznego wydaje sie atrakcyjna,_, poniewa˙z wiele ukÃlad´ow mobilnych, w tym dwie klasy koÃlowych platform mobil-nych, mianowicie klasa (2, 0) i (1, 1), a tak˙ze kinematyka manipulatora z nieholo-nomicznym napedem, daj_, a si_, e sprowadzi´c do takiej postaci._,

UkÃlad Ãla´ncuchowy mo˙zna zapisa´c w nastepuj_, acej postaci_, ˙x1 = u1, ˙x2 = u2, ˙x₃ = x₂u₁, .. . ˙xn = xn−1u1 (2.12)

2.4. UkÃlady Ãla´ncuchowe 21 lub w r´ownowa˙znej postaci bezdryfowego ukÃladu sterowania jako

˙x = g1(x)u1+ g2u2, (2.13) gdzie g1 =           1 0 x₂ x3 .. . xn−1           , g2 =           0 1 0 0 .. . 0           = e2.

Dla ukÃladu (2.13) mo˙zna obliczy´c maÃla flag_, e systemu_,

G₀ = {g₁, g₂}, G1 = {g1, g2, [g2, g1] = e3}, G2 = {g1, g2, e3, [g2, e3] = e4}, .. . Gn−2 = {g1, g2, ..., [g2, en−1] = en}.

Wektor wzrostu ukÃladu Ãla´ncuchowego wynosi (2, 3, 4, ..., n), a stopie´n nieholo-nomiczno´sci jest r´owny n − 2. Wida´c, ˙ze maÃla flaga ukÃladu Ãla´ncuchowego rozpina caÃla przestrze´_, n stanu, a wiec ukÃlad Ãla´_, ncuchowy jest sterowalny.

2.4.1. Posta´c Ãla´ncuchowa dla nieholonomicznych koÃlowych platform mobilnych

Jak wspomniano poprzednio, spo´sr´od nieholonomicznych koÃlowych platform mo-bilnych jedynie dwie klasy mo˙zna przeksztaÃlci´c do postaci Ãla´ncuchowej. Sa to_, klasy, kt´orych r´ownania kinematyki maja dwa wej´scia steruj_, ace: klasa (2, 0)_, i klasa (1, 1).

Klasa (2, 0), do kt´orej zalicza sie najprostsz_, a platform_, e mobiln_, a, czyli tzw._, monocykl, ma nastepuj_, ace r´ownania kinematyki_,

˙q_m =    ˙x ˙y ˙θ   =    cos θ 0 sin θ 0 0 1    Ã v ω ! , (2.14)

gdzie v jest predko´sci_, a liniow_, a, za´s ω pr_, edko´sci_, a k_, atow_, a platformy. Aby powy˙zsze_, r´ownania przeksztaÃlci´c do postaci Ãla´ncuchowej, nale˙zy zastosowa´c globalny dyfeo-morfizm

z1 = θ,

z2 = −x cos θ − y sin θ,

z3 = −x sin θ + y cos θ

oraz statyczne sprze˙zenie zwrotne_,

u1 = ω,

u2 = −v − ωz3.

PrzeksztaÃlcenie to pozwala na globalna transformacj_, e r´owna´_, n kinematyki (2.14) do postaci Ãla´ncuchowej

˙z1 = u1,

˙z2 = u2,

˙z3 = z2u1.

Natomiast klasa (1, 1) o kinematyce r´ownej ˙x = v, ˙y = tan θv, ˙θ = tan β l v, (2.15) ˙ β = ω,

po zastosowaniu lokalnej (| θ |< π/2, | β |< π/2) zmiany wsp´oÃlrzednych [65]_,

z₁ = x,

z2 = _{l cos}tan β_2θ,

z3 = tan θ,

z₄ = y oraz zdefiniowaniu nowych wej´s´c

u₁ = v, u2 = 2tan 2_{β tan θ} l2_cos2_θ v + 1 l cos2_{β cos}2_θω

przyjmuje posta´c Ãla´ncuchowa_,

˙z1 = u1,

˙z2 = u2,

˙z3 = z2u1,

2.4. UkÃlady Ãla´ncuchowe 23 2.4.2. Posta´c Ãla´ncuchowa dla manipulator´ow nieholonomicznych Aby sterowa´c manipulatorem nieholonomicznym, nale˙zy przeksztaÃlci´c jego kine-matyke do postaci Ãla´_, ncuchowej. Taki zabieg pozwala na u˙zycie wszystkich ist-niejacych algorytm´ow sterowania dedykowanych ukÃladom Ãla´_, ncuchowym, np. al-gorytmu Jianga i Nijmeijera [33] gwarantujacego ´sledzenie trajektorii, czy te˙z_, algorytmu sterowania sinusoidalnego [66] przeprowadzajacego ukÃlad do zadanego_, stanu ko´ncowego w okre´slonym horyzoncie czasowym.

W dalszych rozwa˙zaniach ograniczymy sie do nieholonomicznego manipula-_, tora typu tr´ojwahadÃlo, cho´c prezentowana procedura, podana przez Sørdalena w pracy [68], mo˙ze by´c uog´olniona do manipulatora o n przegubach. Kine-matyke nieholonomicznego tr´ojwahadÃla opisan_, a r´ownaniem (2.10) mo˙zna przed-_, stawi´c nastepuj_, aco_,

˙θ1 = u1,

˙θ2 = a2sin θ1u2,

˙θ3 = a3sin θ2cos θ1u2,

˙φ = cos θ1cos θ2u2,

(2.16)

gdzie φ jest orientacja koÃla OW_, 2 w drugim przegubie tr´ojwahadÃla. W pracy [68]

pokazano, ˙ze nie jest mo˙zliwe przeksztaÃlcenie kinematyki nieholonomicznego ma-nipulatora do postaci Ãla´ncuchowej, je˙zeli zmienna φ nie jest dodana do przestrzeni stanu tr´ojwahadÃla, zÃlo˙zonej z kat´ow q_, r = (θ1, θ2, θ3). Wsp´oÃlrzedne transformacji_,

z = h(φ, q_r) i sprze˙zenie zwrotne ν = F (φ, q_{, r}) zaproponowane w [68] sa lokalne_, (obowiazuj_, a jedynie dla θ_, i ∈ (−π₂,π₂), i = 1, 2) i moga by´c zdefiniowane jako_,

z1 = φ, z2 = a2a3_costan θ3_θ1₁, z3 = a3tan θ2, z4 = θ3, (2.17) z nowymi wej´sciami ν₁ = cos θ₁cos θ₂u₂, ν2 = a2a3 ³ u1

cos2_θ1cos3_θ2 + 3a2sin

2_θ

1sin θ2u2

cos θ1cos4θ2

´

.

Kinematyka nieholonomicznego tr´ojwahadÃla przedstawiona w postaci Ãla´ncuchowej przyjmuje w´owczas posta´c

˙z1 = ν1,

˙z₂ = ν₂,

˙z3 = z2ν1,

˙z4 = z3ν1.

Jak wida´c, zar´owno kinematyka manipulatora z nieholonomicznymi sprzegÃlami,_, jak i kinematyka wybranych klas koÃlowych platform mobilnych, dadza si_, e prze-_, ksztaÃlci´c lokalnie lub globalnie do postaci Ãla´ncuchowej.

3.

Modelowanie

nieholonomicznych

manipulator´

ow mobilnych

Kinematyka nieholonomicznych ukÃlad´ow robotycznych przedstawiona w poprzed-nim rozdziale opisuje spos´ob, w jaki zmienne stanu zale˙za wzajemnie od siebie;_, innymi sÃlowy, przez kinematyke rozumie si_, e r´ownania ogranicze´_, n. W sterowa-niu kinematyka zakÃlada si_, e, ˙ze mo˙zliwa jest bezpo´srednia zmiana pr_, edko´sci, tak_, aby realizowana byÃla pewna trajektoria ukÃladu. W rzeczywisto´sci jednak zmiane_, stanu ukÃladu mo˙zna uzyska´c jedynie poprzez sterowanie silnikami (elektrycznymi, pneumatycznymi lub hydraulicznymi), kt´ore przenosza momenty siÃl lub siÃly w ele-_, mentach mechanicznych robota. R´ownania opisujace zachowanie (ruch) ukÃladu_, w odpowiedzi na sygnaÃly pochodzace z nap_, ed´ow b_, edziemy nazywali dynamik_, a_,

ukÃladu.

Istnieje wiele metod otrzymywania r´owna´n dynamiki ukÃladu mechanicznego, spo´sr´od kt´orych formalizm Hamiltona i formalizm Lagrange’a znalazÃly najwieksze_, zastosowanie w robotyce. R´ownania Hamiltona ciesza si_, e du˙z_, a popularno´sci_, a_, [4], [22], gdy˙z wychodzac z rowa˙za´_, n energetycznych, pozwalaja w prosty spos´ob_, uzyskiwa´c nowe algorytmy sterowania, przede wszystkim algorytmy dysypatywne wykorzystujace poj_, ecie bierno´sci ukÃladu [81], [87]. Z kolei formalizm Lagrange’a_, ma pewne zalety, dzieki kt´orym jest cz_, esto spotykany w zastosowaniach roboty-_, cznych [36], [48]. Wynika to z kilku przesÃlanek. Po pierwsze, wsp´oÃlrzednymi_, ukÃladu sa poÃlo˙zenia i pr_, edko´sci uog´olnione, kt´ore s_, a bardziej intuicyjne, ni˙z_, pojecie p_, ed´ow uog´olnionych stosowane w formalizmie Hamiltona. Po drugie,_, r´ownania Lagrange’a pozwalaja na wykorzystanie pewnych strukturalnych wÃla-_, ´sciwo´sci modelu dynamiki. Do wyprowadzenia r´owna´n dynamiki ukÃladu z ogra-niczeniami nieholonomicznymi nale˙zy zastosowa´c zasade d’Alemberta._,

Nale˙zy r´ownie˙z wspomnie´c o jeszcze jednym podej´sciu do otrzymywania r´ow-na´n ruchu ukÃlad´ow z ograniczeniami, zwanym mechanika wakonomiczn_, a. Takie_,

podej´scie zostaÃlo zaproponowane przez Kozlova w pracy [35]. R´o˙zni sie ono_, od klasycznej mechaniki nieholonomicznej sposobem uwzglednienia ogranicze´_, n w ruchu. W podej´sciu wakonomicznym ruch ukÃladu mechanicznego z ogranicze-niami jest rozwa˙zany jako standardowy problem wariacyjny, a r´ownania ruchu otrzymuje sie z rachunku wariacyjnego przy zaÃlo˙zeniu, ˙ze w funkcjonale nale˙zy_, uwzgledni´c ograniczenia naÃlo˙zone na system. Por´ownuj_, ac metod_, e wakonomiczn_, a_, i zasade d’Alemberta, mo˙zna zauwa˙zy´c, ˙ze dla ukÃlad´ow nieholonomicznych obie_, metody daja r´o˙zne r´ownania ruchu. R´ownania te pokrywaj_, a si_, e dla ukÃlad´ow holo-_, nomicznych. Warto podkre´sli´c, ˙ze weryfikacja eksperymentalna r´owna´n ruchu otrzymywanych obiema metodami wykazuje zgodno´s´c trajektorii ruchu otrzy-manych z zasady d’Alemberta z wynikami praktycznie przeprowadzonych ekspery-ment´ow [41], co przesadza o zastosowaniu tej wÃla´snie metody do cel´ow modelo-_, wania ruchu ukÃlad´ow nieholonomicznych.

Znajomo´s´c modelu dynamiki nieliniowego ukÃladu sterowania, jakim jest nie-holonomiczny manipulator mobilny, ma decydujacy wpÃlyw na jako´s´c realizo-_, wanych zada´n. Przy nieznanym modelu opisujacym zachowanie sterowanego_, ukÃladu nieliniowego nie jest mo˙zliwa stabilizacja systemu, a realizowane trajekto-rie lub ´scie˙zki sa obarczone du˙zym bÃl_, edem w stosunku do trajektorii lub ´scie˙zek_, zadanych. Je´sli wiec chcemy zastosowa´c algorytm sterowania zapewniaj_, acy po-_, prawne dziaÃlanie systemu i wysoka jako´s´c realizowanych zada´_, n, np. poprzez osiagni_, ecie du˙zej dokÃladno´sci pozycjonowania, niezb_, edne staje si_, e poznanie mo-_, delu dynamiki rozwa˙zanego nieholonomicznego ukÃladu sterowania.

3.1.

Zasada d’Alemberta

Rozwa˙zmy ukÃlad mechaniczny, kt´orego zachowanie jest opisane przez uog´olnione wsp´oÃlrzedne q ∈ R_, n _{i pr}

,

edko´sci ˙q ∈ Rn_{, speÃlniaj} ,

ace l (l < n) niezale˙znych ogranicze´n fazowych, majacych posta´c Pfaffa_,

A(q) ˙q = 0. (3.1)

Ograniczenia oddziaÃluja na system poprzez siÃly wi_, ez´ow o takiej postaci, aby_, r´ownania byÃly zawsze speÃlnione. D’Alembert przedstawiÃl swoja zasad_, e w postaci_, formuÃly:

SiÃly wiez´ow F , wymuszaj_, ace speÃlnienie ogranicze´_, n nieholonomicznych, nie wykonuja pracy na dopuszczalnych trajektoriach ukÃladu._,

W my´sl powy˙zszej zasady zachodzi nastepuj_, aca zale˙zno´s´c_,

3.2. Model dynamiki we wsp´oÃlrzednych uog´olnionych_, 27 Z kolei r´ownanie ogranicze´n Pfaffa mo˙zna przedstawi´c w r´ownowa˙znej postaci jako

A(q)dq = 0. (3.3)

Z r´owna´n (3.3) i (3.2) wynika, ˙ze FT _{musi by´c kombinacj} ,

a liniowa kolumn_, macierzy A(q), a wiec zachodzi zwi_, azek_,

FT = λTA(q) −→ F = AT(q)λ, gdzie λ ∈ Rl _{jest wektorem mno˙znik´ow Lagrange’a.}

Aby otrzyma´c r´ownania dynamiki ukÃladu z ograniczeniami (3.1), nale˙zy naj-pierw zdefiniowa´c lagran˙zian dla ukÃladu swobodnego, a wiec bez ogranicze´_, n fa-zowych

L(q, ˙q) = K(q, ˙q) − V (q), (3.4)

przy czym poszczeg´olne symbole maja nast_, epuj_, acy sens fizyczny:_,

K(q, ˙q) = 1₂˙qTQ(q) ˙q – energia kinetyczna ukÃladu swobodnego, V (q) – energia potencjalna ukÃladu swobodnego.

R´ownania ruchu dla ukÃladu z ograniczeniami nieholonomicznymi otrzymuje sie_, z zasady d’Alemberta, czyli z uwzglednieniem siÃl przyczepno´sci F , co prowadzi_, do r´owna´n postaci d dt ∂L ∂ ˙q − ∂L ∂q = A T_{(q)λ. (3.5)}

Je˙zeli natomiast na pewne wsp´oÃlrzedne ukÃladu oddziaÃluj_, a uog´olnione siÃly zewn_, e-_, trzne τ , to r´ownania ruchu (3.5) przyjmuja posta´c zmodyfikowan_, a_,

d dt ∂L ∂ ˙q − ∂L ∂q = A T_{(q)λ + B(q)τ, (3.6)} gdzie:

τ ∈ Rm _{– wektor uog´olnionych siÃl zewn} ,

etrznych, m = n − l,

B(q) – macierz wej´sciowa n × m.

3.2.

Model dynamiki we wsp´

oÃlrz

ednych uog´

_,

olnionych

Przed przystapieniem do wyprowadzenia r´owna´_, n dynamiki nieholonomicznego manipulatora mobilnego przyjmijmy nastepuj_, ace zaÃlo˙zenia:_,

1. Holonomiczny manipulator ma bezpo´srednie napedy dla wszystkich stopni_, swobody.

2. Liczba naped´ow dla nieholonomicznej platformy jest r´owna n − l._,

3. R´ownania ruchu manipulatora mobilnego obejmuja model dynamiki oraz_, r´ownanie ogranicze´n nieholonomicznych, natomiast nie wymagaja r´owna´_, n wyj´scia opisujacych wsp´oÃlrz_, edne chwytaka. Wynika to z faktu, ˙ze zakÃlada_, sie, i˙z zadanie sformuÃlowane w przestrzeni zewn_, etrznej mo˙ze by´c prze-_, ksztaÃlcone do przestrzeni wewnetrznej (przegubowej) ramienia manipula-_, tora.

Przy takich zaÃlo˙zeniach, korzystajac z postaci funkcji Lagrange’a (3.4) i postaci_, ogranicze´n (3.1), mo˙zna wyrazi´c r´ownania dynamiki ukÃladu nieholonomicznego (3.6) we wsp´oÃlrzednych uog´olnionych jako_,

Q(q)¨q + C(q, ˙q) ˙q + D(q) = AT(q)λ + B(q)τ, (3.7) gdzie poszczeg´olne elementy modelu oznaczaja:_,

Q(q) – symetryczna, dodatnio okre´slona macierz inercji rozmiaru n × n, C(q, ˙q) – macierz siÃl Coriolisa i siÃl od´srodkowych

C(q, ˙q) = d dtQ(q) − 1 2 ∂ ∂q(Q(q) ˙q),

D(q) – wektor oddziaÃlywa´n potencjalnych, najcze´sciej wektor siÃl grawitacji._, Wektor λ ∈ Rl, nazywany wektorem mno˙znik´ow Lagrange’a, jest najtrudniejsza_, do okre´slenia cze´sci_, a modelu, przy czym model we wsp´oÃlrz_, ednych uog´olnionych_, wymaga znalezienia jawnej postaci tych wyra˙ze´n. Mno˙zniki Lagrange’a λ mo˙zna obliczy´c rozwiazuj_, ac r´ownania (3.1), (3.7) i wiedz_, ac, ˙ze w kierunkach zabro-_, nionych przez ograniczenia nie mo˙ze pojawi´c sie ruch, czyli ˙ze r´ownanie (3.1)_, obowiazuje w ka˙zdej chwili czasu. U˙zywaj_, ac r´ownania (3.7) i r´o˙zniczkuj_, ac po_, czasie r´ownanie ogranicze´n (3.1), otrzymujemy

¨

q = Q(q)−1³−C(q, ˙q) ˙q − D(q) + AT(q)λ + Bτ´, A(q)¨q + ˙A(q) ˙q = 0.

Po wstawieniu pierwszego r´ownania do drugiego i pogrupowaniu wyra˙ze´n dosta-jemy formuÃle okre´slaj_, ac_, a posta´c mno˙znik´ow Lagrange’a jako_,

3.2. Model dynamiki we wsp´oÃlrzednych uog´olnionych_, 29

³

AQ−1AT´λ = AQ−1(C ˙q + D − Bτ ) + ˙A ˙q,

przy czym macierz stojaca przed wektorem mno˙znik´ow Lagrange’a jest macierz_, a_, peÃlnego rzedu, je´sli ograniczenia s_, a niezale˙zne._,

Wida´c, ˙ze mno˙zniki Lagrange’a sa funkcjami uog´olnionych poÃlo˙ze´_, n q i pred-_, ko´sci ˙q, a tak˙ze u˙zytych sterowa´n τ , co dodatkowo utrudnia ich obliczenie [36]. Ponadto model dynamiki we wsp´oÃlrzednych uog´olnionych ma znacznie mniej_, sterowa´n, ni˙z wynosi wymiar przestrzeni stanu q.

3.2.1. Model manipulatora mobilnego (nh, h) we wsp´oÃlrzednych_, uog´olnionych

Niech wektor wsp´oÃlrzednych uog´olnionych manipulatora mobilnego b_, edzie ozna-_, czony jako q = (qT

m, qTr)T, gdzie qm oznacza wektor wsp´oÃlrzednych platformy mo-_,

bilnej, a q_rto wektor wsp´oÃlrzednych przegubowych manipulatora. Przy zaÃlo˙zeniu_, jednorodno´sci k´oÃl, energia potencjalna platformy mobilnej poruszajacej si_, e po_, pÃlaszczy´znie ekwipotencjalnej jest staÃla, nie wpÃlywa zatem na r´ownania ruchu ukÃladu. Przy takim zaÃlo˙zeniu funkcja Lagrange’a dla manipulatora mobilnego jest r´owna

L(q, ˙q) = Km(qm, ˙qm) + Kr(q, ˙q) − Vr(q), (3.8)

gdzie:

Km(qm, ˙qm) =1₂ ˙qmTQm(qm) ˙qm – energia kinetyczna platformy mobilnej, Kr(q, ˙q) = 1₂˙qTQr(q) ˙q – energia kinetyczna manipulatora,

V_r(q) – energia potencjalna manipulatora.

Energia kinetyczna manipulatora zale˙zy zar´owno od wsp´oÃlrzednych przegubowych_,

qr ramienia manipulacyjnego, jak i od wsp´oÃlrzednych platformy mobilnej, na_,

kt´orej zamontowano manipulator. Wynika to z faktu, ˙ze energia kinetyczna ma-nipulatora jest wyra˙zana wzgledem nieruchomego ukÃladu globalnego, a wi_, ec ma-_, nipulator mo˙ze by´c rozwa˙zany jako ukÃlad doÃlaczony do cz_, e´sci mobilnej. Z kolei_, r´ownania ogranicze´n (3.1) w manipulatorze mobilnym (nh, h) odnosza si_, e jedynie_, do wsp´oÃlrzednych platformy mobilnej, jednak wpÃlywaj_, a na zachowanie caÃlego_, ukÃladu.

Wstawmy wyra˙zenie na funkcje Lagrange’a (3.8) do r´owna´_, n dynamiki (3.7) otrzymanych z zasady d’Alemberta. R´ownania dynamiki manipulatora mobil-nego typu (nh, h) przyjma w´owczas posta´c_,

" Q11 Q12 Q21 Q22 # Ã ¨ qm ¨ qr ! + " Qm11 0 0 0 # Ã ¨ qm ¨ qr ! + " C11 C12 C21 C22 # Ã ˙qm ˙qr ! + " Cm11 0 0 0 # Ã ˙qm ˙q_r ! + Ã 0 D ! = " AT_λ 0 # + " B 0 0 I # Ã τm τ_r ! , (3.9) gdzie Qr= " Q₁₁ Q₁₂ Q21 Q22 #

– macierz bezwÃladno´sci manipulatora,

Qm=

"

Qm11 0

0 0

#

– macierz bezwÃladno´sci platformy,

Cr=

"

C11 C12

C₂₁ C₂₂

#

– macierz siÃl Coriolisa i siÃl od´srodkowych manipulatora,

Cm=

"

C_m11 0

0 0

#

– macierz siÃl Coriolisa i siÃl od´srodkowych platformy,

D = −∂Vr

∂q – wektor grawitacji manipulatora, τm – wektor sterowa´n dla platformy,

τr – wektor sterowa´n dla manipulatora.

3.2.2. Model manipulatora mobilnego (nh, nh) we wsp´oÃlrzednych_, uog´olnionych

Ograniczenia nieholonomiczne naÃlo˙zone na ruch manipulatora mobilnego (nh, nh) sa niecaÃlkowalne, dlatego do wyprowadzenia r´owna´_, n dynamiki nale˙zy zastosowa´c zasade d’Alemberta. Takie post_, epowanie prowadzi do r´owna´_, n postaci

Q(q)¨q + C(q, ˙q) ˙q + D(q) = A11(qm)λ1+ A21(qr)λ2+ B(q)τ, (3.10)

gdzie:

Q(q) = Q_r(q) + Q_m(q_m) – macierz bezwÃladno´sci manipulatora mobilnego,

C(q, ˙q) = Cr(q, ˙q) + Cm(qm, ˙qm) – macierz siÃl Coriolisa i siÃl od´srodkowych

ma-nipulatora mobilnego,

D(q) – wektor grawitacji,

Ai1 – macierz ogranicze´n Pfaffa dla i-tego podsystemu, λi – wektor mno˙znik´ow Lagrange’a dla i-tego podsystemu, B(q) – macierz wej´sciowa,