Algorytm kinematyki odwrotnej typu jakobianu pseudoodwrotnego dla manipulatorów mobilnych. Mariusz Janiak 1

Podobne dokumenty
VII. Elementy teorii stabilności. Funkcja Lapunowa. 1. Stabilność w sensie Lapunowa.

Manipulatory i roboty mobilne AR S1 semestr 5

Podstawy robotyki wykład V. Jakobian manipulatora. Osobliwości

Podstawy robotyki. Wykład II. Robert Muszyński Janusz Jakubiak Instytut Informatyki, Automatyki i Robotyki Politechnika Wrocławska

Jakobiany. Kinematykę we współrzędnych możemy potraktować jako operator przekształcający funkcje czasu

2.9. Kinematyka typowych struktur manipulatorów

Podstawy Robotyki Określenie kinematyki oraz dynamiki manipulatora

2.12. Zadania odwrotne kinematyki

Rozdział 3. Tensory. 3.1 Krzywoliniowe układy współrzędnych

II. Równania autonomiczne. 1. Podstawowe pojęcia.

Rozszerzony konspekt preskryptu do przedmiotu Podstawy Robotyki

SIMR 2016/2017, Analiza 2, wykład 1, Przestrzeń wektorowa

Politechnika Wrocławska, Wydział Informatyki i Zarządzania. Modelowanie

Podstawy robotyki - opis przedmiotu

Kinematyka robotów mobilnych

Podstawy robotyki wykład III. Kinematyka manipulatora

dr Mariusz Grządziel 15,29 kwietnia 2014 Przestrzeń R k R k = R R... R k razy Elementy R k wektory;

III. Układy liniowe równań różniczkowych. 1. Pojęcie stabilności rozwiązań.

Notacja Denavita-Hartenberga

Opis systemów dynamicznych w przestrzeni stanu. Wojciech Kurek , Gdańsk

Metody Lagrange a i Hamiltona w Mechanice

Zagadnienia brzegowe dla równań eliptycznych

POSTULATY MECHANIKI KWANTOWEJ cd i formalizm matematyczny

D l. D p. Rodzaje baz jezdnych robotów mobilnych

Wstęp do metod numerycznych Zadania numeryczne 2016/17 1

Informatyka I Lab 06, r.a. 2011/2012 prow. Sławomir Czarnecki. Zadania na laboratorium nr. 6

Obliczanie długości łuku krzywych. Autorzy: Witold Majdak

Egzamin 1 Strona 1. Egzamin - AR egz Zad 1. Rozwiązanie: Zad. 2. Rozwiązanie: Koła są takie same, więc prędkości kątowe też są takie same

1.1 Przegląd wybranych równań i modeli fizycznych. , u x1 x 2

Lokalna odwracalność odwzorowań, odwzorowania uwikłane

Mechanika Robotów. Wojciech Lisowski. 5 Planowanie trajektorii ruchu efektora w przestrzeni roboczej

5. Rozwiązywanie układów równań liniowych

Definicje i przykłady

Formy kwadratowe. Mirosław Sobolewski. Wydział Matematyki, Informatyki i Mechaniki UW. wykład z algebry liniowej Warszawa, styczeń 2009

Wykład 14. Elementy algebry macierzy

Sekantooptyki owali i ich własności

Mechanika Kwantowa. Maciej J. Mrowiński. 24 grudnia Funkcja falowa opisująca stan pewnej cząstki ma następującą postać: 2 x 2 )

Rozkłady wielu zmiennych

Programowanie liniowe

