Αντίστροφη Κινηματική Πώς να τοποθετήσω το χέρι μου εδώ; Αντίστροφη Κινηματική: Επέλεξε αυτές τις γωνίες
ΥΠΑΡΧΕΙ ΛΥΣΗ; Vrml Inverse Kinema9cs - No solu9on Στόχος Για ένα στόχο έξω από τον χώρο εργασίας του ρομπότ δεν υπάρχει λύση Περιορισμοί στις αρθρώσεις => ΟΙ ΠΙΘΑΝΕΣ ΛΥΣΕΙΣ ΝΑ ΜΗΝ ΕΙΝΑΙ ΟΛΕΣ ΕΦΑΡΜΟΣΙΜΕΣ
ΥΠΑΡΧΕΙ ΜΙΑ ΜΟΝΟ ΛΥΣΗ; Goal Για ένα στόχο μέσα στον χώρο εργασίας του ρομπότ πιθανόν να υπάρχουν πάνω από μία λύσεις, ή ακόμα και να μην υπάρχουν λύσεις όπως π.χ. στην περίπτωση ύπαρξης ανυπέρβλητων εμποδίων Κανόνες επιλογής λύσης
Χαρακτηριστικοί χώροι του ρομπότ Δυνατός Χώρος 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
Η μαθηματική περιγραφή του Δυνατού Xώρου εργασίας για τον επίπεδο βραχίονα 3 ων βαθμών ελευθερίας γίνεται από τον Ο.Μ. του άκρου του βραχίονα y o y L 2 L 3 # g 0e = $ cφ sφ 0 x sφ cφ 0 y 0 0 1 0 0 0 0 1 & ( ( ( = ( '( L 1 Επίπεδος βραχίονας 3 β.ε. x φ x o g 0e = Rot(z,φ) x y 0 0 0 0 1 Ο. Μ. Άκρου βραχίονα
Πολλαπλές λύσεις Inverse Kinema9cs - Mul9ple solu9ons 1 Inverse Kinema9cs - Mul9ple solu9ons 2 Κανόνες επιλογής λύσης - Πλησιέστερης - Αποφυγή εμποδίων - Κίνηση μικρών αρθρώσεων Ο αριθμός των λύσεων εξαρτάται από: 1) Αριθμό αρθρώσεων 2) γεωμετρία των συνδέσμων 3) Έύρος κίνησης των αρθρώσεων
Αντίστροφο κινηματικό για επίπεδο πολικό βραχίονα
Να υλοποιηθεί πρόγραμμα που να εκτελεί τα παρακάτω για τον προηγούμενο βραχίονα: (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
Βοηθητικά μαθηματικά εργαλεία Η συνάρτηση 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)
Υπολογισμός της θέσης του καρπού όταν ξέρουμε την θέση και τον προσανατολισμό του άκρου Δεδομένα: Θέλουμε το άκρο του ρομπότ να έχει Σημείο καρπού συντεταγμένες Χ 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
Αντίστροφη κινηματική λύση του επίπεδου βραχίονα με 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 2 1 + l 2 2 + 2l 1 l 2 c c 2 2 = x 2 + y 2 w w l 2 2 1 l 2 2l 1 l 2 1 c 2 1 Αν αυτό δεν ισχύει τότε ο στόχος μας δεν μπορεί να επιτευχθεί. Μπορούμε να γράψουμε s 2 = ± 1 c 2 2 θ 2 = Atan2(s 2,c 2 ) Το διπλό πρόσημο για την λύση του αντιστοιχεί στην πολλαπλή λύση με τον αγκώνα του βραχίονα πάνω ή κάτω. Δρ. Φασουλάς Γιάννης
λύση συνέχεια: Επίσης β = 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 2 2 1 l 2 2l 1 x 2 2 w + y w s ψ = ± 1 c ψ 2 ψ = Atan2(s ψ,c ψ ) όπου η λύση δίνεται για τιμές της ψ από 0 έως 180 ώστε να διατηρηθεί η γεωμετρία του σχήματος Επομένως θ 3 = φ d θ 1 θ 2 Δρ. Φασουλάς Γιάννης
ΠΑΡΑΔΕΙΓΜΑ Έστωοπαρακάτωεπίπεδοςβραχίοναςδύοπεριστροφικώναρθρώσεωνθ 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 και απαντήστεξανάταερώτηματαβκαιγ.
Ιακωβιανή Ρομποτικού Βραχίονα
Αντίστροφη Ιακωβιανή και ταχύτητες αρθρώσεων
Σχέση Ιακωβιανής, δυνάμεων και ροπών
Ιακωβιανή Ρομποτικού Βραχίονα 3. β.ε.