Ρομποτική Μάθημα 5ο «Αντίστροφη κινηματική χειριστών» Γαστεράτος Αντώνιος, Επίκουρος Καθηγητής Εργαστήριο Ρομποτικής και Αυτοματισμών Τομέας Συστημάτων Παραγωγής Τμήμα Μηχανικών Παραγωγής και Διοίκησης Δημοκρίτειο Πανεπιστήμιο Θράκης

Σκοπός του μαθήματος Δεδομένης της επιθυμητής θέσης και προσανατολισμού του τελικού σημείου δράσης να υπολογιστούν οι παράμετροι του χειριστή, για την επίτευξη του επιθυμητού αποτελέσματος

Ανασκόπηση Ειδικές περιπτώσεις του πίνακα περιστροφής Περιστροφή περί τον άξονα Χ κατά γωνία θ Περιστροφή περί τον άξονα Υ κατά γωνία φ Περιστροφή περί τον άξονα Ζ κατά γωνία ψ

Ανασκόπηση Διάνυσμα θέσης Πίνακας περιστροφής Κλιμάκωση Ομογενής πίνακας μετασχηματισμού

Ειδικές περιπτώσεις ομογενούς πίνακα 1. Μεταφορά 2. Περιστροφή Ανασκόπηση

Αναπαράσταση προσανατολισμού γωνίες roll, pitch, yaw γωνίες του Euler XAXA X’ B YAYA ZAZA Y’ B Z’ B γ XAXA X’’ B YAYA ZAZA Y’’BY’’B Z’’BZ’’B β XAXA X’ B YAYA ZAZA Y’’’ B Z’’’ B α X’’’ B X’ B Y’ B Z’ B X’’ B Y’’BY’’B Z’’BZ’’B XAXA Z’ B YAYA ZAZA Y’ B X’ B γ β α XAXA Z’ B YAYA ZAZA Y’ B X’ B Z’’ B X’’ B Y’’ B XAXA Z’’’ B YAYA ZAZA Y’’BY’’B X’’ B Z’’ B X’’’ B Y’’’ B

Ανασκόπηση Ορθό κινηματικό πρόβλημα Δεδομένου του διανύσματος q των μεταβλητών των αρθρώσεων, να υπολογιστούν η θέση και ο προσανατολισμός του τελικού σημείου δράσης: γωνίες roll, pitch, yaw ή γωνίες Euler ?

Ανασκόπηση Σύνδεσμοι και αρθρώσεις Αρθρώσεις ( Joints ) Σύνδεσμοι ( Links ) Τελικό στοιχείο δράσης ( End Effector ) Βάση Ρομπότ ,

Ανασκόπηση Κανόνας των Denavit-Hartenberg Αριθμούμε όλες τις αρθρώσεις από το 1 έως το N, ξεκινώντας από τη βάση και καταλήγοντας στο τελικό σημείο δράσης Προσδιορισμός του πλαισίου βάσης: Προσδιορίζουμε το ένα δεξιόστροφο ορθοκανονικό σύστημα βάσης (Χ 0 Υ 0 Ζ 0 ) με αρχή στη βάση του χειριστή και τον άξονα Ζ 0 παράλληλα στον άξονα κίνησης της άρθρωσης 1 Προσάρτηση πλαισίων σε καθένα από τους συνδέσμους σύμφωνα με τους κανόνες Υπολογισμός των παραμέτρων των συνδέσμων

Ανασκόπηση

Ευθύ κινηματικό πρόβλημα Ευθύ κινηματικό πρόβλημα: δεδομένων των γωνιών ή των μετατοπίσεων των αρθρώσεων να υπολογιστεί η θέση και ο προσανατολισμός του τελικού σημείου δράσης Αντίστροφο κινηματικό πρόβλημα Αντίστροφο κινηματικό πρόβλημα: δεδομένης της θέσης και του προσανατολισμού του τελικού σημείου δράσης, να υπολογιστούν οι μεταβλητές των αρθρώσεων Ποια η δυσκολία στο αντίστροφο κινηματικό πρόβλημα; Μπορεί να μην υπάρχει λύση ή η λύση να μην είναι μοναδική Οι εξισώσεις είναι μη γραμμικές, τριγωνομετρικές, δύσκολες στην επίλυσή τους Το πρόβλημα