Funkcje analityczne. Wykład 4. Odwzorowania wiernokątne. Paweł Mleczko. Funkcje analityczne (rok akademicki 2016/2017) dla każdego s = (s.

Zadania do wykładu Jakościowa Teoria Równań Różniczkowych Zwyczajnych

= i Ponieważ pierwiastkami stopnia 3 z 1 są (jak łatwo wyliczyć) liczby 1, 1+i 3

WEKTORY I WARTOŚCI WŁASNE MACIERZY. = λ c (*) problem przybliżonego rozwiązania zagadnienia własnego dla operatorów w mechanice kwantowej

Układy równań i nierówności liniowych

OPISY PRZESTRZENNE I PRZEKSZTAŁCENIA

Informacja o przestrzeniach Hilberta

ALGEBRA LINIOWA Z ELEMENTAMI GEOMETRII ANALITYCZNEJ. 1. Ciała

INTERPOLACJA I APROKSYMACJA FUNKCJI

Równanie Schrödingera

Ćwiczenia nr 7. TEMATYKA: Krzywe Bézier a

Geometria Lista 0 Zadanie 1

Politechnika Wrocławska, Wydział Informatyki i Zarządzania. Modelowanie

Funkcje analityczne. Wykład 4. Odwzorowania wiernokątne. Paweł Mleczko. Funkcje analityczne (rok akademicki 2017/2018)

przy warunkach początkowych: 0 = 0, 0 = 0

Transformaty. Kodowanie transformujace

1 Relacje i odwzorowania

Więzy i ich klasyfikacja Wykład 2

Manipulator OOO z systemem wizyjnym

Laboratorium Podstaw Robotyki ĆWICZENIE 4

Informacja o przestrzeniach Sobolewa

OBLICZANIE POCHODNYCH FUNKCJI.

Wykład z modelowania matematycznego.

3. KINEMATYKA Kinematyka jest częścią mechaniki, która zajmuje się opisem ruchu ciał bez wnikania w jego przyczyny. Oznacza to, że nie interesuje nas

Układy równań i równania wyższych rzędów

6. Zagadnienie parkowania ciężarówki.

Laboratorium 5 Przybliżone metody rozwiązywania równań nieliniowych

WYKŁAD 6 KINEMATYKA PRZEPŁYWÓW CZĘŚĆ 2 1/11

ALGEBRA LINIOWA Z ELEMENTAMI GEOMETRII ANALITYCZNEJ

Teoretyczne podstawy programowania liniowego

Wykład z Technologii Informacyjnych. Piotr Mika

WYKŁAD 9 METODY ZMIENNEJ METRYKI

Podstawy Automatyki. Wykład 2 - podstawy matematyczne. dr inż. Jakub Możaryn. Warszawa, Instytut Automatyki i Robotyki

Zadania egzaminacyjne

KINEMATYKA I DYNAMIKA CIAŁA STAŁEGO. dr inż. Janusz Zachwieja wykład opracowany na podstawie literatury

PODSTAWY AUTOMATYKI. MATLAB - komputerowe środowisko obliczeń naukowoinżynierskich - podstawowe operacje na liczbach i macierzach.

Dystrybucje, wiadomości wstępne (I)

1. Zbadać liniową niezależność funkcji x, 1, x, x 2 w przestrzeni liniowej funkcji ciągłych na przedziale [ 1, ).

Przekształcanie równań stanu do postaci kanonicznej diagonalnej

1 Rozwiązywanie układów równań. Wyznaczniki. 2 Wektory kilka faktów użytkowych

1 Grupa SU(3) i klasyfikacja cząstek

1. Podstawowe pojęcia

Przekształcenia liniowe

Planowanie przejazdu przez zbiór punktów. zadania zrobotyzowanej inspekcji

PODSTAWY RACHUNKU WEKTOROWEGO

Akwizycja i przetwarzanie sygnałów cyfrowych

Przestrzeń unitarna. Jacek Kłopotowski. 23 października Katedra Matematyki i Ekonomii Matematycznej SGH

2 1 3 c c1. e 1, e 2,..., e n A= e 1 e 2...e n [ ] M. Przybycień Matematyczne Metody Fizyki I

Lista. Przestrzenie liniowe. Zadanie 1 Sprawdź, czy (V, +, ) jest przestrzenią liniową nadr :

Zadania kinematyki mechanizmów

macierze jednostkowe (identyczności) macierze diagonalne, które na przekątnej mają same

Dystrybucje. Marcin Orchel. 1 Wstęp Dystrybucje Pochodna dystrybucyjna Przestrzenie... 5

Obliczenia iteracyjne

ROZWIĄZYWANIE RÓWNAŃ NIELINIOWYCH

Rozdział 2. Krzywe stożkowe. 2.1 Elipsa. Krzywe stożkowe są zadane ogólnym równaniem kwadratowym na płaszczyźnie

W naukach technicznych większość rozpatrywanych wielkości możemy zapisać w jednej z trzech postaci: skalara, wektora oraz tensora.

MODEL MANIPULATORA O STRUKTURZE SZEREGOWEJ W PROGRAMACH CATIA I MATLAB MODEL OF SERIAL MANIPULATOR IN CATIA AND MATLAB

Grafika komputerowa Wykład 8 Modelowanie obiektów graficznych cz. II

Przestrzenie wektorowe

Zestaw zadań 14: Wektory i wartości własne. ) =

