Ρομποτική II. Περιεχόμενα Μαθήματος

Transcript

1 Σχολή Ηλεκτρολόγων Μηχ/κών και Μηχ/κών Υπολογιστών, Ε.Μ.Π., Ακαδημαϊκό Έτος 1-11, 8ο Εξάμηνο Ρομποτική II Ευφυή και Επιδέξια Ρομποτικά Συστήματα Κων/νος Τζαφέστας Τομέας Σημάτων, Ελέγχου & Ρομποτικής Σχολή Ηλεκτρ. Μηχ/κών & Μηχ/κών Υπολ., Ε.Μ.Π. Τηλ.: , (Κτήριο Ηλεκτρ., Γραφείο 1.11) ktzaf@cs.ntua.gr Web: 1 Περιεχόμενα Μαθήματος ΕΝΟΤΗΤΑ-1: Επιδέξιος Ρομποτικός Χειρισμός Έλεγχος Ρομπότ με πλεονάζοντες β.ε. (redundant robots) Έλεγχος «Αλληλεπίδρασης» με το περιβάλλον Έλεγχος Δύναμης / Σύνθετης Μηχανικής Αντίστασης Έλεγχος Οπτικής Οδήγησης Μοντελοποίηση και έλεγχος επιδέξιας «ρομποτικής λαβής» (Συνεργαζόμενα ρομπότ, Ρομποτικά χέρια) ΕΝΟΤΗΤΑ-: Κινούμενα Ρομπότ Αρχιτεκτονικές Ελέγχου Κινούμενων Ρομπότ Σχεδιασμός δρόμου Αποφυγή εμποδίων Σύνθεση αισθητηρίων πληροφοριών Σύνθετοι ρομποτικοί χειριστές - Εφαρμογές

2 Ε.Μ.Π., ΣΗΜΜΥ, Ακαδημαϊκό Έτος 1-11, 8ο Εξάμηνο Μάθημα: Ρομποτική ΙΙ. Διδάσκων: Κ.Τζαφέστας Έλεγχος «Αλληλεπίδρασης» με το Περιβάλλον Έλεγχος Δύναμης Έλεγχος «Εμπέδησης» Έλεγχος «Οπτικής Οδήγησης» 3 Ε.Μ.Π., ΣΗΜΜΥ, Ακαδημαϊκό Έτος 1-11, 8ο Εξάμηνο Μάθημα: Ρομποτική ΙΙ. Διδάσκων: Κ.Τζαφέστας Ρομποτικός Έλεγχος Δύναμης / Εμπέδησης (Σύνθετης Μηχανικής Αντίστασης / Συμμόρφωσης) (Force / Impedance / Compliance Robot Control) 4

3 Ρομποτικός Έλεγχος Δύναμης - Εισαγωγή Έλεγχος μηχανικής εμπέδησης (impedance control) Passive impedance Remote center of compliance (RCC) (εργαλεία παθητικής «συμμόρφωσης») Active Impedance Active stiffness control (έλεγχος «ενεργούς σύνθετης μηχανικής αντίστασης») Έλεγχος δύναμης (force control) Υβριδικός έλεγχος δύναμης/θέσης (hybrid force/motion control) Παραδείγματα - Προσομοίωση 5 Μέθοδος Παθητικής Συμμόρφωσης (passive compliance) Μηχανικά εργαλεία παθητικής συμμόρφωσης Remote Center of Compliance (RCC) Αρχή λειτουργίας RCC (α) συμμόρφωση σε οριζόντια δύναμη (β) συμμόρφωση σε ροπή 6

4 Μέθοδος Παθητικής Συμμόρφωσης (συνέχεια) (1) Βασική Σχέση: f = Κ ε f = [f, f y, f z, N, N y, N z ] ε = [ε, ε y, ε z, α, α y, α z ] : εξωτερική δύναμη/ροπή : μηχανική παραμόρφωση (strain) RCC 3 διαστάσεων K = Κ soft Κ soft Κhard Κ soft Κ soft Κ soft K : μήτρα μηχανικής σκληρότητας («ακαμψίας») (stiffness matri) 7 Μέθοδος Παθητικής Συμμόρφωσης (συνέχεια) () Μηχανικά εργαλεία παθητικής συμμόρφωσης Remote Center of Compliance (RCC) 8

5 Μέθοδος Παθητικής Συμμόρφωσης (συνέχεια) (3) Εργαλεία παθητικής συμμόρφωσης σε ρομποτικές εργασίες συναρμολόγησης (peg-in-hole tasks) 9 Ρομποτικοί Αισθητήρες Δύναμης (force sensors) Αισθητήρας δύναμης: Κατάλληλη μηχατρονική διάταξη αισθητήρων ελαστικής (μηχανικής) παραμόρφωσης (strain gauges strain/deformation transducers) 1

