dynamics equation of motion and trajectory planning of a 2 dof rr robotic arm

Τομέας Συστημάτων Αυτομάτου Ελέγχου Τμήμα Ηλεκτρολόγων Μηχανικών και Τεχνολογίας Υπολογιστών Πολυτεχνική Σχολή Πανεπιστήμιο Πατρών

Μπάρτζης Χρήστος(AM:7321) – [email protected] Παπαδόπουλος Γεώργιος-Μάριος (ΑΜ:7356) - [email protected]

PROJECT

στο μάθημα

“Εισαγωγή στη Ρομποτική”

Ακαδημαϊκό Έτος: 2012-2013

Συμμετέχοντες:

Μπάρτζης Χρήστος (AM: 7321)

Παπαδόπουλος Μάριος-Γεώργιος (ΑΜ: 7356)



Πορεία εργασίας

Κρίνεται σκόπιμο επειδή έγινε αρκετή εργασία πάνω στα ερωτήματα του project αλλά

και στις εργαστηριακές ασκήσεις να παρουαστεί η φιλοσοφία και ο τρόπος που

αντιμετωπίστηκαν με λίγα λόγια. Όσον αφορά το project, υλοποιήθηκε με δυο τρόπους

σχετικά με το προγραμματιστικό του κομμάτι. Κατά την 1η προσέγγιση δεν χρειάζεται να

ειπωθούν αρκετά καθώς παρουσιάζεται παρακάτω και περιλαμβάνει κατά κύριο λόγο

επίλυση εξισώσεων και απεικόνιση των αποτελεσμάτων. Όσον αφορά την 2η,

ακολουθήθηκε πιστά η μεθοδολογία του βιβλίου “Robotics Vision and Control” του Corke

και έγινε χρήση του Robotics Toolbox του Matlab. Σύμφωνα με αυτό, θεωρήσαμε το δικό

μας robot για το Α’ μέρος του project (mdl_rr) και το αντίστοιχο για το δεύτερο μέρος

(mdl_rrp). Με αυτόν τον τρόπο μπορέσαμε να επιβεβαιώσουμε τα αποτελέσματα που

είχαμε με βάση τη μαθηματική μοντελοποίηση και να δούμε γραφικά το robot αλλά και τις

κινήσεις του στα τελευταία ερωτήματα που ζητείται να φτιαχτεί ελεγκτής. Ωστόσο κρίνουμε

σκόπιμο να μην παρουσιάσουμε αναλυτικά τη μεθοδολογία σύμφωνα με το Robotics

Toolbox καθώς αν κάποιος ορίσει το robot μετά με τυποποιημένες εντολές μπορεί να

απαντήσει τα ερωτήματα. Θα καταφύγουμε ωστόσο σε αυτό, σε κάποια συγκεκριμένα

σημεία για να τεκμηριώσουμε τις απαντήσεις μας στα ερωτήματα που τέθηκαν.

Σύμφωνα με το Robotics Toolbox, αυτό που χρειάζεται να δώσουμε στο Matlab είναι οι D-H,

η μάζα των συνδέσμων, τα μήκη τους, ο πίνακας αδρανείας, η δύναμη της βαρύτητας και το

διάνυσμα του κέντρου βάρους κάθε συνδέσμου ως προς την αρχή του συστήματος

συντεταγμένων.

(θ1 = 0o, θ2 = 45o)



Όπως εύκολα διαπιστώνει κανείς, οι D-H παράμετροι είναι:

Σύνδεσμος theta d a alpha

1 θ1 0 0.6 90o

2 θ2 0 0.3 0o

Ερώτημα Α1 (Α1.m):

Στο ερώτημα αυτό μας ζητείται να βρούμε τις δυναμικές εξισώσεις της κίνησης του

ρομποτικού βραχίονα. Η μέθοδος που χρησιμποιείται είναι αυτής της Lagrange-Euler καθώς

τα δεδομένα της ασκήσεως είναι ιδανικά αλλά και ο υπολογισμός της στη Matlab είναι

αρκετά εύκολος.

Για τον 1ο σύνδεσμο έχω:

2 2 2

1 1 1 1 2 2 12 2 2 2 22 2 2

9 9 321 9 9cos(2 ) cos( ) cos( ) sin( ) sin(2 )

400 25 400 25 200

d d d d

t t t dt dt dt dt


2 2 2

1 1 22 2 2 2

2943 9 9 3cos( ) sin(2 ) sin( )

1000 400 50 50

d d

dt dt t

Επεξήγηση:

