Manipulatory i roboty mobilne AR S1 semestr 5

Manipulatory i roboty mobilne
AR S1 semestr 5
Konrad Słodowicz
MN1: Zadanie proste kinematyki manipulatora szeregowego 2DOF
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ć.
1. Przestrzeń robocza manipulatora szeregowego
Zanim jednak zaczniemy jakiekolwiek obliczenia, wyznaczmy przestrzeń roboczą naszego
manipulatora. Zakładamy pewne ograniczenia:
0° ≤q 1≤180 °
.
−135 ° ≤q 2≤135°
Dodatkowo, przyjmujemy długości ramion równe:
l 1=2
.
l 2=0.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 q1 i q2 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:
x1=cos(q1 )⋅l 1
x2=cos (q 1+q 2)⋅l 2
a więc
p x =cos(q1 )⋅l 1+cos (q 1+q 2)⋅l 2
i analogicznie
p y =sin (q 1)⋅l 1+sin( q1+q 2)⋅l 2
gdzie p x oraz
podstawą.
p y to odpowiednie współrzędne położenia chwytaka w układzie związanym z
Do zapisu rotacji, natomiast, używa się tzw. macierzy rotacji o ogólnej postaci:
0
1
[
x⃗1 x⃗0
R= x⃗1 ⃗
y0
x⃗1 z⃗0
y⃗1 x⃗0
y⃗1 y⃗0
y1 z⃗0
⃗
z⃗1 x⃗0
z 1 y⃗0
⃗
z⃗1 z⃗0
]
,
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 1 względem układu 0.
By jednocześnie zapisać przesunięcie i obrót używa się macierzy translacji o postaci:
[
T 4x4 =
R 3x3 P 3x1
0 1x3 11x1
]
,
gdzie R – macierz rotacji, P – macierz przesunięcia o postaci
[]
px
P= p y
pz
, 0 – 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:
0
3
T =01T⋅12T⋅23T .
Wykorzystajmy wspomniane zależności do wyznaczenia macierzy translacji w naszym układzie:
[
]
[
]
[ ]
[
cos q 1 −sin q1
0
sin q1 cos q 1
1T =
0
0
0
0
0
0
1
0
cos q 2 −sin q 2
1
sin q 2 cos q2
2T =
0
0
0
0
0 l1
0 0
1 0
0 1
2
3
1
T= 0
0
0
0
3
T =1T⋅2T⋅3T
0
0
1
0
0
1
0
0
1
0
0
0
0
1
l2
0
0
1
2
]
cos( q1+q2 ) −sin(q1+q2 ) 0 l 2⋅cos (q 1+q 2 )+l 1⋅cos q 1
0
sin( q1+q2 ) cos (q 1+q 2) 0 l 2⋅sin(q 1+q 2 )+l 1⋅sin q1
3T =
0
0
1
0
0
0
0
1
Na podstawie powyższej macierzy, znając kąty q1 oraz q2, można obliczyć położenie
chwytaka oraz jego rotację w układzie współrzędnych związanym z podstawą manipulatora.
MN2: 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:
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 2 +P x 2
Py
=tan α
Px
P
α=arctan y
Px
q 11 =α−β
q 12 =α+β
q 22=−q 21
Do wyznaczenia kątów β oraz q 21 użyjemy twierdzenia cosinusów.
q 21 :
2
2
2
d =l 1 +l 2 −2⋅l 1 l 2⋅cos(180 ° −q 21)
l 2+l 21−d 2
cos(180 °−q 21)= 1
2⋅l 1 l 2
2
2
d −l 1 −l 22
cos( q21 )=
2⋅l 1 l 2
2
d −l 1 2−l 22
q 21=arccos(
)
2⋅l 1 l 2
analogicznie
β :
l 22=l 21+d 2−2⋅d l 1⋅cos(β)
l 2−l 2+d 2
β=arccos( 1 2
)
2⋅l 1 d
stąd ostatecznie otrzymujemy równania:
Py
l 2 −l 22+d 2
−arccos( 1
)
Px
2⋅l 1 d
Py
l 12 −l 22+d 2
q 12=arctan +arccos(
) .
Px
2⋅l 1 d
d 2 −l 12 −l 22
q 21 =arccos(
)
2⋅l 1 l 2
q22 =−q 21
q 11=arctan
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 0+a1 t+a 2 t 2+a 3 t 3 .
Generator trajektorii będzie miał za zadanie znalezienie współczynników powyższego równania. Na
szczęście istnieje możliwość analitycznego ich wyznaczenia. Aby tego dokonać musimy znać:
•
czas początkowy t 0
•
czas końcowy t k
•
położenie początkowe Q0
•
położenie końcowe Qk
•
prędkość początkową Q̇0
•
prędkość końcową Q̇k
Można wtedy ułożyć układ czterech równań, będący układem Cramera:
{
Q 0 (t )=a 0+a1 t 0+a 2 t 02+a 3 t 03
Q k (t)=a 0+a 1 t k +a 2 t k 2+a 3 t k 3
Q̇ 0 (t )=a 1+2 a 2 t 0+3 a 3 t 02
Q̇ k (t)=a1 +2 a 2 t k +3 a 3 t k 2
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 0 będzie zawsze równy 0. 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 0=Q 0
a 1=Q̇0
3 (Qk −Q0 )−2 Q̇ 0 t k −Q̇k t k
a 2=
tk2
2(Q0−Q k )+Q̇ 0 t k +Q̇k t k
a 3=
tk3
Dodatkowo, jeśli chwytak ma zatrzymywać się w kolejnych punktach, pozwala to na przyjęcie
Q̇k = Q̇0 = 0, co dalej ułatwia układ. Daje to równania:
a 0=Q 0
a 1=Q̇0
3 (Qk −Q0 )
a 2=
tk2
2(Q0−Q k )
a 3=
t k3
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) zamiast kątowych każdego członu z
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(q1 )⋅l 1+cos (q 1+q 2)⋅l 2
p y =sin (q 1)⋅l 1+sin( q1+q 2)⋅l 2
Pamiętając, że kąty i położenie są teraz zmienne w czasie, otrzymujemy:
ṗ x =−l 1 q̇1 sin(q1)−l 2 ( q̇ 1+q˙2) sin(q1+q2 )
,
ṗ y =l 1 q̇1 cos( q1 )+l 2 ( q˙1+q˙2) cos (q 1+q 2)
co można zapisać macierzowo jako:
[ ][
][ ]
ṗ x
−l sin( q1 )−l 2 sin (q 1+q 2) −l 2 sin( q1+q 2) q˙1
= 1
⋅
ṗ y
l 1 cos( q1 )+l 2 cos(q1+q 2) l 2 cos (q 1+q 2) q˙2
.
W ogólności równanie zapisywane jest jako V =J (Q , l 1, l 2)⋅Q̇ . Macierz J to macierz Jacobiego
(tzw. jakobian), o wzorze ogólnym
[ ]
δ px
δ q1
J=
δ py
δ q1
δ px
δ q2
δ py
δ q2
.
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 −1( Q , l 1, l 2)⋅V .
Zadania:
MN1:
[plik MN2.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 80°, -30°:
MN2:
[plik MN2.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_zad1.m]
Wyznaczenie trajektorii między dwoma punktami:
b) [plik MN3.m]
Wyznaczenie trajektorii między punktami pod osią x (lecz nadal w przestrzeni roboczej)
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
Wykres wartości kąta q1 (w stopniach) od czasu - q1(t):
Wykres wartości kąta q2 (w stopniach) od czasu - q2(t):
d) [plik Zad4.m]
Wyznaczenie trajektorii przechodzącej przez wiele punktów z odpowiednimi prędkościami:
Opis kodu:
Zmienne l1 i l2 to długości ramion manipulatora. Zmienna tk to długość trwania całego ruchu
manipulatora.
Zmienne v0, v1, v2, v3 oraz vk to wektory prędkości w kolejnych punktach (v0 – początkowa, vk –
końcowa). Domyślnie dobrane zostały tak, by łatwo można było zweryfikować poprawność
animacji.
Zmienne p0, p1, p2, 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ą trajekt2d2.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 = 2.5.
Ponieważ wielomiany obliczane są przy użyciu uproszczonych wzorów (czas początkowy zawsze
0), 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 2, 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.