Σχεδιασμός Τροχιάς Ρομποτικών Χειριστών

Transcript

1 Σχολή Ηλεκτρολόγων Μηχ/κών και Μηχ/κών Υπολογιστών, Ε.Μ.Π., Ακαδημαϊκό Έτος 00809, 7ο Εξάμηνο Μάθημα: Ρομποτική Ι Αυτόματος Έλεγχος Ρομπότ Κων/νος Τζαφέστας Τομέας Σημάτων, Ελέγχου & Ρομποτικής Σχολή Ηλεκτρ. Μηχ/κών & Μηχ/κών Υπολ., Ε.Μ.Π. Τηλ.: (0) (Κτήριο Ηλεκτρ., Γραφείο.36) Eail: We: hp:// Σχεδιασμός Τροχιάς Ρομποτικών Χειριστών

2 Ιεραρχική Δομή Ρομποτικών Συστημάτων Ελέγχου θέσεις στόχου Πρόγραμμα Εργασίας c Ευφυής Ελεγκτής μεταβολή θέσεων στόχου Σχεδιασμός Τροχιάς p(), i p () i Περιορισμοί Αλγόριθμος αντίστροφης κινηματικής σήματα αναφοράς q(), i q () i r r Βρόχοι Ελέγχου a q(), i q () i Στοιχεία Δράσης Μονάδα Παρεμβολής Αισθητήρια Στοιχεία 3 Σχεδιασμός Τροχιάς: Βασικές Έννοιες () Δρόμος (pah): Καμπύλη που διαγράφει το ρομπότ στο χώρο, η οποία ώνει δύο (ή περισσότερες) διάμεσες θέσεις (σημεία διέλευσης) Τροχιά (rajecory): Χρονική ακολουθία διάμεσων θέσεων (στο Καρτεσιανό χώρο) ή διατάξεων (στο χώρο των αρθρώσεων) Τροχιά: ιστορικό θέσεων ή διατάξεων (ήτοι, χρονική συνάρτηση) Ο αλγόριθμος σχεδιασμού τροχιάς κατασκευάζει συναρτήσεις του χρόνου για την εξέλιξη των μεταβλητών θέσης (ή διάταξης) του ρομπότ, οδηγώντας σε μια συνεχή (ομαλή) καμπύλη μεταξύ των ακραίων σημείων (αρχή στόχος), παρεμβάλλοντας ουσιαστικά διάμεσες θέσεις (αλγόριθμοι παρεμβολής) Οι θέσεις αυτές στέλνονται στον αλγόριθμο αντίστροφης κινηματικής, ο οποίος (σε κάθε δειγματοληπτικό διάστημα Τ secs) δίνει τις τολές κίνησης στους τοπικούς βρόχους ελέγχου των αρθρώσεων Εντολή θέσης p(i), Εντολή ταχύτητας p () i τολές κίνησης αρθρώσεων. Τα p(i) και p () i ικανοποιούν περιορισμούς χωρικής ταχύτητας και επιτάχυνσης 4

