3.1. Jakobian geometryczny

Transkrypt

1 Politechnika Poznańska, Katedra Sterowania i Inżynierii Systemów str Kinematyki różniczkowa i statyka 3.1. Jakobian geometryczny Pozycja i orientacja x manipulatora o n stopniach swobody zależy od współrzędnych uogólnionych q = [q 1...q n ] T. Prędkość katow a i liniowa manipulatora w zewnętrznej przestrzeni trójwymiarowej określa zależność: ω = J O (q) q ṗ = J P (q) q (124) Co można zapisać w zwięzłej postaci (równanie kinematyki różniczkowej manipulatora): V = ω ṗ = J ( q) q (125) Gdzie macierz J o wymiarach (6 n) jest tzw. jakobianem geometrycznym manipulatora: J = J O J P (126)

2 Politechnika Poznańska, Katedra Sterowania i Inżynierii Systemów str Pochodna macierzy rotacji Macierz skośniesymetryczna S T + S = a z a y S(a) = a z a x a y a x (127) Właściwości S(αa + βb) = αs(a) + βs(b) S(a)b = a b R(a b) = Ra Rb RS(a)R T = S(Ra) (128) Pochodna macierzy rotacji R Ṙ = S(ω)R (129) Ṙ i = S(ω i )R i

3 Politechnika Poznańska, Katedra Sterowania i Inżynierii Systemów str Prędkość ogniwa Różniczkowanie wektora położenia Rys. 36 p = o 1 + R 1p 1 (13) ṗ = ω r + v + R 1ṗ1 (131) p = ω r + ω (ω r) + 2ω R 1ṗ1 + R 1 p1 + v (132) Dodawanie prędkości katowych Ṙ n = d dt (R 1R 1 2 R n 2 n 1R n 1 n ) Ṙ n = Ṙ 1R 1 2 R n 2 n 1R n 1 n + + R 1R 1 2 R n 2 n 1Ṙn 1 n S(ωn)R n = S(ω1)R 1R 1 2 R n 1 n + +R 1R 1 2 R n 2 n 1S(ωn 1,n)R n 1 n S(ωn)R n = S(ω1)R n + + R n 1S(ωn 1,n)(R n 1) T R n 1R n 1 n S(ωn)R n = S(ω1)R n + + S(R n 1ωn n 1 )R n Ṙ n = S(ω n)r n gdzie ω n = ω 1 + R 1ω 1 1,2 + + R n 1ω n 1 n 1,n (133) czyli ω n = ω 1 + ω 1,2 + + ω n 1,n

4 Politechnika Poznańska, Katedra Sterowania i Inżynierii Systemów str. 4 Notacja DH Prędkość liniowa i-tego ogniwa p i = p i 1 + R i 1r i 1 i 1,i ṗ i = ṗ i 1 + R i 1ṙi 1 i 1,i + Ṙ i 1r i 1 i 1,i ṗ i = ṗ i 1 + R i 1ṙi 1 i 1,i + Ṙ i 1r i 1 i 1,i Prędkość katowa i-tego ogniwa ṗ i = ṗ i 1 + v i 1,i + ω i 1 r i 1,i (134) Ṙ i = R i = R i 1 R i 1 i Ṙi 1R i 1 i S(ω i )R i = S(ω i 1 )R i 1 R i 1 i + R i 1 Ṙ i 1 i + R i 1 S(ω i 1 i 1,i )Ri 1 i S(ω i )R i = S(ω i 1 )R i + S(R i 1 ω i 1 i 1,i )R i S(ω i ) = S(ω i 1 + R i 1 ω i 1 i 1,i ) ω i = ω i 1 + ω i 1,i (135) Złacze przesuwne P: ω i 1,i = v i 1,i = d i z i 1 Złacze obrotowe R: ω i = ω i 1 ṗ i = ṗ i 1 + d i z i 1 + ω i r i 1,i (136) ω i 1,i = θ i z i 1 v i 1,i = ω i 1,i r i 1,i ω i = ω i 1 + θ i z i 1 ṗ i = ṗ i 1 + ω i r i 1,i (137)

