Αντίστροφη Κινηματική

Transcript

1 Αντίστροφη Κινηματική Πώς να τοποθετήσω το χέρι μου εδώ; Αντίστροφη Κινηματική: Επέλεξε αυτές τις γωνίες

2 ΥΠΑΡΧΕΙ ΛΥΣΗ; Vrml Inverse Kinema9cs - No solu9on Στόχος Για ένα στόχο έξω από τον χώρο εργασίας του ρομπότ δεν υπάρχει λύση Περιορισμοί στις αρθρώσεις => ΟΙ ΠΙΘΑΝΕΣ ΛΥΣΕΙΣ ΝΑ ΜΗΝ ΕΙΝΑΙ ΟΛΕΣ ΕΦΑΡΜΟΣΙΜΕΣ

3 ΥΠΑΡΧΕΙ ΜΙΑ ΜΟΝΟ ΛΥΣΗ; Goal Για ένα στόχο μέσα στον χώρο εργασίας του ρομπότ πιθανόν να υπάρχουν πάνω από μία λύσεις, ή ακόμα και να μην υπάρχουν λύσεις όπως π.χ. στην περίπτωση ύπαρξης ανυπέρβλητων εμποδίων Κανόνες επιλογής λύσης

4 Χαρακτηριστικοί χώροι του ρομπότ Δυνατός Χώρος Eργασίας Εφικτός Χώρος Eργασίας Το σύνολο των μαθηματικών λύσεων του προβλήματος + Όρια των αρθρώσεων θ 2 y o L 1 L 2 L + L 1 2 L L 1 2 z o θ 1 x o VRML- Working Space 1

5 Η μαθηματική περιγραφή του Δυνατού Xώρου εργασίας για τον επίπεδο βραχίονα 3 ων βαθμών ελευθερίας γίνεται από τον Ο.Μ. του άκρου του βραχίονα y o y L 2 L 3 # g 0e = $ cφ sφ 0 x sφ cφ 0 y & ( ( ( = ( '( L 1 Επίπεδος βραχίονας 3 β.ε. x φ x o g 0e = Rot(z,φ) x y Ο. Μ. Άκρου βραχίονα

6 Πολλαπλές λύσεις Inverse Kinema9cs - Mul9ple solu9ons 1 Inverse Kinema9cs - Mul9ple solu9ons 2 Κανόνες επιλογής λύσης - Πλησιέστερης - Αποφυγή εμποδίων - Κίνηση μικρών αρθρώσεων Ο αριθμός των λύσεων εξαρτάται από: 1) Αριθμό αρθρώσεων 2) γεωμετρία των συνδέσμων 3) Έύρος κίνησης των αρθρώσεων

7 Αντίστροφο κινηματικό για επίπεδο πολικό βραχίονα

8 Να υλοποιηθεί πρόγραμμα που να εκτελεί τα παρακάτω για τον προηγούμενο βραχίονα: (1) Επιλύει το ευθύ κινηματικό πρόβλημα και απεικονίζει το ρομπότ σε ένα figure για θέσεις που έχει δώσει ο χρήστης (2) Επιλύει το αντίστροφο κινηματικό πρόβλημα (3) Απεικονίζει σε ένα figure τον χώρο εργασίας του παρακάτω βραχίονα. LAB4_Askisi1 clc; clear all; close all fprintf('πολικος Επιπεδος Βραχιονας\n\n'); a=1; while a==1 fprintf('(1)επίλυση του ευθύ κινηµατικού προβλήµατος\n'); fprintf('(2)επίλυση του αντιστροφου κινηµατικού προβλήµατος\n'); fprintf('(3)απεικονιση του χωρου εργασιας\n'); fprintf('(4)τελος προγραµµατος\n\n'); select=input('τι θελεις να γινει;\n'); switch select case 1 fprintf('επελεξεσ ΤΟ ΕΥΘΥ ΚΙΝΗΜΑΤΙΚΟ ΠΡΟΒΛΗΜΑ\n'); th=input('δώσε τιµή για την περιστροφική άρθρωση (σε deg):'); theta=th*pi/180; Αναγωγη deg σε rad d=input('δώσε τιµή για την πρισµατική άρθρωση (σε cm):'); px=d*cos(theta); py=d*sin(theta); fprintf('οι συντεταγµενες του ακρου ειναι:\n'); fprintf(' px=6.2f cm\n py=6.2f cm\n',px,py) line([0 px],[0 py],'linewidth',5) hold on plot(0,0,'ko','linewidth',15) grid on case 2 fprintf('επελεξεσ ΤΟ ΑΝΤΙΣΤΡΟΦΟ ΚΙΝΗΜΑΤΙΚΟ ΠΡΟΒΛΗΜΑ\n'); pxd=input('δώσε τιµή για το px του ακρου (σε cm):'); pyd=input('δώσε τιµή για το py του ακρου (σε cm):'); thetad=atan2(pyd,pxd); thd=thetad*180/pi; d=sqrt(pxd^2+pyd^2); fprintf(' theta=6.2f deg\n d=6.2f cm \n',thd,d); case 3 fprintf('επελεξεσ ΤΗΝ ΠΡΟΒΟΛΗ ΤΟΥ ΧΩΡΟΥ ΕΡΓΑΣΙΑΣ ΤΟΥ ΒΡΑΧΙΟΝΑ\n'); for k=0:.1:2*pi for d=10:1:20; px=d*cos(k); py=d*sin(k); plot(px,py,'.') hold on end end axis equal end case 4 a=0; end

