Rachunek wektorowy. I. Notacja i terminologia: a,a,x,y, y R n wektorywprzestrzenin-wymiarowej, A,B,R,T macierze(zazwyczajowymiarzen n)

Transkrypt

1 Wstęp do Robotyki c W. Szynkiewicz, 9 I. Notacja i terminologia: Rachunek wektorowy a,a,x,y, y R n wektorywprzestrzenin-wymiarowej, A,B,R,T macierze(zazwyczajowymiarzen n) R zbiór liczb rzeczywistych R n = R R... R iloczyn kartezjański }{{} nrazy Wektorkolumnowyx R n jestpostaci: x= x. x n ; x=[x,...,x n ] T ; x i R Długośćlubnormaeuklidesowawektorax R n x = x = x +...+x n Wstęp do Robotyki c W. Szynkiewicz, 9 Rodzaje współrzędnych:. Współrzędne punktu uporządkowana kolumna dwóch(dla płaszczyzny) lub trzech(przestrzeni trójwymiarowej) liczb reprezentujących wektor(odcinek skierowany) o początku w początku układu odniesienia i końcu w danym punkcie. Zarówno punkt, jak jego współrzędne często tak samo oznacza się,np.p.. Współrzędne uogólnione uporządkowana kolumna trzech lub czterech liczb reprezentujących współrzędnepunktup=[p x,p y,] T lubodpowiedniop=[p x,p y,p z,] T. 3. Współrzędne wektora uporządkowana kolumna dwóch lub trzech liczb reprezentujących wektor w pewnymukładzieodniesieniar=[r x,r y,r z ] T. 4. Współrzędne wektora swobodnego wektor o określonym kierunku i zwrocie oraz wielkości lecz, którego punkt zaczepienia nie ma fizycznego znaczenia. Układy współrzędnych będą oznaczane jako{f},{},{}.

2 Wstęp do Robotyki c W. Szynkiewicz, 9 3 II. Iloczyn skalarny: Iloczynskalarny x,y dwóchwektorówx,y R n jestliczbąrzeczywistą x,y =x T y=x y=x y +...+x n y n wówczas x = x,x x = x,x Własności iloczynu skalarnego: a) x,y = y,x przemienność, b) x,y x y nierównośćcauchy ego-schwartz a, c) x+y x + y nierównośćtrójkąta, d)m x,y = mx,y łącznośćwzględemmnożeniaprzezliczbę, e) (x+y),z = x,z + y,z rozdzielnośćwzględemdodawania Wstęp do Robotyki c W. Szynkiewicz, 9 4 Dlawektorówx,y R 3 x,y = x y cosθ,gdzieθ= (x,y) Dla wersorów osi(wektorów jednostkowych) î= ; ĵ= ; ˆk= układu ortokartezjańskiego Oxyz mamy î,î = ĵ,ĵ = ˆk,ˆk = î,ĵ = ĵ,ˆk = ˆk,î = wtymukładziewektora=[a x,a y,a z ] T możnazapisać a=a x î+a y ĵ+a zˆk

3 Wstęp do Robotyki c W. Szynkiewicz, 9 5 Iloczynskalarnydwóchwektorówa,b R 3 a,b =a x b x +a y b y +a z b z Jeślia ib tokątmiędzytymiwektoramispełniazwiązek cos (a,b)= a,b a b Współrzędną wektora a względem osi s(α, β, γ) można zapisać a s = a,s =a x cosα+a y cosβ+a z cosγ stąd w szczególności wynika, że a x = a,î ; a y = a,ĵ ; a z = a,ˆk Wektory a, b nazywamy ortogonalnymi jeśli a,b = ajeśliponadtoa ib towektorytesąprostopadłea b. Wstęp do Robotyki c W. Szynkiewicz, 9 6 III. Iloczyn wektorowy(wewnętrzny) Iloczynwektorowya buporządkowanejparywektorów(a,b)jestwektoremc=a b. Jeśli wektory a i b są kolinearne wówczas iloczyn wektorowy jest wektorem zerowym, w przeciwnym przypadku jest wektorem, którego: modułjestdanyjako: a b = a b sin (a,b)ijestpolemrównoległobokurozpiętegonatych wektorach kierunekjestprostopadłydokażdegozwektorówtj. a,(a b) =; b,(a b) =ijestto kierunek prostopadły do płaszczyzny rozpiętej na tych wektorach zwrottaki,żeuporządkowanatrójka(a,b,a b)matęsamąorientacjęcoprzestrzeńwktórej zdefiniowany jest iloczyn wektorowy. ax b b O a Rysunek : Iloczyn wektorowy

