Podstawy robotyki. Wykład II. Robert Muszyński Janusz Jakubiak Instytut Informatyki, Automatyki i Robotyki Politechnika Wrocławska

Transkrypt

1 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

2 Preliminaria matematyczne 1 przestrzeń euklidesowa (R 3,, ) 3 x, y R 3, x, y = x i y i i=1 x = [x 1, x 2, x 3 ] T y = [y 1, y 2, y 3 ] T iloczyn skalarny (, ) αx 1 + βx 2, y = α x 1, y + β x 2, y x, y = y, x x, x 0

3 Preliminaria matematyczne 2 norma (długość wektora) ( ) x = x, x 1 2 x 0 x = 0 x = 0 αx = α x x + y x + y x, y = x y cos (x, y)

4 Preliminaria matematyczne 2 norma (długość wektora) ( ) x = x, x 1 2 x 0 x = 0 x = 0 αx = α x x + y x + y x, y = x y cos (x, y) metryka euklidesowa (odległość) (ρ(, )) ρ(x, y) = x y ρ(x, y) = 0 x = y ρ(x, y) = ρ(y, x) ρ(x, y) + ρ(y, z) ρ(x, z)

5 Preliminaria matematyczne 3 iloczyn wektorowy ( ) x = [x 1,x 2, x 3 ] T y = [y 1, y 2, y 3 ] T i j k x y = det x 1 x 2 x 3 = y 1 y 2 y 3 = (x 2 y 3 x 3 y 2 )i + (x 3 y 1 x 1 y 3 )j+ (x 1 y 2 x 2 y 1 )k x y = x y sin (x, y)

6 Ruch w R 3 translacja rotacja

7 Elementarna translacja 1

8 Elementarna translacja 1

9 Elementarna translacja 2 p 1 = p 1x i 1 + p 1y j 1 + p 1z k 1 p 0 = p 0x i 0 + p 0y j 0 + p 0z k 0 d 1 0 = d0xi d0y 1 j 0 + d0zk 1 0 p 0 = p 1 + d 1 0

10 Elementarna rotacja 1

11 Elementarna rotacja 1

12 Elementarna rotacja 1

13 Elementarna rotacja 2 p 0x = p 0 i 0 = p 1 i 0 = p 1x i 1 i 0 + p 1y j 1 i 0 + p 1z k 1 i 0 p 0y = p 0 j 0 = p 1 j 0 = p 1x i 1 j 0 + p 1y j 1 j 0 + p 1z k 1 j 0 p 0z = p 0 k 0 = p 1 k 0 = p 1x i 1 k 0 + p 1y j 1 k 0 + p 1z k 1 k 0

14 Elementarna rotacja 3 p 0x = p 0 i 0 = p 1 i 0 = p 1x i 1 i 0 + p 1y j 1 i 0 + p 1z k 1 i 0 p 0y = p 0 j 0 = p 1 j 0 = p 1x i 1 j 0 + p 1y j 1 j 0 + p 1z k 1 j 0 p 0z = p 0 k 0 = p 1 k 0 = p 1x i 1 k 0 + p 1y j 1 k 0 + p 1z k 1 k 0

15 Elementarna rotacja 3 p 0x = p 0 i 0 = p 1 i 0 = p 1x i 1 i 0 + p 1y j 1 i 0 + p 1z k 1 i 0 p 0y = p 0 j 0 = p 1 j 0 = p 1x i 1 j 0 + p 1y j 1 j 0 + p 1z k 1 j 0 p 0z = p 0 k 0 = p 1 k 0 = p 1x i 1 k 0 + p 1y j 1 k 0 + p 1z k 1 k 0 p 0 = R 1 0 p 1

16 Elementarna rotacja 3 p 0x = p 0 i 0 = p 1 i 0 = p 1x i 1 i 0 + p 1y j 1 i 0 + p 1z k 1 i 0 p 0y = p 0 j 0 = p 1 j 0 = p 1x i 1 j 0 + p 1y j 1 j 0 + p 1z k 1 j 0 p 0z = p 0 k 0 = p 1 k 0 = p 1x i 1 k 0 + p 1y j 1 k 0 + p 1z k 1 k 0 p 0 = R 1 0 p 1 gdzie i 1 i 0 j 1 i 0 k 1 i 0 R0 1 = i 1 j 0 j 1 j 0 k 1 j 0 i 1 k 0 j 1 k 0 k 1 k 0

17 Elementarna rotacja 4 Analogicznie p 1x = p 1 i 1 = p 0 i 1 = p 0x i 0 i 1 + p 0y j 0 i 1 + p 0z k 0 i 1 p 1y = p 1 j 1 = p 0 j 1 = p 0x i 0 j 1 + p 0y j 0 j 1 + p 0z k 0 j 1 p 1z = p 1 k 1 = p 0 k 1 = p 0x i 0 k 1 + p 0y j 0 k 1 + p 0z k 0 k 1 p 1 = R 0 1 p 0 gdzie i 0 i 1 j 0 i 1 k 0 i 1 R1 0 = i 0 j 1 j 0 j 1 k 0 j 1 i 0 k 1 j 0 k 1 k 0 k 1