6 Μετρητές μηχανικής παραμόρφωσης (strain gauge transducers) 11 Έλεγχος ενεργούς εμπέδησης (active-impedance control) Απλό παράδειγμα 1 β.ε. Δυναμικό μοντέλο συστήματος: m b k= f F a a a u m a, b a, k a : δυναμικές παράμετροι συστήματος f u : ελεγκτής F : εξωτερική δύναμη Επιθυμητή μηχανική εμπέδηση (desired impedance): md bd kd d F ή πληρέστερα: m b k = 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 : επιθυμητή δύναμη η οποία θέλουμε τελικά να ασκείται από το εξωτερικό περιβάλλον d πάνω στο σύστημα 1

7 Έλεγχος ενεργούς εμπέδησης: Απλό παράδειγμα 1 β.ε. m b k= f F a a a u ( ) ( ) ( ) m b k = F F d d d d d d d d d ( ω = m, ζ = ) d mk d d Ελεγκτής ενεργούς μηχανικής εμπέδησης: f = mu b k F u a a a (1) (3) k : ιδιοσυχνότητα u= b (1) : συντελεστής απόσβεσης : εξίσωση κλειστού βρόχου : Δυναμικό μοντέλο συστήματος : Επιθυμητή μηχανική εμπέδηση (desired impedance) (επιθυμητή συμπεριφορά κλειστού βρόχου) ( ) ( ) ( ) 1 u= d md bd d kd d Fd F (3) Για να επιτύχουμε την επιθυμητή συμπεριφορά (), έχουμε για το u: () (όπου u σήμα ελέγχου) (4) 13 Έλεγχος ενεργούς εμπέδησης: Απλό παράδειγμα 1 β.ε. (συνέχεια) (1) Ο νόμος ελέγχου μηχανικής αντίστασης (3) και (4), γράφεται ουσιαστικά ως: m m m f = m b k F F ( ) ( ) ( ) a a a u a d d d d d d md md md e e b a k a F ή άλλως: όπου: K V K P λ m ( ) ( ) ( ) f = m b k K K K F u a d a a V d P d F ef (όταν F d =) K = λ 1 ( λ m = ( ma md) ) K = λ b και K = λ k F m V m d P m d Παρατήρηση: Το κέρδος ανάδρασης δύναμης K F (K F ) επιτρέπει τη μεταβολή των «αδρανειακών» χαρακτηριστικών του συστήματος (λ m 1) στο κλειστό βρόχο 14

8 Έλεγχος ενεργούς εμπέδησης: Απλό παράδειγμα 1 β.ε. (συνέχεια) () k d b d m d F k c b c E Επαφή με εξωτερικό περιβάλλον E k c : σκληρότητα b c : απόσβεση c : θέσης ισορροπίας c Δυναμική εξίσωση εξωτερικού περιβάλλοντος : b k = F (5) ( ) ( ) c c c c Δυναμική εξίσωση () κλειστού βρόχου συστήματος {M, b, k, f u } (μηχανική αντίσταση συστήματος που επιτυγχάνεται με τον ελεγκτή f u ): ( ) ( ) ( ) m b k = F F d d d d d d d 15 () (5) Έλεγχος ενεργούς εμπέδησης: Απλό παράδειγμα 1 β.ε. (συνέχεια) (3) Δυναμική συμπεριφορά τελικού κλειστού συστήματος {M, b, k, f u } σε επαφή με το εξωτερικό περιβάλλον Ε (k c, b c, c ): ( ) ( ) ( ) ( ) m b b k k = m b b d d c d c d d d d c c k k d d k c c F d kc bd bc d ω = m, ζ = d md ( kd kc) : ιδιοσυχνότητα : συντελεστής απόσβεσης Δύναμη πάνω στο σύστημα από το εξωτερικό περιβάλλον εργασίας, = = στην τελική (μόνιμη) κατάσταση (t ): Fef F( t ) kc ( c f ) όπου f : τελική θέση συστήματος ( ) 1 f kdd kcc ( t = ) = F Άρα: ef ( ) kk d c kc F = F t = F ( ) ( ) c d kd kc ( kd kc) ( kd kc) ( kd kc) d d 16