Για την εύρεση των δυναμικών εξισώσεων Lagrange αρχικά βρίσκουμε τις συντεταγμέςνες

των κέντρων μάζας κάθε συνδέσμου ως προς το αρχικό σύστημα συντεταγμένων. Στη

συνέχεια, βρίσκουμε την γραμμική ταχύτητα και την γωνιακή επιτάχυνση κάθε κέντρου

μάζας (με χρήση Symbolic Toolbox). Μετά προσδιορίζουμε την κινητική ενέργεια κάθε

συνδέσμου και το άθροισμα τους. Επαναλαμβάνουμε ακριβώς την ίδια διαδικασία για τις

δυναμικές ενέργειες των δύο συνδέσμων. Δημιουργούμε το L=K1+K2-P1-P2 και μετά

παραγωγίζουμε για να βρούμε τις απαραίτητες μερικές παραγώγους για την Lagrange

e.o.m. Αρχικά φτιάχνουμε την 1

dL

d , έπειτα την παραγωγίζουμε ως προς τον χρόνο και

έχουμε φτιάξει το 1

d dL

dt d

. Επίσης, φροντίζουμε να δημιουργήσου την παράγωγο 1

dL

d . Η

διαφορά των δυο τελευταίων παραγώγων μας δίνει την πρώτη e.o.m, ακολουθώντας την

ίδια διαδικασία για το θ2 έχουμε και την δεύτερη εξίσωση κίνησης. Σε περίπτωση που μας

ενδιαφέρει να το υλοποιήσουμε με Robotics Toolbox, δεν έχουμε παρά να φορτώσουμε το

robot μας (mdl_rr) και μετά να καλέσουμε την εντολή rne (πρέπει να του δώσουμε και το

θ1,θ2 για το οποίο θα μας υπολογίσει την δυναμική εξίσωση κίνησης).



Ερώτημα Α2 (Α2.m):

Στο δεύτερο ερώτημα μας ζητείται να βρούμε την Ιακωβιανή μήτρα. Για να υπολογιστεί η

Ιακωβιανή μήτρα, αυτό που απαιτείται είναι τα μοναδιαία διανύσματα Zi, και οι

συντεταγμένες των κεντρών Oi. Ένας εύκολος προσδιορισμός τους γίνεται αν θυμηθούμε

ότι οι συντεταγμένες των Zi σε σχέση με το αρχικό σύστημα συντεταγμένων υπολογίζονται

από τα τρεία πρώτα στοιχεία της τρίτης στήλης του πίνακας 0

iT ενώ το Oi δίνεται από τρία

πρώτα στοιχεία του πίνακα 0

iT . Με αυτόν τον τρόπο, αν έχουμε την τρίτη και την τέταρτη

στήλη του 0

iT μπορούμε σχετικά εύκολα να υπολογίσουμε την Ιακωβιανή μήτρα.

1 2

0 1

( )c c

J qz z

με 1 0 2 0

2 1 2 1

( )

( )

c z o o

c z o o

, και

1 1 1 1

1 1 1 10

1

0

0

0 1 0 0

0 0 0 1

c s l c

s c l sT

,

1 2 1 2 1 1 1 2 1 2

1 2 1 2 1 1 1 2 2 10

2

2 2 2 20

0 0 0 1

c c c s s l c l c c

s c s s c l s l c sT

s c l s

Οπότε η Ιακωβιανή μήτρα είναι:

1 2 1 2

1 2 1 2

1 1 1 2 1

2

1 2 1 2 1 2 1

1 1

1 1

2

0.3cθ sθ -0.3cθ sθ

0.3sθ sθ -0.3sθ sθ

0.6cθ cθ + 0.3cθ cθ + 0.6sθ sθ + 0.3cθ sθ θ θ θ θ

sθ sθ

-cθ -cθ

0 0

0.3 0.3J

c c c s

Ερώτημα Α3 (A3a.m και Α3b.m): Στο ερώτημα αυτό μας ζητείται να βρούμε τo 1( )t και το 2 ( )t από 0 έως 5 δευτερόλεπτα

αν γνωρίζουμε τη θέση του robot τις χρονικές στιγμές 0,2,5. Αυτό που πρέπει να κάνουμε

για να επιλύσουμε αυτό το ερώτημα είναι να υπολογίσουμε τα 1 και 2 τις παραπάνω

χρονικές στιγμές με αντίστροφη κινηματική και μετά με χρήση cubic polynomials να βρούμε

τα 1( )t και 2 ( )t . Αρχικά πρέπει από τους πίνακες 0