3 Σχεδιασμός Τροχιάς: Βασικές Έννοιες () Χωρικοί Περιορισμοί της Τροχιάς (spaial consrains): Εκτέλεση Εργασίας στο Χώρο του Τελικού Εργαλείου Δράσης Αποφυγή Εμποδίων, Αποφυγή Ιδιόμορφων Διατάξεων κλπ. Χρονικές Ιδιότητες της Τροχιάς (eporal ariues): Χρονική Διάρκεια απαιτούμη για την εκτέλεση της εργασίας (χρονικές στιγμές διέλευσης από διάμεσα σημεία via poins) Περιορισμοί Ταχύτητας και Επιτάχυνσης στις αρθρώσεις Τροχιά: Χρονική Συνάρτηση Συνεχής και «Ομαλή» Επιθυμητή Χρονική Συνέχεια ως προς την Ταχύτητα και ίσως και ως προς την Επιτάχυνση (για καλύτερο, συνεχεία, έλεγχο του ρομποτικού συστήματος) Σχεδιασμός Τροχιάς: (α) στο Χώρο Εργασίας, ή (β) στο Χώρο Αρθρώσεων Το (α) πλεονεκτεί τροχιά γική, ανεξάρτητη ρομποτικού χειριστή, αλλά απαιτεί αντιστροφή κινηματικής για τον έλεγχο του ρομπότ 5 Σχεδιασμός Τροχιάς: Βασικές Σχέσεις Αρχική θέση φάση επιτάχυνσης (γ) σταθερή ταχύτητα (dp/d) φάση επιβράδυνσης τελικό σημείοστόχος Απαιτούμη μεταβολή θέσης σε κάθε χρονική στιγμή i. Δ pi () pi () (φάση σταθερής ταχύτητας) ή () ( ) Δ pi Δpi γ (όπου γ επιθυμητή επιτάχυνση κατά μήκος του δρόμου) Εντολή θέσης συσσώρευση απαιτούμων μεταβολών θέσης: pi () pi ( ) Δpi () Εντολή ταχύτητας: p () i p ( i ) [ γ ] (αριθμητική ολοκλήρωση) Για καμπύλο δρόμο: ρ ακτίνα καμπυλότητας Αρχικό Σημείο Επιτάχυνση Σημείο διέλευσης Επιβράδυνση Επιτρεπόμη επιτάχυνση ελάχιστη ακτίνα καμπυλότητας ρf(επιτρεπόμη φυγόκτρη επιτάχυνση σε σχέση με το μέτρο της ταχύτητας) Τελικό Σημείο 6

4 Βασικοί αλγόριθμοι παρεμβολής Τα διανύσματα p(i) (τρέχουσα τολή θέσης) ή Δp(i) (απαιτούμη μεταβολή θέσης), τα οποία υπολογίζονται από τον αλγόριθμο σχεδιασμού τροχιάς, τροφοδοτούν τον αλγόριθμο αντίστροφης κινηματικής Απόλυτοι αλγόριθμοι παρεμβολής: p(i) q(i) Αυξητικοί αλγόριθμοι παρεμβολής: p () i q () i (αντίστροφο γεωμετρικό μοντέλο) (αντίστροφο διαφορικό κινηματικό μοντέλο) q() i q( i ) q () i 7 Απόλυτος αλγόριθμος παρεμβολής Σε κάθε βήμα δειγματοληψίας i υπολογίζεται μια νέα διάταξη αναφοράς q(i) (απ ευθείας από το p(i) μέσω αντίστροφου γεωμετρικού μοντέλου), η οποία στέλνεται ως τολή στους βρόχους ελέγχου Δqi () ( qi () qi ( ) ) Ταχύτητα αναφοράς: qi () Σφάλμα θέσης 0 σε όλα τα διάμεσα σημεία θέση επιθυμητή διάμεσο σημείο σφάλμα θέσης ταχύτητα επιθυμητή αναφοράς Δq αναφοράς (τολή) Δq 8

5 Αυξητικός αλγόριθμος παρεμβολής () Σε κάθε βήμα δειγματοληψίας i η ταχύτητα p () i μετασχηματίζεται σε ταχύτητα αναφοράς q () i, η οποία θεωρείται σταθερή σε κάθε βήμα Θέση αναφοράς (τολή): i qi () qk ( ) k Συσσωρευτικά (αθροιστικά) σφάλματα θέσης (επιθυμητή αναφοράς) (υπολογισμός μέσω ψηφιακής ολοκλήρωσης) ταχύτητα επιθυμητή αναφοράς (τολή) θέση επιθυμητή σφάλμα θέσης Δq αναφοράς (τολή) 9 Αυξητικός αλγόριθμος παρεμβολής () Σφάλματα παρακολούθησης τροχιάς στους αυξητικούς αλγορίθμους παρεμβολής Πραγματική Τροχιά Δp (χωρίς διόρθωση) Δp (με διόρθωση) Αρχή Δp Επιθυμητή Τροχιά Στόχος Σχεδιασμός τροχιάς p(i) p * (i) Δp(i) Αντίστροφη Κινηματική Ορθή Κινηματική προς βρόχους ελέγχου q(i) Βρόχος Διόρθωσης του συσσωρευτικού σφάλματος θέσης 0

