�ݺ�ߣ

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

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

Ακαδημαϊκό Έτος: 2012-2013
Συμμετέχοντες:
 Μπάρτζης Χρήστος (AM: 7321)
 Παπαδόπουλος Μάριος-Γεώργιος (ΑΜ: 7356)

Μπάρτζης Χρήστος(AM:7321) – ece7321@upnet.gr
Παπαδόπουλος Γεώργιος-Μάριος (ΑΜ:7356) - ece7356@upnet.gr


Πορεία εργασίας
Κρίνεται σκόπιμο επειδή έγινε αρκετή εργασία πάνω στα ερωτήματα του 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 παράμετροι είναι:
Σύνδεσμος
1
2

theta
θ1
θ2

d
0
0

a
0.6
0.3

alpha
90o
0o

Ερώτημα Α1 (Α1.m):
Στο ερώτημα αυτό μας ζητείται να βρούμε τις δυναμικές εξισώσεις της κίνησης του
ρομποτικού βραχίονα. Η μέθοδος που χρησιμποιείται είναι αυτής της Lagrange-Euler καθώς
τα δεδομένα της ασκήσεως είναι ιδανικά αλλά και ο υπολογισμός της στη Matlab είναι
αρκετά εύκολος.
Για τον 1ο σύνδεσμο έχω:
 2
 2 321
 2
d d
d d
9
9
9
9
cos(22 ) 21  cos(2 ) 21 
cos(2 ) 21  sin(2 ) 1 2 
sin(22 ) 2 1
400
t
25
t
400
t
25
dt dt 200
dt dt


2943
9
9
3  2 2
 d 
 d 
cos( 2 ) 
sin(2 2 )  1   sin( 2 )  1  
2
1000
400
 dt  50
 dt  50 t
2

2

Επεξήγηση:
Για την εύρεση των δυναμικών εξισώσεων Lagrange αρχικά βρίσκουμε τις συντεταγμέςνες
των κέντρων μάζας κάθε συνδέσμου ως προς το αρχικό σύστημα συντεταγμένων. Στη
συνέχεια, βρίσκουμε την γραμμική ταχύτητα και την γωνιακή επιτάχυνση κάθε κέντρου
μάζας (με χρήση Symbolic Toolbox). Μετά προσδιορίζουμε την κινητική ενέργεια κάθε
συνδέσμου και το άθροισμα τους. Επαναλαμβάνουμε ακριβώς την ίδια διαδικασία για τις
δυναμικές ενέργειες των δύο συνδέσμων. Δημιουργούμε το L=K1+K2-P1-P2 και μετά
παραγωγίζουμε για να βρούμε τις απαραίτητες μερικές παραγώγους για την Lagrange
dL
e.o.m. Αρχικά φτιάχνουμε την
, έπειτα την παραγωγίζουμε ως προς τον χρόνο και
d 1
dL
d  dL 
έχουμε φτιάξει το
.Η

 . Επίσης, φροντίζουμε να δημιουργήσου την παράγωγο
d 1
dt  d 1 
διαφορά των δυο τελευταίων παραγώγων μας δίνει την πρώτη e.o.m, ακολουθώντας την
ίδια διαδικασία για το θ2 έχουμε και την δεύτερη εξίσωση κίνησης. Σε περίπτωση που μας
ενδιαφέρει να το υλοποιήσουμε με Robotics Toolbox, δεν έχουμε παρά να φορτώσουμε το
robot μας (mdl_rr) και μετά να καλέσουμε την εντολή rne (πρέπει να του δώσουμε και το
θ1,θ2 για το οποίο θα μας υπολογίσει την δυναμική εξίσωση κίνησης).



