Περισσότερα+για+τις+στροφές+

Transcript

1 ΤεχνολογικόEκπαιδευτικόΊδρυμαKρήτης Ρομποτική «Τοπικήπαραμετροποίησηπινάκωνστροφής,γωνίεςEuler, πίνακαςστροφήςγύρωαπόισοδύναμοάξονα» Δρ.ΦασουλάςΓιάννης 1 Περισσότεραγιατιςστροφές ΗστροφήενόςΣΣμπορείνααντιστοιχηθείσεένα πίνακαστροφής Οπίνακαςστροφήςέχει9στοιχεία Οπίνακαςστροφήςμπορείναπαραμετροποιηθεί μεδιάφορουςτρόπους: ΓωνίεςRoll,Pitch,Yaw ΓωνίεςEuler Στροφήγύρωαπόισοδύναμοάξονα Μεάλλουςτρόπους Δρ.ΦασουλάςΓιάννης

2 Προσανατολισμόςμετοπικήπαραμετροποίηση Προσανατολισμόςμετοπικήπαραμετροποίηση 3Ανεξάρτητοιπαράμετροι R 3Γωνίεςστροφήςγύρωαπότουςβασικούςάξονεςτου αδρανειακούπλαισίου ΒΑΣΙΚΕΣΓΩΝΙΕΣΧaΥaΖ γωνίεςroll,pitch,yaw 3Γωνίεςστροφήςγύρωαπότουςβασικούςάξονεςτου κινουμένουπλαισίου ΓΩΝΙΕΣEULERΖaΥaΧ Γωνίαστροφήςγύρωαπόισοδύναμοάξονα Δρ.ΦασουλάςΓιάννης 3 ΒΑΣΙΚΕΣΓΩΝΙΕΣΣτροφές)ΧaΥaΖ Προσανατολισμόςμετοπικήπαραμετροποίηση {A} rotx A, γ ) eax e Az {A} eay roty A, β ) R γβα,, ) = rotz, α ) roty, β ) rotx, γ) ab XYZ rotz A, α ) { B} Αντίναπεριγράφουμετονπροσανατολισμόμετονπίνακαστροφής Χρησιμοποιούμεμόνοτιςγωνίεςγ,β,απουλέγονταιβασικέςγωνίεςΧeΥe Ζγύρωαπόσταθερούςάξονες. Γωνίεςγ,β,αήαλλιώςγωνίεςRoll,Pitch,Yaw Δρ.ΦασουλάςΓιάννης 4

3 ΓωνίεςEulerZaYaX R γβα,, ) = rotz, α ) roty, β ) rotx, γ) ab XYZ Προσανατολισμόςμετοπικήπαραμετροποίηση {A}"{B} e Az Διαφορετικήερμηνείατου παραπάνωπίνακαστροφής eax {A} eay {A} rotz B, α) roty B, β ) rotx B, γ ) { B} ZYXΓωνίεςEuler=α,β,γ) Αντίναπεριγράφουμετονπροσανατολισμόμετονπίνακαστροφής Χρησιμοποιούμεμόνοτιςγωνίεςα,β,γ,πουλέγονταιΖΥΧγωνίεςEuler γύρωαπότουςκινούμενουςάξονες). Δρ.ΦασουλάςΓιάννης 5 Προσανατολισμόςμετοπική ΓωνίεςEulerRoll,Pitch,Yaw) ab ZYXΓωνίεςEuler rotz B, α) ZYX παραμετροποίηση { A} { B 0)} roty B, β ) R αβγ,, ) = rotz, α ) roty, β ) rotx, γ ) = % cα sα 0 % cβ 0 sβ % = sα cα cγ sγ sβ 0 cβ 0 sγ cγ $ cc α β css α β γ sc α γ csc α β γ ss α γ% = sc α β sss α β γ cc α γ ssc α β γ cs α γ =R ab γβα,, ) sβ cβsγ cβc ) γ rotx B, γ ) { B} XYZ Roll {A} = {B0)} Yaw Pitch 6