5 Politechnika Poznańska, Katedra Sterowania i Inżynierii Systemów str. 5 Notacja ZDH Prędkość liniowa i-tego ogniwa Prędkość katowa i-tego ogniwa p i+1 = p i + R ir i i,i+1 ṗ i+1 = ṗ i + v i,i+1 + ω i r i,i+1 (138) R i+1 = R i R i i+1 Złacze przesuwne P: Ṙ i+1 = ṘiR i i+1 + R i Ṙ i i+1 S(ω i+1 ) = S(ω i + R i ω i i,i+1) ω i+1 = ω i + ω i,i+1 (139) ω i,i+1 = v i,i+1 = d i+1 z i+1 Złacze obrotowe R: ω i+1 = ω i ṗ i+1 = ṗ i + d i+1 z i+1 + ω i r i,i+1 (14) ω i,i+1 = θ i+1 z i+1 v i,i+1 = ω i+1 = ω i + θ i+1 z i+1 ṗ i+1 = ṗ i + ω i r i,i+1 (141)

6 Politechnika Poznańska, Katedra Sterowania i Inżynierii Systemów str Przyspieszenie ogniwa Notacja DH Przyspieszenie liniowe i-tego ogniwa v i = v i 1 + R i 1v i 1 i 1,i + ω i 1 R i 1r i 1 i 1,i v i = v i 1 + R i 1 vi 1 i 1,i + Ṙ i 1v i 1 i 1,i + ω i 1 R i 1r i 1,i + +ω i 1 R i 1ṙi 1 i 1,i + ω i 1 Ṙ i 1r i 1 i 1,i v i = v i 1 + v i 1,i + ω i 1 r i 1,i +ω i 1 (ω i 1 r i 1,i )+2ω i 1 v i 1,i (142) Przyspieszenie katowe i-tego ogniwa ω i = ω i 1 + R i 1 ω i 1 i 1,i ω i = ω i 1 + R i 1 ω i 1 i 1,i + Ṙi 1ω i 1 i 1,i ω i = ω i 1 + ω i 1,i + ω i 1 ω i 1,i (143) W obu wyrażeniach należy uwzględnić prędkości względne: Złacze przesuwne P: ω i 1,i = v i 1,i = d i z i 1 ω i = ω i 1 Złacze obrotowe R: ω i 1,i = θ i z i 1 v i 1,i = ω i 1,i r i 1,i ω i = ω i 1 + θ i z i 1

7 Politechnika Poznańska, Katedra Sterowania i Inżynierii Systemów str. 7 Notacja ZDH Zmieniajac odpowiednie indeksy we wzorach (142,143) otrzymamy zależności na przyspieszenie i-tego ogniwa w tej notacji. Dodatkowo należy podstawić odpowiednie prędkości względne: Złacze przesuwne P: ω i,i+1 = v i,i+1 = d i+1 z i+1 ω i+1 = ω i Złacze obrotowe R: ω i,i+1 = θ i+1 z i+1 v i,i+1 = ω i+1 = ω i + θ i+1 z i+1

8 Politechnika Poznańska, Katedra Sterowania i Inżynierii Systemów str. 8 Propagacja prędkości i przyspieszeń w łańcuchu kinematycznym Notacja ZDH ω i+1 i+1 = Ri+1 i ω i i + Θ i+1 z i+1 i+1 obrotowe (144) ω i+1 i+1 = Ri+1 i ω i i przesuwne (145) v i+1 i+1 = Ri+1 i v i+1 i+1 = Ri+1 i (v i i + ω i i l i i,i+1) obrotowe (146) (v i i + ω i i l i i,i+1) + d i+1 z i+1 i+1 przesuwne (147) ω i+1 i+1 = Ri+1 i ω i i + R i+1 i ω i i Θ i+1 z i+1 i+1 + Θ i+1 z i+1 i+1 obrotowe (148) ω i+1 i+1 = Ri+1 i ω i i przesuwne (149) v i+1 i+1 = Ri+1 i [ v i i + ω i i l i i,i+1 + ω i i (ω i i l i i,i+1)] obrotowe (15) v i+1 i+1 = Ri+1 i [ v i i + ω i i l i i,i+1 + ω i i (ω i i l i i,i+1)]+ +2ω i+1 i+1 d i+1 z i+1 i+1 + d i+1 z i+1 i+1 przesuwne (151)