4 Wstęp do Robotyki c W. Szynkiewicz, 9 7 Własności iloczynu wektorowego: a)a b= (b a) antyprzemienność, b)m(a b)=(ma) b łącznośćwzględemmnożeniaprzezliczbę, c)(a+b) c=a c+b c rozdzielnośćwzględemdodawania Jeślia ib to sin (a,b)= a b a b W układzie ortokartezjańskim mamy î î=ĵ ĵ=ˆk ˆk= î ĵ=ˆk, ĵ ˆk=î, ˆk î=ĵ Wstęp do Robotyki c W. Szynkiewicz, 9 8 IloczynwektorowywR 3 wyznaczamyzewzoru a b=(a y b z a z b y )î+(a z b x a x b z )ĵ+(a x b y a y b x )ˆk= a y a z b y b z î+ a z a x b z b x ĵ+ a x a y b x b y ˆk albo a b= î ĵ ˆk a x a y a z b x b y b z =det î ĵ ˆk a x a y a z b x b y b z Moduł iloczynu wektorowego a b = a y a z b y b z + a z a x b z b x + a x a y b x b y

5 Wstęp do Robotyki c W. Szynkiewicz, 9 9 Macierze Macierzowymiarach(m n) A=[a ij ] n m= Własności: a... a n... a m... a mn Dodawanie macierzy jest łączne i przemienne (A+B)+C=A+(B+C) A+B=B+A Mnożenie macierzy jest łączne A(BC) =(AB)C Mnożenie macierzy w ogólnym przypadku nie jest przemienne AB BA Transpozycja macierzy (A+B) T =A T +B T (A T ) T =A (AB) T =B T A T Wstęp do Robotyki c W. Szynkiewicz, 9 Obrót układu współrzędnych: Opis położenia i orientacji na płaszczyźnie y y j j i x {} i x sin cos Rysunek:ObrótwR î = cosϕ sinϕ, ĵ = stąd otrzymujemy macierz rotacji R= cosϕ sinϕ sinϕ cosϕ sinϕ cosϕ () ()

6 Wstęp do Robotyki c W. Szynkiewicz, 9 Ten sam wynik można uzyskać wiedząc, że iloczyn skalarny wersorów układu{} i układu{} odpowiada rzutowaniu jednych wersorów na drugie: î = î î î, ĵ = ĵ ĵ î (3) ĵ ĵ zatem macierz rotacji R= î î ĵ î î ĵ ĵ ĵ (4) Własności macierzy obrotu(rotacji): macierzrjestortogonalna:rr T =R T R=I,czylikolumny(takżewiersze)macierzyRsą wzajemnieortogonalne,stądwynika,żer T =R Każda kolumna(wiersz) macierzy R jest wektorem jednostkowym detr=+(dlaukładówprawoskrętnych) Macierze R tworzą specjalną grupę obrotów SO() tzn. R SO(). Wstęp do Robotyki c W. Szynkiewicz, 9 y y P x d y p p d {} {} d x x Rysunek3:ObrótiprzesunięciewR p x p = y cosϕ sinϕ sinϕ cosϕ p x p + d x y d y (5) p x p = y cosϕ sinϕ sinϕ cosϕ p x p d x y d y (6)

7 Wstęp do Robotyki c W. Szynkiewicz, 9 3 Postać zwarta p =Rp +d p =R (p d) (7) gdzie R= cosϕ sinϕ d= sinϕ cosϕ d x d y Przykład : Układ{}obróconywzględem{}okąt π 4 iprzesuniętyowektord=[ ] T.Wukładzie{}dany jestwektorp =[ ] T,przedstawićtenwektorwukładzie{} p = cos π 4 sin π 4 sin π 4 cos π 4 + = Wstęp do Robotyki c W. Szynkiewicz, 9 4 MacierzprzekształceniajednorodnegowR P = p x p y p x p y = ; P = p x p y cosϕ sinϕ d x sinϕ cosϕ d y ;P,P współrzędnejednorodne p x p y (8) p x p y = cosϕ sinϕ d x cosϕ d y sinϕ sinϕ cosϕ d x sinϕ d y cosϕ p x p y (9) P = R d P = HP () P = R T R T d P = H P = HP ()

