Wykład5 - Kinematyka prosta i odwrotna

Kinematyka prosta i odwrotna
ES159/259
Przypomnienie: ruch ciała sztywnego
•
Ruch ciała sztywnego jest kombinacją obrotów i przesunięć
– Zdefiniowany przez macierz rotacji (R) i wektor przesunięcia (d)
– Grupa wszystkich takich par (d,R) jest znana jako Specjalna grupa
Euklidesowa (ang. Special Euclidean group, SE(3))
•
•
Można przedstawić ruch ciała sztywnego (obroty i przesunięcia) jako
odpowiednie operacje mnożenia macierzy
Mnożenie macierzy H jest znane jako transformaty homogeniczne:
•
⎡RT
H=⎢
⎣0
Transformacja odwrotna:
H
−1
d⎤
⎥
1⎦
⎡RT
=⎢
⎣0
− RT d ⎤
⎥
1 ⎦
ES159/259
Przypomnienie: transformacje homogeniczne
•
Transformacje podstawowe:
– Trzy czyste przesunięcia, trzy czyste obroty
⎡1
⎢0
Trans x ,a = ⎢
⎢0
⎢
⎣0
⎡1
⎢0
Trans y ,b = ⎢
⎢0
⎢
⎣0
⎡1
⎢0
Trans z,c = ⎢
⎢0
⎢
⎣0
0 0 a⎤
1 0 0⎥⎥
0 1 0⎥
⎥
0 0 1⎦
0 0 0⎤
1 0 b ⎥⎥
0 1 0⎥
⎥
0 0 1⎦
0 0 0⎤
1 0 0⎥⎥
0 1 c⎥
⎥
0 0 1⎦
0
⎡1 0
⎢0 c − s
α
α
Rot x ,α = ⎢
⎢0 sα cα
⎢
0
⎣0 0
⎡ c β 0 sβ
⎢ 0
1 0
Rot y ,β = ⎢
⎢− s β 0 c β
⎢
0 0
⎣ 0
⎡cγ − sγ 0
⎢s
cγ 0
Rot z,γ = ⎢ γ
⎢0
0
1
⎢
0 0
⎣0
ES159/259
0⎤
0⎥⎥
0⎥
⎥
1⎦
0⎤
0⎥⎥
0⎥
⎥
1⎦
0⎤
0⎥⎥
0⎥
⎥
1⎦
Przykład
•
Kąty Eulera : przedstawiliśmy kąty Eulera ZYZ. Jaki jest zbiór
wszystkich możliwych kątów Eulera, który może być użyty do
reprezentacji dowolnej macierzy obrotu?
– XYZ, YZX, ZXY, XYX, YZY, ZXZ, XZY, YXZ, ZYX, XZX, YXY, ZYZ
– ZZY nie może być użyty do opisu dowolnej macierzy obrotu ponieważ dwa
kolejne obroty względem osi Z mogą być złożone w jeden obrót
ES159/259
Przykład
•
Obliczyć transformatę homogeniczną reprezentującą przesunięcie o 3
jednostki wzdłuż osi x, następnie rotację o π/2 względem bieżącej osi z
oraz przesunięcie jednostkowe wzdłuż ustalonej osi y
T = Ty ,1Tx ,3Tz ,π / 2
⎡1
⎢0
=⎢
⎢0
⎢
⎣0
0
1
0
0
⎡1 − 1
⎢1 0
=⎢
⎢0 0
⎢
⎣0 0
0
0
1
0
0⎤ ⎡1
1⎥⎥ ⎢⎢0
0 ⎥ ⎢0
⎥⎢
1 ⎦ ⎣0
0
1
0
0
0
0
1
0
3⎤ ⎡ 0 − 1
0⎥⎥ ⎢⎢1 0
0 ⎥ ⎢0 0
⎥⎢
1 ⎦ ⎣0 0
0 3⎤
0 1⎥⎥
1 0⎥
⎥
0 1⎦
ES159/259
0
0
1
0
0⎤
0⎥⎥
0⎥
⎥
1⎦
Kinematyka prosta - wprowadzenie
•
Zadanie: mając dane wszystkie parametry złączowe manipulatora,
znaleźć pozycję i orientację układu narzędzia (ang. tool frame)
– Układ narzędzia: układ współrzędnych związany z najdalszym elementem
manipulatora
– Układ odniesienia, ukł. bezwładny (ang. inertial frame): ustalony
(nieruchomy) system współrzędnych związany z najbliższym elementem
manipulatora
•
Zatem, chcemy znaleźć odwzorowanie pomiędzy ukł. narzędzia a ukł.
odniesienia
– Będzie to funkcja zależna od wszystkich parametrów złączowych i
fizycznych parametrów (geometrii) manipulatora
•
Czysta geometria: nie musimy brać pod uwagę momentów
skręcających na złączach czy dynamiki
– (na razie !!!)
ES159/259
Oznaczenia
•
Manipulator z n stopniami swobody ma n złączy (obrotowych lub
pryzmatycznych) i n+1 członów (każde złącze łączy dwa człony)
– Zakładamy, że każde złącze ma tylko 1 stopień swobody( złącza sferyczne
lub uniwersalne, mające wiele stopni swobody, mogą być traktowane jako
kombinacja złącz z 1 st. sw. połączonych bezpośrednio ze sobą
– o0 oznacza bazowy układ odniesienia
– on oznacza układ narzędzia
– Złącze i łączy człony i-1 z i
– Układ oi jest stowarzyszony z członem i
•
Zmienne złączowe, qi
⎧θ
qi = ⎨ i
⎩d i
dla zlacza i obrotowego
dla zlacza i pryzmatycznego
ES159/259
Oznaczenia
•
Pokazaliśmy, że transformata homogeniczna pozwala wyrazić pozycję i
orientację oj względem oi
– Szukamy pozycji i orientacji ukł. narzędzia względem bazowego układu
odniesienia
– Krok pośredni polega na znalezieniu macierzy transformacji opisującej
pozycję i orientację oi względem oi-1: oznaczonej Ai
– Teraz można zdefiniować transformację oj w oi jako:
⎧ Ai +1Ai + 2 ...A j −i A j if i < j
⎪
T ji = ⎨
I
if i = j
j −1
⎪
T
if j > i
i
⎩
( )
ES159/259
Wynik:
•
Ostatecznie, pozycja i orientacja ukł. narzędzia względem bazowego
ukł. odniesienia jest dana jedną macierzą transf. homogenicznej:
– Dla manipulatora z n st. Swobody:
⎡Rn0
H=⎢
⎣0
on0 ⎤
0
⎥ = Tn = A1 (q1 )A2 (q2 ) ⋅ ⋅ ⋅ An (qn )
1⎦
•
Podsumowując, aby w pełni określić kinematykę prostą dowolnego
manipulatora z szeregowym łańcuchem kinematycznym, wystarczy
znaleźć transformacje Ai i wykonać mnożenie macierzy
•
Ale można szybciej, ‘na skróty’…
ES159/259
Konwencja zapisu Denavita-Hartenberga (DH)
•
Każda indywidualna transformacja homogeniczna przedstawiona jako
iloczyn czterech transformacji bazowych:
Ai = Rot z,θ i Trans z,d i Trans x ,ai Rot x ,α i
⎡cθ i
⎢s
= ⎢ θi
⎢0
⎢
⎣0
− sθ i
⎡cθ i
⎢s
= ⎢ θi
⎢0
⎢
⎣0
− sθ i cα i
cθ i cα i
sα i
sθ i sα i
− cθ i sα i
cα i
0
0
cθ i
0
0
0 0⎤ ⎡ 1
0 0⎥⎥ ⎢⎢0
1 0⎥ ⎢0
⎥⎢
0 1⎦ ⎣0
0 ⎤ ⎡1
1 0 0 ⎥⎥ ⎢⎢0
0 1 d i ⎥ ⎢0
⎥⎢
0 0 1 ⎦ ⎣0
0 0
ai cθ i ⎤
ai sθ i ⎥⎥
di ⎥
⎥
1 ⎦
ES159/259
0 0 ai ⎤ ⎡ 1 0
1 0 0 ⎥⎥ ⎢⎢0 cα i
0 1 0 ⎥ ⎢0 sα i
⎥⎢
0 0 1 ⎦ ⎣0 0
0
− sα i
cα i
0
0⎤
0⎥⎥
0⎥
⎥
1⎦
Konwencja zapisu Denavita-Hartenberga (DH)
•
Cztery parametry DH:
–
–
–
–
•
ai: długość członu
αi: skręcenie członu
di: przesunięcie (offset) członu
θi: kąt na złączu
Ponieważ każda mac. Ai jest funkcją tylko 1 zmiennej, 3 z powyższych
param. DH będą stałe dla każdego złącza
– di będzie zmienną dla złączy pryzmatycznych a θi - dla złączy obrotowych
•
Ale dla każdego ciała sztywnego potrzeba 6 parametrów aby opisać
jego pozycję i orientację
– Trzy kąty (np. kąty Eulera) i wektor pozycji 3x1
– Więc dlaczego są tylko 4 parametry DH ?...
ES159/259
Istnienie i jednoznaczność
•
•
Kiedy możemy wyrazić transformację homogeniczną używając 4
parametrów DH ?
Jako przykład, rozważmy 2 ukł. współrzędnych o0 i o1
– Istnieje jednoznaczna transformacja homogeniczna pomiędzy tymi dwoma
układami
•
•
Zakładamy, że zachodzi:
1. DH1: xˆ1 ⊥ zˆ0
2. DH2: xˆ1 ∩ zˆ0
Jeśli tak, to istnieje jednoznaczna
transformacja A:
A = Rot z,θ Trans z,d Trans x ,aRot x ,α
⎡R10
=⎢
⎣0
o10 ⎤
⎥
1⎦
ES159/259
Istnienie i jednoznaczność
•
Dowód:
R1 = Rz,θ R x,α
1. Zakładamy, że R10 ma postać:
2. Używamy DH1 w celu weryfikacji postaci R10
0
xˆ1 ⊥ zˆ0 ⇒ x10 ⋅ z00 = 0
⎡r11 ⎤
⇒ ⎢⎢r21 ⎥⎥
⎢⎣r31 ⎥⎦
T
⎡0⎤
⎢0⎥ = r = 0
⎢ ⎥ 31
⎢⎣1⎥⎦
⎡r11 r12
R10 = ⎢⎢r21 r22
⎢⎣ 0 r32
r13 ⎤
r23 ⎥⎥
r33 ⎥⎦
– Wiersze i kolumny R10 muszą być wersorami:
– Postać reszty elementów R10 wynika z własności mac.
rotacji
– Tak więc założenie, ze istnieje jednoznaczne θ i α
dające R10 jest prawdziwe z DH1
ES159/259
2
2
r11 + r21 = 1
2
2
r32 + r33 = 1
Istnienie i jednoznaczność
•
Dowód:
1. Używamy DH2 w celu znalezienia postaci o10
– Ponieważ dwie osie przecinają się, można wyrazić linię pomiędzy dwoma
ukł. wsp. jako liniową kombinację dwóch osi (na płaszczyźnie zdefiniowanej
przez x1 i z0)
xˆ1 ∩ zˆ0 ⇒ o10 = dz00 + ax10
⎡0⎤
⎡cθ ⎤ ⎡acθ ⎤
⇒ o10 = d ⎢⎢0⎥⎥ + a ⎢⎢sθ ⎥⎥ = ⎢⎢asθ ⎥⎥
⎢⎣1⎥⎦
⎢⎣ 0 ⎥⎦ ⎢⎣ d ⎥⎦
ES159/259
Fizyczna baza parametrów DH
•
•
•
•
ai: długość członu, dystans pomiędzy z0 i z1 (wzdłuż x1)
αi: skręcenie członu, kat pomiędzy z0 i z1 (mierzony wokół x1)
di: przesunięcie (offset) członu, dystans pomiędzy o0 i przecięciem z0 i x1
(wzdłuż z0)
θi: kąt złącza, kąt pomiędzy x0 i x1 (mierzony wokół z0)
Konwencja dodatnia:
ES159/259
Przypisanie układów współrzędnych
•
Dla dowolnego n-członowego manipulatora, zawsze możemy tak
dobrać ukł. wsp. żeby założenia DH1 i DH2 były spełnione
– Wybór taki nie jest unikalny, ale końcowy rezultat zawsze będzie taki sam
1. Wybierz zi jako oś obrotu dla złącza i+1
– z0 jest osią obrotu dla złącza 1, z1 jest osią obrotu dla złącza 2, etc
– Jeśli złącze i+1 jest obrotowe, zi jest osią obrotu złącza i+1
– Jeśli złącze i+1 jest pryzmatyczne, zi jest osią przesunięcia dla złącza i+1
ES159/259
Przypisanie układów współrzędnych
2. Wyznacz układ bazowy
–
Może to być dowolny punkt wzdłuż z0
3. Wybierz x0, y0 tak, aby spełnić zasadę ‘prawej ręki’
4. Start procesu iteracyjnego definiującego ukł. i względem ukł. i-1
–
i.
ii.
iii.
Rozważ trzy przypadki zależności zi-1 i zi:
zi-1 i zi nie są koplanarne (nie leżą na wspólnej płaszczyźnie)
zi-1 i zi przecinają się
zi-1 i zi są koplanarne
zi-1 i zi są równoległe
ES159/259
Przypisanie układów współrzędnych
i.
zi-1 i zi nie są koplanarne
–
–
Zawsze istnieje najkrótszy odcinek łączący te osie (najmniejsza odległość
pomiędzy tymi osiami)
Wybierz tą linię jako xi
•
–
oi jest w punkcie przecięcia zi i xi
Wybierz yi zgodnie z konwencją ‘prawej ręki’
ES159/259
Przypisanie układów współrzędnych
ii.
zi-1 i zi przecinają się
–
Wybierz xi tak, aby był normalny do płaszczyzny zdefiniowanej przez zi i zi-1
•
–
oi jest w punkcie przecięcia zi i xi
Wybierz yi zgodnie z konwencją ‘prawej ręki’
ES159/259
Przypisanie układów współrzędnych
iii. zi-1 i zi są równoległe
–
–
–
Nieskończenie wiele normalnych o równej długości pomiędzy zi i zi-1
Dowolny wybór oi gdziekolwiek wzdłuż zi, aczkolwiek jeśli wybierzemy xi
wzdłuż normalnej przecinającej oi-1, wynikowe di będzie równe zero
Wybierz yi zgodnie z konwencją ‘prawej ręki’
ES159/259
Przypisanie układu narzędzia
•
Poprzednie przypisania są poprawne aż do ukł. n-1
– Przypisanie ukł. narzędzia jest najczęściej definiowane przez osie n, s, a:
•
•
•
a jest kierunkiem zbliżenia (ang. approach direction)
s jest kierunkiem ślizgu (ang. ‘sliding’) (kierunek, wzdłuż którego otwiera się i
zamyka chwytak)
n jest kierunkiem normalnym (ang. normal) do a i s
ES159/259
Przykład 1: dwuczłonowy manipulator planarny
•
2 st. swobody: musimy przypisać 3 ukł. wsp.
1. Wybierz oś z0 (oś obrotu dla złącza 1, ukł. bazowy)
2. Wybierz oś z1 (oś obrotu dla złącza 2)
3. Wybierz oś z2 (ukł. narzędzia)
•
•
W tym przypadku dowolnie, jako że brak jest chwytaka
Zamiast tego, zdefiniujmy z2 jako równoległe do z1 i z0 (dla konsekwencji)
4. Wybierz osie xi
•
•
wszystkie osie zi są równoległe
Zatem wybierz oś xi tak aby przecinała oi-1
ES159/259
Przykład 1: dwuczłonowy manipulator planarny
•
Zdefiniowanie parametrów DH
– najpierw definiujemy parametry stałe ai, αi
– potem definiujemy parametry zmienne θi, di
człon
ai
αi
di
θi
1
a1
0
0
θ1
2
a2
0
0
θ2
– elementy αi są 0 bo wszystkie zi są równoległe
– Tak więc tylko θi są zmienne
⎡c1 − s1
⎢s
c1
A1 = ⎢ 1
⎢0
0
⎢
0
⎣0
0 a1c1 ⎤
⎡c 2
⎢s
0 a1s1 ⎥⎥
, A2 = ⎢ 2
⎢0
1 0 ⎥
⎥
⎢
0 1 ⎦
⎣0
− s2
c2
0
0
0 a2c 2 ⎤
0 a2s2 ⎥⎥
1
0 ⎥
⎥
0
1 ⎦
ES159/259
T10 = A1
⎡c12
⎢s
0
T2 = A1A2 = ⎢ 12
⎢0
⎢
⎣0
− s12
c12
0
0
0 a1c1 + a2c12 ⎤
0 a1s1 + a2s12 ⎥⎥
⎥
1
0
⎥
0
1
⎦
Przykład 2: trójczłonowy robot cylindryczny
•
3 st. swob.: trzeba przypisać 4 ukł. wsp.
1.
2.
3.
4.
Wybierz oś z0 (oś obrotu dla złącza 1, ukł. bazowy)
Wybierz oś z1 (oś przesunięcia dla złącza 2)
Wybierz oś z2 (oś przesunięcia dla złącza 3)
Wybierz oś z3 (ukł. narzędzia)
•
•
W tym przypadku znowu dowolnie jako że nie rozważamy chwytaka
Zamiast tego, zdefiniujmy z3 jako równoległą do z2
ES159/259
Przykład 2: trójczłonowy robot cylindryczny
•
Definicja parametrów DH
– najpierw definiujemy parametry stałe ai, αi
– potem definiujemy parametry zmienne θi, di
⎡c1 − s1
⎢s
c1
A1 = ⎢ 1
⎢0
0
⎢
0
⎣0
0⎤
⎡1 0
⎢0 0
0 0 ⎥⎥
, A2 = ⎢
⎢0 − 1
1 d1 ⎥
⎥
⎢
0 1⎦
⎣0 0
0
0⎤
⎡1
⎢0
1 0 ⎥⎥
, A3 = ⎢
⎢0
0 d2 ⎥
⎥
⎢
0 1⎦
⎣0
0
0⎤
1 0 0 ⎥⎥
0 1 d3 ⎥
⎥
0 0 1⎦
0 0
⎡c1 0 − s1 − s1d 3 ⎤
⎢s
0
c1
c1d 3 ⎥⎥
1
0
⎢
T3 = A1A2 A3 =
⎢ 0 − 1 0 d1 + d 2 ⎥
⎢
⎥
0
1 ⎦
⎣0 0
ES159/259
człon
ai
αi
di
θi
1
0
0
d1 θ1
2
0
-90 d2 0
3
0
0
d3 0
Przykład 3: przegub sferyczny
•
3 st. swob.: trzeba przypisać 4 ukł. wsp.
– Odchylenie(yaw), nachylenie(pitch), obrót (θ4, θ5, θ6): wszystkie przecinają
się w punkcie o (środek przegubu)
ES159/259
Przykład 3: przegub sferyczny
•
Definicja parametrów DH
– najpierw definiujemy parametry stałe ai, αi
– potem definiujemy parametry zmienne θi, di
⎡c 4
⎢s
A1 = ⎢ 4
⎢0
⎢
⎣0
0
− s4
0
−1
0
c4
0
0
0⎤
⎡c5
⎢s
0⎥⎥
, A2 = ⎢ 5
⎢0
0⎥
⎥
⎢
1⎦
⎣0
⎡c 4c5c 6 − s 4s6
⎢s c c + c s
4 6
3
T6 = A4 A5 A6 = ⎢ 4 5 6
⎢ − s5 c 6
⎢
0
⎣
0
− s5
0
−1
0
c5
0
0
0⎤
⎡c 6
⎢s
0⎥⎥
, A3 = ⎢ 6
⎢0
0⎥
⎥
⎢
1⎦
⎣0
− c 4c 5s6 − s 4c 6
c 4 s5
− s 4c 5s6 + c 4c 6
s 4 s5
c5
0
s5 c 6
0
− s6
c6
0
0
c 4 s5 d 6 ⎤
s 4s5d 6 ⎥⎥
c5d 6 ⎥
⎥
1 ⎦
ES159/259
0⎤
0 0 ⎥⎥
1 d6 ⎥
⎥
0 1⎦
0
człon
ai
αi
di
θi
4
0
-90 0
θ4
5
0
90
0
θ5
6
0
0
d6 θ6
Co za tydzień…
•
•
•
Więcej przykładów dla popularnych konfiguracji manipulatorów
Co z równoległym łańcuchem kinematycznym?
Kinematyka odwrotna
ES159/259