9 Politechnika Poznańska, Katedra Sterowania i Inżynierii Systemów str Wyznaczanie jakobianu J(q) = J O J P = j O1.. j Oi.. j On j P1.. j Pi.. j Pn (152) Analizujac prędkości względne ogniw dla notacj DH można zapisać: j Oi j Pi = z i 1 obrotowe z i 1 (p p i 1 ) przesuwne z i 1 Podobnie można to zrobić dla notcji ZDH pamietajac o odpowiednim zdefiniowaniu układu (n + 1). Uwaga! Wszystkie wektory musza być wyrażone w układzie (153)

10 Politechnika Poznańska, Katedra Sterowania i Inżynierii Systemów str Jakobian typowych manipulatorów - notacja DH Planarny 3DOF p = J(q) = p = z z 1 z 2 z (p p ) z 1 (p p 1 ) z 2 (p p 2 ) a 1 cos q 1 + a 2 cos(q 1 + q 2 ) + a 3 cos(q 1 + q 2 + q 3 ) a 1 sin q 1 + a 2 sin(q 1 + q 2 ) + a 3 sin(q 1 + q 2 + q 3 ) p 1 = a 1 cos q 1 a 1 sin q 1 p 2 = a 1 cos q 1 + a 2 cos(q 1 + q 2 ) a 1 sin q 1 + a 2 sin(q 1 + q 2 ) J(q) = J O J P = J 41 J 42 J 43 J 51 J 52 J 53 (154)

11 Politechnika Poznańska, Katedra Sterowania i Inżynierii Systemów str. 11 gdzie J 41 = a 1 sin q 1 a 2 sin(q 1 + q 2 ) a 3 sin(q 1 + q 2 + q 3 ), J 42 = a 2 sin(q 1 + q 2 ) a 3 sin(q 1 + q 2 + q 3 ), J 43 = a 3 sin(q 1 + q 2 + q 3 ), J 51 = a 1 cos q 1 + a 2 cos(q 1 + q 2 ) + a 3 cos(q 1 + q 2 + q 3 ), J 52 = a 2 cos(q 1 + q 2 ) + a 3 cos(q 1 + q 2 + q 3 ), J 53 = a 3 cos(q 1 + q 2 + q 3 ) Antropomorficzny J(q) = p = z z 1 z 2 z (p p ) z 1 (p p 1 ) z 2 (p p 2 ) p = p 1 = z = cos q 1 (a 2 cos q 2 + a 3 cos(q 2 + q 3 )) sin q 1 (a 2 cos q 2 + a 3 cos(q 2 + q 3 )) 1 a 2 sin q 2 + a 3 sin(q 2 + q 3 ) p 2 = z 1 = z 2 = a 2 cos q 1 cos q 2 a 2 sin q 1 cos q 2 a 2 sin q 2 sin q 1 cos q 1

12 Politechnika Poznańska, Katedra Sterowania i Inżynierii Systemów str. 12 J(q) = J O J P = sin q 1 sin q 1 cos q 1 cos q 1 1 J 41 J 42 J 43 J 51 J 52 J 53 J 62 J 63 (155) gdzie J 41 = sin q 1 (a 2 cos q 2 + a 3 cos(q 2 + q 3 )), J 42 = cos q 1 (a 2 sin q 2 + a 3 sin(q 2 + q 3 )), J 43 = a 3 cos q 1 sin(q 2 + q 3 )), J 51 = cosq 1 (a 2 cos q 2 + a 3 cos(q 2 + q 3 )), J 52 = sin q 1 (a 2 sin q 2 + a 3 sin(q 2 + q 3 )), J 53 = a 3 sin q 1 sin(q 2 + q 3 )), J 62 = a 2 cos q 2 + a 3 cos(q 2 + q 3 ), J 63 = a 3 cos(q 2 + q 3 ) Stanford J(q) = J O J P = z z 1 z (p p ) z 1 (p p 1 ) z 2 z 3 z 4 z 5 z 3 (p p 3 ) z 4 (p p 4 ) z 5 (p p 5 ) (156)

