Manipulatory i roboty mobilne AR S1 semestr 5

Transkrypt

1 Manipulatory i roboty mobilne AR S semestr 5 Konrad Słodowicz MN: Zadanie proste kinematyki manipulatora szeregowego - DOF Położenie manipulatora opisać można dwojako w przestrzeni kartezjańskiej lub zmiennych konfiguracyjnych (tu: kątów między członami. Aby sprawnie sterować robotem należy znaleźć sposób na przechodzenie pomiędzy owymi przestrzeniami. Przestrzeń kartezjańska Przestrzeń zmiennych konfiguracyjnych Łatwiej jest nam, bowiem, podawać pożądane położenie chwytaka robota w przestrzeni kartezjańskiej, do której jesteśmy przyzwyczajeni, lecz silniki obracające przeguby potrzebują jako wartości zadanych kątów, o które mają się obrócić.. Przestrzeń robocza manipulatora szeregowego Zanim jednak zaczniemy jakiekolwiek obliczenia, wyznaczmy przestrzeń roboczą naszego manipulatora. Zakładamy pewne ograniczenia: q 8 35 q 35. Dodatkowo, przyjmujemy długości ramion równe: l = l =.4. Wobec tego przestrzeń robocza wygląda następująco:

2 . Zadanie proste kinematyki Przejście z przestrzeni zmiennych konfiguracyjnych do przestrzeni kartezjańskiej nazywane jest zadaniem prostym kinematyki. Chcemy więc znaleźć funkcje kątów q i q takie, by w wyniku dawały współrzędne punktu P będącego położeniem chwytaka. Zadanie to rozwiążemy geometrycznie: Z rysunku otrzymujemy równania: x=cos(q l x=cos(q l a więc p x =cos(q l +cos(q l i analogicznie p y =sin (q l +sin(q l gdzie p x oraz p y to odpowiednie współrzędne położenia chwytaka w układzie związanym z podstawą. Do zapisu rotacji, natomiast, używa się tzw. macierzy rotacji o ogólnej postaci: =[ x x y x z x R x y y y z y ], x z y z z z gdzie x, y i z to wersory równoległe do odpowiednich osi układów współrzędnych. Tak zapisana macierz opisuje rotację układu względem układu. By jednocześnie zapisać przesunięcie i obrót używa się macierzy translacji o postaci: T 4x4 =[ R 3x3 P 3x x3 x], P=[ p x p y p z] gdzie R macierz rotacji, P macierz przesunięcia o postaci, macierz zerowa. Macierze translacji można łatwo łączyć, aby ułatwić obliczanie rotacji i przesunięcia między dwoma odległymi układami. Istnieje, bowiem, zależność, gdzie translacja między dwoma układami można zapisać jako iloczyn ciągu translacji jeśli zaczyna on i kończy się na odpowiednich układach. Dla przykładu:

3 3T = T T 3 T. Wykorzystajmy wspomniane zależności do wyznaczenia macierzy translacji w naszym układzie: sin q T =[cosq sin q cosq ] sin q l T =[cosq sin q cos q ] l T 3 =[ ] 3T = T T T 3 sin(q l cos(q +l cosq T 3 =[cos(q sin(q cos(q l sin(q +l sin q ] Na podstawie powyższej macierzy, znając kąty q oraz q, można obliczyć położenie chwytaka oraz jego rotację w układzie współrzędnych związanym z podstawą manipulatora. MN: Zadanie odwrotne kinematyki manipulatora szeregowego Zadaniem odwrotnym kinematyki nazywane jest przejście ze zmiennych kartezjańskich na zmienne konfiguracyjne. Ułatwia to znacznie sterowanie robotem, gdyż umożliwia zadawanie pożądanego położenia jako punkt w układzie kartezjańskim. Program ma wtedy za zadanie wyznaczenie odpowiednich ustawień kątów pomiędzy kolejnymi członami. Należy zauważyć, iż dla manipulatora dwuczłonowego, w przestrzeni roboczej, każdy punkt można osiągnąć maksymalnie dwoma położeniami manipulatora. Na poniższym rysunku na czerwono zaznaczone zostały obszary przestrzeni roboczej, które mogą zostać osiągnięte jedynie jednym położeniem:

4 Zadanie odwrotne rozwiążemy również geometrycznie. Poniższy rysunek przedstawia dwa położenia ramion dające to samo położenie końcowe chwytaka. d = P y +P x P y P x =tan α α=arctan P y P x Do wyznaczenia kątów β oraz q q : q =α β q =α+β q = q użyjemy twierdzenia cosinusów. analogicznie d =l +l l l cos(8 q cos(8 q = l +l d l l cos(q = d l l l l q =arccos( d l l l l

5 β : stąd ostatecznie otrzymujemy równania: l =l +d d l cos(β β=arccos( l l +d l d q =arctan P y arccos( l l +d P x l d q =arctan P y +arccos( l l +d P x l d q =arccos( d l l l l. q = q Ograniczenia przestrzeni roboczej należy osobno zaimplementować w programie. MN3: Generator trajektorii ruchu Choć rozwiązanie zadania odwrotnego pozwala nam na wyznaczenie zmiennych konfiguracyjnych dających pożądane położenie chwytaka, nie wystarczy to do sterowania manipulatorem. Chcemy, bowiem, by położenia ramion zmieniły się z początkowych na końcowe w pewnym, zadanym czasie. Musimy wiec znaleźć funkcję czasu Q(t, która pozwoli na określenie zmiennych konfiguracyjnych w każdym momencie. Postać funkcji Q(t zależy od przebiegu prędkości, jaki chcemy osiągnąć. Z praktycznego punktu widzenia silniki powinny powoli rozpędzić się i spokojnie wyhamować, aby uniknąć błędów statycznych wynikających z bezwładności oraz szybkiego ich zużywania się. Najprostszą, i najlepszą funkcją dającą taki właśnie przebieg jest wielomian 3. stopnia. Wobec tego, ogólna postać funkcji Q(t to: Q(t =a +a t+a t +a 3 t 3. Generator trajektorii będzie miał za zadanie znalezienie współczynników powyższego równania. Na

6 szczęście istnieje możliwość analitycznego ich wyznaczenia. Aby tego dokonać musimy znać: czas początkowy t czas końcowy położenie początkowe Q położenie końcowe Q k prędkość początkową Q prędkość końcową Q k Można wtedy ułożyć układ czterech równań, będący układem Cramera: {Q(t =a+a t t 3 +a +a 3 t Q k (t=a +a +a t 3 k +a 3 Q (t =a +a t +3a 3 t Q k (t=a +a +3a 3 Poprawne jego rozwiązanie pozwoli na łatwe znalezienie współczynników i wyznaczenie odpowiedniej trajektorii. Można jednak ułatwić sobie rozwiązywanie układu wprowadzając kilka uproszczeń. Na początek można przyjąć, iż czas początkowy t będzie zawsze równy. Uproszczenie to znacznie ułatwia rozwiązanie układu. W praktyce, nawet przy poszukiwaniu złożonych trajektorii przechodzących przez wiele zadanych punktów, jedyną zmianą jest to, iż dla kolejnych położeń, zamiast czasów liczonych od początku trajektorii, zadawane są długości odcinków czasu pomiędzy nimi. Rozwiązując układ w ten sposób otrzymujemy równania: a =Q a = Q a = 3(Q Q k Q Q k a 3 = (Q Q + k Q + Q k 3 Dodatkowo, jeśli chwytak ma zatrzymywać się w kolejnych punktach, pozwala to na przyjęcie Q k = =, co dalej ułatwia układ. Daje to równania: a =Q a = Q Q a = 3(Q k Q a 3 = (Q Q k 3 Dzięki tym wzorom możemy wyznaczać już złożone trajektorie omijające przeszkody lub wykonujące bardziej zaawansowane zadania. Pozostaje, jednak jeszcze jeden szczegół. Na samym początku rozwiązaliśmy zadanie odwrotne kinematyki, aby móc podawać współrzędne punktów chwytaka w przestrzeni kartezjańskiej. Przydałaby się więc możliwość podawania również prędkości liniowych chwytaka (początkowej i końcowej zamiasątowych każdego członu z

7 osobna. Ponieważ, jak wiadomo, prędkość jest pochodną położenia po czasie, wyznaczmy wzory na pochodne współrzędnych położenia chwytaka. p x =cos(q l +cos(q l p y =sin (q l +sin(q l Pamiętając, że kąty i położenie są teraz zmienne w czasie, otrzymujemy: ṗ x = l q sin(q l ( q + q sin(q ṗ y =l q cos(q +l ( q + q cos(q co można zapisać macierzowo jako: [ ṗ x l cos(q +l cos(q ṗ y] [ = l sin(q l sin (q +q l sin(q +q l cos(q ] [, q q ]. W ogólności równanie zapisywane jest jako V =J (Q, l, l Q (tzw. jakobian, o wzorze ogólnym =[ δ p x δ p x δ q J δ q ]. δ p y δ p y δ q δ q. Macierz J to macierz Jacobiego Ponieważ prędkości liniowe mają być wartościami zadanymi, a kątowe wielkościami sterującymi, które musimy wyznaczyć, należy odwrócić równanie: Q=J (Q,l, l V. Zadania: MN: [plik MN.m] Kilka przykładów położeń manipulatora. Obliczenia zostały następnie zawarte w funkcji własnej zad_prost.m do użycia w kolejnych zadaniach. Przykład: kąty 8, -3 :

8 MN: [plik MN.m] Przykład pokazujący dwie konfiguracje manipulatora dające to samo położenie chwytaka. Obliczenia zostały następnie zawarte w funkcji własnej zad_odwr.m do użycia w kolejnych zadaniach. MN3: a [plik MN3_zad.m] Wyznaczenie trajektorii między dwoma punktami: b [plik MN3.m] Wyznaczenie trajektorii między punktami pod osią x (lecz nadal w przestrzeni roboczej

9 c [plik Zad3_4.m] Wyznaczenie trajektorii z określonymi prędkościami początkową i końcową. Wykres modułu prędkości liniowej: Niebieski moduł prędkości, Czerwony moduł prędkości końcowej, Zielony moduł prędkości początkowej

10 Wykres wartości kąta q (w stopniach od czasu - q(t: Wykres wartości kąta q (w stopniach od czasu - q(t:

11 d [plik Zad4.m] Wyznaczenie trajektorii przechodzącej przez wiele punktów z odpowiednimi prędkościami: Opis kodu: Zmienne l i l to długości ramion manipulatora. Zmienna tk to długość trwania całego ruchu manipulatora. Zmienne v, v, v, v3 oraz vk to wektory prędkości w kolejnych punktach (v początkowa, vk końcowa. Domyślnie dobrane zostały tak, by łatwo można było zweryfikować poprawność animacji. Zmienne p, p, p, p3, pk wektory zawierające współrzędne kolejnych punktów, które mają zostać osiągnięte przez manipulator. Następnie, używając funkcji własnej zad_odwr.m wyznaczane są dwie możliwe konfiguracje manipulatora dla każdego z punktów. Spośród nich wybierane są następnie te, które nie łamią założonych ograniczeń manipulatora (dowolne, jeśli oba są odpowiednie. Dalej, używając funkcji własnej jacob.m wyznaczane są jakobiany dla wybranych konfiguracji i od razu, na ich podstawie, wyznaczane wektory prędkości kątowych odpowiadające zadanym prędkościom liniowym w kolejnych punktach. Cała trajektoria liczona jest jako połączenie trajektorii pomiędzy kolejnymi punktami. Dla każdej pary, funkcją własną trajektd.m wyznaczane są współczynniki wielomianu trzeciego stopnia opisującego zależność kątów konfiguracyjnych od czasu. Przyjęto, że czasy przemieszczenia pomiędzy punktami są równe i wynoszą tk/4 =.5. Ponieważ wielomiany obliczane są przy użyciu uproszczonych wzorów (czas początkowy zawsze, podczas animacji wskaźnik pokazujący aktualną wartość czasu dla którego rysowana jest pozycja musi zostać zmniejszony tak, by dla każdej z czterech części trajektorii zaczynał liczenie od nowa. Stąd dla trajektorii, 3 i 4, zamiast używać t(i, gdzie i to aktualna klatka animacji, używane jest t(k, gdzie 'k' to 'i' pomniejszone o łączną ilość klatek animacji aż do pierwszej klatki aktualnie rysowanej trajektorii.