2A να εξάγουμε τις γωνίες για τις

αντίστοιχες χρονικές στιγμές. Αυτό γίνεται παρατηρώντας το πρώτο στοιχείο της τρίτης στήλης και το τρίτο στοιχείο της πρώτης στήλης. Από αυτά χρησιμοποιώντας τη σχέση αντίστροφου ημιτόνου έχουμε:

1

1

1

2

sin ( )

sin ( )

x

x



όπου x = γνωστή τιμή από τους δοθέντες πίνακες. Οπότε για t=0s έχω 1 = 2 = 0o, για t=2s

έχω 1 = 2 =90o και για t=5s έχω 1 =90o , 2 = 270o.

Αφότου έχουμε βρει τις γωνίες για τις συγκεκριμένες χρονικές στιγμές, μένει να φτιάξουμε τη συνάρτηση που αναπαριστά τη σχέση τους με το χρόνο. Αυτό γίνεται με cubic polynomials πέμπτης τάξεως. Χρησιμοποιήθαν 5ου βαθμού, θεωρώντας ταχύτητα και επιτάχυνση ίση με το μηδέν για να αποφύγω σε μελλοντικά ερωτήματα ασυνέχειες (στις ροπές ως προς δοσμένο trajectory planning).

Για την γωνία 1 το διάγραμμα από τη χρονική στιγμή μηδέν μέχρι το πέντε δίνεται

παρακάτω:



Και για την γωνία 2 :

Στα αρχεία μας στο Matlab στο μεν A3a.m είναι για τη γωνία 1 ενώ το δε Α3b.m είναι για

τη γωνία 2 .

Οι εξισώσεις για τις 1( )t και 2 ( )t είναι:

5 4 3

1

3 15 5,0 2

32 32 8( )

,2 52

t t tt s

t

t s

και

5 4 3

2 5 4 3 2

3 15 5,0 2

32 32 8( )

2 35 230 700 1000 1007,2 5

81 81 81 81 81 162

t t tt s

tt t t t t

t s

Το παραπάνο ερώτημα προσομοιώθηκε και με συναρτήσεις που παρέχει το robotics

toolbox και διαπιστώθηκε ότι το ρομπότ ακολουθούσε πιστά τα 1( )t και 2 ( )t . Η



συνάρτηση που χρησιμοποιήθηκε για τα cubic polynomials είναι η jtraj αν και δοκιμάστηκαν

και άλλες με μεγαλύτερη ακρίβεια.

Ερώτημα Α4 (A4.m):

Στο ερώτημα αυτό μας ζητείται μέσω των γωνιών του προηγούμενου ερωτήματος να

βρούμε τις επιθυμητές ροπές που ασκούνται. Αυτό μπορεί να πραγματοποιηθεί αν βρούμε

τις απαιτούμενες παραγώγους της 1( )t και τις 2 ( )t όπως ορίζει η εξίσωση των ροπών στο

Α1 ερώτημα. Χρησιμοποιώντας το Symbolic Toolbox του Matlab κάνουμε τις αντίστοιχες

παραγωγίσεις (ορίζουμε όπως και στο Α1 ερώτημα δικιά μας συνάρτηση deriv.m, η οποία

μπορεί και υλοποιεί παραγωγίσεις οι οποίες δεν μπορούν να γίνουν με την diff) για τις

1( )t και τις 2 ( )t και στη συνέχεια αντικαθιστούμε.

Τα διαγράμματα των ροπών βάση του χρόνου είναι τα παρακάτω (το 1ο για την 1( )t και το

2ο για την 2 ( )t ):

Αυτο που μπορεί να παρατηρήσει κάποιος από τα διαγράμματα είναι ότι στο πρώτο της

1( )t , ότι μετά την χρονική στιγμή t=2s κατά το οποίο η γωνία παραμένει σταθερή η ροπή

παραμένει ίση με το μηδέν. Αυτό είναι εντελώς φυσικό αφού δεν έχω μεταβολή στο θ,

οπότε δεν έχω και γωνιακή ταχύτητα για τον 1ο σύνδεσμο, άρα ούτε και ροπή (κίνηση μόνο

για τον 2ο σύνδεσμο-άρα και μεταβολή της ροπής για τον 2ο σύνδεσμο).



Ερώτημα Α5 (A5.m):

Στο ερώτημα αυτό δίνονται τα τ1(t), τ2(t) ίσα με μια δοθείσα γραφική. Μας ζητείται να

βρούμε τις γωνίες 1( )t και 2 ( )t έχοντας ως αρχικές συνθήκες ότι θ1(0)=θ2(0)=0 και 1(0) =