13 Politechnika Poznańska, Katedra Sterowania i Inżynierii Systemów str. 13 p = c 1 s 2 d 3 s 1 d 2 + (c 1 (c 2 c 4 s 5 + s 2 s 5 ) s 1 s 4 s 5 )d 6 s 1 s 2 d 3 + c 1 d 2 + (s 1 (c 2 c 4 s 5 + s 2 s 5 ) + c 1 s 4 s 5 )d 6 z 4 = p = p 1 z = 1 c 2 d 3 + ( s 2 c 4 s 5 + c 2 c 5 )d 6 c 1 s 2 d 3 s 1 d 2 p 3 = p 4 = p 5 s 1 s 2 d 3 + c 1 d 2 c 2 d 3 z 1 = c 1 c 2 s 4 s 1 c 4 s 1 c 2 s 4 + c 1 c 4 s 1 c 1 z 5 = z 2 = z 3 = c 1 s 2 s 1 s 2 c 2 c 1 (c 2 c 4 s 5 + s 2 c 5 ) s 1 s 4 s 5 s 1 (c 2 c 4 s 5 + s 2 c 5 ) + c 1 s 4 s 5 s 2 s 4 s 2 c 4 s 5 + c 2 c 5

14 Politechnika Poznańska, Katedra Sterowania i Inżynierii Systemów str Osobliwości kinematyczne Sa to położenia, w których jakobian manipulatora traci rzad. Przykład: manipulator planarny 2R. Weźmy jakobian położenia dla współrzędnych w płaszczyźnie roboczej manipulatora: J = Wyznacznik tego jakobianu: a 1s 1 a 2 s 12 a 2 s 12 a 1 c 1 + a 2 c 12 a 2 c 12 (157) det(j) = a 1 a 2 s 2 Dla a 1, a 2 punkty osobliwe pojawiaja sie w konfiguracjach ϑ 2 = i ϑ 2 = π (158) Dekompozycja osobliwości W ogólności, dla złożonych struktur kinematycznych jest to bardzo trudne. Dla manipulatorów z nadgarstkiem sferycznym łatwo daje się rozdzielić obliczanie osobliwości na dwa etapy: obliczanie osobliowści ramienia (ruch trzech pierwszych stopni swobody), obliczanie osobliwości nadgarstka sferycznego.

15 Politechnika Poznańska, Katedra Sterowania i Inżynierii Systemów str. 15 Dokonajmy analizy jakobianu manipulatora z nadgarstkiem sferycznym: J = J 11 J 12 (159) J 21 J 22 [ ] J 12 = z 3 z 4 z 5 [ ] J 22 = z 3 (p p 3 ) z 4 (p p 4 ) z 5 (p p 5 ) czyli J 22 = [ ] Zatem det(j) = det(j 21 )det(j 12 ) (16) Osobliwości ramienia: det(j 21 ) = (161) Osobliwości nadgarstka: det(j 12 ) = (162)

16 Politechnika Poznańska, Katedra Sterowania i Inżynierii Systemów str Osobliwości nadgarstka Występuja gdy wektory jednostkowe z 3, z 4, z 5 sa liniowo zależne np. w konfiguracji, gdy z 3, z 5 sa równoległe tj. dla ϑ 5 = lub ϑ 5 = π Osobliwości ramienia Rozpatrzmy je na przykładzie manipulatora antropomorficznego. Wyznacznik jakobianu położenie dla tej struktury wynosi det(j P ) = a 2 a 3 s 3 (a 2 c 2 + a 3 c 23 ). Dla niezerowych długości ramion jest to spełnione, gdy s 3 = (gdy ϑ 3 = lub ϑ 3 = π) lub (a 2 c 2 + a 3 c 23 ) = (konfiguracje, gdy rzut p W na płaszczyznę xy jest zerowy).

