2.12. Zadania odwrotne kinematyki

Transkrypt

1 Politechnika Poznańska, Katedra Sterowania i Inżynierii Systemów str Zadania odwrotne kinematyki Określenie zadania odwrotnego kinematyki T 0 N = [ ] n s a p = r 11 r 12 r 13 p x r 21 r 22 r 23 p y r 31 r 32 r 33 p z Warunek istnienia rozwiazań Strategie rozwiazań: (a) rozwiazania w postaci jawnej: metoda geometryczna, metody algebraiczne (przez podstawianie, przez redukcję do wielomianu), (b) rozwiazania numeryczne: z wykorzystaniem jakobianu, algorytmu iteracyjnego rozwiazywania z.o.k. : metoda największego spadku, metoda Newtona-Raphsona, metoda wykorzystujaca kwaterniony. Odsprzężenie kinematyczne

2 Politechnika Poznańska, Katedra Sterowania i Inżynierii Systemów str Planarny 3DOF Rozwiazanie geometryczne Przykład: Robot planarny o 3 stopniach swobody y a 3 x 3 q 3 p y y 2 y 3 x 2 y 1 y 0 x 0 p a 1 x 1 a 2 q 2 q 1 p x x Rys. 32 T 0 3 = c 123 s a 1 c 1 + a 2 c 12 s 123 c a 1 s 1 + a 2 s p 2 x + p 2 y = a a 2 2 2a 1 a 2 cos(π q 2 ) cos q 2 = p2 x + p 2 y a 2 1 a 2 2 2a 1 a 2 Musi być spełniony warunek: p 2 x + p 2 y a 1 + a 2 α = Atan2(p y, p x ) cos β p 2 x + p 2 y = a 1 + a 2 cos q 2 ( p 2 x + p 2 y + a 2 1 a 2 ) 2 β = arccos więc q 1 = α ± β 2a 1 p 2 x + p 2 y

3 Politechnika Poznańska, Katedra Sterowania i Inżynierii Systemów str. 3 przy czym β (0, π) (istnienie odpowiednich trójkatów) Rozwiazania algebraiczne Przykład 1: Robot planarny o 3 stopniach swobody ϕ = q 1 + q 2 + q 3 (91) p x = a 1 cos q 1 + a 2 cos(q 1 + q 2 ) p y = a 1 sin q 1 + a 2 sin(q 1 + q 2 ) (92) p 2 x + p 2 y = a a a 1 a 2 cos q 2 (93) sin q 2 = ± 1 cos 2 q 2 (94) q 2 = Atan2(sin q 2, cos q 2 ) (95) można odpowiednio pogrupować ( 92) p x = k 1 cos q 1 k 2 sin q 1 p y = k 1 sin q 1 + k 2 cos q 1 (96) gdzie: k 1 = a 1 + a 2 cos q 2, k 2 = a 2 sin q 2 po podstawieniu do (96) zależności r = k k 2 2 γ = Atan2(k 2, k 1 ) k 1 = r cos γ, k 2 = r sin γ (97)

4 Politechnika Poznańska, Katedra Sterowania i Inżynierii Systemów str. 4 otrzymujemy p x r = cosγ cos q 1 sin γ sin q 1 p y r = cos γ sin q 1 + sinγ cos q 1 czyli cos(γ + q 1 ) = p x, sin(γ + q r 1) = p y r więc q 1 = Atan2(p y, p x ) Atan2(k 2, k 1 ) (98) Inny sposób polega na podstawieniu rozwiazania na q 2 do układu równań (92) i rozwiazania układu równań. Wówczas otrzymujemy: cos q 1 = (a 1 + a 2 cos q 2 )p x + a 2 p y sin q 2 p 2 x + p 2 y sin q 1 = (a 1 + a 2 cos q 2 )p y a 2 p x sin q 2 p 2 x + p 2 y a następnie q 1 = Atan2(sin q 1, cos q 1 ) (99) Współrzędna uogólniona q 3 wyznaczymy ostatecznie z równania (91): q 3 = ϕ q 1 q 2 (100)

5 Politechnika Poznańska, Katedra Sterowania i Inżynierii Systemów str Manipulator z nadgarstkiem sferycznym Odsprzężenie kinematyczne Rys. 33 Podział zadania na dwa prostsze: zadanie odwrotne kinematyki dla pozycji nadgarstka p w, zadanie odwrotne kinematyki dla orientacji. Krok 1: Znajdujemy wektor p w (q 1, q 2, q 3 ) łacz acy poczatek układu odniesienia z punktem centralnym nadgarstka p w (q 1, q 2, q 3 ) = p d 6 R 0 6k = p d 6 a (101)

6 Politechnika Poznańska, Katedra Sterowania i Inżynierii Systemów str. 6 czyli p wx p wy = p x d 6 r 13 p y d 6 r 23 (102) p wz p z d 6 r 33 Krok 2: Rozwiazać zadanie odwrotne kinematyki dla wektora p w i znaleźć (q 1, q 2, q 3 ) Krok 3: Znaleźć macierz R 0 3(q 1, q 2, q 3 ) (z zadania prostego kinematyki) Krok 4: Znaleźć zestaw katów Eulera odpowiadajacy macierzy R 3 6(q 4, q 5, q 6 ): wzór (23, 24) dla katów odpowiednio (ϕ, ϑ, ψ) reprezentacji ZYZ R 0 6 = R 0 3R 3 6 R 3 6 = (R 0 3) 1 R 0 6 = (R 0 3) T R 0 6 (103)