2 (0) =0. Αυτό που στην ουσία ζητείται δηλαδή είναι το ευθύ δυναμικό μοντέλο και η

προσομοίωση του για δοθείσες ροπές για χρονικό διάστημα τριών δευτερολέπτων.

Αυτό που κάναμε σε αυτό το ερώτημα ήταν να θέσουμε τους κάθε συνδέσμους με βάση το

κέντρο μάζας τους ως προς το αρχικό σύστημα συντεταγμένων και μετά να βρούμε την

κινητική ενέργεια των δύο αυτών μαζών (ο πρώτος σύνδεσμος έχει μόνο γωνιακή ταχύτητα

ενώ ο δεύτερος έχει γωνιακή και γραμμική, οπότε και οι κινητικές ενέργειες θα έχουν

αντίστοιχα ίσους όρους). Στη συνέχεια με παραγωγίσεις βρήκαμε το αριστερό μέλος της

Lagrange εξίσωσης, 1 1

d K K

dt

και

2 2

d K K

dt

.

Στη συνέχεια ορίζουμε τις δυνάμεις τις βαρύτητας ως προς κάθε σύνδεσμο (προσέχοντας η

φορά των διανυσμάτων να συμβαδίζει με τον τρόπο που έχουμε ορίσει τα συστήματα

συντεταγμένων) και θεωρούμε σε πρώτο χρόνο γενικά τις ροπές ως μεταβλητές (με χρήση

Symbolic Toolbox-προσέχοντας και εκεί τις φορές τους). Στη συνέχεια, υπολογίζουμε τις

γενικευμένες δυνάμεις 1 2,Q Q και στην ουσία έχουμε δημιουργήσει δυο Lagrange εξισώσεις

αφού βρήκαμε και το δεξί μέλος της εξίσωσης. Αυτές είναι οι:

1

1 1

d K KQ

dt

και

2

2 2

d K KQ

dt

Αφότου έχουμε βρει τις εξισώσεις σε γενική μορφή και με χρήση μεταβλητών, περνάμε στις

μεταβλητές αριθμητικές τιμές και στις ροπές τις δοθέντες από το ερώτημα εξισώσεις. Αυτά

γίνονται με την εντολή subs του Symbolic Toolbox, καθώς επίσης για τις ροπές

χρησιμοποιήθηκε η εντολή Heaviside (για τη βηματική απόκριση) και η dirac για την

κρουστική. Το μόνο που μας μένει είναι να λύσουμε την εξίσωση ως προς 1( )t και 2 ( )t

και να απεικονίσουμε τα αποτελέσματα. Για να επιλύσουμε την παραπάνω εξίσωση

φρντίζουμε να δημιουργήσουμε τις κατάλληλες συνθήκες για να χρησιμοποιήσουμε την

ode45. Όταν την χρησιμοποιούμε για χρονικό διάστημα τριών δευτερολέπτων, βάζουμε τα

αποτελέσματα σε ένα πίνακα και στη συνέχεια τα απεικονίζουμε. Το αποτέλεσμα φαίνεται

στο παρακάτω διάγραμμα (φροντίσαμε να απεικονίσουμε και τις 1d

dt

και 2d

dt

ώστε να

έχουμε πιο καλή γνώση των αποτελεσμάτων και καλύτερα συμπεράσματα).

Το συμπέρασμα το οποίο προκύπτει είναι ότι το robot στην μεν 1( )t , πηγαίνει από το

μηδέν στις 300ο ενώ στην δε 2 ( )t κάνει ταλαντώσεις με πλάτος 100ο(αύξουσες).



Δοκιμάσαμε να δούμε πως είναι το robot στην πράξη με χρήση εντολών του Robotic

Toolbox (fdyn), αυτό που παρατηρήθηκε είναι ότι το robot, έκανε πολύ γρήγορες και

απότομες κινήσεις. Υποθέτουμε πως στην πράξη θα είχε διαλυθεί καθώς οι κινητήρες δεν

θα άντεχαν τόσο γρήγορες και σπασμωδικές κινήσεις σε πολύ μικρό χρονικό διάστημα.

Ερώτημα Β1 (Β1.m): Στο ερώτημα αυτό μας ζητείται να βρούμε τις δυναμικές εξισώσεις κίνησης με την προϋπόθεση ότι έχουμε τοποθετήσει στην άκρη του ρομποτικού βραχίονα μια μάζα 1kg. Για τον 1ο σύνδεσμο έχω:

2 2 2