17 Politechnika Poznańska, Katedra Sterowania i Inżynierii Systemów str Analiza redundancji Jakobian manipulatora przedstawia odwzorowanie liniowe z przestrzeni prędkości uogólnionych złaczy w przestrzeń prędkości końcówki manipulatora. Rys. 37 Przestrzeń przeciwdziedziny oraz przestrzeń zerowa dim R(J) + dim N(J) = n Oznaczmy przez q rozwiazanie układu v = J(q) q, a przez P macierz o wymiarach (n n) taka, że R(P) N(J). Wtedy wektor określony równaniem: q = q + P q a (163) dla dowolnego wektora q a jest też rozwiazaniem tego układu. J q = J q + JP q a = J q = v, ponieważ JP q a = dla dowolnego wektora q a.

18 Politechnika Poznańska, Katedra Sterowania i Inżynierii Systemów str Odwrotne zadanie kinematyki różniczkowej Gdy liczba stopni swobody jest równa wymiarowi przestrzeni zewnętrznej n = r q = J 1 (q)v. (164) q(t) = t q(ξ)dξ + q(). q(t k+1 ) = q(t k ) + q(t k ) t, (165) Manipulatory redundantne Gdy n > r g( q) = 1 2 qt C q, Posłużymy się metoda mnożników Lagrange a: g( q, λ) = 1 2 qt C q + λ T (v J q), Poszukiwane rozwiazanie powinno spełnić następujace warunki: ( g q ) T = oraz ( ) T g =. λ Na podstawie pierwszego z nich: C q J T λ =, skad wynika, że Daje ostatecznie rozwiazanie: q = C 1 J T λ. (166) v = JC 1 J T λ. λ = (JC 1 J T ) 1 v, q = C 1 J T (JC 1 J T ) 1 v. (167)

19 Politechnika Poznańska, Katedra Sterowania i Inżynierii Systemów str. 19 Pomnóżmy lewostronnie przez J i gdy C = U: q = J v, (168) gdzie J = J T (JJ T ) 1 i jest macierza pseudoinwersji prawostronnej. Wróćmy do równania (163): g( q) = 1 2 ( q q a) T ( q q a ) będziemy minimalizować normę wektora q q a. g( q, λ) = 1 2 ( q q a) T ( q q a ) + λ T (v J q), której minimalizacja prowadzi do rozwiazania: q = J v + (U J J) q a, (169) Sposób wyboru wektora q a dla wykorzystania redundantnych stopni swobody manipulatora: ( ) T w(q) q a = k a, (17) q gdzie k a >, w(q) jest dodatkowa funkcja celu współrzędnych uogólnionych (np. manipulatywność).

20 Politechnika Poznańska, Katedra Sterowania i Inżynierii Systemów str Osobliwości kinematyczne Zwróćmy uwagę, że rozwiazania (164) i (168) wymagaja pełnego rzędu jakobianu. Gdy jakobian traci rzad: 1. zastosować technikę SVD (rozkładu według wartości szczególnych macierzy J) 2. odwrotność jakobianu przy tłumionych najmniejszych kwadratach DLS (ang. damped least-squares inverse) w postaci: J = J T (JJ T + k 2 U) 1, (171) gdzie k jest współczynnikiem tłumienia, który warunkuje lepsze rozwiazanie problemu (minimalizacja funkcji kosztów) g( q) = 1 2 (v J q)t (v J q) k2 q T q. (172)

21 Politechnika Poznańska, Katedra Sterowania i Inżynierii Systemów str Jakobian analityczny Rys. 38 x = Ω p, ẋ = x = k(q), (173) ṗ = p q q = J p(q) q, Ω = Ω q q = J o(q) q, (174) Ω = J o(q) q = J A (q) q, (175) ṗ J p (q) J A (q) = k(q) q. (176)