Επιλυσημότητα Δεδομένου του πίνακα μετασχηματισμού: Για 6 dof υπάρχουν 12 εξισώσεις με 6 αγνώστους Από τις 9 εξισώσεις του πίνακα περιστροφής, μόνο οι 3 είναι ανεξάρτητες

Ο χώρος εργασίας Χώρος εργασίας: ο χώρος που είναι προσβάσημος από το τελικό σημείο δράσης Επιδέξιος χώρος εργασίας: ο χώρος που είναι προσβάσημος από το τελικό σημείο δράσης, σε οποιοδήποτε προσανατολισμό Προσβάσημος χώρος εργασίας: ο χώρος όπου το ρομπότ μπορεί να έχει πρόσβαση κατά έναν τουλάχιστον προσανατολισμό

Χώρος εργασίας

Προσβάσημος χώρος εργασίας: ο κύκλος με ακτίνα 2l Επιδέξιος χώρος εργασίας: το κέντρο του παραπάνω κύκλου Το μήκος καθενός από τους συνδέσμους είναι l

Χώρος εργασίας Προσβάσημος χώρος εργασίας: ο δακτύλιος με εσωτερική ακτίνα l 1 -l 2 και εξωτερική ακτίνα l 1 +l 2 Επιδέξιος χώρος εργασίας: δεν υπάρχει Τα μήκη των δύο συνδέσμων είναι διαφορετικά

Ποιος είναι ο προσβάσημος χώρος; Έστω l 1, l 2 σταθερές και  3 μεταβλητή l2l2 l3l3 l1l1 Έστω  1 μεταβλητή Έστω η  2 μεταβλητή Παράδειγμα

Ύπαρξη λύσεων Όταν ο χειριστής διαθέτει λιγότερους από έξι βαθμούς ελευθερίας, δεν είναι σε θέση να επιτύχει γενικούς σκοπούς θέσης και προσανατολισμού στον τρισδιάστατο χώρο Για λιγότερους βαθμούς ελευθερίας αναζητείται το πλησιέστερο εφικτό πλαίσιο

Μία λύση είναι αποδεκτή αν ο στόχος ανήκει στο χώρο εργασίας του χειριστή Ο υπολογισμός του χώρου εργασίας είναι δύσκολος. Πρακτικά διευκολύνεται από τον ειδικό σχεδιασμό του χειριστή Το αντίστροφο κινηματικό πρόβλημα μπορεί να επιδέχεται περισσότερες της μιας λύσεις 2 λύσεις Πολλαπλές λύσεις

Σε περίπτωση περισσοτέρων της μιας λύσεων, πώς επιλέγεται κάποια; Η πλησιέστερη λύση Ύπαρξη εμποδίων, που περιορίζουν το χώρο εργασίας Πολλαπλές λύσεις Εμπόδιο

2 σετ λύσεων: {  4,  5 } 1,  6 {  4,  5 } 2,  6 Πολλαπλές λύσεις

O PUMA μπορεί να επιτύχει το ίδιο αποτέλεσμα με 8 διαφορετικές λύσεις Για καθεμιά από τις 4 δεξιά, υπάρχουν δύο δυνατές τοποθετήσεις του καρπού, σύμφωνα με:

Ένας χειριστής είναι επιλύσημος αν οι μεταβλητές των αρθρώσεων μπορούν να προσδιορισθούν από έναν αλγόριθμο, που να μπορεί να υπολογίσει όλες τις πιθανές λύσεις λύσεις κλειστού τύπου αριθμητικές λύσεις Λύσεις Μέθοδοι επίλυσης 1. Αλγεβρικές μέθοδοι 2. Γεωμετρικές μέθοδοι 2. Γεωμετρικές μέθοδοι

Όλα τα συστήματα με περιστροφικές ή πρισματικές αρθρώσεις, που έχουν συνολικά 6 βαθμούς ελευθερίας εν σειρά είναι επιλύσημα Γενικά η λύση είναι αριθμητική Τα ρομπότ με αναλυτική λύση: ορισμένοι τεμνόμενοι άξονες αρθρώσεων και πολλές φορές  i = 0, +/-90 o Μέθοδοι επίλυσης