8 Wstęp do Robotyki c W. Szynkiewicz, 9 5 Obroty w przestrzeni trójwymiarowej z z P y x k k j i j i y x Rysunek4:ObrótwR 3 W układzie{} p =p x î +p y ĵ +p zˆk W układzie{} p =p x î +p y ĵ +p zˆk () (3) Wstęp do Robotyki c W. Szynkiewicz, 9 6 p x =p î =p î =p x î î +p y ĵ î +p zˆk î (4) p y =p ĵ =p ĵ =p x î ĵ +p y ĵ ĵ +p zˆk ĵ (5) p z =p ˆk =p ˆk =p x î ˆk +p y ĵ ˆk +p zˆk ˆk (6) Równanie w postaci wektorowej gdzie p = Rp R= î î ĵ î ˆk î î ĵ ĵ ĵ ˆk ĵ î ˆk ĵ ˆk ˆk ˆk (7) (8) podobnie gdzie p = Rp R= î î ĵ î ˆk î î ĵ ĵ ĵ ˆk ĵ î ˆk ĵ ˆk ˆk ˆk (9) ()

9 Wstęp do Robotyki c W. Szynkiewicz, 9 7 ponieważiloczynskalarnyjestprzemiennytzn.î ĵ =ĵ î,więc R= R = R T () Własności macierzy obrotu(rotacji): macierzrjestortogonalna:rr T =R T R=I 3,czylikolumny(takżewiersze)macierzyRsą wzajemnieortogonalne,stądwynika,żer T =R Każda kolumna(wiersz) macierzy R jest wektorem jednostkowym R(u w)=(ru) (Rw) detr=((rî) (Rĵ)) ((Rˆk))= (Rˆk) =+(dlaukładówprawoskrętnych) Macierze R tworzą specjalną grupę obrotów SO(3). Wstęp do Robotyki c W. Szynkiewicz, 9 8 Składanie obrotów: p = Rp,p = Rp,p = Rp Względem osi bieżącego układu: stąd p = R Rp R= R R kolejne macierze mnożymy prawostronnie. Względem osi układu ustalonego: R= R R kolejne macierze mnożymy lewostronnie.

10 Wstęp do Robotyki c W. Szynkiewicz, 9 9 Przykład : Składanie obrotów nie jest przemienne. z z y x y z y x y O x O x z Rysunek5:Obrótwokółosiz,anastępniey z y y z z y O x z y O x x x Rysunek6:Obrótwokółosiy,anastępniez Wstęp do Robotyki c W. Szynkiewicz, 9 Przykład3:Układ{}jestobróconywzględem{}okątϕwokółosiz.Znaleźćmacierz R z z j O j i i y y x x Rysunek7:Obrótwokółosiz î î =cosϕ,ĵ î = sinϕ,ĵ ĵ =cosϕ,î ĵ =sinϕ,ˆk ˆk =

11 Wstęp do Robotyki c W. Szynkiewicz, 9 Macierz R=R z,ϕ = R z, = I 3 R z,ϕ R z,θ =R z,ϕ+θ R z,ϕ=r z, ϕ cosϕ sinϕ sinϕ cosϕ Podobnie można wyznaczyć macierze obrotu odpowiednio wokół osi x oraz y R x,ϕ = cosϕ sinϕ sinϕ cosϕ R y,ϕ = cosϕ sinϕ sinϕ cosϕ Wstęp do Robotyki c W. Szynkiewicz, 9 Inne wybrane reprezentacje obrotu: reprezentacja oś-kąt(obrót wokół dowolnej osi) obroty wokół osi układu bieżącego kąty Euler a( zestawów) np. kąty Eulera Z-Y-Z obroty wokół osi układu ustalonego np. roll-pitch-yaw(kołysanie-kiwanie-myszkowanie) parametry Eulera(kwaternion jednostkowy) Dalej omówimy trzy pierwsze reprezentacje obrotu.