18 Elementarna rotacja 5 Biorąc pod uwagę, że i 1 i 0 j 1 i 0 k 1 i 0 i 0 i 1 j 0 i 1 k 0 i 1 R0 1 = i 1 j 0 j 1 j 0 k 1 j 0 R1 0 = i 0 j 1 j 0 j 1 k 0 j 1 i 1 k 0 j 1 k 0 k 1 k 0 i 0 k 1 j 0 k 1 k 0 k 1

19 Elementarna rotacja 5 Biorąc pod uwagę, że i 1 i 0 j 1 i 0 k 1 i 0 i 0 i 1 j 0 i 1 k 0 i 1 R0 1 = i 1 j 0 j 1 j 0 k 1 j 0 R1 0 = i 0 j 1 j 0 j 1 k 0 j 1 i 1 k 0 j 1 k 0 k 1 k 0 i 0 k 1 j 0 k 1 k 0 k 1 mamy R1 0 = (R0 1 ) 1 = (R0 1 ) T det R 1 0 = ±1, R 1 0 SO(3)

20 Rotacje wokół osi układu współrzędnych wokół osi Z i 1 i 0 = cos θ j 1 i 0 = sin θ k 1 i 0 = 0 i 1 j 0 = sin θ j 1 j 0 = cos θ k 1 j 0 = 0 i 1 k 0 = 0 j 1 k 0 = 0 k 1 k 0 = 1 θ θ cos θ sin θ 0 R Z,θ = sin θ cos θ R Z,0 = I 3, R Z,θ1 R Z,θ2 = R Z,θ1 +θ 2 R 1 Z,θ = R Z, θ

21 Rotacje wokół osi układu współrzędnych wokół osi X wokół osi Y R X,θ = 0 cos θ sin θ 0 sin θ cos θ cos θ 0 sin θ R Y,θ = sin θ 0 cos θ

22 Złożenia 1 złożenie translacji d 1 0 d 2 1 0x 0 y 0 z 0 = 0x 1 y 1 z 1 = 0x 2 y 2 z 2 p 0 p 1 p 2 p 0 = d p 1 p 0 = d p 2 p 1 = d p 2 p 0 = d d2 1 + p 2 d 2 0 = d d 2 1

23 Złożenia 2 złożenie rotacji R 1 0 R 2 1 0x 0 y 0 z 0 = 0x 1 y 1 z 1 = 0x 2 y 2 z 2 p 0 p 1 p 2 p 0 = R 1 0 p 1 p 0 = R 2 0 p 2 p 1 = R 2 1 p 2 p 0 = R 1 0 R2 1 p 2 R 2 0 = R 1 0 R 2 1

24 Reprezentacje macierzy rotacji 1 r 11 r 12 r 13 ] R = r 21 r 22 r 23 = [R 1 R 2 R 3 r 31 r 32 r 33 Ograniczenia na kolumny macierzy R R 1 = R 2 = R 3 = 1 R 1 R 2 = 0 R 2 R 3 = 0 R 1 R 3 = 0 9 zmiennych 6 równań = 3 zmienne niezależne

25 Reprezentacje macierzy rotacji 2 kąty Eulera ZYZ (φ, θ, ψ) R0 1 = R Z,φ R Y,θ R Z,ψ = θ = s φ c θ c ψ + c φ s ψ s φ c θ s ψ + c φ c ψ s φ s θ s θ c ψ s θ s ψ c θ 0 φ < 2π 0 θ < π 0 ψ < 2π kąty Roll Pitch Yaw (φ, θ, ψ) R 1 0 = R Z,φ R Y,θ R X,ψ 0 φ < 2π π 2 θ < π 2 0 ψ < 2π

26 Transformacje jednorodne Definicja Przekształcenie p 0 = R 1 0 p 1 + d 1 0 nazywamy ruchem ciała sztywnego, gdy macierz R jest ortogonalna. Załóżmy, że mamy dwa ruchy wówczas ich złożenie Porównując otrzymujemy p 0 = R 1 0 p 1 + d 1 0 p 1 = R 2 1 p 2 + d 2 1 p 0 = R 1 0 R 2 1 p 2 + R 1 0 d d 1 0 = R 2 0 p 2 + d 2 0 R 2 0 = R 1 0 R 2 1 d 2 0 = d R 1 0 d 2 1

27 Transformacje jednorodne 2 [ R 1 0 d ] [ R 2 1 d ] = [ R 1 0 R 2 1 R 1 0 d2 1 + d Ruch ciała sztywnego może być reprezentowany przez transformację [ ] [ ] R d T = R SO(3) T 1 R = T R T d zwaną transformacją jednorodną. Po rozszerzeniu reprezentacji punktów p o dodatkową składową [ ] p P = 1 mamy P 0 = T 1 0 P 1 T 1 0 SE(3) ]

28 Bazowe macierze jednorodne generujące SE(3) (translacja) a Trans(x, a) = b Trans(y, b) = Trans(z, c) = c

29 Bazowe macierze jednorodne generujące SE(3) (rotacja) cos α sin α 0 Rot(x, α) = 0 sin α cos α cos β 0 sin β Rot(y, β) = sin β 0 cos β cos γ sin γ 0 0 sin γ cos γ 0 0 Rot(z, γ) = n x o x a x d x T0 1 n y o y a y d y = n z o z a z d z