Τεχνολογικό Eκπαιδευτικό Ίδρυμα Kρήτης Διατμηματικό Μεταπτυχιακό Πρόγραμμα "Προηγμένα συστήματα παραγωγής, αυτοματισμού και ρομποτικής" Βιομηχανική Ρομποτική «Τοπική παραμετροποίηση πινάκων στροφής, γωνίες Euler, πίνακας στροφής γύρω από ισοδύναμο άξονα» Δρ. Φασουλάς Γιάννης 1
Περισσότερα για τις στροφές Η στροφή ενός ΣΣ μπορεί να αντιστοιχηθεί σε ένα πίνακα στροφής Ο πίνακας στροφής έχει 9 στοιχεία Ο πίνακας στροφής μπορεί να παραμετροποιηθεί με διάφορους τρόπους: Γωνίες Roll, Pitch, Yaw Γωνίες Euler Με άλλους τρόπους Δρ. Φασουλάς Γιάννης 2
Προσανατολισμός με τοπική παραμετροποίηση Προσανατολισμός με τοπική παραμετροποίηση 3 Ανεξάρτητοι παράμετροι R 3 Γωνίες στροφής γύρω από τους βασικούς άξονες του αδρανειακού πλαισίου ΒΑΣΙΚΕΣ ΓΩΝΙΕΣ Χ- Υ- Ζ 3 Γωνίες στροφής γύρω από τους βασικούς άξονες του κινουμένου πλαισίου ΓΩΝΙΕΣ EULER Ζ- Υ- Χ Γωνία στροφής γύρω από ισοδύναμο άξονα Δρ. Φασουλάς Γιάννης 3
ΒΑΣΙΚΕΣ ΓΩΝΙΕΣ (Στροφές) Χ- Υ- Ζ Προσανατολισμός με τοπική παραμετροποίηση {A} rot(x A, γ ) e Az eax {A} eay rot(y A, β ) R ( γβα,, ) = rot(z, α ) rot(y, β ) rot(x, γ) ab XYZ rot(z A, α ) { B} Αντί να περιγράφουμε τον προσανατολισμό με τον πίνακα στροφής Χρησιμοποιούμε μόνο τις γωνίες γ,β,α που λέγονται βασικές γωνίες Χ- Υ- Ζ γύρω από σταθερούς άξονες. Γωνίες γ,β,α ή αλλιώς γωνίες Roll, Pitch, Yaw Δρ. Φασουλάς Γιάννης 4
Προσανατολισμός με τοπική παραμετροποίηση Γωνίες Euler Z- Y- X {A} {B} R ( γβα,, ) = rot(z, α ) rot(y, β ) rot(x, γ) ab XYZ e Az Διαφορετική ερμηνεία του παραπάνω πίνακα στροφής eax {A} eay {A} rot(z B, α) rot(y B, β ) rot(x B, γ ) { B} ZYX Γωνίες Euler = (α,β,γ) Αντί να περιγράφουμε τον προσανατολισμό με τον πίνακα στροφής Χρησιμοποιούμε μόνο τις γωνίες α, β, γ, που λέγονται ΖΥΧ γωνίες Euler (γύρω από τους κινούμενους άξονες). Δρ. Φασουλάς Γιάννης 5
Γωνίες Euler (Roll, Προσανατολισμός Pitch, με Yaw) τοπική ab ZYX Γωνίες Euler rot(z B, α) ZYX παραμετροποίηση { A} { B( 0)} rot(y B, β ) R ( αβγ,, ) = rot(z, α) rot(y, β) rot(x, γ ) = cα sα 0 cβ 0 sβ 1 0 0 = sα cα 0 0 1 0 0 cγ sγ 0 0 1 sβ 0 cβ 0 sγ cγ cc css sc csc + ss = sc sss + cc ssc cs sβ cβsγ cβcγ α β α β γ α γ α β γ α γ α β α β γ α γ α β γ α γ =R ab( γβα,, ) rot(x B, γ ) { B} XYZ Roll {A} = {B(0)} Yaw Pitch 6
Σύνοψη {A} {B} Προσανατολισμός με τοπική παραμετροποίηση R ab βασικές γωνίες Χ(γ)- Υ(β)- Ζ(α) Ζ(α)- Υ(β)- Χ(γ) γωνίες Euler R ab (!,",) XYZBASE = rot(z,) rot(y,") rot(x,! ) = Rot(,",! ) ZYXEULER cα sα 0 cβ 0 sβ 1 0 0 = s c 0 0 1 0 0 c s α α γ γ = 0 0 1 sβ 0 cβ 0 sγ cγ cc css sc csc + ss sc sss + cc ssc cs sβ cβsγ cβcγ α β α β γ α γ α β γ α γ α β α β γ α γ α β γ α γ Υπολογισμός των γωνιών α, β, γ αν ξέρουμε τον πίνακα στροφής Δρ. Φασουλάς Γιάννης 7
Η συνάρτηση Arctan2(y,x) ή Atan2(y,x) 0 θ 90 για + x + y 90 θ 180 για x + y θ= arc tan 2(y,x) = 180 θ 90 για x y 90 θ 0 για + x y Επιστρέφει την γωνία στο σωστό τεταρτηµόριο y x
Υπολογισμός των γωνιών α, β, γ αν ξέρουμε τον πίνακα στροφής cos(!) = + r 2 11 + r 2 21, " 90! 90!90 o < " < 90 o Αν β= (±) 90 ο έχουμε Ιδιάζον σημείο cos(β)=0 Δρ. Φασουλάς Γιάννης 9
10 Δρ. Φασουλάς Γιάννης
11 Δρ. Φασουλάς Γιάννης
Ιδιάζον σημείο όταν β=(±) 90 ο R(!,",) XYZBASE = rot(z,) rot(y,") rot(x,! ) = Rot(,",! ) ZYXEULER " c! c! s " s! s! c c! s " c + s! s s! s! s " s + c! c s! s " c! c! s!s " s c! = " r 11 r 12 r 13 r 21 r 22 r 23 r 31 r 32 r 33 Αν β= (±) 90 ο έχουμε Ιδιάζον σημείο cos(β)=0 Έστω τώρα! =! " 2 R = 0 sin ψ cosψ 0 cosψ sin ψ 1 0 0! + " = 1.!"()*"+,- *./*0*1*)-. cos! x y" cos! x" cos! y"! sin! x" sin! y" R(!, " 0 " sin(! + ) " cos(! + ) ( 2, ) = * sin! x y" sin! x" cos! y" cos! x" sin! y" 0 cos(! + ) " sin(! + ) * ZYX"EULER * Atan( x)= Atan(x) cos x + (*! sin! x" 1 0 0 2 ) ) sin x + ( * cos! x ) " 2 Μία σύμβαση να επιλέξουμε το α=0 Οπότε γ=- Atan2(r 12,r 22 )! "! "! "! "! "! " 2 2 cos 2x cos x, sin x 12 sin 2x 2sin x cos x
13 Δρ. Φασουλάς Γιάννης
ΖΥZ Γωνίες Euler R ab (!,", ) ZYZEULER = rot(z,!) rot(y,") rot(z, ) = c! c s! s c! s s! c c! s " s! c + c! s s! s + c! c s! s " s " c s " s ( * * * * ) Υπάρχει ιδιάζον σημείο στην περίπτωση που R = I 3 γιατί υπάρχουν άπειρες επιλογές για την τιμή της γωνίας α R ab (!,0,"!) ZYZ"EULER = I Στην περίπτωση πού το άκρο του βραχίονα κινείται μέσα στον χώρο τότε η αρπάγη του ρομπότ θα έχει τον προσανατολισμό R(t) ο οποίος είναι συνάρτηση του χρόνου. Μπορούμε, προκειμένου να περιγράψουμε τον προσανατολισμό, αντί για τον πίνακα στροφής να χρησιμοποιήσουμε τις γωνίες EULER- ΖΥΖ. Όμως η απεικόνιση του πίνακα στροφής στις γωνίες zyz για την περίπτωση που αναφέρουμε παραπάνω δεν είναι μονοσήμαντη. 14
Υπολογισμός των γωνιών α, β, γ από τον πίνακα στροφής R ab (!,",) ZYZEULER = c! c s! s c! s s! c c! s " s! c + c! s s! s + c! c s! s " s " c s " s ( * * * * )! = " r 11 r 12 r 13 r 21 r 22 r 23 r 31 r 32 r 33 if sin(!) = + r 2 11 + r 21, 0 " <! < 180 " sin(!) " 0 Oταν sin(β)=0 έχουμε ιδιάζον σημείο και κατά σύμβαση μπορούμε να επιλέξουμε το α=0 15 Δρ. Φασουλάς Γιάννης
Oταν sin(β)=0 κατά σύμβαση μπορούμε να επιλέξουμε το α=0 16 Δρ. Φασουλάς Γιάννης
FROM CRAIG S BOOK 17 Δρ. Φασουλάς Γιάννης
Γωνία στροφής και ισοδύναμος άξονας Γωνία στροφής γύρω από ισοδύναμο άξονα (θεώρημα του EULER) x k x {A} z k z α 2 k xvθ + cθ k xk yvθ k zsθ k xk zvθ + k ysθ 2 R k,θ = k xk yvθ + k zsθ k yvθ + cθ k yk zvθ k xsθ 2 k xk zvθ k ysθ k yk zvθ + k xsθ k zvθ + cθ β k θ p ab k y y 2 2 x y sinβ = k + k cosβ = k z v θ = 1 c θ sinα = cosα = k k= k k k:μοναδιαίο άνυσμα k k k y 2 2 x y + k k x x y z 2 2 x + k y Δρ. Φασουλάς Γιάννης 18
Γωνία στροφής και ισοδύναμος άξονας Γωνία στροφής και ισοδύναμος άξονας περιστροφής από πίνακα στροφής Έστω ένας πίνακας στροφής R με στοιχεία r ij, αποδεικνύεται ότι σε αυτόν τον πίνακα στροφής αντιστοιχεί ο άξονας περιστροφής k και η γωνία περιστροφής θ : r32 r23 r11 + r22 + r33 1 1 θ= arccos, k r13 r 31 2 = 2sin θ r21 r 12 Η αντιστοιχία «Πίνακας Στροφής» σε έκφραση «Άξονα- Γωνίας» δεν είναι μοναδική π.χ. Αν επιλεχθεί σαν γωνία η 2π- θ τότε ο άξονας που βρίσκουμε είναι ο - k 19 Δρ. Φασουλάς Γιάννης
ΠΑΡΑΔΕΙΓΜΑ Ι Γωνία στροφής και ισοδύναμος άξονας Παράδειγμα (1/2) r + r + r 1 2 r32 r23 1 k= r13 r 31 2sin θ r21 r12 11 22 33 θ= arccos Δρ. Φασουλάς Γιάννης 20
ΠΑΡΑΔΕΙΓΜΑ Ι Γωνία στροφής και ισοδύναμος άξονας Παράδειγμα (2/2) r11 + r22 + r 33 = 0 Η γωνία υπολογίζεται ως 1 0 θ = arc cos( ) = 120 2 r + r + r 1 2 r32 r23 1 k= r13 r 31 2sin θ r21 r12 11 22 33 θ= arccos και ο αντίστοιχος άξονας περιστροφής είναι 1 1 1 1 1 T k = (,, + ) 3 2 3 2 2 3 2 Δρ. Φασουλάς Γιάννης 21
ΠΑΡΑΔΕΙΓΜΑ ΙΙ Γωνία στροφής και ισοδύναμος άξονας {A} {B} R ab = Rot(k, θ)rot(z, ϕ) Rot(k,θ) Rot(z, φ) άξονας k=στροφή κατά Rot(z,ψ) του άξονα y του {Α} y = 0 1 0 R ab ( ψ, θ, φ) = cosψ sin ψ 0 0 sψ k x k = sin ψ cosψ 0 1 = c = k ψ y 0 0 1 0 0 k z s v c s c v c s + ψ θ c s sψsθ cθ 2 ψ θ + θ ψ ψ θ ψ θ 2 sψcψ vθ cψ vθ cθ sψsθ R k,θ cφ sφ 0 sφ cφ 0 0 0 1 Rot(z,φ) Δρ. Φασουλάς Γιάννης 22
Παράδειγμα 2.9 Craig! k {B } {A } A P =! " 1 2 3 k =! " 0.707 0.707 0, = 30 {A}! P g A A! = " 1 0 0 1 0 1 0 2 0 0 1 3 0 0 0 1 " g AB = g B! B = {B} 1 0 0 "1 0 1 0 "2 0 0 1 "3 0 0 0 1 ( ( ( ( 0.933 0.067 0.354!1.13 0.067 0.933!0.354!1.13!0.354 0.354 0.866 0.05 0 0 0 1 g A! B! = Rot(k,") = g AB = g A! 0.933 0.067 0.354 0 0.067 0.933 0.354 0 0.354 0.354 0.866 0 0 0 0 1 A g A! B! g B! B Όταν ο άξονας στροφής k δεν περνάει από το κέντρο των συντεταγμένων του {Α} α) προκαλεί μετακίνηση του στρεφόμενου πλαισίου από την αρχική θέση, και β) προκαλεί στροφή ίδια με αυτήν που θα είχε το πλαίσιο αν ο άξονας- k περνούσε από το κέντρο των συντεταγμένων του {Α}. o ) ) ) ) (
Στις γωνίες Euler εμφανίζεται ιδιομορφία όταν ο μεσαίος άξονας γίνει παράλληλος με τον πρώτο ή τον τρίτο άξονα περιστροφής Roll- Pitch- Yaw angles R=rotX(γ)rotY(β)rotZ(α) Tool frame z = approach vector!! a = " Orienta on vector between the fingers of the robot s griper a x a y a z, z axis tool frame! n =! o!! a!! o = " o x o y o z! R = " n x o x a x n y o y a y n z o z a z