9 Βοηθητικά μαθηματικά εργαλεία Η συνάρτηση arctan2(y,x) επιστρέφει την γωνία στο σωστό τεταρτημόριο y 0 θ 90 για + x + y 90 θ 180 για x + y θ= arc tan 2(y,x) = 180 θ 90 για x y 90 θ 0 για + x y Νόμος των συνημητόνων x Αν θέλω να υπολογίσω µία άγνωστη γωνία θ, υπολογίζω το ηµίτονο και το συνηµίτονο αυτής και χρησιµοποιώ την atan2(y,x) sin(θ) = β cos(θ) = α θ = a tan 2(β,α) c 2 = a 2 + b 2 2abcos(ab) a c Τριγωνομετρικές ταυτότητες cos(x ± y) = cos(x)cos(y) sin(x)sin(y) b sin(x ± y) = sin(x)cos(y) ± sin(y)cos(x)

10 Υπολογισμός της θέσης του καρπού όταν ξέρουμε την θέση και τον προσανατολισμό του άκρου Δεδομένα: Θέλουμε το άκρο του ρομπότ να έχει Σημείο καρπού συντεταγμένες Χ d, Υ d και να σχηματίζει γωνία φ d ως (wrist point) προς τον άξονα x. x w,y w Ζητούμενο: οι γωνίες των αρθρώσεων q 1, q 2, q 3 y θ 2 θ 3 l 1 l 2 l 3 φ d x d, y d Από τις γωνίες των αρθρώσεων και τον επιθυμητό προσανατολισμό μπορώ να γράψω ότι φ d =θ 1 +θ 2 +θ 3 θ 1 x Oι συντεταγμένες του καρπού x w, y w μπορούν να υπολογιστούν εύκολα από τις παρακάτω σχέσεις: x d x w = l 3 cos(φ d ) x w = x d l 3 cos(φ d ) y d y w = l 3 sin(φ d ) y w = y d l 3 sin(φ d ) Επίσης, οι συντεταγμένες του καρπού x w, y w μπορούν να υπολογιστούν εύκολα καιόπως παρακάτω: x w = l 1 c 1 + l 2 c 12 Δρ. Φασουλάς Γιάννης y w = l 1 s 1 + l 2 s 12

11 Αντίστροφη κινηματική λύση του επίπεδου βραχίονα με 3 περιστροφικές αρθρώσεις Λύση: Αν τετραγωνίσουμε τις δύο τελευταίες εξισώσεις και τις προσθέσουμε, έχουμε x w = l 1 c 1 + l 2 c 12 y w = l 1 s 1 + l 2 s 12 φ d = θ 1 +θ 2 +θ 3 x 2 w + y 2 w = l l l 1 l 2 c c 2 2 = x 2 + y 2 w w l l 2 2l 1 l 2 1 c 2 1 Αν αυτό δεν ισχύει τότε ο στόχος μας δεν μπορεί να επιτευχθεί. Μπορούμε να γράψουμε s 2 = ± 1 c 2 2 θ 2 = Atan2(s 2,c 2 ) Το διπλό πρόσημο για την λύση του αντιστοιχεί στην πολλαπλή λύση με τον αγκώνα του βραχίονα πάνω ή κάτω. Δρ. Φασουλάς Γιάννης

12 λύση συνέχεια: Επίσης β = Atan2(y w,x w ) y θ 2 Σημείο καρπού x w,y w L 3 θ 3 L 2 Παρατηρήστε ότι θ 1 = β ±ψ L 1 ψ θ 2 Για τον υπολογισμό της ψ από το νόμο των συνημίτονων στο γραμμοσκιασμένο τρίγωνο θ 1 β θ 1 x c ψ = x 2 + y 2 w w + l l 2 2l 1 x 2 2 w + y w s ψ = ± 1 c ψ 2 ψ = Atan2(s ψ,c ψ ) όπου η λύση δίνεται για τιμές της ψ από 0 έως 180 ώστε να διατηρηθεί η γεωμετρία του σχήματος Επομένως θ 3 = φ d θ 1 θ 2 Δρ. Φασουλάς Γιάννης

13 ΠΑΡΑΔΕΙΓΜΑ Έστωοπαρακάτωεπίπεδοςβραχίοναςδύοπεριστροφικώναρθρώσεωνθ 1,θ 2 μεμήκη συνδέσμωνl 1 =L 2 (δενυπάρχουνπεριορισμοίστιςαρθρώσειςτουρομπότδηλαδή,τα θ 1,θ 2 μπορούνναπάρουνοποιαδήποτετιμή). (x e,y e ) α)πόσεςλύσειςυπάρχουνγιατοαντίστροφοκινηματικόπρόβλημαγιατηνδεδομένη θέση(x e =0,y e =L 1 );.Σχεδιάστετοβραχίονασεαυτέςτιςλύσεις. β) Υπάρχει κάποιο σημείο (x,y) του χώρου εργασίας για το οποίο υπάρχουν άπειρες λύσειςγιατοαντίστροφοκινηματικόπρόβλημα;ανναι,ναυποδείξετεαυτήτηνθέση. γ) Υπάρχει κάποιο σημείο (x,y) του χώρου εργασίας για το οποίο υπάρχει μοναδική λύσηγιατοαντίστροφοκινηματικόπρόβλημα?ανναι,ναυποδείξετεαυτήτηνθέση.αν νομίζεται ότι υπάρχουν περισσότερα από ένα σημεία σχεδιάστε τον γεωμετρικό τόπο τωνσημείωναυτών. δ) Σχεδιάστε το χώρο εργασίας του βραχίονα στην περίπτωση που L 2 = L 1 /2 και απαντήστεξανάταερώτηματαβκαιγ.

14 Ιακωβιανή Ρομποτικού Βραχίονα

15 Αντίστροφη Ιακωβιανή και ταχύτητες αρθρώσεων

16 Σχέση Ιακωβιανής, δυνάμεων και ροπών

17 Ιακωβιανή Ρομποτικού Βραχίονα 3. β.ε.