4 Προσανατολισμόςμετοπική παραμετροποίηση Σύνοψη {A}"{B} R ab Ζα)eΥβ)eΧγ)γωνίεςEuler βασικέςγωνίεςχγ)eυβ)eζα) R ab γ,β,α) XYZ BASE = rotz,α) roty,β) rotx,γ ) = Rotα,β,γ ) ZYX EULER "cα = $$ sα $ 0 sα cα 0 0 " cβ 0%% $$ 0 1 % $ sβ 0 sβ " %% $$0 cγ 0 cβ % $0 sγ 0 sγ %% = cγ % $cα cβ sα cβ sβ ) c α sβ s α s α sβ c α cβ Υπολογισμόςτωνγωνιώνα,β,γανξέρουμετονπίνακαστροφής c α sβ s α % s α sβ c α cβ Δρ.ΦασουλάςΓιάννης 7. Representing Pose in 3-Dimensions Leonhard Euler ) was a Swiss mathematician and physicist who dominated eighteenth century mathematics. He was a student of Johann Bernoulli and applied new mathematical techniques such as calculus to many problems in mechanics and optics. He also developed the functional notation, y = Fx), that we use today. In robotics we use his rotation theorem and his equations of motion in rotational dynamics. He was prolific and his collected wors fill 75 volumes. Almost half of this was produced during the last seventeen years of his life when he was completely blind. It is common practice to refer to all 3-angle representations as Euler angles but this is underspecified since there are twelve different types to choose from. The particular angle sequence is often a convention within a particular technological field. The ZYZ sequence.13) is commonly used in aeronautics and mechanical dynamics, and is used in the Toolbox. The Euler angles are the 3-vector = φ, θ, ψ). For example, to compute the equivalent rotation matrix for = 0.1, 0., 0.3) we write >> R = rotz0.1) roty0.) rotz0.3); or more conveniently >> R = eulr0.1, 0., 0.3) R = The inverse problem is finding the Euler angles that correspond to a given rotation matrix >> gamma = treulr) 9

5 ΗσυνάρτησηArctany,x)ήAtany,x) 0 θ 90 για x y 90 θ 180 για x y θ = arc tan y,x) = 180 θ 90 για x y ) 90 θ 0 για x y Επιστρέφει την γωνία στο σωστό τεταρτηµόριο y x Υπολογισμόςτωνγωνιώνα,β,γανξέρουμετον πίνακαστροφής A B R = R AB ΙσοδύναμοςσυμβολισμόςπουχρησιμοποιείταιστοβιβλίοτουCraig cosβ) = r 11 r 1, 90 β o < β < 90 o Ανβ=±)90 ο έχουμειδιάζονσημείοcosβ)=0 10

6 11 1

7 Ιδιάζονσημείοότανβ=±)90 ο Rγ,β,α) XYZ BASE = rotz,α) roty,β) rotx,γ ) = Rotα,β,γ ) ZYX EULER c α c α s α c α s α s γ s α s α c α s α c α = c γ Ανβ=±)90 ο έχουμειδιάζονσημείοcosβ)=0 " 0 sin ψ cosψ% β = π Έστωτώρα R = $ 0 cosψ sin ψ $ r 11 r 1 r 13 r 1 r r 3 r 31 r 3 r 33 α γ = ψ Rα, π 0 sinα γ) cosα γ), γ) = 0 cosα γ) sinα γ) ZYX EULER Μίασύμβασηείναιναεπιλέξουμετοα=0 x y x y x y x y x y x y cos cos cos sin sin sin sin cos cos sin Atan x)= Atanx) Οπότεγ=eAtanr 1,r )γιαβ=eπ/ 13 14