Σε ένα χειριστή, το σύνολο των πλαισίων των τελικών στόχων αποτελεί τον προσβάσημο χώρο εργασίας Αν n<6, ο προσβάσημος χώρος εργασίας είναι μέρος ενός υποχώρου διαστάσεως n Για την περιγραφή του χώρου εργασίας, υπολογίζουμε τις εξίσωσεις της ευθείας κινηματικής και μεταβάλουμε τις μεταβλητές των αρθρώσεων Ο χώρος εργασίας του χειριστή όταν n<6

for q 1 =0:q 1max for q 2 =0:q 2max … (x,y,z,r,p,y)=f(q 1,q 2,…q n ) … end Υπολογισμός χώρου εργασίας

Παράδειγμα Ο υποχώρος για τον επίπεδο χειριστή 3 βαθμών ελευθερίας προσδιορίζεται από: Όπου τα x και y προσδιορίζουν τη θέση του καρπού και το φ τον προσανατολισμό του τελικού συνδέσμου. Ο χώρος δημιουργείται επειδή τα x,y,φ επιτρέπεται να λάβουν τυχαίες τιμές Οποιοδήποτε πλαίσιο δεν περιγράφεται από τον παραπάνω πίνακα, δεν ανήκει στο χώρο του χειριστή Τα φυσικά χαρακτηριστικά των συνδέσμων και των αρθρώσεων περιορίζουν το χώρο του χειριστή l 1 l 2 l 3  3  2  1 x y {0} {H}

Παράδειγμα l 1 l 2 l 3  3  2  1 x y {0} {H} for θ 1 =0:180 for θ 2 =0:360 for θ 3 =0:360 (x,y,z,r,p,y)=f(q 1,q 2,…q n ) if y>0 end

Ο τελικός στόχος απαιτεί 6 βαθμούς ελευθερίας Αν ο χειριστής διαθέτει n<6 DoF, στη γενική περίπτωση δεν είναι σε θέση να επιτύχει τον τελικό στόχο Πιθανός συμβιβασμός: επίτευξη ενός στόχου όσο το δυνατόν εγγύτερα στον επιθυμητό στόχο 1) Δεδομένου του πλαισίου του στόχου,να υπολογιστεί ο τροποποιημένος στόχος στον υποχώρο του χειριστή, κατά το δυνατόν εγγύτερα του 2) Υπολογισμός του αντίστροφου κινηματικού. Μπορέι να μην υπάρχει πιθανή λύση, αν ο στόχος δεν ανήκει στο χώρο εργασίας του χειριστή Ο χώρος εργασίας του χειριστή όταν n<6

Παράδειγμα Να δοθεί μία περιγραφή του υποχώρου του για τον πολικό χειριστή του σχήματος όπου τα x, y μπορούν να λάβουν οποιαδήποτε τιμή. Ο προσανατολισμός περιορίζεται εξαιτίας του άξονα Ζ 2, του οποίου η διεύθυνση εξαρτάται από τα x, y. Ο άξονας Υ 2 έχει πάντα κατεύθυνση προς τα κάτω, ενώ ο άξονας Χ 2 προκύπτει από το εξωτερικό γινόμενο των προηγουμένων δύο. Έτσι Χ1Χ1 Ζ1Ζ1 Ζ2Ζ2 Χ2Χ2 Υ1Υ1 Υ2Υ2

Παράδειγμα Έτσι ο υποχώρος προκύπτει ότι είναι Χ1Χ1 Ζ1Ζ1 Ζ2Ζ2 Χ2Χ2 Υ1Υ1 Υ2Υ2

Οι κινηματικές εξισώσεις του βραχίονα είναι: Το τελικό σημείο του στόχου προσδιορίζεται από 3 αριθμούς x, y και φ, ως εξής: (φ περιστροφή περί τον άξονα Ζ) Αλγεβρική επίλυση {0} l 1 l 2 l 3  3  2  1 Χ Υ {H} x, y, φ

Με σύγκριση των δύο εξισώσεων λαμβάνουμε το παρακάτω σύστημα: Υψώνοντας στο τετράγωνο και αθροίζοντας τις δύο τελευταίες εξισώσεις: Αλγεβρική επίλυση