1 1 1 1 2 2 12 2 2 22 2 2

27 18 483 18 27cos(2 ) cos( ) sin( ) sin(2 )

400 25 400 25 200

d d d d

t t t dt dt dt dt


2 2 2

1 1 22 2 2 2

2943 27 9 3cos( ) sin(2 ) sin( )

500 400 25 16

d d

dt dt t



Η διαδικασία εύρεσης της δυναμικής εξίσωσης είναι ακριβώς ίδια με το ερώτημα A1 με την

μόνη διαφορά ότι για τη μάζα που βρίσκεται στον End-Effector βρίσκουμε τις

συντεταγμένες της ως προς το αρχικό σύστημα συντεταγμένων και με βάση αυτές

υπολογίζουμε την κινητική και την δυναμική της ενέργεια - δουλεύουμε με την μέθοδο

Lagrange. Δημιουργούμε το L=K1+K2+Κ3-P1-P2-P3 και μετά παραγωγίζουμε για να βρούμε τις

απαραίτητες μερικές παραγώγους για την Lagrange e.o.m. Αρχικά φτιάχνουμε την 1

dL

d ,

έπειτα την παραγωγίζουμε ως προς τον χρόνο και έχουμε φτιάξει το 1

d dL

dt d

. Επίσης,

φροντίζουμε να δημιουργήσου την παράγωγο 1

dL

d . Η διαφορά των δυο τελευταίων

παραγώγων μας δίνει την πρώτη e.o.m, ακολουθώντας την ίδια διαδικασία για το θ2 έχουμε

και την δεύτερη εξίσωση κίνησης. Σε περίπτωση που μας ενδιαφέρει να το υλοποιήσουμε

με Robotics Toolbox, δεν έχουμε παρά να φορτώσουμε το robot μας (mdl_rrp) και μετά να

καλέσουμε την εντολή rne (πρέπει να του δώσουμε και το θ1,θ2 για το οποίο θα μας

υπολογίσει την δυναμική εξίσωση κίνησης). Η μόνη διαφορά ώς προς το Α1, με τον δεύτερο

τρόπο υλοποίησης, είναι η προσθήκη της εντολής payload στο mdl_rr. Ωστόσο μας ζητείται

να δώσουμε πόσο ακριβώς απέχει από τον end-effector η μάζα, και εμείς δίνουμε την

μικρότερη δυνατή τιμή για να προσεγγίσουμε όσο πιο καλά γίνεται το ερώτημα.

Ερώτημα Β2α (Β2a1.m και B2a2.m):

Στο ερώτημα αυτό μας ζητείται να βρούμε τις συντεταγμένες των αρθρώσεων ως προς το

χρόνο, δεδομένων των θέσεων του end-effector σε συγκεκριμένες χρονικές στιγμές και

υποθέτοντας ότι στις στιγμές ακριβώς πριν και ακριβώς μετά από αυτές που έχουν δοθεί οι

γωνιακές ταχύτητες είναι μηδέν.

Αρχικά, πρέπει να βρούμε για τις συγκεκριμένες χρονικές στιγμές, την τιμή των γωνιών με

ανάστροφη κινηματική. Εφόσον δίνεται η θέση του end-effector σημαίνει ότι θα

ανατρέξουμε στην 4η στήλη του πίνακα 0

2 και στα τρία πρώτα στοιχεία της. Παρατηρούμε

ότι το 2 μπορούμε να το βρούμε από το τρίτο στοιχείο καθώς είναι η μόνη άγνωστη

παράμετρος για αυτό. Οπότε έχω 1

2 sin ( )0.3

x ,x = γνωστή τιμή από τους δοθέντες

πίνακες. Aφου έχω προσδιορίσει το 2 μπορώ από το 1ο ή το 2ο στοιχείο της 4ης στήλης να

προσδιορίσω το 1 . Εμείς για πχ το προσδιορίζουμε με βάση το 1ο. Έστω

2cos y ό οπότε λύνοντας ως προς 1 θα έχω 1

1 cos0.6 0.3

z

y

.



Εφαρμόζοντας τους παραπάνω τύπους στους δοθέντες πίνακες έχω:

Στη συνέχεια, όπως και στο ερώτημα Α3 χρησιμοποιούμε cubic polynomials 5ου βαθμού για

να βρούμε την τροχειά (trajectory planning) δηλαδή τα 1( )t και 2 ( )t . Ο λόγος που

χρησιμοποιούμε πολυώνυμα 5ου βαθμού και όχι 3ου είναι για να αποφύγουμε ασυνέχειες

