Σχολή Ηλεκτρολόγων Μηχ/κών και Μηχ/κών Υπολογιστών, Ε.Μ.Π., Ακαδημαϊκό Έτος 7-8, 7ο Εξάμηνο Μάθημα: Ρομποτική Ι Αυτόματος Έλεγχος Ρομπότ 3 (Έλεγχος Δύναμης) Κων/νος Τζαφέστας Τομέας Σημάτων, Ελέγχου & Ρομποτικής Σχολή Ηλεκτρ. Μηχ/κών & Μηχ/κών Υπολ., Ε.Μ.Π. Τηλ.: (1) 77-3687 (Κτήριο Ηλεκτρ., Γραφείο 1.36) E-mail: ktzaf@softlab.ntua.gr Web: http://www.softlab.ntua.gr/~ktzaf/ 1 Ε.Μ.Π., ΣΗΜΜΥ, Ακαδημαϊκό Έτος 7-8, 7ο Εξάμηνο Μάθημα: Ρομποτική Ι. Διδάσκων: Κ.Τζαφέστας Ρομποτικός Έλεγχος Δύναμης / Μηχανικής Αντίστασης (Fore / Impedane Robot Manipulation Control)
Ρομποτικός Έλεγχος Δύναμης - Εισαγωγή Έλεγχος μηχανικής αντίστασης (impedane ontrol) Passive impedane Remote enter of ompliane (RCC) (εργαλεία παθητικής «συμμόρφωσης») Ative Impedane Ative stiffness ontrol (έλεγχος «ενεργούς μηχανικής αντίστασης») Έλεγχος δύναμης (fore ontrol) Υβριδικός έλεγχος θέσης/δύναμης (hybrid fore/impedane ontrol) Παραδείγματα - Προσομοίωση 3 Μέθοδος Παθητικής Συμμόρφωσης (passive ompliane) Μηχανικά εργαλεία παθητικής συμμόρφωσης Remote Center of Compliane (RCC) Αρχή λειτουργίας RCC (α) συμμόρφωση σε οριζόντια δύναμη (β) συμμόρφωση σε ροπή 4
Μέθοδος Παθητικής Συμμόρφωσης (συνέχεια) (1) Βασική Σχέση: f Κ ε f [f x, f y, f z, N x, N y, N z,] : εξωτερική δύναμη/ροπή ε [ε x, ε y, ε z, α x, α y, α z,] : μηχανική παραμόρφωση (strain) RCC 3 διαστάσεων K Κ soft Κ soft Κhard Κ soft Κ soft Κ soft K : μήτρα μηχανικής σκληρότητας (ακαμψίας) (stiffness matrix) 5 Μέθοδος Παθητικής Συμμόρφωσης (συνέχεια) () Μηχανικά εργαλεία παθητικής συμμόρφωσης Remote Center of Compliane (RCC) 6
Μέθοδος Παθητικής Συμμόρφωσης (συνέχεια) (3) Εργαλεία παθητικής συμμόρφωσης σε ρομποτικές εργασίες συναρμολόγησης (peg-in-hole tasks) 7 Ρομποτικοί Αισθητήρες Δύναμης (fore sensors) Αισθητήρας δύναμης: Κατάλληλη μηχατρονική διάταξη αισθητήρων ελαστικής (μηχανικής) παραμόρφωσης (strain gauges strain/deformation transduers) 8
Αισθητήρες ελαστικής παραμόρφωσης (strain gauge transduers) 9 Έλεγχος ενεργούς μηχανικής αντίστασης (ative-impedane ontrol) Απλό παράδειγμα 1 β.ε. Δυναμικό μοντέλο συστήματος: mx bx kx f F a a a u m a, b a, k a : δυναμικές παράμετροι συστήματος f u : ελεγκτής F : εξωτερική δύναμη Επιθυμητή μηχανική αντίσταση (desired impedane): md x bd x kd x xd F Fd ή πληρέστερα: m x x b x x k x x F F ( ) ( ) ( ) ( ) d d d d d d d m d, b d, k d : επιθυμητές δυναμικές παράμετροι «κλειστού βρόχου» συστήματος (μοντέλο αναφοράς) k d b d -F d m d F F : δύναμη ασκούμενη από το εξωτερικό περιβάλλον πάνω στο σύστημα F d : επιθυμητή δύναμη η οποία θέλουμε τελικά x x να ασκείται από το εξωτερικό περιβάλλον d πάνω στο σύστημα 1
Έλεγχος ενεργούς μηχανικής αντίστασης: Απλό παράδειγμα 1 β.ε. mx bx kx f F a a a u ( ) ( ) ( ) m x x b x x k x x F F d d d d d d d d d ( ω m, ζ ) d mk d d Ελεγκτής ενεργούς μηχανικής αντίστασης: f mu bx kx F u a a a (1) (3) k : ιδιοσυχνότητα u x b (1) : συντελεστής απόσβεσης : εξίσωση κλειστού βρόχου : Δυναμικό μοντέλο συστήματος : Επιθυμητή μηχανική αντίσταση (desired impedane) (επιθυμητή συμπεριφορά κλειστού βρόχου) ( ) ( ) ( ) 1 u xd md bd x d x kd xd x Fd F (3) Για να επιτύχουμε την επιθυμητή συμπεριφορά (), έχουμε για το u: () (όπου u σήμα ελέγχου) (4) 11 Έλεγχος ενεργούς μηχανικής αντίστασης: Απλό παράδειγμα 1 β.ε. (συνέχεια) (1) Ο νόμος ελέγχου μηχανικής αντίστασης (3) και (4), γράφεται ουσιαστικά ως: m m m f mx b x x k x x F F ( ) ( ) ( ) a a a u a d d d d d d md md md e x ex bx a kx a F ή άλλως: K V K P K F ( ) ( ) ( ) ( 1) f mx bx kx K x x K x x K F u a d a a V d P d F με K m a F, KV KFbd και KP KFkd md Παρατήρηση: Το κέρδος ανάδρασης δύναμης K F επιτρέπει τη μεταβολή των «αδρανειακών» χαρακτηριστικών του συστήματος (στο κλειστό βρόχο) ef (όταν F d ) 1
Έλεγχος ενεργούς μηχανικής αντίστασης: Απλό παράδειγμα 1 β.ε. (συνέχεια) () k d b d m d F k b E Επαφή με εξωτερικό περιβάλλον E k : σκληρότητα b : απόσβεση x : θέσης ισορροπίας x x Δυναμική εξίσωση εξωτερικού περιβάλλοντος : b x x k x x F (5) ( ) ( ) Δυναμική εξίσωση () κλειστού βρόχου συστήματος {M, b, k, f u } (μηχανική αντίσταση συστήματος που επιτυγχάνεται με τον ελεγκτή f u ): ( ) ( ) ( ) m x x b x x k x x F F d d d d d d d 13 Έλεγχος ενεργούς μηχανικής αντίστασης: Απλό παράδειγμα 1 β.ε. (συνέχεια) (3) Δυναμική συμπεριφορά τελικού κλειστού συστήματος {M, b, k, f u } σε επαφή με το εξωτερικό περιβάλλον Ε (k, b, x ): () (5) ( ) ( ) ( ) ( ) mx b b x k k x mx bx bx d d d d d d d k kx d d kx F d k bd b d ω m, ζ d md ( kd k) : ιδιοσυχνότητα : συντελεστής απόσβεσης Δύναμη πάνω στο σύστημα από το εξωτερικό περιβάλλον εργασίας, στην τελική (μόνιμη) κατάσταση (t ): Fef F( t ) k ( x xf ) όπου x f : τελική θέση συστήματος x ( ) 1 f x kdxd kx ( t ) F Άρα: ef ( ) kk d k F F t x x F ( ) ( ) d kd k ( kd k) ( kd k) ( kd k) d d 14
Έλεγχος ενεργούς μηχανικής αντίστασης ρομποτικού χειριστή - Γενικά Γενική Περίπτωση (ρομποτικός χειριστής) (a) Σύστημα ελέγχου (ροπές τ i ) με ανάδραση δύναμης F (και θέσης y) (b) Μηχανική αντίσταση που θέλουμε να επιτύχουμε μέσω του ρομποτικού ελεγκτή ενεργούς μηχανικής αντίστασης 15 Δυναμική ρομποτικού χειριστή (1) Δυναμικό μοντέλο ρομποτικού χειριστή (υπενθύμιση): M( q) q h( q, q ) τ J F (στο χώρο των γενικευμένων μεταβλητών q i των αρθρώσεων) όπου Μ: μήτρα αδρανείας ρομπότ (συχνά συμβολίζεται και ως D(q)) h: όροι Coriolis και φυγοκέντρου δυνάμεως τ: ροπές (γενικευμένες μεταβλητές δράσης) στις αρθρώσεις F e : εξωτερική δύναμη που ασκείται από το εξωτερικό περιβάλλον και J : Ιακωβιανή μήτρα του ρομπότ e (Δ1) Μοντέλο Lagrange : Μοντέλο Newton-Euler : τ i d dt i i i K K P q q q fi mv i i Ni I iωi ωi ( Iiωi) K: κινητική ενέργεια P: δυναμική ενέργεια 16
Δυναμική ρομποτικού χειριστή () Δυναμικό μοντέλο ρομποτικού χειριστή στο χώρο εργασίας (task-spae): * * M p h F F (Δ-α) όπου M * : μήτρα αδρανείας ρομπότ στο χώρο εργασίας, h * : όροι Coriolis και φυγοκέντρου δυνάμεως στο χώρο εργασίας, F a : (τj F a ) γενικευμένη δύναμη στο χώρο εργασίας οφειλόμενη στη δράση των κινητήριων στοιχείων του ρομπότ, F e : εξωτερική δύναμη που ασκείται από το εξωτερικό περιβάλλον. Έχουμε: p J q p J q J q 1 1 και (Δ1): M q h J Fa J Fe q M h M J Fa Fe 1 1 Jq JM h JM J F F Επομένως τελικά: και a με ( ) * 1 * ( M J M ) M J * h h q * 1 1 M Q Q J M J e ( a e) ( ) (Δ-β) 17 Δυναμικός έλεγχος ενεργούς μηχανικής αντίστασης ρομποτικού χειριστή (1) Επιθυμητή μηχανική αντίσταση (desired impedane) ρομποτικού χειριστή στο χώρο εργασίας (επιθυμητή δυναμική συμπεριφορά κλειστού βρόχου) ( ) ( ) ( ) M p p B p p K p p F F d d d d d d d e Δυναμική εξίσωση ρομποτικού χειριστή στο χώρο εργασίας (Δ-α): * * M p h F F a e Δυναμικός ελεγκτής ενεργούς μηχανικής αντίστασης: (βασισμένος στη μεθοδολογία ελέγχου υπολογισμένης ροπής (omputed-torque ontrol) τ J Fa (Ε) ( u p : εξίσωση κλειστού βρόχου ) * * Fa M u h Fe με το σήμα ελέγχου u να δίνεται από τη σχέση (ώστε στο κλειστό βρόχο να έχουμε την (Ε1)) 1 u pd Md Bd ( p d p ) Kd ( pd p) ( Fd Fe) (Ε3) σφάλμα ταχύτητας σφάλμα θέσης σφάλμα δύναμης (Ε1) 18
Δυναμικός έλεγχος ενεργούς μηχανικής αντίστασης ρομποτικού χειριστή () p d p d p d F d e p x - e x p - x - K d e F B d ανάδραση δύναμης ανάδραση ταχύτητας ανάδραση θέσης Δυναμικός Ελεγκτής (Ε),(E3) τ Ρομποτικός Χειριστής (Δ) ανάδραση δύναμης ανάδραση ταχύτητας ανάδραση θέσης p p F e Σχηματικό διάγραμμα (blok-diagram) δυναμικού ρομποτικού ελεγκτή ενεργούς μηχανικής αντίστασης (υπολογισμένης ροπής) 19 Δυναμικός έλεγχος ενεργούς μηχανικής αντίστασης ρομποτικού χειριστή (3) Ρομποτικό Μοντέλο: M *,h * p d p d p d F d e p x - e x p - x - K d e F B d ανάδραση δύναμης ανάδραση ταχύτητας J( q ) (task-spae) ανάδραση θέσης - * h( q, q ) * M( q ) * 1 - MM Γ(q) Ρομποτικός Χειριστής (Δ) ανάδραση δύναμης ανάδραση ταχύτητας (joint spae) ανάδραση θέσης Σχηματικό διάγραμμα (blok-diagram) δυναμικού ρομποτικού ελεγκτή ενεργούς μηχανικής αντίστασης (υπολογισμένης ροπής) d τ q q F e
Απλοποιημένος έλεγχος ενεργούς μηχανικής αντίστασης ρομποτικού χειριστή Θεωρούμε: M * I και h * οπότε ο ελεγκτής (Ε)-(Ε3) μπορεί να γραφεί: τ ( ) J p B e K e K e F d d p d p Fd F e όπου: e p p και e F F p d F d e (Ε4) Εαν επιπλέον F d και K Fd I, καθώς και Β d, pd, τότε η (Ε4) γράφεται: (Ε5) τ K qd J K e J K J e d p d q όπου: και ονομάζεται ative-stiffness ontrol, όπου: eq qd q K d : μήτρα «σκληρότητας» (stiffness matrix) ορισμένη στο χώρο εργασίας K qd J K d J : ισοδύναμη μήτρα «σκληρότητας» στο χώρο αρθρώσεων 1 Έλεγχος δύναμης για ρομποτικό χειριστή (Fore ontrol) - Εισαγωγή ενεργός μηχανική αντίσταση ρομπότ K d M d B d Fe p Δύναμη πάνω στο ρομποτικό χειριστή από το εξωτερικό περιβάλλον εργασίας, στην τελική (μόνιμη) κατάσταση (t ): ( t ) K ( ) F F p p ef e e e f όπου p f : τελική θέση (στη μόνιμη κατάσταση) του στοιχείου δράσης του ρομπότ 1 p p t K K K p K p F Άρα: p e εξωτερικό περιβάλλον p d K e B e ( ) [ ] ( ) f d e d d e e d 1 ( t ) K ( K K ) K ( ) Fef Fe e d e d pe pd Fd Βασική αρχή: Στον έλεγχο δύναμης θέτουμε: K d F ef F e (t ) F d (δηλαδή δε λαμβάνουμε υπ όψη στον έλεγχο το σφάλμα θέσης) E σφάλμα θέσης επιθυμητή δύναμη
Έλεγχος δύναμης ρομποτικού χειριστή Ρομποτικός έλεγχος δύναμης βασισμένος στο μοντέλο (υπολογισμένης ροπής): (omputed torque) ( u p : εξίσωση κλειστού βρόχου ) τ J F F u h F a * * a M e (F1) με το σήμα ελέγχου u να δίνεται από τη σχέση (ώστε στο κλειστό βρόχο να έχουμε την (Ε1)) ( ) ( ) u M B p F F K p K F F 1 d d d e Bd Fd d e ανάδραση ταχύτητας ανάδραση δύναμης (F) Σε σύγκριση με το δυναμικό ελεγκτή ενεργούς μηχανικής αντίστασης (impedane ontrol), στον έλεγχο δύναμης (fore ontrol) θέτουμε K d, δηλαδή: «Σκληρότητα» του συστήματος ελέγχου του ρομποτικού χειριστή μηδέν (μηδενική φαινόμενη «σκληρότητα» -apparent stiffness- στο χώρο εργασίας) Με άλλα λόγια δεν έχουμε ανάδραση σφάλματος θέσης παρά μόνο ανάδραση σφάλματος δύναμης (και ταχύτητας για ευστάθεια), οπότε τελικά: F ef F e (t ) F d (επιθυμητή δύναμη) 3 Υβριδικός έλεγχος δύναμης/τροχιάς ρομποτικού χειριστή (hybrid ontrol) Βασική Αρχή έλεγχος δύναμης έλεγχος θέσης Διαφορετικός τύπος ελέγχου σε διαφορετικές κατευθύνσεις στο χώρο εργασίας: Έλεγχος θέσης / δύναμης / ενεργούς μηχανικής-αντίστασης /... κλπ. ανάλογα με την υπο-εργασία 4
Υβριδικός έλεγχος δύναμης/τροχιάς ρομποτικού χειριστή (συνέχεια) (1) Παραδείγματα μηχανικών εργασιών που απαιτούν την εφαρμογή υβριδικού ελέγχου δύναμης/θέσης (σε διαφορετικούς άξονες στο χώρο του τελικού στοιχείου δράσης) 5 Υβριδικός έλεγχος δύναμης/τροχιάς ρομποτικού χειριστή (συνέχεια) () Βασικό σχήμα υβριδικού ελέγχου δύναμης/τροχιάς (1) Επιθυμητή Τροχιά Εντολή κατεύθυνσης ελέγχου {s i } (i1,..,n) Επιθυμητή Δύναμη - - Μετασχηματισμοί Συντεταγμένων [S] [I] [S] R R Μετασχηματισμοί Συντεταγμένων R R Έλεγχος Τροχιάς Έλεγχος Δύναμης Αισθητήρες Θέσης Ρομποτικός Χειριστής Αισθητήρας Δύναμης Θέση Δύναμη Sdiag[s i ] s i 1 έλεγχος τροχιάς s i έλεγχος δύναμης 6
Υβριδικός έλεγχος δύναμης/τροχιάς ρομποτικού χειριστή (συνέχεια) (3) Βασικό σχήμα υβριδικού ελέγχου δύναμης/τροχιάς () Επιθυμητή Τροχιά Εντολή κατεύθυνσης ελέγχου {s i } (i1,..,n) Επιθυμητή Δύναμη - - Μετασχηματισμοί Συντεταγμένων Έλεγχος Τροχιάς Έλεγχος Δύναμης ( ) u p ( ) u F Μετασχηματισμοί Συντεταγμένων R [S] [I] [S] R Αισθητήρες Θέσης R ανάστροφη δυναμική u τ D (.)h Ρομπότ R Αισθητήρας Δύναμης Θέση Δύναμη Sdiag[s i ] s i 1 έλεγχος τροχιάς s i έλεγχος δύναμης 7 Ρομποτικός έλεγχος μηχανικής αντίστασης Παράδειγμα R-R D (1) βαθμοί ελευθ. D, επίπεδο 1 y O l 1 l 1 m 1,I 1 y 1 O 1 q 1 m,i l q x y Ε O Ε x 1 όπου: v d ( p dt ) και: p l l ls l s 1 1 1 1 1 1 x Ε θ Δυναμικό μοντέλο Lagrange : τ i d dt i i i K K P q q q K ml q Iq 1 1 1 1 1 1 1 1 1 1ˆ ( s 1 1 1 sin( q1) ) P mgl s K mvv I q q 1 1 1 P mgls ˆ l s s q q 1 1 1 ( 1 sin ( 1 )) lsq l s q q v lq l q q 1 1 1 1 1 1 1 1 1 1 K: κινητική ενέργεια P: δυναμική ενέργεια 1ος σύνδεσμος ος σύνδεσμος 8
Ρομποτικός έλεγχος μηχανικής αντίστασης Παράδειγμα R-R D () Άρα: d dt lsq 1 1 1 ls 1 q1 q lq 1 1 1 l 1 q1 q lsq l s q q ll ssqq q 1 1 1 1 1 1 1 1 1 1 lq 1 1 1 l 1 q1 q ll 1 1 1q 1q1 q lq 1 l q q ll q q q 1 1 1 1 1 vv vv vv K ml q Iq I q q m l q l q q ll q q q 1 1 1 1 1 1 1 1 1 1 1 1 K ml q Iq I q q 1 1 1 1 1 1 q 1 m l1q1 l q1 q ll 1 q 1 q ll 1 s q 1 q q P q q mgl ˆ s mgˆ ls l s mgl ˆ mgˆ l l 1 1... 1 1 1 1 1 1 1 1 1 1 1 1 9 Ρομποτικός έλεγχος μηχανικής αντίστασης Παράδειγμα R-R D (3) Τελικά: Ρομποτικό Δυναμικό Μοντέλο D Χειριστή β.ε. I ml I m l l ll q I m l ll q τ1 1 1 1 1 1 1 1 mll s qq q mgl 1 1 1ˆ mg 1 1 ˆ l l 1 1 1 τ I ml ll 1 q 1 I ml q mllsq 1 1 mgl ˆ 1 τ M q 1 11 M q 1 1 h q h qq g 1 11 1 1 τ M q 1 M q h q g 1 11 1 όπου: M11 I1 ml 1 1 I m ( l1 l ll 1 ) M I ml M1 M1 I m ( l ll 1 ) h1 h11 h11 mll 1 s g1 m1gl ˆ 11 mg ˆ( l 1 1 l1) g mgl ˆ 1 Δηλαδή: 3
Ρομποτικός έλεγχος μηχανικής αντίστασης Παράδειγμα R-R D (4) Είναι: Θέτουμε: M M M h h q h qq g Ρομποτικός Ελεγκτής Ενεργούς Μηχανικής Αντίστασης (υπολογισμένης ροπής) με F Mu Mu h F ax 11 1 1 1 11 1 1 h M M 1 h h q g 11 1 με ( ) * 1 * ( M J M ) M J * 1 1 M Q Q J M J * h h q τ J F 1 11 ax J F 1 ay και 1 ax όπου, θέτοντας: με: * * * 11 1 1 1 τ J F J F ex και ay ay (J: Ιακωβιανή μήτρα) F Mu Mu h F 1 1 1 B d Md Bd, K d Md Kd και Μ d M d e p p και e F F έχουμε: p d F d e * * * 1 1 [ ] [ ] u p B e B e K e K e M e M e 1 dx d, xx px d, xy py d, xx px d, xy py d, xx Fx d, xy Fy u p B e B e K e K e M e M e dy d, yx px d, yy py d, yx px d, yy py d, yx Fx d, yy Fy ey 31 Ε.Μ.Π., ΣΗΜΜΥ, Ακαδημαϊκό Έτος 7-8, 7ο Εξάμηνο Μάθημα: Ρομποτική Ι. Διδάσκων: Κ.Τζαφέστας Ρομποτικός Έλεγχος Δύναμης / Μηχανικής Αντίστασης (Παράδειγμα Εφαρμογής) 3
Δυναμικός έλεγχος ενεργούς μηχανικής αντίστασης ρομποτικού χειριστή Επιθυμητή μηχανική αντίσταση (desired impedane) ρομποτικού χειριστή στο χώρο εργασίας (επιθυμητή δυναμική συμπεριφορά κλειστού βρόχου) ( ) ( ) ( ) M p p B p p K p p F F d d d d d d d e Δυναμική εξίσωση ρομποτικού χειριστή στο χώρο εργασίας (Δ-α): * * M p h F F a Δυναμικός ελεγκτής ενεργούς μηχανικής αντίστασης: e (βασισμένος στη μεθοδολογία ελέγχου υπολογισμένης ροπής (omputed-torque ontrol) τ J Fa (Ε) ( u p : εξίσωση κλειστού βρόχου ) * * Fa M u h Fe με το σήμα ελέγχου u να δίνεται από τη σχέση (ώστε στο κλειστό βρόχο να έχουμε την (Ε1)) ( ) ( ) ( ) u p M B p p K p p F F 1 d d d d d d d e σφάλμα ταχύτητας σφάλμα θέσης σφάλμα δύναμης (Ε1) (Ε3) 33 Έλεγχος δύναμης ρομποτικού χειριστή Ρομποτικός έλεγχος δύναμης βασισμένος στο μοντέλο: τ J F F u h F a * * a M e (F1) ( u p : εξίσωση κλειστού βρόχου ) με το σήμα ελέγχου u να δίνεται από τη σχέση (ώστε στο κλειστό βρόχο να έχουμε την (Ε1)) ( ) ( ) u M B p F F K p K F F 1 d d d e Bd Fd d e ανάδραση ταχύτητας ανάδραση δύναμης (F) Σε σύγκριση με το δυναμικό ελεγκτή ενεργούς μηχανικής αντίστασης (impedane ontrol), στον έλεγχο δύναμης (fore ontrol) θέτουμε K d, δηλαδή: «Σκληρότητα» του συστήματος ελέγχου του ρομποτικού χειριστή μηδέν (μηδενική φαινόμενη «σκληρότητα» -apparent stiffness- στο χώρο εργασίας) Με άλλα λόγια δεν έχουμε ανάδραση σφάλματος θέσης παρά μόνο ανάδραση σφάλματος δύναμης (και ταχύτητας για ευστάθεια), οπότε τελικά: F ef F e (t ) F d (επιθυμητή δύναμη) 34
Έλεγχος δύναμης ρομποτικού χειριστή Παράδειγμα R-R D (1) βαθμοί ελευθ. D, επίπεδο 1 y O l 1 l 1 m 1,I 1 y 1 O 1 q 1 m,i l q x y Ε O Ε x 1 x Ε θ Ρομποτικό δυναμικό μοντέλο: τ M q M q h q h qq g 1 11 1 1 1 11 1 1 τ M q M q h q g 1 1 11 1 όπου: M I ml I m l l ll M I ml M1 M1 I m ( l ll 1 ) h1 h11 h11 mll 1 s g1 mgl 1ˆ 11 mg ˆ( l 1 1 l1) g mgl ˆ ( ) 11 1 1 1 1 1 1 35 Έλεγχος δύναμης ρομποτικού χειριστή - Παράδειγμα R-R D () Ρομποτικό Δυναμικό Μοντέλο ρομπότ β.ε. (στο χώρο αρθρώσεων) με: τ M( q) q h( q, q ) J F M M M h h q h qq g 11 1 1 1 11 1 1 h M M 1 h h q g 11 1 e Ρομποτικό Δυναμικό Μοντέλο ρομπότ β.ε. στο χώρο δράσης (task-spae) τ J όπου: F a και: F p h F * * a M με ( ) * 1 * ( M J M ) M J * 1 1 M Q Q J M J * h h q e (J: Ιακωβιανή μήτρα) 36
Ρομποτικός έλεγχος συμμόρφωσης - Παράδειγμα R-R dof - Αποτελέσματα (1) y (m) 1 8 6 4 - -4 Fex,Fey (grf) 3 x 14.5 1.5 1.5 -.5-6 -8-1 5 1 15 x (m) -1-1.5 -.5 1 1.5 time t (se) Ελεγκτής τροχιάς υπολογισμένης ροπής (στο χώρο των αρθρώσεων) (joint-spae trajetory omputed-torque ontroller) τ M( q) u h( q, q ) u q d Bd ( q d q ) Kd ( qd q) σφάλμα ταχύτητας σφάλμα θέσης 37 Ρομποτικός έλεγχος συμμόρφωσης - Παράδειγμα R-R dof - Αποτελέσματα () y (m) 1 8 6 4 - -4-6 -8 Fex,Fey (grf) 8 6 4 - -4-1 5 1 15 x (m) -6.5 1 1.5 time t (se) Ελεγκτής ενεργούς μηχανικής αντίστασης (στο χώρο δράσης) (task-spae ative impedane ontroller) * * τ J Fa Fa M u h Fe 1 u pd Md Bd ( p d p ) Kd ( pd p) ( Fd Fe) σφάλμα ταχύτητας σφάλμα θέσης σφάλμα δύναμης 38
Ρομποτικός έλεγχος συμμόρφωσης - Παράδειγμα R-R dof - Αποτελέσματα (3) 1 1 y (m) 8 6 4 - -4 O y x O Fey (task-spae) (grf) 1 8 6 4-6 -8-1 x y 5 1 15 x (m).5 1 1.5 time t (se) Υβριδικός έλεγχος δύναμης / μηχανικής αντίστασης (στο χώρο εργασίας) (task-spae hybrid fore/impedane robot manipulation ontrol) Έστω R O -xyz, το γενικό πλαίσιο του χώρου εργασίας, και R -x y z το τοπικό πλαίσιο αναφοράς στο σημείο επαφής O R η μήτρα στροφής του τοπικού πλαισίου αναφοράς R ως προς το R Ο 39 τ J Υβριδικός ρομποτικός έλεγχος δύναμης / τροχιάς - Παράδειγμα Υβριδικός ρομποτικός έλεγχος δύναμης / τροχιάς, βασισμένος στη δομή του δυναμικού ελέγχου ενεργούς μηχανικής αντίστασης F a F u h F * * a M ( ) ( ) ( ) 1 ( ) ( ) ( ) ( ) ( ) ( ) d Md Bd p Kd p d e e O ( ) O ( ) R u ( F ) e R Fe ( ) u u p e e F F ( ) O O ( ) O O όπου ep R ep R ( pd p) και e p R e p R ( p d p ) ( ) ( ) Για το K d μπορούμε να θέσουμε: Kd S Kd όπου η μήτρα K d επιλέγεται κανονικά για έλεγχο μηχανικής αντίστασης, ενώ η μήτρα S (seletion matrix) είναι στο συγκεκριμένο παράδειγμα Sdiag[s x,s y ] με s x (,s y ), για έλεγχο δύναμης στον άξονα x (y ) 1, για έλεγχο τροχιάς στον άξονα x (y ) ( ή γενικά s x (,s y ) στο διάστημα [,1] ) 4