Ερώτημα Α2 (Α2.m):
Στο δεύτερο ερώτημα μας ζητείται να βρούμε την Ιακωβιανή μήτρα. Για να υπολογιστεί η
Ιακωβιανή μήτρα, αυτό που απαιτείται είναι τα μοναδιαία διανύσματα Zi, και οι
συντεταγμένες των κεντρών Oi. Ένας εύκολος προσδιορισμός τους γίνεται αν θυμηθούμε
ότι οι συντεταγμένες των Zi σε σχέση με το αρχικό σύστημα συντεταγμένων υπολογίζονται
από τα τρεία πρώτα στοιχεία της τρίτης στήλης του πίνακας Ti 0 ενώ το Oi δίνεται από τρία
πρώτα στοιχεία του πίνακα Ti 0 . Με αυτόν τον τρόπο, αν έχουμε την τρίτη και την τέταρτη
στήλη του Ti 0 μπορούμε σχετικά εύκολα να υπολογίσουμε την Ιακωβιανή μήτρα.
c
J (q)   1
 z0

c1  z0  (o2  o0 )
c2 
, και
 με
z1 
c2  z1  (o2  o1 )

 c1 0 s1 l1c1 
 c1c 2



0
 s1 0 c1 l1s1  , T 0   s1c 2
T1 
2
 0 1
 s 2
0
0 



0
1 
 0 0
 0
Οπότε η Ιακωβιανή μήτρα είναι:

c1s 2
 s1s 2
c 2
0

s1 l1c1  l2c1c 2 

c1 l1s1  l2c 2 s1 

0
l2 s 2

0
1


0.3cθ1sθ 2
-0.3cθ1sθ 2




0.3sθ1sθ 2
-0.3sθ1sθ 2


 0.6cθ1cθ1 + 0.3cθ1cθ 2 + 0.6sθ1sθ1 + 0.3cθ 2sθ1 0.3cθ 2 cθ12  0.3cθ 2 sθ12 
J 

sθ1
sθ1




-cθ1
-cθ1




0
0



Ερώτημα Α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 A2 να εξάγουμε τις γωνίες για τις
αντίστοιχες χρονικές στιγμές. Αυτό γίνεται παρατηρώντας το πρώτο στοιχείο της τρίτης
στήλης και το τρίτο στοιχείο της πρώτης στήλης.
Από αυτά χρησιμοποιώντας τη σχέση αντίστροφου ημιτόνου έχουμε:
1  sin 1 ( x)

 2  sin 1 ( 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 ) είναι:
 3 t 5 15 t 4 5 t 3
 32  32  8 , 0  t  2s

1 (t )  


, 2  t  5s


2

3 t 5 15 t 4 5 t 3


, 0  t  2s


32
32
8
και  2 (t )  
5
4
3
2
 2 t  35 t  230 t  700 t  1000 t  1007 , 2  t  5s
 81
81
81
81
81
162


Το παραπάνο ερώτημα προσομοιώθηκε και με συναρτήσεις που παρέχει το 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 εξίσωσης,

d  K  K
d  K  K
και 
.



dt  1  1
dt   2   2

Στη συνέχεια ορίζουμε τις δυνάμεις τις βαρύτητας ως προς κάθε σύνδεσμο (προσέχοντας η
φορά των διανυσμάτων να συμβαδίζει με τον τρόπο που έχουμε ορίσει τα συστήματα
συντεταγμένων) και θεωρούμε σε πρώτο χρόνο γενικά τις ροπές ως μεταβλητές (με χρήση
Symbolic Toolbox-προσέχοντας και εκεί τις φορές τους). Στη συνέχεια, υπολογίζουμε τις
γενικευμένες δυνάμεις Q1 , Q2 και στην ουσία έχουμε δημιουργήσει δυο Lagrange εξισώσεις
αφού βρήκαμε και το δεξί μέλος της εξίσωσης. Αυτές είναι οι:
d  K  K
d  K  K
 Q2
 Q1 και




dt   2   2
dt  1  1