Απ’ όπου λαμβάνεται μια έκφραση για το c 2 (επειδή c 12 =c 1 c 2 -s 1 s 2 και s 12 = c 1 s 2 -c 2 s 1 ) : Αλγεβρική επίλυση Πότε υπάρχει λύση; cosθ  [-1,1] Ποια είναι η φυσική έννοια της μη ύπαρξης λύσης; Ο τελικός στόχος βρίσκεται εκτός του επιδέξιου χώρου εργασίας

Είναι διαθέσημες δύο πιθανές λύσεις για την θ 2, γιατί; και θέτοντας k 1 =l 1 +l 2 c 2 και k 2 =l 2 s 2, τότε: Αλγεβρική επίλυση θ2θ2 cosθ 2

k1k1 k2k2 22 l1l1 l2l2 Τότε : k 1 =r cos , k 2 =r sin , και επομένως : x/r= cos  cos  1 - sin  sin  1 y/r= cos  sin  1 - sin  cos  1 ή cos(  +  1 ) = x/r και sin(  +  1 ) =y/r  r Αλγεβρική επίλυση Έστω

Επομένως:  +θ 1 = atan2(y/r,x/r) = atan2(y,x) και έτσι: θ 1 = atan2(y,x) - atan2(k 2,k 1 ) Επομένως επειδή c φ = c 123 και s φ = s 123, η θ 3 είναι: θ 1 +θ 2 + θ 3 = atan2(s φ, c φ ) = φ Σημ: Αν x=y=0, τότε η θ 1 μπορεί να λάβει όποια τυχαία τιμή Αλγεβρική επίλυση

Ανάλυση της χωρικής γεωμετρίας του βραχίονα σε χωριστά προβλήματα επίπεδης γεωμετρίας Εφαρμογή του “ νόμου των συνημίτονων ” και Πυθαγορείου θεωρήματος : x 2 +y 2 =l 1 2 +l l 1 l 2 cos(180 ο +  2 ) Γεωμετρική επίλυση l 1 l 2 l 3  3  2  1 Χ Υ {H} φ y x

Δηλαδή: Για να υπάρχει το τρίγωνο θα πρέπει : Από το σχήμα : β = atan2 ( x,y ) και όταν 0  ψ  180 ο, τότε θ 1 =β  ψ όπου το + ισχύει για θ 2 > 180 ο και το - για θ 2 < 180 ο Γεωμετρική επίλυση ψ β l 1 l 2 l 3  3  2  1 Χ Υ {H} φ y x

Τελικά επειδή θ 1 +θ 2 +θ 3 = φ επιλύουμε ως προς θ 3 Γεωμετρική επίλυση ψ β l 1 l 2 l 3  3  2  1 Χ Υ {H} φ y x

Οι τριγωνομετρικές εξισώσεις είναι δύσκολες στην επίλυσή τους, αφού μια μεταβλητή θ εμφανίζεται συνήθως ως cosθ και sinθ Εισάγοντας τη μεταβλητή u, έτσι ώστε u = tan(θ/2) τότε: cosθ = (1-u 2 )/(1+u 2 ) sinθ = 2u /(1+u 2 ) Αλγεβρική επίλυση με αναγωγή σε πολυώνυμα

Παράδειγμα Να μετασχηματίσετε την τριγωνομετρική σχέση: σε αλγεβρική και να λύσετε ως προς το θ. Αντικαθιστώντας με τις σχέσεις της προηγούμενης διαφάνειας και πολλαπλασιάζοντας με ή Αν η παραπάνω λύση είναι μιγαδική, δεν υπάρχει πραγματική λύση στην αρχική τριγωνομετρική εξίσωση. Αν a+c=0, τότε υπάρχει λύση θ=180. Στην υλοποίηση με υπολογιστή θα πρέπει η διαίρεση με το μηδέν να λαμβάνεται υπόψη.

Η επίλυση κατά Pieper Η μέθοδος αυτή εφαρμόζεται σε χειριστές όπου τρεις διαδοχικοί άξονες (συνήθως οι άξονες του καρπού) τέμνονται σε ένα σημείο Το σημείο αυτό είναι t

Η επίλυση κατά Pieper και επειδή

