Ρομποτική Ι: Διαφορική Κινηματική Ανάλυση

Transcript

1 Σχολή Ηλεκτρολόγων Μηχ/κών και Μηχ/κών Υπολογιστών, Ε.Μ.Π., Ακαδημαϊκό Έτος 8-9, 7ο Εξάμηνο Ρομποτική Ι: Διαφορική Κινηματική Ανάλυση Κων/νος Τζαφέστας Τομέας Σημάτων, Ελέγχου & Ρομποτικής Σχολή Ηλεκτρ. Μηχ/κών & Μηχ/κών Υπολ., Ε.Μ.Π. Τηλ.: ( (Κτήριο Ηλεκτρ., Γραφείο.36 ktzaf@softlab.tua.gr Web: Διαφορική Κινηματική Ανάλυση Ρομποτικών Χειριστών

2 Διαφορική Κινηματική Ανάλυση Ορθή διαφορική κινηματική ανάλυση Γενικευμένες ταχύτητες αρθρώσεων {q i } Εύρεση ταχύτητας (v, ω (γραμμική/γωνιακή τελικού στοιχείου δράσης ρομπότ Μετασχηματισμός από το χώρο αρθρώσεων στο χώρο δράσης (εργασίας proprioceptio Ανάστροφη διαφορική κινηματική ανάλυση Ταχύτητα τελικού στοιχείου δράσης (v, ω Εύρεση {q i } Σχεδιασμός δρόμου ρομπότ 3 Διαφορικές κινηματικές ρομποτικές εξισώσεις: Παράδειγμα ( βαθμοί ελευθ. D, επίπεδο l (p x, p Ε O Ε q x Ε θ z Ορθό Κινηματικό μοντέλο: ( βαθμοί ελευθερίας: q και q Δεδομένα: «Ταχύτητες» αρθρώσεων q, q Εύρεση: Ταχύτητα (γραμμική/γωνιακή τελικού στοιχείου δράσης [p x,p, ω z ] Τ O O l q x x p x l cos(q + l cos(q + q p l si(q + l si(q + q θ z q + q J(q,q p x (dp x /dt q (l s +l s q l s p (dp /dt q (l c +l c + q l c ω z θ z q + q p x p θ z (l s +l s l s (l c +l c l c q q 4

3 Ορθή διαφορική κινηματική ανάλυση: Παράδειγμα ( 3 βαθμοί ελευθ. D, επίπεδο Ε l 3 x Ε O Ε q 3 θ z Κινηματική μοντέλο: Δεδομένα: «Ταχύτητες» αρθρώσεων q, q, q 3 Εύρεση: Ταχύτητα (γραμμική/γωνιακή τελικού στοιχείου δράσης [p x,p, ω z ] Τ x O 3 l q p x l c + l c + l 3 c 3 p l s + l s + l 3 s 3 θ z q + q + q 3 O O l q x x p x p θ z (l s +l s +l 3 s 3 (l s +l 3 s 3 l 3 s 3 (l c +l c +l 3 c 3 (l c +l 3 c 3 l 3 c 3 q q q 3 J(q, q, q 3 5 Διαφορική Κινηματική: Ιακωβιανή Μήτρα Ρομποτικού Χειριστή Έστω: p(q,q,q 3 [ p x (q,q,q 3,p (q,q,q 3, θ z (q,q,q 3 ] Τ ϑ p ( q, q, q ϑp ( q, q, q ϑp ( q, q, q dp dq + dq + dq x x 3 x 3 x 3 ϑq 3 ϑq ϑq3 ϑ p ( q, q, q ϑp ( q, q, q ϑp ( q, q, q dp dq + dq + dq ϑq 3 ϑq ϑq3 ϑθ ( q, q, q ϑθ ( q, q, q ϑθ ( q, q, q dθ dq + dq + dq z z 3 z 3 z 3 ϑq 3 ϑq ϑq3 dp dp dp J dq x dθ z όπου dq dq dq dq 3 και J ϑ p ϑp ϑp ϑq ϑq ϑq ϑp ϑp ϑ p ϑq ϑq ϑq ϑθ ϑθ ϑθ ϑq ϑq ϑq x x x 3 z 3 z z z 3 Ιακωβιανή Μήτρα (Jacobia 6

4 Μετασχηματισμοί απειροστών περιστροφών Μήτρα απειροστής περιστροφής (dθ x γύρω από τον άξονα x: Rx( dθx cos( dθx si( dθx dθx si( dθ cos( dθ dθ Αντίστοιχα: dθ R( dθ dθ x x x dθ z Rz( dθz dθz Ισχύει: dθz dθ R( dθx, dθ, dθz Rx( dθx R( dθ Rz( dθz dθz dθx dθ dθ και: ( dθz+ dθz ( dθ dθ + Rd ( θx, dθ, dθz Rd ( θx, dθ, dθz ( dθz+ dθz ( dθx+ dθx ( dθ + dθ ( dθ + dθ x x R( dθ + dθ, dθ + dθ, dθ + dθ x x x z z Διάνυσμα απειροστής στροφής dθ dθ dθ dθ x z 7 Υπολογισμός Ιακωβιανής Μήτρας ( Έστω: d dp r dθ v p E E E ω E το (6x διάνυσμα απειροστής μετατόπισης (μεταφοράς dr E και στροφής dθ E του τελικού στοιχείου δράσης Ε όπου v E, ω E : γραμμική και γωνιακή ταχύτητα του τελικού στοιχείου δράσης Ε Διαφορικό κινηματικό μοντέλο: p J q όπου q [q,, q ] : (x διάνυσμα των ταχυτήτων των αρθρώσεων J J... J L L L J όπου, J J... J J L J i A i A A A Ιακωβιανή Μήτρα (6x : (3x διανύσματα στήλης «συνεισφορά» του q i (ταχύτητα άρθρωσης i στη γραμμική και γωνιακή ταχύτητα του τελικού στοιχείου δράσης v J iq +J iq + +J iq E L δηλαδή: L L ω J iq+j iq+ +J iq E A A A 8

5 Υπολογισμός Ιακωβιανής Μήτρας ( J b r L i J A i J b i i, E b i L i b i J A i,..., i i : για στροφική άρθρωση όπου b i- :o άξονας της άρθρωσης i r i-,e : διάνυσμα Ο i- Ο Ε : για πρισματική άρθρωση R i- ( q q b όπου: b [,, ] (στη μεθοδολογία Deavit-Harteberg r A ( q q r - A ( q q r όπου: r [,,, ],...,,..., i, E i- i άρθρωση i b i- O i- z O A( q, q A( q A( q A ( q,, q A ( q,, q Ai-( q i- i i- i i- i A ( q,, q A ( q,, q A ( q x b i- r i-,e O E dr Ε - - dθ Ε 9 Υπολογισμός Ιακωβιανής Μήτρας Παράδειγμα: Ρομποτικός Βραχίονας 3-R A (q O Ε l 3 q 3 l z q l q z 3 3 z x 3 b x O b x O z b O c -s s c l x c s l s, A (q, -s c l c A (q,q A (q A (q A 3(q A (q,q A (q 3 3 A 3(q 3 c 3 s 3 l 3 s 3 -s 3 c 3 l 3 c 3 c c -s c s l c s s c c s s l s s -s c l + l c c c 3 -s c s 3 c (l s + l 3 s 3 s c 3 c s s 3 s (l s + l 3 s 3 -s 3 c 3 l + l c + l 3 c 3

6 Υπολογισμός Ιακωβιανής Μήτρας Ρομποτικoύ Βραχίονα 3-R (συνέχεια ( b ( + l s ( ( c l s r s l s + l s, E 3 3 l + l c + l c b r s c ( + l s 3 3 ( ls ( lc + lc c l s s ls+, E b s c r ( lcs 33 ( lss ( lc, E Υπενθύμιση : J b r L i J A i i i, E b i : για στροφική άρθρωση i J L i b i J A i : για πρισματική άρθρωση Υπολογισμός Ιακωβιανής Μήτρας Ρομποτικoύ Βραχίονα 3-R (συνέχεια ( c l s + l s 3 3 s l s + l s 3 3 J s l s l s c l s l s L b r, E l l c l c c l s + l s 3 3 c l c + l c 3 3 s c s l s l s s l c l c J L b r, E lc + lc ls ls lcs lcc s J c l ss l sc L b r 3, E lc ls J A s J c A s J c A 3

7 Αντίστροφη Διαφορική Κινηματική Δεδομένης επιθυμητής ταχύτητας τελικού στοιχείου δράσης (v, ω Εύρεση {q i } (i,, v p E ( ω J q q E : ορθό διαφορικό κινηματικό μοντέλο Εάν η J(q είναι αντιστρέψιμη, δηλαδή: det(j(q, τότε : q J-( q p q : ταχύτητες αρθρώσεων για να επιτύχουμε επιθυμητή ταχύτητα τελικού στοιχείου δράσης Εκφυλισμός διάταξης ρομποτικού βραχίονα. Ιδιόμορφες διατάξεις (sigular cofiguratios q για τις οποίες: det(j(q Υπάρχει τουλάχιστον μία διεύθυνση κατά την οποία το ρομπότ δεν μπορεί να κινηθεί p 3 Αντίστροφη Διαφορική Κινηματική: Παράδειγμα ( βαθμοί ελευθ. D, επίπεδο O O l q x l (p x, p Ε O Ε q x x Ε θ z Ορθό Κινηματικό μοντέλο: ( βαθμοί ελευθερίας: q και q v x (l s +l s l s v (l c +l c l c J(q, q det( J(q, q l l si(q q q det( J(q, q, όταν si(q, δηλαδή όταν : q ή π (ιδιόμορφες διατάξεις q l c l s v v x q ll s ( lc + lc ( ls + l s J - (q, q : αντίστροφο κινηματικό μοντέλο 4

8 Ρομπότ με πλεονάζοντες βαθμούς ελευθερίας ( N(J q R Χώρος αρθρώσεων (joit space J p R m R(J p R(J: rage space (σύνολο δυνατών ταχυτήτων στο χώρο εργασίας N(J: ull space (μηδενικός χώρος J Χώρος εργασίας (task space q N( J Jq ( Όταν >m, και rak(jm (δηλαδή, J: πλήρους τάξης τότε έχουμε (-m πλεονάζοντες (redudat βαθμούς ελευθερίας dimn(j m dimn(j + dimr(j 5 Ρομπότ με πλεονάζοντες βαθμούς ελευθερίας ( Βέλτιστη λύση της διαφορικής κινηματικής εξίσωσης για πλεονάζοντα ρομπότ (redudat robots Διαφορική κινηματική εξίσωση: J q p J: m x, rak(j m Ελαχιστοποίηση συνάρτησης κόστους: F( q (/ q (/ qq Mέθοδος Lagrage: F' q λ q q λ ( Jq p q F'( q, λ F'( q, λ λ (, (/ - - mi q - J λ Jq- p qj + p όπου J + : ψευδοαντίστροφη της Ιακωβιανής J J ( J J + 6

9 Ρομπότ με πλεονάζοντες βαθμούς ελευθερίας (3 Γενική βέλτιστη λύση της διαφορικής κινηματικής εξίσωσης για πλεονάζοντα ρομπότ (redudat robots Διαφορική κινηματική εξίσωση: J q p Ελαχιστοποίηση συνάρτησης κόστους: F( q ( ( q k Q ( q k ( Q: smmetric, positive defiite x, k R Mέθοδος Lagrage: F' q λ ( ( q k Q ( q k λ ( Jq p q F'( q, λ F'( q, λ λ (, - - mi ( - Qq k J λ Jq- p J: m x, rak(j m qj p+ ( I J J k # # Q Q (I : μοναδιαία μήτρα x N( J J Q όπου # weighted pseudoiverse of the Jacobia matrix J Q J ( J Q J # Q 7 Ρομπότ με πλεονάζοντες βαθμούς ελευθερίας (4 Γενική βέλτιστη λύση της διαφορικής κινηματικής εξίσωσης : Απόδειξη ( - Qq k J λ Jq- p ( ( q Q J λ + k Jq- p q Q J λ + k ( JQ J λ + Jk p rak(jm q Q J λ + k ( ( λ JQ J p Jk ( + ( q Q J JQ J p I Q J JQ J J k J # Q J # Q 8

10 Ιεραρχική Δομή Ρομποτικών Συστημάτων Ελέγχου Πρόγραμμα Εργασίας c εντολή στόχου Ευφυής Ελεγκτής Σχεδιασμός Δρόμου p(, i p ( i Περιορισμοί Αλγόριθμος αντίστροφης κινηματικής σήματα αναφοράς q(, i q ( i r r b Βρόχοι Ελέγχου a q(, i q ( i Στοιχεία Δράσης Μονάδα Παρεμβολής Αισθητήρια Στοιχεία 9 Έλεγχος αναλυμένης ταχύτητας (Resolved motio rate cotrol q(t q(i Υπολογισμός j A j και A j (j,, Υπολογισμός Ιακωβιανής J( q Βρόχος διόρθωσης (correctio loop p p *( i A :3,4 - p(i+ + p p( i+ p * ( i dt επιθυμητή τροχιά Επίλυση αντίστροφης διαφορικής κινηματικής p J( q q Εύρεση q q( i+ q ( i + q ( i dt dt: samplig time i i+ q( i+, q ( i προς ελεγκτή θέσης ρομπότ