Αφότου έχουμε βρει τις εξισώσεις σε γενική μορφή και με χρήση μεταβλητών, περνάμε στις
μεταβλητές αριθμητικές τιμές και στις ροπές τις δοθέντες από το ερώτημα εξισώσεις. Αυτά
γίνονται με την εντολή subs του Symbolic Toolbox, καθώς επίσης για τις ροπές
χρησιμοποιήθηκε η εντολή Heaviside (για τη βηματική απόκριση) και η dirac για την
κρουστική. Το μόνο που μας μένει είναι να λύσουμε την εξίσωση ως προς 1 (t ) και  2 (t )
και να απεικονίσουμε τα αποτελέσματα. Για να επιλύσουμε την παραπάνω εξίσωση
φρντίζουμε να δημιουργήσουμε τις κατάλληλες συνθήκες για να χρησιμοποιήσουμε την
ode45. Όταν την χρησιμοποιούμε για χρονικό διάστημα τριών δευτερολέπτων, βάζουμε τα
αποτελέσματα σε ένα πίνακα και στη συνέχεια τα απεικονίζουμε. Το αποτέλεσμα φαίνεται
d1
d 2
στο παρακάτω διάγραμμα (φροντίσαμε να απεικονίσουμε και τις dt και dt ώστε να
έχουμε πιο καλή γνώση των αποτελεσμάτων και καλύτερα συμπεράσματα).
Το συμπέρασμα το οποίο προκύπτει είναι ότι το robot στην μεν 1 (t ) , πηγαίνει από το
 (t ) κάνει ταλαντώσεις με πλάτος 100ο(αύξουσες).
μηδέν στις 300ο ενώ στην δε 2


Δοκιμάσαμε να δούμε πως είναι το robot στην πράξη με χρήση εντολών του Robotic
Toolbox (fdyn), αυτό που παρατηρήθηκε είναι ότι το robot, έκανε πολύ γρήγορες και
απότομες κινήσεις. Υποθέτουμε πως στην πράξη θα είχε διαλυθεί καθώς οι κινητήρες δεν
θα άντεχαν τόσο γρήγορες και σπασμωδικές κινήσεις σε πολύ μικρό χρονικό διάστημα.

Ερώτημα Β1 (Β1.m):
Στο ερώτημα αυτό μας ζητείται να βρούμε τις δυναμικές εξισώσεις κίνησης με την
προϋπόθεση ότι έχουμε τοποθετήσει στην άκρη του ρομποτικού βραχίονα μια μάζα 1kg.
 2 18
 2 483  21 18
d d
d d
27
27
cos(22 ) 21  cos(2 ) 21 
 sin(2 ) 1 2 
sin(22 ) 2 1
2
400
t
25
t
400 t
25
dt dt 200
dt dt


2943
27
9
3  2 2
 d 
 d 
cos( 2 ) 
sin(2 2 )  1   sin( 2 )  1  
2
500
400
 dt  25
 dt  16 t
2

2



Η διαδικασία εύρεσης της δυναμικής εξίσωσης είναι ακριβώς ίδια με το ερώτημα A1 με την
μόνη διαφορά ότι για τη μάζα που βρίσκεται στον End-Effector βρίσκουμε τις
συντεταγμένες της ως προς το αρχικό σύστημα συντεταγμένων και με βάση αυτές
υπολογίζουμε την κινητική και την δυναμική της ενέργεια - δουλεύουμε με την μέθοδο
Lagrange. Δημιουργούμε το L=K1+K2+Κ3-P1-P2-P3 και μετά παραγωγίζουμε για να βρούμε τις
dL
απαραίτητες μερικές παραγώγους για την Lagrange e.o.m. Αρχικά φτιάχνουμε την
,
d 1
d  dL 
έπειτα την παραγωγίζουμε ως προς τον χρόνο και έχουμε φτιάξει το

 . Επίσης,
dt  d 1 

dL
. Η διαφορά των δυο τελευταίων
d 1
παραγώγων μας δίνει την πρώτη 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 μπορούμε να το βρούμε από το τρίτο στοιχείο καθώς είναι η μόνη άγνωστη