Η επίλυση κατά Pieper και άρα

Η επίλυση κατά Pieper χρησιμοποιώντας τις εκφράσεις για τους τότε όπου

Η επίλυση κατά Pieper και επομένως το τετράγωνο του μέτρου του είναι και τελικά όπου

Η επίλυση κατά Pieper η εξίσωση αυτή είναι ανεξάρτητη της θ 1 ενώ η επίλυση της θ 2 είναι σχετικά απλή Για την επίλυση της θ 3 διακρίνουμε 1. Αν a 1 =0, τότε r=k 3, όπου το r=x 2 +y 2 +z 2, γνωστό και το k 3 είναι συνάρτηση μόνο του θ 3 και η εξίσωση επιλύεται με αναγωγή σε πολυώνυμα 2. Αν sα 1 =0, τότε z=k 4, όπου το z είναι γνωστό και η εξίσωση επιλύεται με αναγωγή σε πολυώνυμα 3. Διαφορετικά με απαλοιφή των s 2 και c 2 παίρνουμε

Η επίλυση κατά Pieper Αφού υπολογίσουμε τη θ 3, υπολογίζουμε τη θ 2, από την εξίσωση και τη θ 1 από την

Η επίλυση κατά Pieper Αφού υπολογίσουμε τις θ 3, θ 2 και θ 1, είναι δυνατός ο υπολογισμός του πίνακα περιστροφής, για θ 4 =0 και τελικά από τον πίνακα υπολογίζουμε τις γωνίες Euler για τον καρπό

t PUMA 560

Επειδή οι 3 τελικές αρθρώσεις ικανοποιούν τη συνθήκη του Pieper, υπολογίζουμε αρχικά τις θ 1, θ 2 και θ 3

PUMA 560 Στη συνέχεια υπολογίζουμε τις θ 4, θ 5 και θ 6 από τον πίνακα περιστροφής

PUMA 560 αν και αν r 13 =r 33 =0 Επομένως υπάρχουν δύο σύνολα τιμών για την θ 1, δύο για τη θ 3 και δύο για τις {θ 4, θ 5, θ 6 }, συνολικά 8 συνδυασμοί για τη λύση του αντίστροφου κινηματικού προβλήματος για τον PUMA 560

Πλαίσια με καθιερωμένο όνομα 1. Ο χρήστης υποδεικνύει στο σύστημα τη θέση του πλαισίου του σταθμού εργασίας {S}. Το πλαίσιο αυτό δίνεται σε σχέση με το {Β} 2. Ο χρήστης ορίζει το εργαλείο που χειρίζεται το ρομπότ. Κάθε εργαλείο μπορεί να έχει διαφορετικό {T} 3. Ο χρήστης υποδεικνύει το στόχο, ορίζοντας το πλαίσιο {G} 4. Το σύστημα υπολογίζει τις παραμέτρους των αρθρώσεων για την ομαλή μετακίνηση έτσι ώστε τελικά {T} = {G} {Β} {W}{W} {T}{T} {S}{S} {G}{G}

Εργασία Για το κυλινδρικό ρομπότ του παρακάτω σχήματος να επιλύσετε το αντίστροφο κινηματικό πρόβλημα Άρθρωση 1 η Άρθρωση 2 η ΧΒΧΒ ΥΒΥΒ ΖΒΖΒ {Β} Β ΖΤΖΤ {Τ}Β{Τ}Β Άρθρωση 3 η Άρθρωση 4 η ΥΤΥΤ ΧΤΧΤ l1l1

Ασκήσεις στο Matlab Για το χειριστή του σήματος (l 1 =4, l 2 =3 και l 3 =2) Να αναπτύξετε ένα πρόγραμμα για την επίλυση του χειριστή και να το ελέγξετε για τις ακόλουθες περιπτώσεις: Για επαλήθευση χρησιμοποιήστε το πρόγραμμα που φτιάξατε στο προηγούμενο μάθημα Επίσης από το Robotics Toolbox να κάνετε επαλήθευση των αποτελεσμάτων με τη συνάρτηση ikine Ως το επόμενο μάθημα μόνο με ηλεκτρονικό ταχυδρομείο l 1 l 2 l 3  3  2  1 x y {0} {H}

Ερωτήσεις