docRuch ciała sztywnego w przestrzeni euklidesowej
download 
Reklamacja Transkrypt
Ruch ciała sztywnego w przestrzeni euklidesowej
Podstawy robotyki Wykład II Ruch ciała sztywnego w przestrzeni euklidesowej Robert Muszyński Janusz Jakubiak Instytut Informatyki, Automatyki i Robotyki Politechnika Wrocławska Podstawy robotyki – wykład II Ruch ciała sztywnego w R 3 Preliminaria matematyczne 1 I przestrzeń euklidesowa (R3 , h·, ·i) x, y ∈ R3 , hx, yi = 3 X xi yi i=1 x = [x1 , x2 , x3 ]T I y = [y1 , y2 , y3 ]T iloczyn skalarny (h·, ·i) hαx1 + βx2 , yi = αhx1 , yi + βhx2 , yi hx, yi = hy, xi hx, xi 0 Podstawy robotyki – wykład II Ruch ciała sztywnego w R 3 Preliminaria matematyczne 2 I norma (długość wektora) (k · k) 1 kxk = hx, xi 2 kxk 0 kxk = 0 ⇔ x=0 kαxk = |α|kxk kx + yk ¬ kxk + kyk hx, yi = kxkkyk cos ^(x, y) Podstawy robotyki – wykład II Ruch ciała sztywnego w R 3 Preliminaria matematyczne 2 I norma (długość wektora) (k · k) 1 kxk = hx, xi 2 kxk 0 kxk = 0 ⇔ x=0 kαxk = |α|kxk kx + yk ¬ kxk + kyk hx, yi = kxkkyk cos ^(x, y) I metryka euklidesowa (odległość) (ρ(·, ·)) ρ(x, y) = kx − yk ρ(x, y) = 0 ⇔ x=y ρ(x, y) = ρ(y, x) ρ(x, y) + ρ(y, z) ρ(x, z) Podstawy robotyki – wykład II Ruch ciała sztywnego w R 3 Preliminaria matematyczne 3 I iloczyn wektorowy ( · × · ) x = [x1 ,x2 , x3 ]T i x × y = det x1 y1 y = [y1 , y2 , y3 ]T j x2 y2 k x3 = y3 = (x2 y3 − x3 y2 )i + (x3 y1 − x1 y3 )j+ (x1 y2 − x2 y1 )k ✕ kx × yk = kxkkyk sin ^(x, y) Podstawy robotyki – wykład II Ruch ciała sztywnego w R 3 Ruch w R 3 I translacja I rotacja Podstawy robotyki – wykład II Ruch ciała sztywnego w R 3 Elementarna translacja 1 Podstawy robotyki – wykład II Ruch ciała sztywnego w R 3 Elementarna translacja 1 Podstawy robotyki – wykład II Ruch ciała sztywnego w R 3 Elementarna translacja 2 p1 = p1x i1 + p1y j1 + p1z k1 p0 = p0x i0 + p0y j0 + p0z k0 1 1 1 1 d0 = d0x i0 + d0y j0 + d0z k0 p0 = p1 + d10 Podstawy robotyki – wykład II Ruch ciała sztywnego w R 3 Elementarna rotacja 1 Podstawy robotyki – wykład II Ruch ciała sztywnego w R 3 Elementarna rotacja 1 Podstawy robotyki – wykład II Ruch ciała sztywnego w R 3 Elementarna rotacja 1 Podstawy robotyki – wykład II Ruch ciała sztywnego w R 3 Elementarna rotacja 2 p0x = p0 i0 = p1 i0 = p1x i1 i0 + p1y j1 i0 + p1z k1 i0 p0y = p0 j0 = p1 j0 = p1x i1 j0 + p1y j1 j0 + p1z k1 j0 p = p k = p k = p i k +p j k +p k k 0z 0 0 1 0 1x 1 0 1y 1 0 1z 1 0 Podstawy robotyki – wykład II Ruch ciała sztywnego w R 3 Elementarna rotacja 3 p0x = p0 i0 = p1 i0 = p1x i1 i0 + p1y j1 i0 + p1z k1 i0 p =p j =p j =p i j +p j j +p k j 0y 0 0 1 0 1x 1 0 1y 1 0 1z 1 0 p = p k = p k = p i k +p j k +p k k 0z 0 0 1 0 1x 1 0 1y 1 0 1z 1 0 Podstawy robotyki – wykład II Ruch ciała sztywnego w R 3 Elementarna rotacja 3 p0x = p0 i0 = p1 i0 = p1x i1 i0 + p1y j1 i0 + p1z k1 i0 p =p j =p j =p i j +p j j +p k j 0y 0 0 1 0 1x 1 0 1y 1 0 1z 1 0 p = p k = p k = p i k +p j k +p k k 0z 0 0 1 0 1x 1 0 1y 1 0 1z 1 0 p0 = R01 p1 Podstawy robotyki – wykład II Ruch ciała sztywnego w R 3 Elementarna rotacja 3 p0x = p0 i0 = p1 i0 = p1x i1 i0 + p1y j1 i0 + p1z k1 i0 p =p j =p j =p i j +p j j +p k j 0y 0 0 1 0 1x 1 0 1y 1 0 1z 1 0 p = p k = p k = p i k +p j k +p k k 0z 0 0 1 0 1x 1 0 1y 1 0 1z 1 0 p0 = R01 p1 gdzie i1 i0 j1 i0 k1 i0 1 R0 = i1 j0 j1 j0 k1 j0 i1 k0 j1 k0 k1 k0 Podstawy robotyki – wykład II Ruch ciała sztywnego w R 3 Elementarna rotacja 4 Analogicznie p1x = p1 i1 = p0 i1 = p0x i0 i1 + p0y j0 i1 + p0z k0 i1 p1y = p1 j1 = p0 j1 = p0x i0 j1 + p0y j0 j1 + p0z k0 j1 p1z = p1 k1 = p0 k1 = p0x i0 k1 + p0y j0 k1 + p0z k0 k1 p1 = R10 p0 gdzie i0 i1 j0 i1 k0 i1 R10 = i0 j1 j0 j1 k0 j1 i0 k1 j0 k1 k0 k1 Podstawy robotyki – wykład II Ruch ciała sztywnego w R 3 Elementarna rotacja 5 Biorąc pod uwagę, że i1 i0 j1 i0 k1 i0 1 R0 = i1 j0 j1 j0 k1 j0 i1 k0 j1 k0 k1 k0 Podstawy robotyki – wykład II i0 i1 j0 i1 k0 i1 0 R1 = i0 j1 j0 j1 k0 j1 i0 k1 j0 k1 k0 k1 Ruch ciała sztywnego w R 3 Elementarna rotacja 5 Biorąc pod uwagę, że i1 i0 j1 i0 k1 i0 1 R0 = i1 j0 j1 j0 k1 j0 i1 k0 j1 k0 k1 k0 mamy R10 = (R01 )−1 = (R01 )T det R01 = ±1, Podstawy robotyki – wykład II i0 i1 j0 i1 k0 i1 0 R1 = i0 j1 j0 j1 k0 j1 i0 k1 j0 k1 k0 k1 R01 ∈ SO(3) Ruch ciała sztywnego w R 3 Rotacje wokół osi układu współrzędnych I wokół osi Z i1 i0 = cos θ i1 j0 = sin θ i1 k0 = 0 j1 i0 = − sin θ j1 j0 = cos θ j1 k0 = 0 θ RZ,θ k1 i0 = 0 k1 j0 = 0 k1 k0 = 1 cos θ − sin θ 0 = sin θ cos θ 0 0 0 1 θ RZ,θ1 RZ,θ2 = RZ,θ1 +θ2 RZ,0 = I3 , −1 RZ,θ = RZ,−θ Podstawy robotyki – wykład II Ruch ciała sztywnego w R 3 Rotacje wokół osi układu współrzędnych I wokół osi X RX ,θ I wokół osi Y RY,θ 1 0 0 = 0 cos θ − sin θ 0 sin θ cos θ cos θ 0 sin θ 1 0 = 0 − sin θ 0 cos θ Podstawy robotyki – wykład II Ruch ciała sztywnego w R 3 Złożenia 1 I złożenie translacji d10 d21 0x0 y0 z0 =⇒ 0x1 y1 z1 =⇒ 0x2 y2 z2 p0 p0 = d10 + p1 p1 p0 = d20 + p2 p2 p1 = d21 + p2 p0 = d10 + d21 + p2 d20 = d10 + d21 Podstawy robotyki – wykład II Ruch ciała sztywnego w R 3 Złożenia 2 I złożenie rotacji 0x0 y0 z0 R01 R12 =⇒ 0x1 y1 z1 =⇒ 0x2 y2 z2 p0 p0 = R01 p1 p1 p0 = R02 p2 p2 p1 = R12 p2 p0 = R01 R12 p2 R02 = R01 R12 Podstawy robotyki – wykład II Ruch ciała sztywnego w R 3 Reprezentacje macierzy rotacji 1 r11 r12 r13 h i R = r21 r22 r23 = R1 R2 R3 r31 r32 r33 Ograniczenia na kolumny macierzy R kR1 k = kR2 k = kR3 k = 1 R1 R2 = 0 R2 R3 = 0 R1 R3 = 0 9 zmiennych – 6 równań = 3 zmienne niezależne Podstawy robotyki – wykład II Ruch ciała sztywnego w R 3 Reprezentacje macierzy rotacji 2 I kąty Eulera ZYZ (φ, θ, ψ) R01 = RZ,φ RY,θ RZ,ψ = 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θ 0 ¬ φ < 2π I 0¬θ<π 0 ¬ ψ < 2π π π ¬θ< 2 2 0 ¬ ψ < 2π kąty Roll–Pitch–Yaw (φ, θ, ψ) R01 = RZ,φ RY,θ RX ,ψ 0 ¬ φ < 2π Podstawy robotyki – wykład II − Ruch ciała sztywnego w R 3 Transformacje jednorodne Definicja Przekształcenie p0 = R01 p1 + d10 nazywamy ruchem ciała sztywnego, gdy macierz R jest ortogonalna. Załóżmy, że mamy dwa ruchy p0 = R01 p1 + d10 p1 = R12 p2 + d21 wówczas ich złożenie p0 = R01 R12 p2 + R01 d21 + d10 = R02 p2 + d20 Porównując otrzymujemy R02 = R01 R12 Podstawy robotyki – wykład II d20 = d10 + R01 d21 Ruch ciała sztywnego w R 3 Transformacje jednorodne 2 " R01 d10 0 1 #" # " R 1 R 2 R01 d21 + d10 R12 d21 = 0 1 0 1 0 1 # Ruch ciała sztywnego może być reprezentowany przez transformację " R d T = 0 1 # " R ∈ SO(3) T −1 RT = 0 −R T d 1 # zwaną transformacją jednorodną. Po rozszerzeniu reprezentacji punktów p o dodatkową składową " # P= p 1 mamy P0 = T01 P1 Podstawy robotyki – wykład II T01 ∈ SE (3) Ruch ciała sztywnego w R 3 Bazowe macierze jednorodne generujące SE (3) (translacja) 0 1 0 0 0 0 1 0 a 0 0 1 0 1 0 0 0 0 1 0 0 b 0 1 0 1 0 0 0 0 1 0 0 0 c 1 1 0 Trans(x, a) = 0 0 1 0 Trans(y , b) = 0 0 1 0 Trans(z, c) = 0 0 Podstawy robotyki – wykład II Ruch ciała sztywnego w R 3 Bazowe macierze jednorodne generujące SE (3) (rotacja) 1 0 Rot(x, α) = 0 0 0 cos α sin α 0 cos β 0 Rot(y , β) = − sin β 0 cos γ sin γ Rot(z, γ) = 0 0 nx ny 1 T0 = nz 0 Podstawy robotyki – wykład II ox oy oz 0 0 1 0 0 0 0 0 1 sin β 0 cos β 0 0 0 0 1 − sin γ cos γ 0 0 ax ay az 0 0 − sin α cos α 0 0 0 1 0 0 0 0 1 dx dy dz 1 Ruch ciała sztywnego w R 3