12 Wstęp do Robotyki c W. Szynkiewicz, 9 3 Obrót wokół dowolnej osi: Mającdanywektora R 3,wektorjednostkowyk, k =ikątϕznaleźćwektorb R 3 powstaływ wynikuobrotuawokółkokątϕ. k x (k x a) C k oœ obrotu k x a k(k a) a b O Rysunek8:Obrótwokółosikokątϕ Wektoramożnazdekomponowaćnaskładowąrównoległąa iskładowąprostopadłąa doosiobrotu a=k(k a) k (k a) }{{}}{{} a a Tylkoskładowaa jestzmienianawwynikuobrotu. Wstęp do Robotyki c W. Szynkiewicz, 9 4 Wektor b można wyznaczyć ze wzoru Rodrigues a: b=k(k a)+sinϕ(k a) cosϕk (k a) () lub innej jego postaci: b=a+(sinϕ)k a+( cosϕ)k (k a) (3) Definiując operator macierzowy iloczynu wektorowego w postaci macierzy K K= taki, że k z k y k z k x k y k x Ka=k a a następnie podstawiając macierz K do wzoru(3) otrzymamy b=a+(sinϕ)ka+( cosϕ)k a stąd wyciągając a można wyznaczyć macierz obrotu R R=I+(sinϕ)K+( cosϕ)k (4)

13 Wstęp do Robotyki c W. Szynkiewicz, 9 5 InnysposóbobliczeniaobliczeniamacierzyR k,ϕ : Znającwektork=[k x,k y,k z ] T definiującyośobrotuikątϕmożnawyprowadzićmacierzobrotur k,ϕ. z k z k x O k k y y x Rysunek 9: Obrót wokół dowolnej osi Obrót wokół dowolnej osi można przedstawić jako złożenie następujących obrotów: R k,ϕ =R z,α R y,β R z,ϕ R y, β R z, α (5) Wstęp do Robotyki c W. Szynkiewicz, 9 6 Obroty wykonywane są wokół osi układu ustalonego. sinα= k y, cosα= k x +ky k x k x +k y sinβ= k x+k y, cosβ=k z cosϕ c ϕ, sinϕ s ϕ, v ϕ =versϕ= c ϕ podstawiając powyższe zależności do(5) mamy R k,ϕ = k xv ϕ +c ϕ k x k y v ϕ k z s ϕ k x k z v ϕ +k y s ϕ k x k y v ϕ +k z s ϕ k yv ϕ +c ϕ k y k z v ϕ k x s ϕ k x k z v ϕ k y s ϕ k y k z v ϕ +k x s ϕ k zv ϕ +c ϕ (6)

14 Wstęp do Robotyki c W. Szynkiewicz, 9 7 Macierz obrotu R SO(3) można zawsze wyrazić w postaci pojedynczego obrotu wokół odpowiedniej osi o odpowiedni kąt R=R k,ϕ gdziekwektorjednostkowydefiniującyoś,zaśϕkątobrotuwokółtejosi.znającelementyr i,j macierzyr kąt ϕ obliczamy Tr(R) ϕ=arccos =arccos r +r +r 33 oraz k= sinϕ r 3 r 3 r 3 r 3 r r Reprezentacja oś-kąt nie jest jednoznaczna ponieważ R k,ϕ =R k, ϕ Jeśliϕ=toR=I,aośobrotujestnieokreślona. Reprezentację oś-kąt można przedstawić w postaci R=[r x,r y,r z ] T =[ϕk x,ϕk y,ϕk z ] T Długość wektora R jest równa mierze kąta ϕ. Wstęp do Robotyki c W. Szynkiewicz, 9 8 Przykład4:MacierzRpowstałazobrotuokąt9 wokółosiz,następnieokąt6 wokółosiy orazo kąt3 wokółosix.obliczyćwynikowąmacierzobrotuorazodpowiadającąjejparametryzacjęoś-kąt. Wynikowa macierz obrotu ma postać R=R x,3 R y,6 R z,9 = Odpowiadająca tej macierzy reprezentacja oś-kąt jest następująca: Kąt obrotu jest równy ϕ=arccos = zaś oś określa wektor k=, 3 3, 3 T

15 Wstęp do Robotyki c W. Szynkiewicz, 9 9 Zadanie : Danesądwaukładyortokartezjańskie{}=(O,x,y,z )i{}=(o,x,y,z ).Kątmiędzyosiami x,y jestrówny 3 π,kątmiędzyosiamix,z jestrówny π 6 orazkątmiędzyosiamiy,z jestrówny π 3. Obliczyćelementymacierzyrotacji R(przejściezukładu{}do{}).Czyjesttylkojednorozwiązanie tego zadania? z x y 6 3 z _ 3 y x Rysunek:Obrotywprzestrzeni R 3 Wstęp do Robotyki c W. Szynkiewicz, 9 3 Obroty wokół osi układu bieżącego: Kąty Eulera Z-Y-Z: z z z z y x O y y x x x y Rysunek : Reprezentacja kątów Eulera ZestawkątówEuleraZ-Y-Zotrzymamyprzeztrzyobroty:wokółosiz okątϕ,następniewokółosiy o kątθiwokółosiz okątψ.

16 Wstęp do Robotyki c W. Szynkiewicz, 9 3 Wynikowa macierz obrotu: R=R z,ϕ R y,θ R z,ψ = c ϕ c θ c ψ s ϕ s ψ c ϕ c θ s ψ s ϕ c ψ c ϕ s θ s ϕ c θ c ψ +c ϕ s ψ s ϕ c θ s ψ +c ϕ c ψ s ϕ s θ s θ c ψ s θ s ψ c θ (7) Problemodwrotny:Mającdanewartościr i,j elementówmacierzyrznaleźćkątyϕ,θ,ψ. Jeślisinθ θ=atan ϕ=atan ( ) ± r3+r 3,r 33 r 3 s θ s θ, r 3 r 3 ψ=atan, r 3 s θ s θ Jeżeliθ=lubθ=πwówczasmożnawyznaczyćjedyniesumęalboróżnicękatówϕ±ψ. Kiedyr 33 =,wówczasc θ =is θ =,zatemθ=. ϕ+ψ=atan(r,r )=atan( r,r ) Kiedyr 33 =,wówczasc θ = is θ =,zatemθ=π ϕ ψ=atan( r, r )=atan( r, r ) W obu przypadkach mamy nieskończenie wiele rozwiązań. Wstęp do Robotyki c W. Szynkiewicz, 9 3 Obroty wokół osi układu ustalonego: Kołysanie-kiwanie-myszkowanie(roll-pitch-yaw): x myszkowanie (yaw) z ko³ysanie (roll) O kiwanie (pitch) y Rysunek : Obroty wokół osi układu ustalonego R=R z,ϕ R y,θ R x,ψ = c ϕ c θ s ϕ c ψ +c ϕ s θ s ψ s ϕ s ψ +c ϕ s θ c ψ s ϕ c θ c ϕ c ψ +s ϕ s θ s ψ c ϕ s ψ +s ϕ s θ c ψ s θ c θ s ψ c θ c ψ określone na zbiorze V= {(ϕ,θ,ψ) π } θπ,<ϕ<π, ψ<π (8)

17 Wstęp do Robotyki c W. Szynkiewicz, 9 33 Przekształcenie odwrotne: Dlaθ ± π mamy ϕ=atan(r,r ), ϕ=ϕ+π (9) θ=atan( r 3,cosϕr +sinϕr ) (3) ψ=atan(sinϕr 3 cosϕr 3, sinϕr +cosϕr ) (3) Jeśliθ=± π wówczasrozwiązaniedegenerujesięimożemyobliczyćtylkosumęlubróżnicękątówϕiψ. Zazwyczaj przyjmuje się ϕ = i rozwiązanie znajduje się z zależności: Dlaθ= π θ= π, ϕ=, ψ=atan(r,r ) Dlaθ= π θ= π, ϕ=, ψ= atan(r,r ) Wstęp do Robotyki c W. Szynkiewicz, 9 34 P= p x p y p z = p MacierzprzekształceniajednorodnegowR 3 p R d R d R d p = = Rp+d R R Rd+ d =HP (3) (33) R d = R T R T d (34) H= R 3 3 d 3 f 3 s = obrót przesunięcie perspektywa skalowanie (35)

18 Wstęp do Robotyki c W. Szynkiewicz, 9 35 Zbiór elementarnych przekształceń jednorodnych Zbiór wszystkich macierzy jednorodnych H stanowi specjalną grupę euklidesową izometrii przestrzeni trójwymiarowej. Zbiór podstawowych przekształceń jednorodnych generujących tę grupę jest następujący: Trans x,a = a Trans y,b = b Trans z,c = c dla translacji odpowiednio wzdłuż osi x, y, z, Rot x,α = cα sα sα cα Rot y,ϕ = cϕ sϕ sϕ cϕ Rot z,θ = cθ i sθ i sθ i cθ i dlaobrotuodpowiedniowokółosix,y,z(gdzies() sin()orazc() cos()). Wstęp do Robotyki c W. Szynkiewicz, 9 36 Interpretacje macierzy przekształcenia jednorodnego: {} z P P z k k d P O j y x i {} O j i y T= R d x. Jako opis układu opisuje układ{} względem układu{}. Kolumny macierzy rotacji(macierzy kosinusówkierunkowych) Rsąwersoramiokreślającymikierunkiosiukładu{}wukładzie{}. Wektor dokreślapołożeniepoczątkuukładu{}wukładzie{}..jakoprzekształcenieodwzorowujące.macierz Todwzorowuje P P. 3.Jakooperatorprzekształcenia.MacierzTdziałanawektor P tworzącwektor P (przesuniętyi obrócony w układzie{})

19 Wstęp do Robotyki c W. Szynkiewicz, 9 37 Przykład 5: Układortokartezjański{}=(O,x,y,z )początkowopokrywałsięzukładem{}=(o,x,y,z ), następniezostałobróconyokątϕ= π 6 wokółosiz iprzesuniętyod=6wzdłużosix.punktpma współrzędnewukładzie{}współrzędne P=[,,3] T.ObliczyćwspółrzędnepunktuPwukładzie{}. Rozwiązanie: Transformacja współrzędnych z układu{} do{} opisana jest wzorem: P= T P Macierz przekształcenia jednorodnego ma postać: T= cosϕ sinϕ d sinϕ cosϕ Zapisując we współrzędnych jednorodnych współrzędne punktu P, możemy obliczyć P= T 3 P= = Wstęp do Robotyki c W. Szynkiewicz, 9 38 Ruch ciała sztywnego: Ruch ciała(bryły) sztywnego w przestrzeni euklidesowej można przedstawić jako gładkie odwzorowanie czasuwgrupęprzesunięć R 3 orazspecjalnągrupęobrotów SO(3) c: R R 3 SO(3) = SE(3), H SE(3), (36) które opisuje w każdej chwili czasu położenie i orientację układu związanego z ciałem względem układu przestrzeni(układu odniesienia). KażdyelementH SE(3)formalnienależydo R : H R (R R 9 d R 3 ) przy czym macierz rotacji R spełnia 6 niezależnych warunków ortogonalności R T R=I 3 stąd wymiar specjalnej grupy euklidesowej dim SE(3) = 6, czyli ciało swobodne ma 6 stopni swobody.

20 Wstęp do Robotyki c W. Szynkiewicz, 9 39 Macierze skośniesymetryczne S T +S= (37) dlamacierzysowymiarze3 3oelementachs ij,i,j=,,3mamy9równań s ij +s ji = (38) ponieważs ii =orazdlai js ij = s ji tomacierzs 3 3 S= s s s s 3 s s 3 Jeśliwektora=[a x,a y,a z ] T tos(a) S(a)=[a ]= a z a y a z a x a y a x Macierzeskośnie-symetryczneS(î),S(ĵ),S(ˆk)dlawersorówî,ĵ,ˆk S(î)= ; S(ĵ)= ; S(ˆk)= (39) (4) (4) Wstęp do Robotyki c W. Szynkiewicz, 9 4 Własności macierzy skośniesymetrycznych: dladowolnychwektorówa,b R 3 orazskalarówα,βmamys(αa+βb)=αs(a)+βs(b) liniowość dladowolnegowektorap R 3 mamys(a)p=a p dladowolnychprzekształceńr,s SO(3)idowolnychwektorówa,b R 3 zpoprzednichwłasności wynika, iż RS(a)R T b = R(a R T b)=(ra) (RR T b) = (Ra) b = S(Ra)b (4) Stąd wynika, że RS(a)R T =S(Ra) (43) Lewa strona równania reprezentuje przekształcenie macierzy S(a) przez podobieństwo. Niech macierz obrotu będzie funkcją jednego kąta θ, stąd R = R(θ) SO(3). Pochodna macierzy względem θ dana jest wzorem dr dθ =SR(θ) (44)

21 Wstęp do Robotyki c W. Szynkiewicz, 9 4 Przykład:6JeśliR=R x,θ jestpodstawowąmacierząobrotutomożnapokazać,że S= dr dθ RT = sinθ cosθ cosθ sinθ = cosθ sinθ sinθ cosθ =S(î) (45) stąd otrzymujemy dr x,θ dθ =S(î)R x,θ podobnie dostajemy pozostałe pochodne (46) dr y,θ dθ =S(ĵ)R y,θ ; dr z,θ dθ =S(ˆk)R z,θ (47) Wstęp do Robotyki c W. Szynkiewicz, 9 4 Prędkość kątowa i liniowa z s Z b y b s k b {s} O s s d b {b} O b x b s v b y s x s Rysunek 3: Ruch ciała sztywnego prędkości Prędkość kątowa: Załóżmy,żemacierzobrotujestzmiennawczasieR=R(t) SO(3).PonieważmacierzR(t)jest ortogonalna, więc R(t)R(t) T =I różniczkując obustronnie względem t jako pochodną iloczynu otrzymamy dr dt R(t)T +R(t) drt dt (48) = (49)

22 Wstęp do Robotyki c W. Szynkiewicz, 9 43 Definiujemy macierz S dr dt R(t)T wówczas transpozycja macierzy S S T = dr dt R(t)T ) T =R(t) drt dt równanie(49) oznacza wówczas, że (5) (5) S+S T = (5) zatem macierz S zdefiniowana wzorem(5) jest skośniesymetryczna. Mnożąc obie strony równania(5) przezr(t)iwiedząc,żer(t)jestmacierząortogonalnąotrzymujemypochodnąṙ(t)macierzyr(t) względem czasu dr(t) dt =Ṙ(t)=S(t)R(t) (53) Macierz S(t) może być jednoznacznie reprezentowana w postaci S(ω(t)) przez wektor prędkości kątowej ω(t) układu obracającego się względem układu ustalonego w chwili t. Pochodną macierzy R(t) względem czasu wyrazić Ṙ(t) = S(t)R(t) = S(ω(t))R(t) (54) gdzie ω(t) wektor prędkości kątowej. Wstęp do Robotyki c W. Szynkiewicz, 9 44 Niech c(t)= R(t) d(t) ċ(t) = Ṙ(t) ḋ(t) Prędkość(macierz prędkości) w układzie przestrzeni V s =ċ(t)c (t)= Ω s ḋ Ω s d gdzieω s =ṘRT prędkośćkątowawukładzieinercjalnym (55) Prędkość(macierz prędkości) w układzie ciała(związanym z ciałem) V b =c (t)ċ(t)= Ω b R T ḋ gdzieω b =R T Ṙ prędkośćkątowawukładzieciała. MacierzeprędkościkątowychΩ s iω b sąokreśloneprzezwektoryprędkościkątowychω s orazω b ω ω ω 3 =ω [ω ]=Ω= ω 3 ω ω 3 ω ω ω (56) (57)