x
) ,x = γνωστή τιμή από τους δοθέντες
0.3
πίνακες. Aφου έχω προσδιορίσει το  2 μπορώ από το 1ο ή το 2ο στοιχείο της 4ης στήλης να
παράμετρος για αυτό. Οπότε έχω  2  sin 1 (

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



z
cos 2  y   ό οπότε λύνοντας ως προς 1 θα έχω 1  cos 1 
.
 0.6  0.3 y 



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

1
2

0s
0o

2s
0o

4s
0o

6s
60o

8s
60o

0o

30o

30o

45o

45o

Στη συνέχεια, όπως και στο ερώτημα Α3 χρησιμοποιούμε cubic polynomials 5ου βαθμού για
να βρούμε την τροχειά (trajectory planning) δηλαδή τα 1 (t ) και  2 (t ) . Ο λόγος που
χρησιμοποιούμε πολυώνυμα 5ου βαθμού και όχι 3ου είναι για να αποφύγουμε ασυνέχειες
στα διαγράμματα της ροπής στα επόμενα ερωτήματα (θεωρούμε δηλαδή ότι οι
επιταχύνσεις μόλις πριν φτάσει η φύγει από κάθε δοσμένη θέση είναι μηδέν.
Εφαρμόζωντας τον παραπάνω συλλογισμό, έχουμε τα παρακάτω διαγράμματα το μεν
πρώτο για την 1 , το δε δεύτερο για τη  2 :



Το παραπάνω ερώτημα προσομοιώθηκε και με συναρτήσεις που παρέχει το 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 με τη μάζα στην αρπάγη).
Εφαρμόζοντας τις ευρεθείσες τροχιές του προηγούμενου ερωτήματος στον κατάλληλο τύπο
υπολογισμού της ροπής κάθε φορά (για t  4 σε αυτόν του Α1 και για t 4 σε αυτόν του Β1)


έχουμε τα παρακάτω διαγράμματα. Τα συμπεράσματα που μπορεί κάποιος να εξάγει είναι
ότι όταν η θ είναι σταθερή τότε η ροπή είναι μηδέν (δεν μας προκαλεί εντύπωση, είναι
απόλυτα φυσιολογικό και προσδοκώμενο). Επίσης, στο δεύτερο διάγραμμα βλέπουμε ότι
υπάρχει ασυνέχεια την χρονική στιγμή t=4s. Αυτό συμβαίνει γιατί εκείνη τη χρονική στιγμή
αλλάζει ο τρόπος υπολογισμού της ροπής αφού προστίθεται η μάζα ενός κιλού, οπότε
αλλάζει η δυναμική εξίσωση υπολογισμού των ροπών.

Αν προσπαθούσαμε να υπολογίσουμε τη ροπή με βάση τις εντολές που υπάρχουν στο
Robotics Toolbox δεν έχουμε παρά να φορτώσουμε το robot μας (mdl_rrp) και μετά να
καλέσουμε την εντολή rne βάζοντας και την χρονική στιγμή που επιθυμούμε να
υπολογίσουμε τις ροπές.

Ερώτημα Β3 :
Στα επόμενα δυο ερωτήματα
controller :

έχουμε να σχεδιάσουμε έναν feedforward cancellation

  D( d ) d  h( d , d )  C( d )  Kp( d   )  Kv( d   )
με δοθέντες τις τιμές για τα Κp και Kv και να απεικονίσουμε τις τιμές για τα 1 (t ) και  2 (t ) .
Οι πρώτοι τρεις όροι αποτελούν παράγοντα feedforward ενώ οι δυο τελευταίοι είναι όροι
αποτελεί feedback (position και velocity gain) όρο. Για να σχεδιάσουμε τον ελεγκτή μας



μπορούμε να θεωρήσουμε ότι ο feedforward όρος γραμμικοποιεί την μη-γραμμική
δυναμική εξίσωση γύρω από το λειτουργικό σημείο ( d , d , d ) . Aν θεωρήσουμε ότι η
γραμμικοποίηση μας είναι ιδανική τότε η δυναμική του σφάλματος e   d   δίνεται από
την ( d )e  Kv e  K p e  0 . 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

More Related Content

Dynamics equation of motion and Trajectory planning of a 2 DOF RR robotic arm