22 Politechnika Poznańska, Katedra Sterowania i Inżynierii Systemów str. 22 y x 4 p y y 1 p y 4 y y 2 y 3 a 3 a 2 x 3 q 3 q 2 x 2 x a 1 x 1 q 1 p x x Rys. 39 x = p = R 4 = Ω p x p y cos(q 1 + q 2 + q 3 ) sin(q 1 + q 2 + q 3 ) sin(q 1 + q 2 + q 3 ) cos(q 1 + q 2 + q 3 ) 1 a 1 cos q 1 + a 2 cos(q 1 + q 2 ) + a 3 cos(q 1 + q 2 + q 3 ) = a 1 sin q 1 + a 2 sin(q 1 + q 2 ) + a 3 sin(q 1 + q 2 + q 3 ) q 1 + q 2 + q 3.. a 1 cos q 1 + a 2 cos(q 1 + q 2 ) + a 3 cos(q 1 + q 2 + q 3 ) a 1 sin q 1 + a 2 sin(q 1 + q 2 ) + a 3 sin(q 1 + q 2 + q 3 ).

23 Politechnika Poznańska, Katedra Sterowania i Inżynierii Systemów str. 23 Jakobian geometryczny J(q) = J 41 J 42 J 43 J 51 J 52 J 53, gdzie J 41 = a 1 sin q 1 a 2 sin(q 1 + q 2 ) a 3 sin(q 1 + q 2 + q 3 ), J 42 = a 2 sin(q 1 + q 2 ) a 3 sin(q 1 + q 2 + q 3 ), J 43 = a 3 sin(q 1 + q 2 + q 3 ), J 51 = a 1 cos q 1 + a 2 cos(q 1 + q 2 ) + a 3 cos(q 1 + q 2 + q 3 ), J 52 = a 2 cos(q 1 + q 2 ) + a 3 cos(q 1 + q 2 + q 3 ), J 53 = a 3 cos(q 1 + q 2 + q 3 ). Jakobian analityczny J A (q) = J 41 J 42 J 43 J 51 J 52 J 53,

24 Politechnika Poznańska, Katedra Sterowania i Inżynierii Systemów str. 24 Pochodne katów Eulera z y x Rys. 4 [ ω x ω y ω z ] T = ϕ [ 1 ] T, [ [ ] T [ ] T ω x ω y ω z = ϑ sin ϕ cos ϕ, ] T [ ω x ω y ω z = ψ cos ϕ sin ϑ sin ϕ sin ϑ cos ϑ sin ϕ cos ϕ sin ϑ ω = cos ϕ sin ϕ sin ϑ Ω = T(Ω) Ω. 1 cos ϑ ω = T(Ω) ẋ = T A (Ω)ẋ, v U ] T. J = T A (Ω)J A. (177)

25 Politechnika Poznańska, Katedra Sterowania i Inżynierii Systemów str Algorytmy rozwiazuj ace zadanie odwrotne kinematyki q(t k+1 ) = q(t k ) + J 1 (q(t k ))v(t k ) t, (178) gdzie t jest odpowiednio dobranym przedziałem czasu. e = x d x. (179) ė = ẋ d ẋ, ė = ẋ d J A (q) q (18) Odwrotność/pseudoinwersja jakobianu Rys. 41 Współrzędne uogólnione q, które maja odpowiadać zadanemu wektorowi x, sa przyjmowane za poprawne wtedy i tylko wtedy, gdy norma błędu x d k(q) jest mniejsza od pewnej zadanej wartości - wielkość wejściowa algorytmu obliczeniowego. Gdy J A (q) jest macierza kwadratowa i nieosobliwa. q = J 1 A (q)(ẋ d + Ke), (181) ė + Ke =. (182) Dla manipulatorów redundantnych równanie (181) przyjmuje postać (odpowiada równaniu (169)): q = J A (q)(ẋ d + Ke) + (U J A J A) q a, (183)