23 Wstęp do Robotyki c W. Szynkiewicz, 9 45 odwzorowanie jest wzajemnie jednoznaczne i określa wektorową reprezentację prędkości kątowej Ω s =[ω s ] Ω b =[ω b ] (58) MacierzeprędkościV s iv b możnazatemzastąpićwektorami v s = ν s ω s, vb = ν b ω b (59) gdzie ν s =ḋ+d ωs, ν b =R T ḋ prędkość liniowa odpowiednio w układzie przestrzeni i układzie ciała. Wstęp do Robotyki c W. Szynkiewicz, 9 46 Interpretacja prędkości ciała sztywnego w układzie przestrzeni i układzie ciała: DanyjestpunktowspółrzędnychjednorodnychP= [ p T, ] T wukładzieciała.współrzędnetegopunktuw układzie przestrzeni w chwili t: r(t) = R(t) d(t) p(t) (6) prędkość punktu P względem układu przestrzeni ṙ(t) = Ṙ(t) ḋ(t) p (6) prędkość punktu P wyrażona w układzie ciała R(t) d(t) Ṙ(t) ḋ(t) p =Vb p = ω b p+rḋ (6) prędkość w układzie ciała jest prędkością ruchu względem układu przestrzeni punktu P którego współrzędne w układzie ciała są ustalone widzianą z układu ciała. (Uwaga: Punkt P nie porusza się względem układu ciała.)

24 Wstęp do Robotyki c W. Szynkiewicz, 9 47 Prędkość punktu P w układzie przestrzeni ṙ(t) = =V s Ṙ(t) ḋ(t) r(t) = R(t) d(t) r(t) ω s (r(t) d(t))+ḋ(t) = (63) prędkość punktu P w układzie przestrzeni ma składową pochodzącą od obrotu wektora(r d) z prędkością kątowąω s orazskładowąwynikającązruchupoczątkuukładuciałaḋ.