6 Έλεγχος επιλυμένης ταχύτητας q(0) (Resolved oion rae conrol) q(i) Υπολογισμός j A j και A 0 j (j,,n) Υπολογισμός Ιακωβιανής J( q) Βρόχος διόρθωσης (correcion loop) p p *() i A0 n :3,4 p(i) p p( i ) p * ( i) d επιθυμητή τροχιά Επίλυση αντίστροφης διαφορικής κινηματικής p J( q) q Εύρεση q q( i ) q ( i) q ( i) d d: sapling ie i i q( i ), q ( i) προς ελεγκτή θέσης ρομπότ ξ() Σχεδιασμός τροχιάς μέσω πολυωνυμικών συναρτήσεων παρεμβολής (a) Έστω ξ μια μεταβλητή q i (στο χώρο των αρθρώσεων) ή p i (στο χώρο του τελικού στοιχείου δράσης) ξ f ξ0 ξ f Αρχικές / Τελικές Συνθήκες (oundary condiions) για τροχιά συνεχή, με συνεχή η παράγωγο (ταχύτητα) ξ f ξ(0) ξ 0 ξ( f ) ξ f ξ(0) ξ ξ ( ) ξ 0 f f Πολυωνυμική Συνάρτηση Παρεμβολής: (3ου βαθμού, 4 παράμετροι) (cuic polynoials) ξ () a a a a a0 ξ0 a ξ 0 a... a 3...

7 Σχεδιασμός τροχιάς μέσω πολυωνυμικών συναρτήσεων παρεμβολής () Πολυωνυμική Συνάρτηση Παρεμβολής (3ου βαθμού, 4 παράμετροι) (cuic polynoials) (συνέχεια) ξ () a a a a a0 ξ0, ξ a f a0 a f a f a 3 f, ξ, ξ a a 3a 0 f f 3 f ΕΠΕΚΤΑΣΗ Κυβικές Τροχιές Παρεμβολής με Ενδιάμεσα Σημεία (via poins) και Συνεχή Ταχύτητα a0 ξ0, a ξ0, a 3 ( ξ 0) f ξ ξ0 ξf, f f f ξ 0 Α Β Γ ξ f a ( ξ ξ ) ( ξ ξ ) 3 3 f 0 0 f f f 0 A B Γ f 3 ξ() Σχεδιασμός τροχιάς μέσω πολυωνυμικών συναρτήσεων παρεμβολής () Έστω ξ μια μεταβλητή q i (στο χώρο των αρθρώσεων) ή p i (στο χώρο του τελικού στοιχείου δράσης) ξ f ξ 0 ξ0 0 0 f ξ f Αρχικές / Τελικές Συνθήκες (oundary condiions) ξ(0) ξ 0 ξ( f ) ξ f ξ(0) ξ 0 ξ ( ) f ξ f ξ (0) ξ ξ ( ) ξ 0 f f 5 Πολυωνυμική Συνάρτηση Παρεμβολής: ξ () a0 a a a 5 (5ου βαθμού, 6 παράμετροι) Ειδική περίπτωση, όταν ισχύουν οι συνθήκες: a0 ξ0 a 3... a ( ξf ξ ) ( 0 ξ0 ξf) ξ0 a ξ0 ξf 0, 4... a f ξ0/ a 5... a 5 0 ( πολυώνυμο 4ου βαθμού) 4

8 Σχεδιασμός τροχιάς μέσω πολυωνυμικών συναρτήσεων παρεμβολής (3) Τροχιές τριών φάσεων (επιτάχυνση, σταθερή ταχύτητα, επιβράδυνση), με πολυώνυμα παρεμβολής 4ου βαθμού (και συνέχεια ως προς την επιτάχυνση) Ειδική Περίπτωση: Αρχική και Τελική Θέση «στάσει» ξ ( ξf ξ0 )( / f Δ) στην ευθεία που ώνει τα ξ 0 ξ 0 και ξ f ξ f ξ f ξ f ξ f ξ f ξ f Εαν ( ξ f ξ 0) ( ξ 0 ξ f ) f 4ου βαθμού ξ 0 ξ 0 ξ 0 ξ 0 Φ Δ Δ Δ Δ Φ Φ3 Πολυώνυμα παρεμβολής 4ου βαθμού f Φ: Φ3: ( ξ 0 ξ 0) 0 ξ ( ξ ξ 0 0) ξ Δ Δ Ισχύουν! ( ξf ξ ) f ξ 0 ( ξf ξf) ξ Δ Δ 5 Γραμμικός Έλεγχος Απλής Ρομποτικής Άρθρωσης 6

