Το γενικό σχήμα ελέγχου κίνησης 1 Η βασικότερη συνιστώσα του συστήματος ελέγχου κάθε ρομποτικού χειριστή είναι το σύστημα ελέγχου κίνησης. Ο ρόλος του συστήματος ελέγχου κίνησης είναι να ελέγξει τις κινήσεις των επί μέρους ‘αρθρώσεων’ κατά τέτοιο τρόπο ώστε ο Μηχανικό μέρος Αισθητήρες Έλεγχος Κίνησης αρθρώσεων Διαταραχές Στόχος Σχεδιασμός μονοπατιού χειριστής να κινείται στην εκάστοτε επιθυμητή θέση ακολουθώντας όσο γίνεται καλύτερα το επιθυμητό μονοπάτι κίνησης, παρά τις διαταραχές από το περιβάλλον. Για να επιτελέσει το ρόλο του, το σύστημα ελέγχου κίνησης πρέπει να εκτελέσει δύο διακριτές λειτουργίες : α) τον σχεδιασμό του μονοπατιού (path) που πρέπει να ακολουθήσει το ρομπότ στο χώρο - διαδικασία από την οποία προκύπτουν οι επιθυμητές τροχιές (trajectories) των επί μέρους αρθρώσεων και β) τον έλεγχο των επί μέρους αρθρώσεων προκειμένου να εκτελέσουν τις επιθυμητές τροχιές. Και οι δύο λειτουργίες πρέπει να εκτελούνται σε πραγματικό χρόνο. Τροχιές αρθρώσεων

Έλεγχος αρθρώσεων (Έλεγχος χαμηλού επιπέδου – low level control) 2 Στο χαμηλότερο επίπεδο, το σύστημα ελέγχου κίνησης αναλαμβάνει να ελέγξει την γωνία στροφής κάθε άρθρωσης, «κλείνοντας» τους αντίστοιχους βρόχους ελέγχου. Για κάθε άρθρωση χρησιμοποιείται συνήθως ένας μικροελεγκτής … και βεβαίως η κατάλληλη ανάδραση Στον καταχωρητή επιθυμητής θέσης ΄φορτώνεται΄ συνεχώς – εν όσo η κίνηση εξελίσσεται – η επιθυμητή γωνία στροφής από τον ‘σχεδιαστή’ τροχιάς. Η πραγματική γωνία στροφής καταγράφεται συνεχώς στον καταχωρητή πραγματικής θέσης, με την βοήθεια του αισθητηρίου γωνιακής θέσης – ο βρόχος ελέγχου κλείνει «ψηφιακά». Ας σημειωθεί ότι κινητήριος μηχανισμός μπορεί να είναι και πνευματικός ή υδραυλικός. Αποτελείται τότε από αναλογική βαλβίδα ή σερβοβαλβίδα + επενεργητή (κύλινδρο ή κινητήρα) Τυπική συχνότητα δειγματοληψίας και ελέγχου : 1 ΚHz Αισθητήριο γωνιακής θέσης (encoder) Kαταχωρητής επιθυμητής θέσης Ελεγχος (PID ή..) Kαταχωρητής πραγματικής θέσης D/A + - θ d (t) Ελεγκτής κίνησης άρθρωσης Συνήθως μικροελεγκτής θ(t) Κινητήρας Μειω- τήρας Ενισχυτής ισχύος Κινητήριος μηχανισμός

To πλήρες σύστημα ελέγχου κίνησης 3 Μηχανικό Σύστημα Επεξεργαστής Συντονισμού κινήσεων Ελεγκτής της Μηχανής Επεξεργαστής για έλεγχο του Ν-τού άξονα Μέτρηση θέσης Ν ος άξονας κίνησης Επεξεργαστής για έλεγχο του 1 ου άξονα Μέτρηση θέσης Actuator Ενισχυτής ισχύος 1 ος άξονας κίνησης