9 Έλεγχος ενεργούς μηχανικής εμπέδησης ρομποτικού χειριστή - Γενικά Γενική Περίπτωση (ρομποτικός χειριστής) (a) Σύστημα ελέγχου (ροπές τ i ) με ανάδραση δύναμης F (και θέσης y) (b) Μηχανική αντίσταση που θέλουμε να επιτύχουμε μέσω του ρομποτικού ελεγκτή ενεργούς μηχανικής εμπέδησης 17 Δυναμική ρομποτικού χειριστή (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 ci Ni = I ciωi ωi ( Iciωi) K: κινητική ενέργεια P: δυναμική ενέργεια 18

10 Δυναμική ρομποτικού χειριστή () Δυναμικό μοντέλο ρομποτικού χειριστή στο χώρο εργασίας (task-space): * * 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) ( ) (Δ-β) 19 Δυναμικός έλεγχος ενεργούς μηχανικής εμπέδησης ρομποτικού χειριστή (1) Επιθυμητή μηχανική εμπέδηση (desired impedance) ρομποτικού χειριστή στο χώρο εργασίας (επιθυμητή δυναμική συμπεριφορά κλειστού βρόχου) ( ) ( ) ( ) 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 Δυναμικός ελεγκτής ενεργούς εμπέδησης: (βασισμένος στη μεθοδολογία ελέγχου υπολογιζόμενης ροπής (computed-torque control) τ = J F F u h F a * * a = M e (Ε) ( u= p : εξίσωση κλειστού βρόχου ) με το σήμα ελέγχου u να δίνεται από τη σχέση (ώστε στο κλειστό βρόχο να έχουμε την (Ε1)) ( ) ( ) ( ) u= p M B p p K p p F F 1 d d d d d d d e σφάλμα ταχύτητας σφάλμα θέσης σφάλμα δύναμης (Ε1) (Ε3)

11 Δυναμικός έλεγχος ενεργούς μηχανικής εμπέδησης ρομποτικού χειριστή () p d p d p d F d e p - e p - - K d e F B d ανάδραση δύναμης ανάδραση ταχύτητας ανάδραση θέσης Δυναμικός Ελεγκτής (Ε),(E3) τ Ρομποτικός Χειριστής (Δ) ανάδραση δύναμης ανάδραση ταχύτητας ανάδραση θέσης p p F e Σχηματικό διάγραμμα (block-diagram) δυναμικού ρομποτικού ελεγκτή ενεργούς μηχανικής αντίστασης (υπολογιζόμενης ροπής) 1 Δυναμικός έλεγχος ενεργούς μηχανικής εμπέδησης ρομποτικού χειριστή (3) Ρομποτικό Μοντέλο: M *,h * p d p d p d F d e p - e p - - K d e F B d ανάδραση δύναμης ανάδραση ταχύτητας J( q ) (task-space) ανάδραση θέσης - * h( q, q ) * M( q ) * 1 - MM Γ(q) Ρομποτικός Χειριστής (Δ) ανάδραση δύναμης ανάδραση ταχύτητας (joint space) ανάδραση θέσης Σχηματικό διάγραμμα (block-diagram) δυναμικού ρομποτικού ελεγκτή ενεργούς εμπέδησης (impedance control) d τ q q F e

12 Δυναμικός έλεγχος ενεργούς μηχανικής εμπέδησης ρομποτικού χειριστή (4) Ρομποτικό Μοντέλο: M *,h * p d p d p d F d Έλεγχος Εμπέδησης (Ε3) u Αντίστροφη Δυναμική (Ε) τ Ρομποτικός Χειριστής (Δ) q q F e ανάδραση δύναμης ανάδραση ταχύτητας J( q ) (task-space) ανάδραση θέσης Γ(q) ανάδραση δύναμης ανάδραση ταχύτητας (joint space) ανάδραση θέσης Ευθεία κινηματική Γενική δομή ρομποτικού ελεγκτή εμπέδησης (impedance control) 3 Απλοποιημένος έλεγχος ενεργούς μηχανικής ακαμψίας (stiffness) ρομποτικού χειριστή Θεωρούμε: 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 όπου: και ονομάζεται active-stiffness control, όπου: eq = qd q K d : μήτρα «ακαμψίας» (stiffness matri) ορισμένη στο χώρο εργασίας K qd = J K d J : ισοδύναμη μήτρα «ακαμψίας» στο χώρο αρθρώσεων 4

13 Έλεγχος δύναμης για ρομποτικό χειριστή (Force control) - Εισαγωγή ενεργός μηχανική αντίσταση ρομπότ 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 σφάλμα θέσης επιθυμητή δύναμη 5 Έλεγχος δύναμης ρομποτικού χειριστή Ρομποτικός έλεγχος δύναμης βασισμένος στο μοντέλο (υπολογιζόμενης ροπής): (computed 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 ανάδραση ταχύτητας ανάδραση δύναμης Σε σύγκριση με το δυναμικό ελεγκτή ενεργούς εμπέδησης (impedance control), στον έλεγχο δύναμης (force control) θέτουμε K d =, δηλαδή: «Ακαμψία» του συστήματος ελέγχου του ρομποτικού χειριστή = μηδέν (μηδενική φαινόμενη «ακαμψία» -apparent stiffness- στο χώρο εργασίας) Με άλλα λόγια δεν έχουμε ανάδραση σφάλματος θέσης παρά μόνο ανάδραση σφάλματος δύναμης (και ταχύτητας για ευστάθεια), οπότε τελικά: F ef = F e (t ) F d (επιθυμητή δύναμη) (F) 6

14 Άμεσος Έλεγχος δύναμης (1) (direct force control) F d e F - K F pf e p - K P - u 1 M d Δυναμική συστήματος p Περιβάλλον p e p p - B d K e F e Άμεσος Έλεγχος Δύναμης με εσωτερικό βρόχο θέσης 7 Άμεσος Έλεγχος δύναμης () (direct force control) Άμεσος Έλεγχος Δύναμης με εσωτερικό βρόχο θέσης (συνέχεια) ( ) ( ) u = KP pf p Bd p p = K F F F F d e M p= u d ( ) F = K p p e e e ( ) M d p Bd p KP p= = KPKF ( Fd Fe) ( / ) p = F K p e e e M F B F K I K K F = K K K F Δυναμική Εξίσωση κλειστού βρόχου d e d e P e F e P e F d Proportional (P) Force Control: Μηδενικό μόνιμο σφάλμα δύναμης e F K F = K = const. Proportional Integral (PI) Force Control: F FP FI ( ) FP K = K K. dτ t 8

15 Άμεσος Έλεγχος δύναμης (3) (direct force control) F d ef - K F pf K P - u 1 M d Δυναμική συστήματος p Περιβάλλον p e p p - B d K e F e Άμεσος Έλεγχος Δύναμης με εσωτερικό βρόχο ταχύτητας M F B F K K K F = K K K F d e d e P e F e P e F d Δυναμική Εξίσωση κλειστού βρόχου P και PI force controller: Μηδενικό μόνιμο σφάλμα δύναμης e F 9 Υβριδικός έλεγχος δύναμης/τροχιάς ρομποτικού χειριστή (hybrid control) Βασική Αρχή έλεγχος δύναμης έλεγχος θέσης Διαφορετικός τύπος ελέγχου σε διαφορετικές κατευθύνσεις στο χώρο εργασίας: Έλεγχος θέσης / δύναμης / ενεργούς μηχανικής εμπέδησης /... κλπ. ανάλογα με την υπο-εργασία 3

16 Υβριδικός έλεγχος δύναμης/τροχιάς ρομποτικού χειριστή (συνέχεια) (1) Παραδείγματα μηχανικών εργασιών που απαιτούν την εφαρμογή υβριδικού ελέγχου δύναμης/θέσης (σε διαφορετικούς άξονες στο χώρο του τελικού στοιχείου δράσης) 31 Υβριδικός έλεγχος δύναμης/τροχιάς ρομποτικού χειριστή (συνέχεια) () Βασικό σχήμα υβριδικού ελέγχου δύναμης/τροχιάς (1) Επιθυμητή Τροχιά Εντολή κατεύθυνσης ελέγχου {s i } (i=1,..,n) Επιθυμητή Δύναμη - - Μετασχηματισμοί Συντεταγμένων [S] [I] [S] c R c R Μετασχηματισμοί Συντεταγμένων c R c R Έλεγχος Τροχιάς Έλεγχος Δύναμης Αισθητήρες Θέσης Ρομποτικός Χειριστής Αισθητήρας Δύναμης Θέση Δύναμη S=diag[s i ] s i =1 έλεγχος τροχιάς s i = έλεγχος δύναμης 3

17 Υβριδικός έλεγχος δύναμης/τροχιάς ρομποτικού χειριστή (συνέχεια) (3) Βασικό σχήμα υβριδικού ελέγχου δύναμης/τροχιάς () Επιθυμητή Τροχιά Εντολή κατεύθυνσης ελέγχου {s i } (i=1,..,n) Επιθυμητή Δύναμη - - Μετασχηματισμοί Συντεταγμένων Έλεγχος Τροχιάς Έλεγχος Δύναμης ( c ) u p ( c ) u F Μετασχηματισμοί Συντεταγμένων c R [S] [I] [S] c R Αισθητήρες Θέσης c R ανάστροφη δυναμική u τ D (.)h Ρομπότ c R Αισθητήρας Δύναμης Θέση Δύναμη S=diag[s i ] s i =1 έλεγχος τροχιάς s i = έλεγχος δύναμης 33 Ρομποτικός έλεγχος εμπέδησης Παράδειγμα R-R D (1) βαθμοί ελευθ. D, επίπεδο 1 y O l 1 l c1 m 1,I 1 y 1 O 1 q 1 m,i l c q y Ε O Ε 1 όπου: vc= d ( p dt c) και: p lc lc c c = ls l s c 1 Ε θ Δυναμικό μοντέλο Lagrange : τ = i d dt i i i K K P q q q K = ml q Iq c = 1 1ˆ c ( s sin( q1) ) P mgl s = K = mvv I q q 1 1 c c 1 P = mgls ˆ l s s q q c ( 1 = sin ( 1 )) lsq l s q q v c = lcq l c q q c c 1 1 K: κινητική ενέργεια P: δυναμική ενέργεια 1ος σύνδεσμος ος σύνδεσμος 34

18 Άρα: d dt Ρομποτικός έλεγχος εμπέδησης Παράδειγμα R-R D () c c lsq lcs 1 q1 q lcq lcc 1 q1 q = lsq l s q q ll ssqq q c c c c lcq lcc 1 q1 q ll 1 ccc 1 1q 1q1 q = lq c c 1 l q q ll cq q q 1 c 1 1 c 1 1 vv vv vv = K ml q Iq I q q m l q l q q ll c q q q = 1 c c 1 1 c 1 1 K ml q Iq I q q 1 c q 1 m l1q1 l c q1 q ll 1 cc q 1 q ll 1 cs q 1 q q = P q q = mgl ˆ s mgˆ ls l s = mgl ˆ c mgˆ lc l c c c 1 1 c c 1 35 Τελικά: Ρομποτικός έλεγχος εμπέδησης Παράδειγμα R-R D (3) Ρομποτικό Δυναμικό Μοντέλο D Χειριστή β.ε. = I ml I m l l ll c q I m l ll c q τ1 1 1 c1 1 c 1 c 1 c 1 c mll s qq q mgl 1c 1 1ˆ c mg c1 1 ˆ lc l c 1 1 c 1 τ I ml c llc 1 c q 1 I ml c q mllsq 1 c 1 mglc ˆ c 1 = τ = M q 1 11 M q 1 1 h q h qq g τ = M q 1 M q h q g όπου: M11 = I1 ml 1 c1 I m ( l1 lc ll 1 cc ) M = I ml c M1 = M1 = I m ( lc ll 1 cc ) h1 = h11 = h11 = mll 1 cs g1= m1gl ˆ c1c1 mg ˆ( lc 1 1 lcc1) g= mgl ˆ cc1 Δηλαδή: 36

19 Είναι: Θέτουμε: Ρομποτικός έλεγχος εμπέδησης Παράδειγμα R-R D (4) M= M M = h = h q h qq g Ρομποτικός Ελεγκτής Ενεργούς Μηχανικής Εμπέδησης (υπολογιζόμενης ροπής) με F = Mu Mu h F a 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 a J F 1 ay και 1 a όπου, θέτοντας: με: * * * τ = J F J F e και ay ay (J: Ιακωβιανή μήτρα) F = Mu Mu h F 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 d d, p d, y py d, p d, y py d, F d, y Fy u = p B e B e K e K e M e M e dy d, y p d, yy py d, y p d, yy py d, y F d, yy Fy ey 37 Ε.Μ.Π., ΣΗΜΜΥ, Ακαδημαϊκό Έτος 1-11, 8ο Εξάμηνο Μάθημα: Ρομποτική ΙΙ. Διδάσκων: Κ.Τζαφέστας Ρομποτικός Έλεγχος Δύναμης / Εμπέδησης (Παράδειγμα Εφαρμογής) 38

20 Δυναμικός έλεγχος ενεργούς εμπέδησης ρομποτικού χειριστή Επιθυμητή μηχανική εμπέδηση (desired impedance) ρομποτικού χειριστή στο χώρο εργασίας (επιθυμητή δυναμική συμπεριφορά κλειστού βρόχου) ( ) ( ) ( ) M p p B p p K p p = F F d d d d d d d e Δυναμική εξίσωση ρομποτικού χειριστή στο χώρο εργασίας (Δ-α): p h = F F * * M Δυναμικός ελεγκτής ενεργούς εμπέδησης: a e (βασισμένος στη μεθοδολογία ελέγχου υπολογισμένης ροπής (computed-torque control) τ = 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) 39 Έλεγχος δύναμης ρομποτικού χειριστή Ρομποτικός έλεγχος δύναμης βασισμένος στο μοντέλο: τ = 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 ανάδραση ταχύτητας ανάδραση δύναμης Σε σύγκριση με το δυναμικό ελεγκτή ενεργούς εμπέδησης (impedance control), στον έλεγχο δύναμης (force control) θέτουμε K d =, δηλαδή: «Ακαμψία» του συστήματος ελέγχου του ρομποτικού χειριστή = μηδέν (μηδενική φαινόμενη «ακαμψία» -apparent stiffness- στο χώρο εργασίας) Με άλλα λόγια δεν έχουμε ανάδραση σφάλματος θέσης παρά μόνο ανάδραση σφάλματος δύναμης (και ταχύτητας για ευστάθεια), οπότε τελικά: F ef = F e (t ) F d (επιθυμητή δύναμη) (F) 4

21 Έλεγχος δύναμης ρομποτικού χειριστή Παράδειγμα R-R D (1) βαθμοί ελευθ. D, επίπεδο 1 y O l 1 l c1 m 1,I 1 y 1 O 1 q 1 m,i l c q y Ε O Ε 1 Ε θ Ρομποτικό δυναμικό μοντέλο: τ = M q M q h q h qq g τ = M q M q h q g όπου: M I ml I m l l ll c M I ml M1 = M1 = I m ( lc ll 1 cc ) h1 = h11 = h11 = mll 1 cs g1= mgl 1ˆ c1c1 mg ˆ( lc 1 1 lcc1) g = mgl ˆ c ( ) 11 = 1 1 c1 1 c 1 c = c c 1 41 Έλεγχος δύναμης ρομποτικού χειριστή - Παράδειγμα R-R D () Ρομποτικό Δυναμικό Μοντέλο ρομπότ β.ε. (στο χώρο αρθρώσεων) με: τ = M( q) q h( q, q ) J F M= M M = h = h q h qq g h M M 1 h h q g 11 1 e Ρομποτικό Δυναμικό Μοντέλο ρομπότ β.ε. στο χώρο δράσης (task-space) τ = 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: Ιακωβιανή μήτρα) 4

22 Ρομποτικός έλεγχος συμμόρφωσης - Παράδειγμα R-R dof - Αποτελέσματα (1) y (cm) Fe,Fey (grf) (cm) time t (sec) Ελεγκτής τροχιάς υπολογιζόμενης ροπής (στο χώρο των αρθρώσεων) (joint-space trajectory computed-torque controller) τ = M( q) u h( q, q ) ( ) K ( ) u= qd Bd qd q d qd q σφάλμα ταχύτητας σφάλμα θέσης 43 Ρομποτικός έλεγχος συμμόρφωσης - Παράδειγμα R-R dof - Αποτελέσματα () y (cm) Fe,Fey (grf) (cm) time t (sec) Ελεγκτής ενεργούς μηχανικής εμπέδησης (στο χώρο εργασίας) (task-space active impedance controller) * * τ = J Fa Fa = M u h Fe 1 u= pd Md Bd ( p d p ) Kd ( pd p) ( Fd Fe) σφάλμα ταχύτητας σφάλμα θέσης σφάλμα δύναμης 44

23 Ρομποτικός έλεγχος συμμόρφωσης - Παράδειγμα R-R dof - Αποτελέσματα (3) 1 1 y (cm) O y O c Fey (task-space) (grf) c y c (cm) time t (sec) Υβριδικός έλεγχος δύναμης / εμπέδησης (στο χώρο εργασίας) (task-space hybrid force/impedance robot manipulation control) Έστω R O -yz, το γενικό πλαίσιο του χώρου εργασίας, και R c - c y c z c το τοπικό πλαίσιο αναφοράς στο σημείο επαφής O R c η μήτρα στροφής του τοπικού πλαισίου αναφοράς R c ως προς το R Ο 45 τ = J Υβριδικός ρομποτικός έλεγχος δύναμης / τροχιάς - Παράδειγμα Υβριδικός ρομποτικός έλεγχος δύναμης / τροχιάς βασισμένος στη δομή του δυναμικού ελέγχου ενεργούς εμπέδησης F a F u h F * * a = M ( c) ( c) ( c) 1 ( c) ( c) ( c) ( c) ( c) ( c) = d Md Bd p Kd p d e e O ( c) O ( ) R c u ( F ) c e = R c Fe ( ) u= u p e e F F ( c) O O ( c) O O όπου ep = Rc ep = Rc ( pd p) και e p = Rc e p = Rc ( p d p ) ( c) ( c) Για το K d μπορούμε να θέσουμε: Kd = S Kd όπου η μήτρα K d επιλέγεται κανονικά για έλεγχο εμπέδησης, ενώ η μήτρα S (selection matri) είναι στο συγκεκριμένο παράδειγμα S=diag[s,s y ] με s (,s y ) =, για έλεγχο δύναμης στον άξονα c (y c ) 1, για έλεγχο τροχιάς στον άξονα c (y c ) ( ή γενικά s (,s y ) στο διάστημα [,1] ) 46

24 Ε.Μ.Π., ΣΗΜΜΥ, Ακαδημαϊκό Έτος 1-11, 8ο Εξάμηνο Μάθημα: Ρομποτική ΙΙ. Διδάσκων: Κ.Τζαφέστας Ρομποτικός Έλεγχος Οπτικής Οδήγησης (Robot Visual Servoing) 47 Διατάξεις Οπτικού Συστήματος Camera Placement Configurations R e t R e R c e R c R c t R t eye-in-hand ( hand-eye ) διάταξη Κάμερα προασαρτημένη στο τελικό εργαλείο δράσης του ρομπότ fied camera ( static-eye ) διάταξη Κάμερα σταθερά τοποθετημένη στο περιβάλλον εργασίας Πλαίσια Αναφοράς (): βάση ρομπότ, (e): end-effector, (c): camera, (t): target/task (αντικείμενο/εργασία) 48

25 Αρχιτεκτονικές Ελέγχου Οπτικής Οδήγησης 1. Οπτική οδήγηση Έλεγχος θέσης/τροχιάς ρομπότ a. Ιεραρχικός έλεγχος (look-and-move) (οπτικό σύστημα παρέχει σήματα αναφοράς σε ρομποτικό ελεγκτή αρθρώσεων) b. Απ ευθείας έλεγχος αρθρώσεων (direct visual servo). Πλαίσιο αναφοράς για σφάλμα ελέγχου a. Έλεγχος βασισμένος στη θέση (position-based): σφάλμα ορισμένο σε κάποιο Καρτεσιανό πλαίσιο αναφοράς b. Έλεγχος βασισμένος στην εικόνα (image-based): σφάλμα ορισμένο στο χώρο των οπτικών χαρακτηριστικών (feature space) 49 Αρχιτεκτονικές Ελέγχου (συνέχεια) (1/4) (c) p d - Καρτεσιανός Νόμος Ελέγχου Cartesian-space control law q d τ Ελεγκτές αρθρώσεων (joint controllers) Ενισχυτές (power amplifiers) Ανάδραση q αισθητήρες αρθρώσεων (c) pˆ Εκτίμηση Θέσης (pose estimation) f Εξαγωγή Οπτικών Χαρακτηριστικών (feature etraction) Εικόνα (video) Position-based / look-and-move Aρχιτεκτονική Βασισμένου στη Θέση / Ιεραρχικού Ελέγχου 5

26 Αρχιτεκτονικές Ελέγχου (συνέχεια) (/4) f d - Νόμος Ελέγχου Οπτικών Χαρακτηριστικών Feature-space control law q d τ Ελεγκτές αρθρώσεων (joint controllers) Ενισχυτές (power amplifiers) Ανάδραση q αισθητήρες αρθρώσεων f Εξαγωγή Οπτικών Χαρακτηριστικών (feature etraction) Εικόνα (video) Image-based / look-and-move Aρχιτεκτονική Βασισμένου στην Εικόνα / Ιεραρχικού Ελέγχου 51 Αρχιτεκτονικές Ελέγχου (συνέχεια) (3/4) (c) p d - Καρτεσιανός Νόμος Ελέγχου Cartesian-space control law τ Ελεγκτές αρθρώσεων (joint controllers) Ενισχυτές (power amplifiers) (c) pˆ Εκτίμηση Θέσης (pose estimation) f Εξαγωγή Οπτικών Χαρακτηριστικών (feature etraction) Εικόνα (video) Position-based / direct visual servo Aρχιτεκτονική Βασισμένου στη Θέση / Αμέσου Ελέγχου 5

27 Αρχιτεκτονικές Ελέγχου (συνέχεια) (4/4) f d - Νόμος Ελέγχου Οπτικών Χαρακτηριστικών Feature-space control law τ Ελεγκτές αρθρώσεων (joint controllers) Ενισχυτές (power amplifiers) f Εξαγωγή Οπτικών Χαρακτηριστικών (feature etraction) Εικόνα (video) Image-based / direct visual servo Aρχιτεκτονική Βασισμένου στην Εικόνα / Αμέσου Ελέγχου 53 Έλεγχος με βάση τη θέση (1/) Position-Based Visual Servo Control Οπτικά χαρακτηριστικά εξάγονται από την εικόνα και χρησιμοποιούνται για την εκτίμηση της «θέσης» του «στόχου» (target pose) ως προς την κάμερα... Συνάρτηση Κινηματικού Σφάλματος : E(p e )= where p e : robot (end-effector) pose E(.): m, where : robot workspace (robot poses) Γενικά: SE 3 = 3 SO 3 54

28 Έλεγχος με βάση τη θέση (/) Position-Based Visual Servo Control (cont d) Σημείο-προς-Σημείο (point-to-point positioning): Τοποθέτηση σημείου p του ρομποτικού εργαλείου, με end-effector coordinates p (e), σε επιθυμητή θέση p d ( e) Epp = Pd Ae P Έστω: = 3 u 3 =K E pp ( ) ( ) Static-eye: u ˆ ˆ c ˆ e 3 = K Ac Pd Ae P Συνάρτηση κινηματικού σφάλματος Hand-eye: Aˆ e c ( ) pˆ c d βαθμονόμηση κάμερας κινηματική ρομπότ ( e) ˆ e ˆ( c) ( e) ˆ e ˆ( c) ˆ( c) u3 = K Ac Pd P = K Ac Pd P σφάλμα ανεξάρτητο κινηματικής ρομπότ Σήμα Καρτεσιανού ελέγχου (επιθυμητή μεταβολή θέσης ρομποτικού εργαλείου) : βαθμονόμηση κάμερας (τοποθέτηση ως προς ρομποτικό εργαλείο) : παρατηρήσεις στόχου μέσω της κάμερας (camera observations) 55 Έλεγχος με βάση την εικόνα (1/8) Image-Based Visual Servo Control Camera Projection Models Y Μοντέλο Προοπτικής Προβολής (Perspective Projection) X P=(,y,z) u π ( yz,, ) λ f=(u,v) = = v z y Z viewpoint λ αντικείμενο λ: εστιακή απόσταση (focal length) image plane Μοντέλο Ορθογραφικής Προβολής (Orthographic Projection) u oyz (,, ) = = σ v y (σ: fied scale) (ειδική περίπτωση) Μοντέλο Αφινικής Προβολής (Affine Projection) u u ( c) = v A P v (A: 3) 56

29 Έλεγχος με βάση την εικόνα (/8) Image-Based Visual Servo Control (cont d) Image Jacobian («Ιακωβιανή Μήτρα Εικόνας») f = Jc( p) p J c f: feature vector (k 1), π.χ.: f=[u 1,v 1,, u n,v n ] J: (k m) Ιακωβιανή Εικόνας f1 p1 f1 pm f = = p f p f p k 1 k m ( n=(k/) σημεία ) 57 Έλεγχος με βάση την εικόνα (3/8) Image-Based Visual Servo Control / «Eye-in-Hand» Παράδειγμα εξαγωγής Ιακωβιανής Μήτρας Εικόνας R c P (c) P vz = zωy ωz V λ uz y = ωz zω Vy λ z z = ( vω uω ) V λ y z [ y z] [ y z] Έστω: ( c) P = P = = P V Τ ( c) Τ ( c) ( c) ( c) Ωe e λ λ λ u = u z z = z z λ λ λ v = y v = y y z z z z 58

30 Έλεγχος με βάση την εικόνα (4/8) Παράδειγμα εξαγωγής Ιακωβιανής Μήτρας Εικόνας (συνέχεια) λ u uv λ u u = V Vz ω ωy vωz z z λ λ λ u λ v uv v = Vy Vz ω ωy uωz z z λ λ V u uv u V λ λ y v u z z λ λ Vz f = v = λ u λ v uv ω u z z λ λ ωy ωz 59 Έλεγχος με βάση την εικόνα (5/8) Για κίνηση σε επίπεδο: λ v V u Z V y v = λ u ωz Z και για περισσότερα (n) χαρακτηριστικά (σημεία) στην εικόνα, παίρνουμε για τη χρονική μεταβολή του διανύσματος χαρακτηριστικών : λ v 1 Z1 u 1 λ u1 v 1 Z1 V f = = V y u n λ ωz v n v Z n n λ u n Zn J c 6

31 Έλεγχος με βάση την εικόνα (6/8) Image-Based Visual Servo Control (cont d) Έλεγχος Οπτικής Οδήγησης Επίλυση Συστήματος: (Ιακωβιανή κάμερας J: k m) ΣχετικήΚίνησηΣτόχου Κάμερας Επιθυμητή Κίνηση της Κάμερας (ρομποτικού εργαλείου) Εαν 1 = p (c) =J f k m: Εαν τώρα: k m, rank( J) = min( k, m) Εαν k < m: p (c) = J f ( I J J) c Εαν k > m: p (c) =J f όπου όπου J = J ( JJ ) 1 = f = J p p (c) 1 J ( J J) J (c) 61 Έλεγχος με βάση την εικόνα (7/8) Καρτεσιανός Έλεγχος Επιλυμένης Ταχύτητας (resolved rate) u (c) Έστω: = K p (c) ( ) ε =Δ f = f f όπου f d d (1) Σήμα καρτεσιανού ελέγχου ρομπότ (επιθυμητή κίνηση ρομποτικού εργαλείου) το επιθυμητό διάνυσμα χαρακτηριστικού Στόχος του ελεγκτή ασυμπτωτικήμείωσησφάλματοςε: Από () (3) () ε = f f = K ε (4) ε= K ε (3) Αλλά, ως γνωστόν, έχουμε: Νόμος Ελέγχου: p (c) c f (4) p (c) K J c (c) u = J = J ε K c = ε 6

32 Έλεγχος με βάση την εικόνα (8/8) Image-Based Visual Servo Control (cont d) Πρόβλεψη κίνησης Παρακολούθηση χαρακτηριστικών στην εικόνα («παράθυρο» πρόβλεψης για αναζήτηση/παρακολούθηση χαρακτηριστικών) ft h= ft h Jc( p) u (h=δt) Κατάστρωση εξισώσεων στο χώρο κατάστασης Προβλήματα «ιχνηλάτησης» στόχου βέλτιστη εκτίμησης (π.χ. Φίλτρο Kalman,... ) 63