9 Συνάρτηση μεταφοράς απλής ρομποτικής άρθρωσης () Κινητήρας συνεχούς ρεύματος (DC): υψηλός λόγος ροπήςισχύος, γραμμική χαρακτηριστική (ροπήςταχύτητας) και μικρές σταθερές χρόνου (ροπή αδρανείας φορτίου) v ρ R ρ ρ i ρ e θ J ρ τ * τ J τ θ (συντελεστής γραμμικής τριβής) J θ (συντελεστής τριβής φορτίου) λόγος μετάδοσης n Ισοδύναμο Ηλεκτρικό Κύκλωμα Οδήγησης Κινητήρα DC Μηχανικό Σύστημα Κινητήρα DC (μετάδοση κίνησης με οδοντωτούς τροχούς, gears) Ισχύει: rθ rθ ή N Nθ όπου r, r ακτίνες τροχών και N, N : αριθμοί οδόντων r N Λόγος μετάδοσης: n r N < (μειωτήρας) θ n θ θ n θ θ n θ () 7 Συνάρτηση μεταφοράς απλής ρομποτικής άρθρωσης () Μηχανικό Σύστημα Κινητήρα J * () τ() () τ() () άξονας κινητήρα τ : ροπή που αναπτύσσει ο κινητήρας * τ : ροπή από το φορτίο (ανηγμένο στον άξονα του κινητήρα) () * * θ Ισχύει (διατήρηση έργειας): τ τ θ τ τ n τ (3) θ άξονας φορτίου () (3) τ () J θ () θ () n τ () (4) J θ () τ () θ () (4) (5) ( ) ( ) τ () J θ () θ () τ () nj θ () n θ () τ() J n J () n () (6) J () (5) τ s ( J s ) μηχανική σταθερά χρόνου (Σ) 8

10 Συνάρτηση μεταφοράς απλής ρομποτικής άρθρωσης (3) Ηλεκτρικό Σύστημα Οδήγησης του DC Κινητήρα τ K i ρ όπου Κ Τ : σταθερά ροπής κινητήρα (N./A) i ρ : ένταση ρεύματος ρότορα και diρ vρ Rρ iρ ρ e όπου R ρ : ηλεκτρική αντίσταση κυκλώματος ρότορα d ρ : αυτεπαγωγή ρότορα e : αντιηλεκτρεγερτική δύναμη του κινητήρα e K Κ : σταθερά αναλογίας αντιηλεκτρικής δύναμης Άρα, μετασχηματίζοντας κατά aplace: vρ Ks vρ ( Rρ ρs) iρ Ks iρ ( ρs Rρ) και vρ Ks θ ηλεκτρική τ K iρ K σταθερά (Σ) ( s ρ Rρ) χρόνου 9 Συνάρτηση μεταφοράς απλής ρομποτικής άρθρωσης (4) Μηχανικό Υποσύστημα τ ( J s ) sθ s μηχανική σταθερά (Σ) χρόνου τ s ( J s ) Ηλεκτρικό Υποσύστημα v ρ i ρ K ρ ρ ( s R ) e K τ sθ ηλεκτρική σταθερά χρόνου (Σ) vρ Ks τ K iρ K ( s ρ Rρ) 0