26 Politechnika Poznańska, Katedra Sterowania i Inżynierii Systemów str Jakobian transponowany Rys. 42 Alternatywne rozwiazanie polega na zdefiniowaniu funkcji Lapunowa: V (e) = 1 2 et Ke, (184) Funkcja ta spełnia następujace warunki definicyjne funkcji Lapunowa: V (e) > e oraz V () =. Po zróżniczkowaniu V (e) otrzymamy: V (e) = e T Kė = e T Kẋ d e T K ẋ, V (e) = e T Kẋ d e T KJ A (q) q. (185) Dokonamy teraz wyboru prędkości uogólnionych w następujacy sposób: co w konsekwencji prowadzi do równania: q = J T A(q)Ke, (186) V (e) = e T Kẋ d e T KJ A (q)j T A(q)Ke. (187)

27 Politechnika Poznańska, Katedra Sterowania i Inżynierii Systemów str Bład orientacji Obliczenie błędu (a) położenia i (b) orientacji r z n n d x y O (a) Rys. 43 (b) e p = p d p (188) ė p = ṗ d ṗ (189) Orientację zadana oznaczymy przez R d = [n d o d a d ], a aktualna przez R = [n o a]. Bład orientacji obliczamy na podstawie wzoru: e o = r sin θ. (19) Rot(r, θ) = R d R T. (191) Wzór powyższy opisuje, jakiego przekształcenia rotacji należy dokonać, aby uzyskać pokrywanie się układów R i R d.

28 Politechnika Poznańska, Katedra Sterowania i Inżynierii Systemów str. 28 Obliczymy najpierw prawa stronę wzoru (191): R d R T = n dx n x + o dx o x + a dx a x n dx n y + o dx o y + a dx a y n dx n z + o dx o z + a dx a z n dy n x + o dy o x + a dy a x n dy n y + o dy o y + a dy a y n dy n z + o dy o z + a dy a z n dz n x + o dz o x + a dz a x n dz n y + o dz o y + a dz a y n dz n z + o dz o z + a dz a z (192) Gdy odejmiemy element (1,2) od elementu (2,1) i przyrównamy tę różnicę do 2k z sin θ, to otrzymamy: 2r z sin θ = (n dy n x n dx n y ) + (o dy o x o dx o y ) + (a dy a x a dx a y ) r z sin θ = 1 2 [(n n d) z + (o o d ) z + (a a d ) z ] indeks z określa odpowiednia składowa iloczynów wektorowych zdefiniowanych wyżej. Odejmiemy element (3,1) od elementu (1,3) i przyrównamy różnicę do 2k y sin θ: r y sin θ = 1 2 [(n n d) y + (o o d ) y + (a a d ) y ] Postępujac analogicznie, odejmiemy od elementu (2,3) element (3,2) i przyrównamy do 2k x sin θ, co daje r x sin θ = 1 2 [(n n d) x + (o o d ) x + (a a d ) x ] Na podstawie powyższych wzorów możemy natychmiast napisać, że e o = r sin θ = 1 2 (n n d + o o d + a a d ) (193) Z uwagi na funkcje sin kat θ (reprezentujacy bład orientacji) powinien się zawierać w przedziale π/2 < θ < π/2.

29 Politechnika Poznańska, Katedra Sterowania i Inżynierii Systemów str. 29 Obliczenie pierwszej i drugiej pochodnej błędu orientacji Na poczatku wyznaczymy pochodna pierwszego składnika wzoru (193): d dt (n n d) = ṅ n d + n ṅ d = S(n d )ṅ + S(n)ṅ d. (194) Pochodne wektorów jednostkowych obliczymy według następujacych wzorów: ṅ = ω n = S(n)ω oraz ṅ d = ω d n d = S(n d )ω d, przy czym ω oraz ω d sa prędkościami katowymi układów n, o, a oraz n d, o d, a d względem układu podstawowego. Podstawiwszy powyższe zależności do wzoru (194), otrzymujemy: d dt (n n d) = S(n d )S(n)ω S(n)S(n d )ω d. Przez analogię do tego wzoru zapiszemy pozostałe pochodne iloczynów wektorowych: oraz d dt (o o d) = S(o d )S(o)ω S(o)S(o d )ω d d dt (a a d) = S(a d )S(a)ω S(a)S(a d )ω d. Na podstawie tych wzorów pochodna wzoru (193) względem czasu przekształcimy do postaci: ė o = 1 2 (S(n)S(n d) + S(o)S(o d ) + S(a)S(a d )) ω d (S(n d)s(n) + S(o d )S(o) + S(a d )S(a)) ω.