8 ΖΥZΓωνίεςEulerεφαρμογήστονσφαιρικόκαρπό) R ab α,β,γ ) ZYZ EULER = rotz,α) roty,β) rotz,γ ) = c α s α c α s α c α s β s α c α s α c α s α c β Υπάρχειιδιάζονσημείοστηνπερίπτωσηπουβ=0,καιοπίνακαςγίνεται R = I 3 Δηλαδή,οάξοναςτηςθ 6 γίνεταιπαράλληλοςμεαυτόντηςθ 4 υπάρχουνάπειρεςεπιλογέςγιατηντιμήτηςγωνίαςα R ab α,0, α) ZYZ EULER = I Στηνπερίπτωσηπούτοάκροτουβραχίονακινείταιμέσαστονχώροτότεη αρπάγητουρομπότθαέχειτονπροσανατολισμόrt)οοποίοςείναι συνάρτησητουχρόνου.μπορούμε,προκειμένουναπεριγράψουμετον προσανατολισμό,αντίγιατονπίνακαστροφήςναχρησιμοποιήσουμετις γωνίεςeuleraζυζ.όμωςηαπεικόνισητουπίνακαστροφήςστιςγωνίεςzyz γιατηνπερίπτωσηπουαναφέρουμεπαραπάνωδενείναιμονοσήμαντη. 15 Υπολογισμόςτωνγωνιώνα,β,γαπότονπίνακα στροφής R ab α,β,γ) ZYZ EULER = c α s α c α s α c α s α c α s α c α s α = r 11 r 1 r 13 r 1 r r 3 r 31 r 3 r 33 if sinβ) = r 31 r 3, 0 Ο < β <180 Ο sinβ) 0 Oτανsinβ)=0έχουμειδιάζονσημείοκαικατάσύμβαση μπορούμεναεπιλέξουμετοα=0 16

9 Oτανsinβ)=0κατάσύμβασημπορούμεναεπιλέξουμετοα=0 17 Γωνίαστροφήςκαιισοδύναμοςάξονας Γωνίαστροφήςγύρωαπόισοδύναμοάξονα θεώρηματουeuler) x x {A} z z α xvθ cθ x yvθ zsθ x zvθ ysθ % R,θ = % x yvθ zsθ yvθ cθ y zvθ xsθ % x zvθ ysθ y zvθ xsθ zvθ cθ β $ θ p ab y y x y sinβ = cosβ = z v θ = 1 c θ sinα = cosα = x " = $ :μοναδιαίοάνυσμα y y x y $ % $ z x x y Δρ.ΦασουλάςΓιάννης 19

10 Γωνίαστροφήςκαιισοδύναμοςάξονας Γωνίαστροφήςκαιισοδύναμοςάξοναςπεριστροφής απόπίνακαστροφής ΈστωέναςπίνακαςστροφήςRμεστοιχείαr ij,αποδεικνύεται ότισεαυτόντονπίνακα στροφήςαντιστοιχείοάξοναςπεριστροφήςκαιηγωνία περιστροφήςθ: " r3 r3 $ r11 r r33 1% 1 θ = arccos, r13 r ) 31 =, sin θ r r Ηαντιστοιχία«ΠίνακαςΣτροφής»σεέκφραση«ΆξοναaΓωνίας»δενείναιμοναδική π.χ.ανεπιλεχθείσανγωνίαηπaθτότεοάξοναςπουβρίσκουμεείναιοa 0 Δρ.ΦασουλάςΓιάννης ΠΑΡΑΔΕΙΓΜΑΙ Γωνίαστροφήςκαιισοδύναμοςάξονας Παράδειγμα1/) " r r r 1 ) r3 r3 1 = r r, sin θ, θ = arccos% 13 31, - r1 r1. Δρ.ΦασουλάςΓιάννης 1

11 ΠΑΡΑΔΕΙΓΜΑΙ Γωνίαστροφήςκαιισοδύναμοςάξονας Παράδειγμα/) r11 r r 33 = 0 Η γωνία υπολογίζεται ως 1 0 θ = arc cos ) = 10 " r r r 1 ) r3 r3 1 = r r, sin θ, θ = arccos% 13 31, - r1 r1. και ο αντίστοιχος άξονας περιστροφής είναι T =,, ) Δρ.ΦασουλάςΓιάννης ΠΑΡΑΔΕΙΓΜΑΙΙ Γωνίαστροφήςκαιισοδύναμοςάξονας R ab = Rot, θ)rotz, ϕ) {A}"{B} Rot,θ) Rotz,φ) άξονας=στροφήκατάrotz,ψ)τουάξοναyτου{α} 0$ y = 1 " 0% R ab ψ, θ, φ) cosψ sin ψ 0$ 0$ sψ $ x $ = % sin ψ cosψ 0 % 1 = % c = % % % % ψ % y % 0 0 1% 0 % 0 % z % s v c s c v c s = ψ θ c s sψsθ cθ ψ θ θ ψ ψ θ ψ θ sψcψ vθ cψ vθ cθ sψsθ R,θ % cφ sφ 0 sφ cφ 0 ) 0 0 1) Rotz,φ) Δρ.ΦασουλάςΓιάννης 3