11 Συνάρτηση μεταφοράς απλής ρομποτικής άρθρωσης (5) v ρ i ρ K ρ ρ ( s R ) τ ( J s ) sθ s e K sθ vρ Ks () s K s ( J s ) ( ρ s Rρ )... v ρ Άρα: e G( s) K sθ sθ s όπου: Ισχύει: sθ () s K v ( s R )( J s ) K K ρ ρ ρ K G( s) ( s R )( J s ) ρ ρ sθ G v K G() s ρ K G vρ s[ ( J ) ρ s ( RJ ρ ρ ) s ( R ρ KK ) ] Συνάρτηση μεταφοράς απλής ρομποτικής άρθρωσης (6) Απλοποίηση μοντέλου: αγνοούμε την επίδραση της αυτεπαγωγής ρ του ρότορα ( ρ 0) Δηλαδή: Θεωρούμε την ηλεκτρική σταθερά χρόνου (Τ e ρ /R ρ ) του κινητήρα πολύ μικρή σε σχέση με τη μηχανική σταθερά χρόνου (Τ e J / ) K G v s[ ( R J ) s ( R K K )] ή, ισοδύναμα: όπου: ρ ρ ρ K G v s[ s ] ρ K K ( R K K ) ρ : Συνάρτηση Μεταφοράς (ανοικτού βρόχου) απλής άρθρωσης : σταθερά ίσχυσης του κινητήρα RJ ρ ( R K K ) ρ : σταθερά χρόνου του κινητήρα

12 Βασικός βρόχος ελέγχου θέσης απλής ρομποτικής άρθρωσης () θ, d e θ K P Ks D v ρ i ρ K ρ ρ ( s R ) τ ( J s ) sθ s n θ e K sθ Ελεγκτής θέσης PD (proporionalderivaive): v ( K K s) e Αντιστάθμιση δύο όρων: θέσης και ταχύτητας (δύο όροι : ανάλογος και παραγώγου) Δηλαδή, ελεγκτής θέσης PD: u () v() Ke() K e() όπου: e θ : σφάλμα θέσης άρθρωσης θ, d ρ P θ D θ, d : επιθυμητή (desired, reference) θέση άρθρωσης ρ e θ θ θ (επιθυμητή θέση τρέχουσα θέση) P (σήμα ελέγχου, οδήγησης του κινητήρα της άρθρωσης) D θ 3 Βασικός βρόχος ελέγχου θέσης απλής ρομποτικής άρθρωσης () θ, d e θ K P Ks D GPD n : συνάρτηση μεταφοράς ανοικτού βρόχου v ρ θ θ G( s) n θ Συνάρτηση μεταφοράς ανοικτού βρόχου: όπου: ή ( ρ 0): G ( K K s) G PD P D K G vρ s[ ( J ) ρ s ( RJ ρ ρ ) s ( R ρ KK ) ] K G v s[ ( R J ) s ( R K K )] ρ ρ ρ Συνάρτηση μεταφοράς κλειστού βρόχου: Όταν ρ 0: H H PD PD( ) P D GPD() s G ( s ) K ( K K s) s s[ ( RJ ) s ( R KK) ] K ( K Ks) ρ ρ PD P D 4

13 H Βασικός βρόχος ελέγχου θέσης απλής ρομποτικής άρθρωσης (3) Συνάρτηση μεταφοράς κλειστού βρόχου μεμονωμένης ρομποτικής άρθρωσης με αντισταθμιστή θέσης PD K ( K K s) s ( R J ) s ( R KK KK) s KK PD( ) P D ρ ρ D P ανάδραση ταχύτητας (derivaive er) ανάδραση θέσης (proporional er) Επιλογή ισχύσεων ελέγχου θέσης και ταχύτητας Δευτεροβάθμιο Σύστημα: χ.π. s ( ζω ) ( ) n s ωn ζ: συντελεστής απόσβεσης ω n : ιδιοσυχνότητα ταλαντώσεων (χωρίς απόσβεση) εύρος ζώνης ( ) KKP ω n Rρ J ( Rρ KK KKD) ζ ( KK)( RJ ) P ρ 5 Βασικός βρόχος ελέγχου θέσης απλής ρομποτικής άρθρωσης (4) Προδιαγραφές Σχεδίασης Αντισταθμιστή PD Ελέγχου Απλής Ρομποτικής Άρθρωσης ωr ω n < (ω r : δομική συχνότητα συντονισμού συστήματος) ( Rρ J ) < RJ K KK ( ) ( ) P ωr ωr ω n < KP ( ) ρ Ενίσχυση ανάλογου όρου (proporional gain) ζ (κρίσιμη ή υπερκρίσιμη απόσβεση) ( R ρ KK KK D) ζ ( KK)( RJ ) P ρ K D P ( K K )( RρJ) Rρ K K K 6