Wstęp do metod numerycznych Uwarunkowanie Eliminacja Gaussa. P. F. Góra

Transkrypt:

Na prawach rękopisu INSTYTUT INFORMATYKI AUTOMATYKI I ROBOTYKI POLITECHNIKI WROCŁAWSKIEJ Raport serii??? nr??/26 Algorytm kinematyki odwrotnej typu jakobianu pseudoodwrotnego dla manipulatorów mobilnych. Mariusz Janiak 1 Słowa kluczowe: przestrzeń endogeniczna, jakobian analityczny, algorytm sterowania, manipulator mobilny Wrocław 26

Spis treści 1 Wprowadzenie 2 2 Podstawowe pojęcia 2 3 Algorytm kinematyki odwrotnej 3 4 Modyfikacja algorytmu standardowego 5 5 Symulacje 5 6 Zakończenie 9

1 Wprowadzenie System robotyczny złożony z platformy mobilnej z zainstalowanym na niej manipulatorem stacjonarnym nazywamy manipulatorem mobilnym [2]. W ostatnich latach wzrosło zainteresowanie tego typu robotami. Po pierwsze, są one ciekawym obiektem badań dla naukowców zajmujących sie robotyką, po drugie istnieje szerokie pole zastosowań praktycznych dla tego typu konstrukcji takich jak funkcje usługowe, zadania specjalne, zastosowania rozrywkowe. Jednym z kluczowych zagadnień uwzględnianych przy sterowaniu manipulatorem mobilnym jest rozwiązanie zadania kinematyki odwrotnej. Ze względu na charakter ograniczeń, znalezienie kinematyki odwrotnej wymaga wyznaczenia sterowań realizujących ruch robota, a tym samym zaplanowania jego trajektorii. W pracach [1, 3] zaprezentowano algorytmy kinematyki odwrotnej dla nieholonomicznych robotów mobilnych wywodzące się z metody kontynuacji. W artykule [2] przedstawiono metodę endogenicznej przestrzeni konfiguracyjnej unifikującej algorytmy dla robotów stacjonarnych i mobilnych poprzez wprowadzenie wspólnych dla obu typów robotów notacji kinematyki i sformułowaniu problemu kinematyki odwrotnej, wprowadzając uogólnienie algorytmów dla manipulatorów mobilnych. Podstawową wadą przedstawionych tam jakobianowych algorytmów kinematyki odwrotnej jest to, że w rozwiązaniu nie uwzględniają naturalnych ograniczeń nakładanych na zmienne stanu robota przez jego konstrukcje. Przykładem takiego układu z ograniczeniami może być samochód, który ma ograniczone przyśpieszenie, jak również kąt skrętu kół przednich. Nieuwzględnienie ograniczeń tego typu przez algorytm powoduje, że trudno będzie go zaimplementować w rzeczywistym robocie. W pracy zajmiemy się ważonym algorytmem jakobianu pseudoodwrotnego [2], a dokładniej zaproponujemy jego modyfikację polegającą na uzmiennieniu wykorzystywanej w nim normy przestrzeni endogenicznej, co zapewni dodatkowy stopień swobody, pozwalający w pewnym zakresie wpływać na sposób działania algorytmu. Poprzez zmianę normy będziemy kontrolować wartości wybranych zmiennych stanu platformy mobilnej tak, by ograniczyć zakres ich zmian. Układ pracy jest następujący. Podrozdział drugi zawiera podstawowe pojęcia i sformułowania dotyczące ciągłego zadania kinematyki, jakobianu analitycznego oraz pseudoodwrotności jakobianu. W podrozdziale trzecim zaprezentowano algorytm kinematyki odwrotnej typu ważonego jakobianu pseudoodwrotnego wraz z jego wersję dyskretną z ograniczonym pasmem. W rozdziale czwartym pokazano jak poprzez uzmiennienie normy można ograniczyć wartości wybranej zmiennej stanu platformy mobilnej. Działanie zmodyfikowanego algorytmu kinematyki odwrotnej zilustrowano symulacjami w podrozdziale piątym. Podrozdział szósty podsumowuje otrzymane wyniki. 2 Podstawowe pojęcia Przedmiotem rozważań jest manipulator mobilny składający się z nieholonomicznej platformy mobilnej z zainstalowanym na niej manipulatorem sztywnym. Oznaczmy posturę platformy przez q R n, wektor sterowań platformy przez u R m, wektor konfiguracji manipulatora przez x R p, zaś wektor pozycji efektora w przestrzeni zadaniowej przez y R r. Przyjmując taką notację, kinematykę manipulatora mobilnego możemy zapisać w postaci bezdryfowego układu sterowania z funkcją wyjścia { m q = G(q)u = i=1 g i(q)u i, (1) y = k(q, x) = (k 1 (q, x),..., k r (q, x)). Przyjmiemy, że występujące w (1) funkcje sterujące są całkowalne z kwadratem na pewnym interwale [, T ], u( ) L 2 m [, T ]. Dla tak zdefiniowanego układu przestrzeń liniową X = L2 m [, T ] Rp będziemy nazywali endogeniczną przestrzenią konfiguracyjną manipulatora mobilnego, a należące do niej pary (u( ), x) jego konfiguracjami endogenicznymi.przyjmując stałą macierz funkcji wagowych R(t), R(t) = R T (t) >, t [, T ] oraz dodatniookreśloną macierz wagową W = W T >, możemy zdefiniować w przestrzeni X iloczyn skalarny w następujący sposób (u 1 ( ), x 1 ), (u 2 ( ), x 2 ) RW = 1 Praca została wykonana w ramach grantu statutowego T u T 1 (t)r(t)u 2 (t)dt + x T 1 W x 2, (2) 2

co czyni z X przestrzeń Hilberta z normą zdefiniowaną jako (u( ), x) RW = T u T (t)r(t)u(t)dt + x T W x Przy ustalonej posturze początkowej platformy q R n i horyzoncie T > odwzorowanie zdefiniowane przez K q,t : X R r 1/2. (3) K q,t (u( ), x) = k(ϕ q,t (u( )), x) = y(t) (4) traktujemy jako kinematykę chwilową manipulatora mobilnego. Odpowiadający jej jakobian analityczny J q,t (u( ), x) : X R r, (5) będący pochodną kinematyki (4), obliczamy następująco J q,t (u( ), x)(v( ), w) = d dα K q,t (u( ) + αv( ), x + αw) = α= T C(T, x) Φ(T, s)b(s)v(s)ds + D(T, x)w, gdzie C(t, x) = k q (q(t), x), t Φ(t, s) = A(t)Φ(t, s), Φ(s, s) = I n, A(t) = q (G(q(t))u(t)), B(t) = G(q(t)), q(t) = ϕ q,t(u( )), D(t, x) = k x (q(t), x). Najważniejszą zaletą wykorzystania jakobianu analitycznego do rozwiązania odwrotnego zadania kinematyki jest zastąpienie nieliniowego równania (4) przez liniowe równanie jakobianowe postaci J q,t (u( ), x)(v( ), w) = η. (7) W konfiguracjach nieosobliwych istnieje rozwiązanie równania jakobianowego dla dowolnego η R r. 3 Algorytm kinematyki odwrotnej Mając daną kinematykę (4) oraz pozycję zadaną efektora w przestrzeni zadaniowej y d R r i położenie początkowe manipulatora (q, x ) R n+p, zadanie odwrotne kinematyki dla manipulatora mobilnego polega na znalezieniu, na określonym horyzoncie sterowania T, konfiguracji (u( ), x) X takiej, że K q,t (u( ), x) = y d. Załóżmy, że istnieje rozwiązanie odwrotnego zadania kinematyki. Według metody endogenicznej przestrzeni konfiguracyjnej [2] jako to rozwiązanie można przyjąć granice ( ) ud (t) = lim x d θ + trajektorii układu danego równaniem ( ) d uθ (t) = γ dθ x(θ) [ R γ 1 (t)bθ T (t)φt θ (T, t)ct θ ( uθ (t) x(θ) ) ( ) J # q (u,t θ( ), x(θ))e(θ) (t) = ] (T, x) W 1 Dθ T (T, x) D 1 q (u,t θ( ), x(θ))e(θ), gdzie J # q (u( ), x) oznacza prawą pseudoodwrotność jakobianu (5), tzn. J,T q,t (u( ), x)j # q (u( ), x) = I,T r, θ R, zaś D 1 q,t (u( ), x) jest odwrotnością macierzy zręczności manipulatora mobilnego danej wzorem D q,t (u( ), x) = D(T, x)w 1 D T (T, x)+ T C(T, x) Φ(T, s)b(s)r 1 (s)b T (s)φ T (T, s)ds C T (T, x), 3 (6) (8) (9) (1)

a e(θ) = K q,t (u θ ( ), x(θ)) yd błędem śledzenia trajektorii. Uzyskany tym sposobem algorytm nazywamy ważonym pseudoodwrotnym jakobianowym algorytmem kinematyki odwrotnej dla manipulatorów mobilnych. Z praktycznego punktu widzenia, implementując przedstawiony powyżej algorytm kinematyki odwrotnej, łatwiej operować na skończenie wymiarowej reprezentacji (parametryzacji) endogenicznej przestrzeni konfiguracyjnej niż na niej samej. Do tego zmuszeni jesteśmy do przekształcenia algorytmu z postaci ciągłej na jej dyskretny odpowiednik. By osiągnąć pierwszy cel należy przyjąć ortogonalną bazę w L 2 m [, T ] z funkcjami bazowymi ϕ (t), ϕ 1 (t),..., a następnie sterowania platformy u(t) wyrazić w postaci skończonego szeregu funkcji bazowych z odpowiednimi współczynnikami wagowymi. Dzięki tej operacji uzyskamy reprezentację endogenicznej przestrzeni konfiguracyjnej z ograniczonym pasmem (u(t), x) (F (t)λ, x), (11) gdzie λ jest wektorem zawierającym współczynniki szeregu, λ R s, s = m+ m i=1 s i, s i jest długością rozwinięcia i-tej składowej sterowania, a F (t) jest macierzą blokową diagonalną F (t) = block diag{f 1 (t), F 2 (t),..., F m (t)}, której każdy blok składa się z zestawu funkcji bazowych F i (t) = [ϕ (t), ϕ 1 (t),..., ϕ si (t)]. Teraz w odniesieniu do (11), wektor sterowań platformy możemy wyrazić w postaci s i u i (t) = λ ij ϕ j (t). (12) j= Łatwo zauważyć, że według formuły (11), dana konfiguracja endogeniczną (u( ), x) X jest reprezentowana przez punkt (λ, x) R s+p. Ustalając więc s-elementową parametryzację przestrzeni sterowań platformy, otrzymujemy skończenie wymiarową endogeniczną przestrzeń konfiguracyjną z ograniczonym pasmem X = R s+p. Mając na względzie (3) przestrzeń tą można unormować w następujący sposób (λ, x) RW = (λ T P λ + x T W x) 1/2, (13) gdzie macierz wagowa P = T F T (t)r(t)f (t)dt. W przyjętej parametryzacji jakobianowi (5) odpowiada jakobian analityczny z ograniczonym pasmem J q,t (λ, x) : X R r (14) danej w postaci Teraz równanie (9) przyjmuje postać [ T J q,t (λ, x) = C(T, x) Φ(T, s)b(s)f (s)ds D(T, x) d dθ ( λ(θ) x(θ) zawierające macierz zręczności zdefiniowaną przez ) = γ J # q,t (λ(θ), x(θ))e(θ) = ]. (15) (16) γs J q T 1,T (λ(θ), x(θ)) D q,t (λ(θ), x(θ))e(θ), D q,t (λ(θ), x(θ)) = J q,t (λ(θ), x(θ))s J T q,t (λ(θ), x(θ)), (17) [ ] P 1 gdzie S = W 1 jest odwrotnością macierzy wagowej. Ostatecznie postawione na początku tego podrozdziału odwrotne zadanie kinematyki możemy rozwiązać za pomocą ważonego pseudoodwrotnego jakobianowego algorytmu kinematyki odwrotnej z ograniczonym pasmem (16) w postaci dyskretnej określonego poniższą formułą ( ) ( ) λ θ+1 λ θ x θ+1 = x θ γ J # q,t (λ(θ), x(θ))e(θ). (18) Algorytm w tej postaci będziemy dalej nazywali algorytmem standardowym. 4

4 Modyfikacja algorytmu standardowego Zmodyfikujmy definicję normy (13) tak, by jej wartość była zależna od krzywej (λ(θ), x(θ)) X (λ, x) θ RW = (λ T P θ λ + x T W x) 1/2. (19) Zabieg ten dostarcza nam dodatkowego stopnia swobody, pozwalającego wpływać na efekt działania algorytmu. Zmieniając odpowiednio normę będziemy kontrolować wartości wybranych zmiennych stanu platformy mobilnej. W szczególności będzie nam zależeć by ograniczyć zakres ich zmian. Ponieważ macierz P określa część macierzy wagowej S, dlatego w dalszych rozważaniach zajmiemy się wyłącznie tą drugą. Przyjmiemy dla uproszczenia, że macierz S jest diagonalna. Rozpiszmy równie (18) tak, by wyeksponować sposób modyfikacji poszczególnych współczynników λ gdzie i = 1, 2,..., s oraz λ θ+1 i = λ θ i γsθ i ( J 1i θ θ 1 + J 2i θ θ 2 +... + J ri θ θ r ), (2) θ 1 = ( D θ ) 1 11 eθ 1 + ( D θ ) 1 12 eθ 2 +... + ( D θ ) 1 1r eθ r, θ 2 = ( D θ ) 1 21 eθ 1 + ( D θ ) 1 22 eθ 2 +... + ( D θ ) 1 2r eθ r,. θ r = ( D θ ) 1 r1 eθ 1 + ( D θ ) 1 r2 eθ 2 +... + ( D θ ) 1 rr e θ r, zaś J ij θ oznacza ij-ty element macierzy J q,t (λ(θ), x(θ)), ( D θ ) 1 ij ij-ty element odwrotności macierzy zręczności D 1 q (λ(θ), x(θ)), a,t eθ i i-ty element wektora błędu e(θ). Wprowadźmy następujące prawo opisujące ewolucję elementów macierzy wagowej S wzdłuż krzywej (λ(θ), x(θ)) X ) Si (S θ = min ε i imax, γk max Γ θ 1, (21) i gdzie ε i oznacza maksymalny zakres zmian elementów λ i, k max maksymalną liczbę kroków algorytmu, S imax maksymalną wartość jaką może przyjąć element S i oraz Γ θ i = ( J θ 1i θ 1 + J θ 2i θ 2 +... + J θ ri θ r ). Zakładając, że Γ θ 1 i Γ θ i otrzymamy algorytm ważonego jakobianu pseudoodwrotnego ze zmienną normą ( ) ( ) ( λ θ+1 λ θ ε i x θ+1 = x θ k max sign(γ θ i ). ) γ J #, (22) q,t (λ(θ), x(θ))e(θ) będący modyfikacją standardowego algorytmu (18) Znając wartość początkową λ oraz zakres zmian ε i możemy utrzymać wybrany współczynnik λ i w zadanych granicach, a przez to pośrednio ograniczyć zmianę wartości wybranej współrzędnej stanu platformy mobilnej. W standardowym algorytmie jakobianu pseudoodwrotnego jako rozwiązanie przyjmuje się granicę (8). Takie rozwiązanie nie może być zastosowane w zmodyfikowanym algorytmie (22) ponieważ przy k max + algorytm przestałby działać, λ θ+1 i = λ θ i. Z praktycznego punktu widzenia nie jest to wielką wadą, ponieważ podczas symulacji często przyjmuje się maksymalną liczbę kroków k max jako warunek stopu algorytmu (18). Dodatkowo w otoczeniu punktu docelowego y d, Γ θ i konieczne jest wprowadzenie górnego ograniczenia S imax w celu zapewnienia stabilności numerycznej algorytmu (22). 5 Symulacje Przedstawiony w poprzednim podrozdziale algorytm kinematyki odwrotnej typu ważonego jakobianu pseudoodwrotnego ze zmienną normą, został wykorzystany do sterowania manipulatorem mobilnym, składającym się z platformy mobilnej typu samochod kinematyczny z zainstalowanym na pokładzie manipulatorem o strukturze RTR, rys. 1. Wektor postury platformy q = (q 1, q 2, q 3, q 4 ) = (x, y, φ, ψ) R 4 składa się z pozycji i orientacji 5

l 2 x 3 l 3 x 2 l q 4 x 1 (q,q ) 1 2 q 3 Rysunek 1: Manipulator RTR zamontowany na samochodzie kinematycznym platformy oraz kąta skrętu przednich kół. Wektor x = (x 1, x 2, x 3 ) R 3 oznacza konfigurację manipulatora. Współrzędne zadaniowe y = (y 1, y 2, y 3 ) R 3 związane są z kartezjańską pozycją efektora manipulatora. Kinematykę (1) tak skonstruowanego manipulatora mobilnego możemy zapisać w następujący sposób { q1 = u 1 cos q 3 cos q 4, q 2 = u 1 sin q 3 cos q 4, q 3 = u 1 sin q 4, q 4 = u 2, y = y 1 y 2 = q 1 + (l 2 + l 3 cos x 3 ) cos(q 3 + x 1 ) q 2 + (l 2 + l 3 cos x 3 ) sin(q 3 + x 1 ). y 3 x 2 + l 3 sin x 3 W symulacjach każde ze składowych sterowania reprezentowano za pomocą dwóch harmonicznych s 1 = s 2 = 2 w bazie Furiera: ϕ = 1, ϕ 1 = sin 2πt, ϕ 2 = cos 2πt, ϕ 3 = sin 4πt, ϕ 4 = cos 4πt. Okres sterowania wynosił T = 1. Analizowany problem polegał na rozwiązaniu odwrotnego zadania kinematyki tak, by jednocześnie ograniczyć kąt skrętu przednich kół platformy do zbioru wartości q 4 ( π 2, π 2 ). Stąd algorytm (22) zastosowano w taki sposób, by ograniczyć sterowanie u 2, czyli wyłącznie dla współczynników λ 6,7,...,1, związanych z tym sterowaniem. Przedstawione poniżej symulacje przeprowadzono dla trzech różnych początkowych położeń platformy, oznaczonych przez C 1, C 2, C 3 (kolejno coraz dalej od celu) przy niezmienionej początkowej jej orientacji i położeniu jej kół, stałej konfiguracji początkowej manipulatora, takich samych wartościach λ, y d, k max, S imax, ɛ i, R, R oraz W. Wyniki symulacji zebrane zostały w tab. 1. Dla porównania zamieszczono w niej również wyniki symulacji dla standardowego algorytmu. Ścieżki ruchu manipulatora mobilnego oraz odpowiadające im trajektorie q 4 dla algorytmu ze zmienną normą pokazano na rys. 2-4. Analogiczne rozwiązania uzyskane ze standardowego algorytmu zamieszczono na rys. 5-7 Tabela 1: Wyniki symulacji. Algorytm Zmienna norma ZN Stała norma SN Punkt C 1 C 2 C 3 C 1 C 2 C 3 Liczba kroków 54 86 72 173 5 5 Błąd końcowy 5.29 1 7 5.64 1 7 8.62 1 7 6.67 1 7 12 23 q 4 ( π 2, π 2 ) TAK TAK TAK NIE NIE NIE 6

Rysunek 2: Ścieżka ruchu robota oraz trajektoria q 4, punkt C 1, alg. ZN. Rysunek 3: Ścieżka ruchu robota oraz trajektoria q 4, punkt C 2, alg. ZN. Rysunek 4: Ścieżka ruchu robota oraz trajektoria q 4, punkt C 3, alg. ZN. 7

Rysunek 5: Ścieżka ruchu robota oraz trajektoria q 4, punkt C 1, alg. SN. Rysunek 6: Ścieżka ruchu robota oraz trajektoria q 4, punkt C 2, alg. SN. Rysunek 7: Ścieżka ruchu robota oraz trajektoria q 4, punkt C 3, alg. SN. 8

6 Zakończenie Wprowadzenie modyfikacji do standardowej wersji algorytmu ważonego jakobianu paseudoodwrotnego w postaci zmiennej normy pozwala wpływać na działanie algorytmu tak, by ograniczyć wartości wybranych zmiennych stanu manipulatora mobilnego. Jako przykład w pracy przedstawiono zastosowanie algorytmu ze zmienną normą do ograniczenia kąta skrętu przednich kół platformy mobilnej typu samochod kinematyczny. Symulacje pokazały, że po wprowadzeniu modyfikacji algorytm kinematyki odwrotnej znajduje rozwiązanie w mniejszej liczbie kroków oraz dla większych odległości od celu. W przykładach algorytm ze stałą normą znalazł rozwiązanie jedynie dla punktu C 1, kiedy to po modyfikacji rozwiązał każde z trzech postawionych zadań. Ponadto w każdym z badanych przypadków przy stałej normie wartości zmiennej stanu q 4 wykraczały z przedziału ( π 2, π 2 ). Dalsze badania będą miały na celu wyjaśnienie mechanizmów przyczyniających się do polepszenia wyników generowanych przez algorytm ze zmienną normą. Będziemy również chcieli zastosować prezentowany algorytm do sterowania innymi rodzajami manipulatorów mobilnych. Literatura [1] J.T. Wen A. Divelbiss, S. Seereeram. Kinematic path planning for robots with holonomic and nonholonomic constraints. New York, Springer-Verlag 1998, s. 127 15. [2] J. Jakubiak K. Tchoń. Endogenous configuration space approach to mobile manipulators: a derivation and performance assessment of jacobian inverse kinematics algorithms. Int. J. Control., 23, volume 76, number 14, s. 1387 1419. [3] E.D. Sontag. A general approach to path planning for systems without drift. New York, Springer-Verlag 1998, s. 151 168. 9

Mariusz Janiak Instytut Informatyki Automatyki i Robotyki Politechniki Wrocławskiej ul. Janiszewskiego 11/17 5-372 Wrocław Niniejszy raport otrzymują: 1. OINT............................................................................................ - 1 egz. 3. Autor............................................................................................ - 3 egz. Razem : 4 egz. Raport wpłynął do redakcji I-6 w maju 26 roku. 1