Σερβομηχανισμός (σύστημα ελέγχου) θέσης Κίνηση από σημείου εις σημείο (point to point motion) 4 Αν είναι επιθυμητό ο σερβομηχανισμός θέσης να κινηθεί από μια θέση σε μια άλλη, δεν είναι ιδιαιτέρως σοφό να το κάνει με τον «βίαιο» τρόπο μιας εντολής βαθμίδας. Τέτοιες εντολές χρησιμοποιούμε στα συστήματα ελέγχου μόνο για να μελετήσομε την δυναμική τους συμπεριφορά. Ένας απλός τρόπος προκειμένου να επιτύχει κανείς ‘ομαλή’ κίνηση μεταξύ δύο θέσεων, είναι να ‘σχεδιάσει’ ένα τραπεζοειδές προφίλ ταχύτητας. Να ζητήσει δηλαδή από την άρθρωση να ξεκινήσει με σταθερή επιτάχυνση a, να επιταχύνει μέχρι να επιτευχθεί η μέγιστη ταχύτητα, να συνεχίσει ομαλή κίνηση με ταχύτητα V max και την κατάλληλη στιγμή να αρχίσει επιβραδυνόμενη κίνηση μέχρι ακινητοποιήσεως. Το αντίστοιχο προφίλ θέσης έχει τότε την μορφή του σχήματος : Ένα τμήμα μορφής παραβολής ( αν η επιθυμητή επιτάχυνση είναι σταθερή) ακολουθούμενο από ένα ευθύγραμμο τμήμα (σταθερή ταχύτητα και ένα τελευταίο τμήμα και πάλι σχήματος παραβολής. v t θ Vmax tata tata tvtv s t sasa sasa svsv Εντολή βαθμίδας

Σερβομηχανισμός (σύστημα ελέγχου) θέσης Τραπεζοειδές προφίλ ταχύτητας 5 Για συμμετρικό προφίλ ταχύτητας ισχύουν οι παρακάτω σχέσεις : V max = a t a (1) Ομαλά επιταχυνόμενη κίνηση s a = ½ a t a 2 =½ V max 2 /a (2) Ομαλά επιταχυνόμενη κίνηση S = 2s a + s v (3) Συνολικό διάστημα κίνησης T = 2 t a + t v (4) Συνολικός χρόνος κίνησης s v = V max t v = (a t a ) t v (5) Ομαλή ευθύγραμμη κίνηση Συνήθως ζητείται να σχεδιασθεί προφίλ με δεδομένα : την συνολική μετακίνηση S, την επιτάχυνση a και την μέγιστη ταχύτητα κίνησης. Από τις παραπάνω σχέσεις μπορούν τότε να υπολογισθούν τόσο οι χρόνοι των επί μέρους κινήσεων όσο και τα επί μέρους διαστήματα. Το ζητούμενο προφίλ θέσης δίδεται τότε από τις σχέσεις : v t θ Vmax tata tata tvtv s t sasa sasa svsv s(t) = ½ a t 2 για t < t a s(t) = s a + V max (t - t a ) για t a <= t < (t a + t v ) s(t) = (s a +s v ) +V max (t-t a -t v ) - ½ a (t-t a -t v ) 2 για (t a + t v ) <= t <= T

Σερβομηχανισμός (σύστημα ελέγχου) θέσης Τριγωνικό προφίλ ταχύτητας 6 Πολλές φορές, την φάση της επιταχυνόμενης κίνησης ακολουθεί αμέσως η φάση της επιβραδυνόμενης : Είτε επειδή έτσι το ζητούμε για να ελαχιστοποιήσομε τους χρόνους μετάβασης είτε γιατί με δεδομένα τα S, Vmax και a, ο σερβομηχανισμός ενδέχεται να μην ‘προλάβει’ να αναπτύξει την επιθυμητή ταχύτητα. Το προφίλ της ταχύτητας τότε έχει μορφή τριγωνική της δε θέσης αποτελείται από δύο παραβολικά τμήματα. Αν έχει επιλεγεί η μέγιστη επιτάχυνση, τότε η κίνηση αυτή είναι κίνηση ελαχίστου χρόνου μετάβασης. Το ζητούμενο προφίλ θέσης δίδεται τότε από τις σχέσεις : s(t) = ½ a t 2 για t < t a s(t) = (S/2) + (a t a ) (t-t a ) - ½ a (t-t a ) 2 για t a <= t <= 2t a Αν κατά το προηγούμενο, δίδεται το συνολικό διάστημα S, η επιτάχυνση a και η μέγιστη ταχύτητα επιτρεπόμενη ταχύτητα κίνησης, τότε, το διάστημα της επιταχυνόμενης κίνησης, s a = V max 2 /a καθορίζει το είδος του προφίλ : Αν 2s a S >= V max 2 /a τότε το προφίλ είναι τραπεζοειδές. Σε αντίθετη περίπτωση είναι τριγωνικό. v t Vmax tata tata s t sasa sasa Ενδιαφέρον παρουσιάζει και η περίπτωση κατά την οποία ζητείται τραπεζοειδές προφίλ με δεδομένα το S και τους χρόνους t a και t v. Αποδεικνύεται εύκολα τότε ότι η ζητούμενη επιτάχυνση είναι a = S/(t a 2 + t a t v ), οπότε μπορούν να προκύψουν και όλα τα υπόλοιπα μεγέθη της κίνησης. Οι παραπάνω σχέσεις είναι γραμμένες για ευθύγραμμη κίνηση, ισχύουν όμως και για περιστροφική, αρκεί το διάστημα s να αντικατασταθεί με γωνία φ, η γραμμική ταχύτητα με γωνιακή ταχύτητα και η γραμμική επιτάχυνση με γωνιακή επιτάχυνση.

Σερβομηχανισμός θέσης Το γενικότερο πρόβλημα του σχεδιασμού της μετάβασης από μια θέση σε μια άλλη (trajectory planning) Στην γενικότερη των περιπτώσεων, το πρόβλημα του σχεδιασμού του προφίλ θέσης στο χρόνο, τίθεται ως εξής : Να σχεδιασθεί το κατάλληλο προφίλ s(t) (trajectory) για την μετάβαση από την θέση s 0 όπου ο σερβομηχανισμός έχει ταχύτητα v 0, στην θέση s tf όπου ο σερβομηχανισμός πρέπει να έχει ταχύτητα v tf, εντός χρόνου t f. Αν για το s(t) επιλεγεί πολυωνυμική συνάρτηση τρίτου βαθμού, οι συντελεστές της μπορούν να υπολογισθούν με την βοήθεια των παραπάνω οριακών συνθηκών. Έστω : Τότε : Εφαρμόζοντας τις οριακές συνθήκες θέσης και ταχύτητας, έχομε : s t s tf s0s0 tftf Αν επιλύσομε το σύστημα των εξισώσεων μπορούμε να υπολογίσομε τους συντελεστές του κυβικού πολυωνύμου: Αν θέσει κανείς οριακές συνθήκες και για την επιτάχυνση, τότε χρειάζεται να χρησιμοποιήσει πολυώνυμο 5 ου βαθμού, οι συντελεστές του οποίου προσδιορίζονται με αντίστοιχο τρόπο.

Σερβομηχανισμός θέσης Σύγκριση τραπεζοειδούς με κυβικό προφίλ κίνησης Η χρήση τραπεζοειδούς προφίλ χρησιμοποιείται τις περισσότερες φορές καθ’ όσον αυτό που συνήθως θέτομε ως απαίτηση είναι (ασφαλώς) η συνολική διαδρομή, η μέγιστη επιτρεπτή ταχύτητα και η μέγιστη επιτάχυνση. Το «κυβικό» προφίλ, εκτός της συνολικής διαδρομής απαιτεί μόνο τον συνολικό χρόνο κίνησης και «αποφασίζει» μόνο του για την επιτάχυνση – που μεταβάλλεται γραμμικά – και για την ταχύτητα κίνησης. Παράγει πιο «ομαλή» μετακίνηση και μπορεί να αποδειχθεί ότι απαιτεί τα ελάχιστα ποσά ενέργειας. Στα σχήματα φαίνονται οι χρονικές συναρτήσεις (προφίλ) θέσης, ταχύτητας και επιτάχυνσης για συνολική μετακίνηση σερβομηχανισμού γωνιακής θέσης κατά π ακτίνια. (α) με τραπεζοειδές προφίλ ταχύτητας και χρόνους : t a = 0.2s, t v = 0.6s (β) με κυβικό προφίλ θέσης και συνολικό χρόνο μετακίνησης t f = 1s (α)(β)

Σερβομηχανισμός θέσης Υλοποίηση 9 Δημιούργησε μια ρουτίνα που να τρέχει κάθε φορά που το ρολόι του συστήματος «μετρήσει» χρόνο Δt -τυπικά για σερβοσυστήματα 1ms. Στην εν λόγω ρουτίνα πρέπει να «τρέχει» το παρακάτω πρόγραμμα : θ d =0; e(rror) = 0; C(ommand) =0; t=0; Ι =0; Repeat t=t+Δt; i= i+1; Υπολόγισε το θ d (t); % τραπεζοειδές, κυβικό ή άλλο προφίλ Διάβασε την πραγματική θέση θ(t); Υπολόγισε το σφάλμα θέσης e(t) = θ d (t) - θ(t); Υπολόγισε τον αναλογικό όρο του PID : Kp* e(t) ; Υπολόγισε τον ολοκληρωτικό όρο του PID : Kι*I t ; % Ι t = Ι t-1 + (e t-1 + e t ) Δt / 2 το ολοκλήρωμα του σφάλματος Υπολόγισε τον διαφορικό όρο του PID : Kd*[(e t - e t-1 ) / Δt] ; Υπολόγισε την εντολή ελέγχου : C = Kp* e(t) + Kι*I t + Kd*[(e t - e t-1 ) / Δt] ; Στείλε την στον Α/D μετατροπέα; until θ = Θ ; % (συνολική μετατόπιση)  (s) Κ Τ m s + 1 Va(s) + D(s) Ra Ka _ 1 s Ke Kc θ(s) + _ θ d (s) Κi s Κp Κ d s PID Κινητήρας Ελεγκτής Υπολογισμός προφίλ θ d (t)