7 Politechnika Poznańska, Katedra Sterowania i Inżynierii Systemów str Manipulator sferyczny Redukcja do wielomianu (T 0 1) 1 T 0 3 = T 1 2T 2 3 p 1 w = p wx c 1 + p wy s 1 p wz = d 3 s 2 d 3 c 2 (104) p wx s 1 + p wy c 1 d 2 czyli t = tan ϑ 1 2 c 1 = 1 t2 1+t 2 s 1 = 2t 1+t 2 (d 2 + p wy )t 2 + 2p wx t + d 2 p wy = 0 t = p wx ± p 2 w x + p 2 w y d 2 2 d 2 + p wy ϑ 1 = 2Atan2(p wx ± p 2 w x + p 2 w y d 2 2, d 2 + p wy ) (105) p wx c 1 + p wy s 1 p wz = d 3s 2 d 3 c 2 ϑ 2 = Atan2(p wx c 1 + p wy s 1, p wz ) (106) d 3 = (p wx c 1 + p wy s 1 ) 2 + p 2 w z (107) tylko dla d 3 > 0

8 Politechnika Poznańska, Katedra Sterowania i Inżynierii Systemów str Manipulator antropomorficzny Przykład: Robot Yasukawa z 3 x 3 z 4 y 3 z, z, x z 2 p 5 0 x 5 x 4 z 5 x, x 0 1 Rys. 34 Tablica 1 Parametry ZDH dla robota Yasukawa L-3 Nr ogniwa α i 1 a i 1 d i θ i = q i σ i σ i q π/2 0 0 q a 2 0 q a 3 0 q π/2 0 0 q 5 0 1

9 Politechnika Poznańska, Katedra Sterowania i Inżynierii Systemów str. 9 T 0 5 = T 0 1T 1 2T 2 3T 3 4T 4 5 (108) Pomnóżmy obustronnie to równanie przez (T 0 1) 1 : (T 0 1) 1 T 0 5 = T 1 2T 2 3T 3 4T 4 5 (109) Lewa strona tego równania ma postać: r 11 cos q 1 + r 21 sin q 1 r 12 cos q 1 + r 22 sin q 1 r 13 cos q 1 + r 23 sin q 1 r 11 sin q 1 + r 21 cos q 1 r 12 sin q 1 + r 22 cos q 1 r 13 sin q 1 + r 23 cos q 1 r 31 r 32 r p x cos q 1 + p y sin q 1 p x sin q 1 + p y cos q 1 (110) p z 1 Prawa strona wzoru (109) przyjmie postać: sin(q 2 + q 3 + q 4 ) sin q 5 cos q cos(q 2 + q 3 + q 4 ) (111) p x sin q 1 + p y cos q 1 = 0 (112) co pozwoli wyznaczyć współrzędna uogólniona q 1 : q 1 = Atan2(p y, p x ) (113)

10 Politechnika Poznańska, Katedra Sterowania i Inżynierii Systemów str. 10 sin q 5 = r 11 sin q 1 + r 21 cos q 1 cos q 5 = r 12 sin q 1 + r 22 cos q 1 (114) q 5 = Atan2( r 11 sin q 1 + r 21 cos q 1 r 12 sin q 1 + r 22 cos q 1 ) (115) cos(q 2 + q 3 + q 4 ) = r 33, sin(q 2 + q 3 + q 4 ) = r 13 cos q 1 + r 23 sin q 1 (116) q 2 + q 3 + q 4 = Atan2(r 13 cos q 1 + r 23 sin q 1, r 33 ) (117) p z -q 3 B a 3 a 2 O q p x +py Rys. 35 cos q 3 = p2 x + p 2 y + p 2 z a 2 2 a 2 3 2a 2 a 3 (118) q 3 = Atan2( 1 cos 2 q 3, cos q 3 ) (119) q 2 = Atan2(p z, p 2 x + p 2 y) ± Atan2(a 3 sin q 3, a 2 + a 3 cos q 3 ) (120) q 4 = Atan2(r 13 cos q 1 + r 23 cos q 1, r 33 ) q 2 q 3 (121)

11 Politechnika Poznańska, Katedra Sterowania i Inżynierii Systemów str Nadgarstek sferyczny R 3 6 = [n 3 s 3 a 3 ] = n 3 x s 3 x a 3 x n 3 y s 3 y a 3 y n 3 z s 3 z a 3 z Jeżeli założymy, że ϑ 5 (0, π), to otrzymujemy rozwiazanie: ϑ 4 = Atan2(a 3 y, a 3 x) ϑ 5 = Atan2( (a 3 x) 2 + (a 3 y) 2, a 3 z) (122) ϑ 6 = Atan2(s 3 z, n 3 z) Z kolei jeśli ϑ 5 ( π, 0), rozwiazanie przyjmuje postać: ϕ = Atan2( a 3 y, a 3 x) ϑ = Atan2( (a 3 x) 2 + (a 3 y) 2, a 3 z) ψ = Atan2( s 3 z, n 3 z) (123)