30 Politechnika Poznańska, Katedra Sterowania i Inżynierii Systemów str. 3 Wprowadzimy następujace oznaczenie: L = 1 2 (S(n d)s(n) + S(o d )S(o) + S(a d )S(a)), co w konsekwencji daje równanie: ė o = L T ω d Lω. (195) Biorac teraz pod uwagę wzory (189) i (195), możemy napisać: ė = ėo = LT ω d LJ o (q) q = LT ω d L J q. ė p p d J p (q) q ṗ d U (196) Przejdziemy teraz do obliczenia drugiej pochodnej błędu e. Korzystajac ze wzoru (196), możemy zapisać: ë = ëo = LT ω d + L T ω d L (J q+ J q) L ë p p d U (197) gdzie J q L T = 1 2 [S(n)S(S(n d)ω d ) + S(S(n)ω)S(n d ) + S(a)S(S(a d )ω d )+ + S(S(a)ω)S(a d ) + S(o)S(S(o d )ω d ) + S(S(o)ω)S(o d )]. Wzór (197) możemy przedstawić również w alternatywnej postaci: ë = L T ω d + L T ω d Lω L ω. (198) p d p

31 Politechnika Poznańska, Katedra Sterowania i Inżynierii Systemów str Porównanie algorytmów 3.8. Statyka dw τ = τ T dq dw f = F T dp + N T ωdt dw f = F T J p dq + N T J O dq = f T J(q)dq δw τ = τ T δq δw f = f T δq W stanie równowagi statycznej δw τ δw f = τ = J(q) T f (199) Dualność kineto-statyczna τ J T (J ) (J ) Rys. 44 Przeciwdziedzina oraz przestrzeń zerowa odwzorowania J T

32 Politechnika Poznańska, Katedra Sterowania i Inżynierii Systemów str. 32 Przeciwdziedzina odwzorowania J T jest podzbiór R(J T ) przestrzeni R N tych sił uogólnionych działajacych w złaczach, które równoważa siły działajace w środku chwytaka w każdej konfiguracji manipulatora. Z kolei przestrzenia zerowa odwzorowania J T nazywamy podprzestrzeń N(J T ) przestrzeni R r sił działajacych na chwytak, które nie wymagaja żadnych równoważacych je sił uogólnionych w złaczach w każdej konfiguracji manipulatora. Z tych definicji wynika, że każda siłę f N(J T ) równoważa siły reakcji zadanej struktury mechanicznej. A więc gdy manipulator znajduje się w konfiguracji osobliwej, pozostaje on w równowadze niezależnie od sił zewnętrznych f działajacych na chwytak. Zależność pomiędzy dwoma podprzestrzeniami wyraża się następujaco: N(J) R (J T ) oraz R(J) N (J T ), gdzie oznacza uzupełnienie ortogonalne. Oznacza to, że jeżeli znany jest tylko jakobian manipulatora, to możemy zdefiniować przekształcenie kinematyczne prędkości oraz zależności statyczne przez określenie przeciwdziedziny oraz podprzestrzeni zerowej odpowiedniego jakobianu i jego transpozycji Transformacja prędkości i sił Patrz punkt 4.1

33 Politechnika Poznańska, Katedra Sterowania i Inżynierii Systemów str Elipsoida manipulowalności q T q = 1 v T J T J v = 1 w(q) = det(jj T ) (2) lub w(q) = det(jj T )