στα διαγράμματα της ροπής στα επόμενα ερωτήματα (θεωρούμε δηλαδή ότι οι

επιταχύνσεις μόλις πριν φτάσει η φύγει από κάθε δοσμένη θέση είναι μηδέν.

Εφαρμόζωντας τον παραπάνω συλλογισμό, έχουμε τα παρακάτω διαγράμματα το μεν

πρώτο για την 1 , το δε δεύτερο για τη 2 :

t 0s 2s 4s 6s 8s

1 0o 0o 0o 60o 60o

2 0o 30o 30o 45o 45o



Το παραπάνω ερώτημα προσομοιώθηκε και με συναρτήσεις που παρέχει το robotics

toolbox και διαπιστώθηκε ότι το ρομπότ ακολουθούσε πιστά τα 1( )t και 2 ( )t . Η

συνάρτηση που χρησιμοποιήθηκε για τα cubic polynomials είναι η jtraj αν και δοκιμάστηκαν

και άλλες με μεγαλύτερη ακρίβεια.

Ερώτημα B2b (B2b.m):

Σε αυτό το ερώτημα ζητείται να υποθέσουμε ότι την χρονική στιγμή t=4+s τοποθετούμε μια

μάζα βάρους 1Kg στον end-effector. Με δεδομένες τις τροχιές των 1( )t και 2 ( )t από το

προηγούμενο ερώτημα μας ζητείται να υπολογίσουμε τις επιθυμητές ροπές 1( )t και 2 ( )t .

Η διαφορά με το ερώτημα Α4 , όσον αφορά τη μεθοδολογία, είναι ότι μετά τη χρονική

στιγμή t=4s το ρομπότ αλλάζει ανάστροφη δυναμική εξίσωση κίνησης. Δηλαδή, αυτό που

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

στιγμή t=4s αυτήν του ερωτήματος Α1 (δηλαδή του robot χωρίς τη μάζα στην αρπάγη), ενώ

μετά το πέρας της χρονικής στιγμής αυτή που υπολογίσαμε στο ερώτημα Β1 (δηλαδή του

robot με τη μάζα στην αρπάγη).

Εφαρμόζοντας τις ευρεθείσες τροχιές του προηγούμενου ερωτήματος στον κατάλληλο τύπο

υπολογισμού της ροπής κάθε φορά (για 4t σε αυτόν του Α1 και για 4t σε αυτόν του Β1)



έχουμε τα παρακάτω διαγράμματα. Τα συμπεράσματα που μπορεί κάποιος να εξάγει είναι

ότι όταν η θ είναι σταθερή τότε η ροπή είναι μηδέν (δεν μας προκαλεί εντύπωση, είναι

απόλυτα φυσιολογικό και προσδοκώμενο). Επίσης, στο δεύτερο διάγραμμα βλέπουμε ότι

υπάρχει ασυνέχεια την χρονική στιγμή t=4s. Αυτό συμβαίνει γιατί εκείνη τη χρονική στιγμή

αλλάζει ο τρόπος υπολογισμού της ροπής αφού προστίθεται η μάζα ενός κιλού, οπότε

αλλάζει η δυναμική εξίσωση υπολογισμού των ροπών.

Αν προσπαθούσαμε να υπολογίσουμε τη ροπή με βάση τις εντολές που υπάρχουν στο

Robotics Toolbox δεν έχουμε παρά να φορτώσουμε το robot μας (mdl_rrp) και μετά να

καλέσουμε την εντολή rne βάζοντας και την χρονική στιγμή που επιθυμούμε να

υπολογίσουμε τις ροπές.

Ερώτημα Β3 :

Στα επόμενα δυο ερωτήματα έχουμε να σχεδιάσουμε έναν feedforward cancellation

controller :

( ) ( , ) ( ) ( ) ( )d d d d d d dD h C Kp Kv

με δοθέντες τις τιμές για τα Κp και Kv και να απεικονίσουμε τις τιμές για τα 1( )t και 2 ( )t .

Οι πρώτοι τρεις όροι αποτελούν παράγοντα feedforward ενώ οι δυο τελευταίοι είναι όροι

αποτελεί feedback (position και velocity gain) όρο. Για να σχεδιάσουμε τον ελεγκτή μας



μπορούμε να θεωρήσουμε ότι ο feedforward όρος γραμμικοποιεί την μη-γραμμική

δυναμική εξίσωση γύρω από το λειτουργικό σημείο ( , , )d d d . Aν θεωρήσουμε ότι η

γραμμικοποίηση μας είναι ιδανική τότε η δυναμική του σφάλματος de δίνεται από

την ( ) 0d

v pe K e K e . To σχηματικό διάγραμμα που προκύπτει είναι το εξής:

Κατά την σχεδίαση υπήρξε το πρόβλημα υλοποίησης της βηματικής απόκρισης και πόσο

μάλλον της πρώτης και δεύτερης παραγώγου της, η οποία είναι η κρουστική. Για να

αποφύγουμε αυτό το πρόβλημα αρχικά υποθέσαμε ότι δεν δίνουμε τη θέση, ταχύτητα και

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

και για ταχύτητα και την επιτάχυνση. Με αυτό τον τρόπο αποφεύγαμε το σήμειο στο οποίο

λόγω της κρουστικής γινόταν άπειρο καθώς στο διακριτό χρόνο η dirac είχε πεπερασμένη

αριθμητική τιμή αλλά η προσέγγιση μας δεν ήταν στο συνεχές χρόνο όπως όριζε η άσκηση

και δη η γραφική που δινόταν. Για να προσπεράσουμε αυτό το πρόβλημα, καταφύγαμε σε

μια προσέγγιση που χρησιμοποιείται και στην πραγματικότητα. Με cubic polynomials

δίναμε την θέση του robot σε διάφορες χρονικές στιγμές και βρίσκαμε την τροχειά του (από

το 0+ στο 0.5- και από το 0.5+ έως το 5- για τη θ1 και από 0+ εως 1- και από 1+ εως 5- για το θ2

η τροχειά είναι σταθερή οπότε μας νοιάζει μόνο το σημείο μεταβολής στη βηματική

απόκριση). Για να αποφύγουμε το εμπόδιο που δημιουργούσε η βηματική απόκριση κατά

την απότομη μετάβαση της, θεωρήσαμε για απειροελάχιστο χρονικό διάστημα 0.0001 sec

ότι μεταβαίνει η γραφική στο επιθυμητό επίπεδο. Με τον τελευταίο τρόπο δημιουργήσαμε

μια τεχνητή βηματική απόκριση και υλοποιήσαμε τον ελεγκτή μας.

Για να εκτελέσει κάποιος την προσομοίωση μας πρέπει να φορτώσει το Robotics Toolbox

και μετά να καλέσει την εντολή sl_fforward. Στο Simulink αρχείο που θα εμφανιστεί πρέπει

να ορίσει συγκεκριμένες μεταβλητές και να φορτώσει τα κατάλληλα δυναμικά μοντέλα

(ορθό και ανάστροφο) των robot στις χρονικές στιγμές που πρέπει (το ένα είναι με μάζα

στην αρπάγη, το άλλο χωρίς) και να βάλει δυο scopes ένα στο qd και ένα στο q της εξόδου



ώστε να παρατηρεί τη διαφορά. Επίσης, πρέπει νωρίτερα να έχει βάλει τα σωστά κέρδη στα

Kp και Kv.

Στις προσομοιώσεις που έγιναν είχαμε τη δυνατότητα να δούμε το robot να κινείται. Στο

παρακάτω σχεδιάγραμμα φαίνεται η αρχική του θέση, η οποία και για τα δυο επόμενα

ερωτήματα είναι η ίδια.

Στο παρακάτω σχήμα φαίνεται το Simulink αρχείο που δημιουργήθηκε με μικρές

τροποποιήσεις να γίνονται κατά τη διάρκεια της προσομοίωσης με το ορθό και ανάστροφο

δυναμικό μοντέλο με και χωρίς τη μάζα.



Και στις δυο περιπτώσεις που ακολουθούν η τροχειά που πρέπει να ακολουθήσουν τα θ1

και θ2 είναι η παρακάτω (με τον τρόπο που προσομοιώθηκε ώστε να γίνεται απότομη η

βηματική).

Ερώτημα B3a :

Στο πρώτο σκέλος της ασκήσεως μας δίνεται ότι ο σχεδιαστής έχει γνώση του ότι κατά την

χρονική στιγμή t=4s προστίθεται μάζα ενός κιλού. Αυτό εμμέσως σημαίνει ότι, κατά το

χρονικό διάστημα από μηδέν εως τέσσερα δευτερόλεπτα, έχουμε το ορθό και ανάστροφο

δυναμικό μοντέλο χωρίς τη μάζα, ενώ για το επόμενο έχουμε αυτά με τη μάζα. Στην έξοδο

του συστήματος το robot φτάνει μέχρι το σημείο που φαίνεται στο σχήμα παρακάτω:



H έξοδος του για την δοθείσα είσοδο είναι η παρακάτω:

Παρατηρούμε ότι ο ελεγκτής προσπαθεί να φτάσει στο σημείο το οποίο του δίνεται ως

reference αλλά έχει ένα σφάλμα 10o περίπου για το θ1 και 40ο περίπου για το θ2. Επίσης μας

προκαλεί εντύπωση το γεγονός ότι μετά τη χρονική στιγμή δυο και ενώ το reference θ2 δεν

έχει αλλάξει το σφάλμα μεγαλώνει αντί για να μικραίνει μέχρι την χρονική στιγμή τέσσερα,

σε αντίθεση με αυτό του θ1 που διαρκώς μικραίνει. Επίσης, είναι άξιο αναφοράς το γενονός

ότι μετά τη χρονική στιγμή t=4s που αλλάζει το ορθό και ανάστροφο δυναμικό μοντέλο

(αφού προστίθεται η μάζα στην αρπάγη) το σφάλμα μικραίνει πιο γρήγορα από ότι πριν και

για τη θ1 και για τη θ2. Ωστόσο γενικά μπορούμε να πούμε ότι ο ελεγκτής παρακολουθεί το

reference σήμα παρότι υπάρχει σφάλμα και προσπαθεί να το φτάσει.

Ερώτημα B3b:

Η διαφορά με το προηγούμενο ερώτημα έγκειται στο ότι ενώ η μάζα προστίθεται στην

αρπάγη του robot τη χρονική στιγμή t=4s o σχεδιαστής δεν το γνωρίζει. Αυτό σημαίνει ότι

μέχρι τη χρονική στιγμή t=4s το ορθό και το ανάστροφο δυναμικό μοντέλο είναι το ίδιο, ενώ

μετά από αυτό το διάστημα ενώ το ανάστροφο δυναμικό μοντέλο αλλάζει και κανονικά

πρέπει να αλλάξει και το ορθό, ο σχεδιαστής το αφήνει το ίδιο. Οπότε, εφαρμόζωντας τις

παραπάνω υποθέσεις στο robot μας βλέπουμε ότι η τελική του θέση (επόμενη εικόνα) δεν

είναι η επιθυμώμενη.



Στο επόμενο διάγραμμα παρουσιάζεται η έξοδος του συστήματος μας.



Tα συμπεράσματα που προκύπτουν από το παραπάνω διάγραμμα διαφέρουν από το

προηγούμενο ερώτημα στο ότι μετά τη χρονική στιγμή t=4s, ο ελεγκτής χάνει το reference

όσον αφορά το θ2 και αυξάνεται το σφάλμα. Αυτό που μας προκαλεί εντύπωση είναι ότι για

τη θ1 το παρακολουθεί και το προσεγγίζει με παραπλήσια ακρίβεια με την περίπτωση όπου

το ορθό δυναμικό μοντέλο ήταν το σωστό. Ωστόσο, γενικά συμπεραίνουμε ότι ο ελεγκτής

μας δεν είναι καλός αφού το σφάλμα αυξάνει με την πάρωδο του χρόνου στην τελική τιμή.

Επίσης, δίνεται η επιθυμητή τελική τιμή του robot για να γίνουν πιο εύκολα κατανοητά τα

παραπάνω και να εξαχθούν πιο εύκολα συμπεράσματα για τον ελεγκτή και την απόδοση

του σε καθένα από τα δυο ερωτήματα.

Ερώτηση Bonus:

Σαν τελευταίο ερώτημα μας ζητείται να σχολιάσουμε τα Kp και Kv . Τα Kp και Kv είναι

τετραγωνικοί πίνακες κέρδους για την θέση και την ταχύτητα αντίστοιχα. Στην επιλογή του

Kp πρέπει να ικανοποιούνται κάποιες συνθήκες για να υπάρχει Unicity of the Equilibrium.

Επίσης, όσον αφορά την επιλογή των Kp και Kv πινάκων πρέπει να μπορέσει ο σχεδιαστής να

εγγυηθεί παγκόσμια ασυμπτωτική ομοιόμορφη ευστάθεια (global uniform asymptotic

stability) της αρχής του χώρου κατάστασης σχετικά με την εξίσωση κλειστού βρόγχου (για

περαιτέρω πληροφορίες: “Control of Robots Manipulators in Joint Space”, R. Kelly, V.

Santibanez and A. Lorνa , σελίδα 263-284).

dynamics equation of motion and trajectory planning of a 2 dof rr robotic arm

Technology