Force and Visual Servoing (PDF, 910Kb, 32 σελίδες) - Εθνικό ...

Σχολή Ηλεκτρολόγων Μηχ/κών και Μηχ/κών Υπολογιστών, 

Ε.Μ.Π., Ακαδημαϊκό Έτος 2010-11, 8ο Εξάμηνο 

Ρομποτική II 

Ευφυή και Επιδέξια Ρομποτικά Συστήματα 

Κων/νος Τζαφέστας 

Τομέας Σημάτων, Ελέγχου & Ρομποτικής 

Σχολή Ηλεκτρ. Μηχ/κών & Μηχ/κών Υπολ., Ε.Μ.Π. 

Τηλ.: 210 772-3687, (Κτήριο Ηλεκτρ., Γραφείο 21.11) 

Email: ktzaf@cs.ntua.gr 

Web: http://www.softlab.ntua.gr/~ktzaf/ 

Εθνικό Μετσόβιο Πολυτεχνείο, Σχολή ΗΜ&ΜΥ. Μάθημα: Ρομποτική ΙΙ (K.Τζαφέστας) 

Περιεχόμενα Μαθήματος 

• ΕΝΟΤΗΤΑ-1: Επιδέξιος Ρομποτικός Χειρισμός 

– Έλεγχος Ρομπότ με πλεονάζοντες β.ε. (redundant robots) 

– Έλεγχος «Αλληλεπίδρασης» με το περιβάλλον 

• Έλεγχος Δύναμης / Σύνθετης Μηχανικής Αντίστασης 

• Έλεγχος Οπτικής Οδήγησης 

– Μοντελοποίηση και έλεγχος επιδέξιας «ρομποτικής λαβής» 

(Συνεργαζόμενα ρομπότ, Ρομποτικά χέρια) 

• ΕΝΟΤΗΤΑ-2: Κινούμενα Ρομπότ 

– Αρχιτεκτονικές Ελέγχου Κινούμενων Ρομπότ 

– Σχεδιασμός δρόμου – Αποφυγή εμποδίων 

– Σύνθεση αισθητηρίων πληροφοριών 

• Σύνθετοι ρομποτικοί χειριστές - Εφαρμογές 


1 

2

Ε.Μ.Π., ΣΗΜΜΥ, Ακαδημαϊκό Έτος 2010-11, 8ο Εξάμηνο 

Μάθημα: Ρομποτική ΙΙ. Διδάσκων: Κ.Τζαφέστας 

Έλεγχος «Αλληλεπίδρασης» 

με το Περιβάλλον 

• Έλεγχος Δύναμης – Έλεγχος «Εμπέδησης» 

• Έλεγχος «Οπτικής Οδήγησης» 




Ρομποτικός Έλεγχος Δύναμης / 

Εμπέδησης (Σύνθετης Μηχανικής 

Αντίστασης / Συμμόρφωσης) 

(Force / Impedance / Compliance Robot Control) 


3 

4

Ρομποτικός Έλεγχος Δύναμης - 

Εισαγωγή 

• Έλεγχος μηχανικής εμπέδησης (impedance control) 

– Passive impedance – Remote center of compliance (RCC) 

(εργαλεία παθητικής «συμμόρφωσης») 

– Active Impedance – Active stiffness control 

(έλεγχος «ενεργούς σύνθετης μηχανικής αντίστασης») 

• Έλεγχος δύναμης (force control) 

• Υβριδικός έλεγχος δύναμης/θέσης 

(hybrid force/motion control) 

• Παραδείγματα - Προσομοίωση 


Μέθοδος Παθητικής Συμμόρφωσης 

(passive compliance) 

Μηχανικά εργαλεία παθητικής συμμόρφωσης 

Remote Center of Compliance (RCC) 

Αρχή λειτουργίας RCC 

(α) συμμόρφωση σε οριζόντια δύναμη 

(β) συμμόρφωση σε ροπή 


5 

6


(συνέχεια) (1) 

RCC 3 διαστάσεων 

Βασική Σχέση: 

f = [f x , f y , f z , N x , N y , N z ] 

ε = [ε x , ε y , ε z , α x , α y , α z ] 

K = 

f = Κ · ε 

Κsoft 0 

Κsoft Κhard Κsoft Κsoft 0 Κ soft 

: εξωτερική δύναμη/ροπή 

: μηχανική παραμόρφωση 

(strain) 

K : μήτρα μηχανικής σκληρότητας («ακαμψίας») 

(stiffness matrix) 




Μηχανικά εργαλεία παθητικής συμμόρφωσης 

Remote Center of Compliance (RCC) 


7 

8



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

εργασίες συναρμολόγησης (peg-in-hole tasks) 


Ρομποτικοί Αισθητήρες Δύναμης 

(force sensors) 

Αισθητήρας δύναμης: Κατάλληλη μηχατρονική διάταξη 

αισθητήρων ελαστικής (μηχανικής) παραμόρφωσης 

(strain gauges – strain/deformation transducers) 


9 

10

Μετρητές μηχανικής παραμόρφωσης 

(strain gauge transducers) 


Έλεγχος ενεργούς εμπέδησης 

(active-impedance control) 

Απλό παράδειγμα 1 β.ε. 

⋅ + ⋅ + ( − ) = 

( ) ( ) ( ) 

d d d d d d d 

Δυναμικό μοντέλο συστήματος: 

mx + bx + kx= f + F 

a a a u 

ma, ba, ka: δυναμικές παράμετροι συστήματος 

f u : ελεγκτής 

F : εξωτερική δύναμη 

Επιθυμητή μηχανική εμπέδηση (desired impedance): 

md x bd x kd x xd F ή πληρέστερα: 

m x − x + b x − x + k x − x = F −F 

k d 

b d 

-Fd md F 

F : δύναμη ασκούμενη από το εξωτερικό 

περιβάλλον πάνω στο σύστημα 

0 

x xd Fd : επιθυμητή δύναμη η οποία θέλουμε τελικά 

να ασκείται από το εξωτερικό περιβάλλον 

πάνω στο σύστημα 


11 

m d, b d, k d: επιθυμητές 

δυναμικές παράμετροι 

«κλειστού βρόχου» 

συστήματος 

(μοντέλο αναφοράς) 

12

Έλεγχος ενεργούς εμπέδησης: 

Απλό παράδειγμα 1 β.ε. 

mx + bx + kx= f + F 

a a a u 

( ) ( ) ( ) 

: Δυναμικό μοντέλο συστήματος 

md xd − x + bd xd − x + kd xd − x = Fd −F 

(2) 

( ω0= kd m , 

d 

ζ = 

2 

bd 

mk d d 

) 

: Επιθυμητή μηχανική εμπέδηση 

(desired impedance) 

(επιθυμητή συμπεριφορά κλειστού βρόχου) 

: ιδιοσυχνότητα 

Ελεγκτής ενεργούς μηχανικής εμπέδησης: 

f = mu+ bx + kx−F u a a a 

(3) 

(1) 

⇒ u= x 

(1) 

: συντελεστής απόσβεσης 

: εξίσωση κλειστού βρόχου 

( ) ( ) ( ) 

−1 

u= xd + md ⎡⎣bd xd − x + kd xd −x − Fd −F 

⎤⎦ 

(3) 

(όπου u σήμα ελέγχου) 

Για να επιτύχουμε την επιθυμητή συμπεριφορά (2), έχουμε για το u: 



Απλό παράδειγμα 1 β.ε. (συνέχεια) (1) 

Ο νόμος ελέγχου μηχανικής αντίστασης (3) και (4), γράφεται ουσιαστικά ως: 

m m m 

f = mx + b x − x + k x −x− F −F 


(4) 

( ) ( ) ( ) 

a a a 

u a d d 

md + bx a+ kx a −F 

d 

e x 

md d d 

ex md 

d 

eF 

ή άλλως: 

όπου: 

K V K P λ m 

( ) ( ) ( ) 

f = mx + bx+ kx + K x − x + K x − x + K F 

u a d a a V d P d F 

(όταν F d=0) 

K = λ −1( 

λ m = ( ma md) 

) K = λ b και K = λ k 

F m 

V m d 

P m d 

Παρατήρηση: Το κέρδος ανάδρασης δύναμης K F (K F≠0) επιτρέπει τη μεταβολή των 

«αδρανειακών» χαρακτηριστικών του συστήματος (λ m≠1) στο κλειστό βρόχο 

13 

14

k d 

b d 

(2) 

⇒ 

(5) 



m d 

x c 

x 

F 

k c 

b c 

Δυναμική εξίσωση “εξωτερικού περιβάλλοντος”: 

b x− x + k x− x =−F 

(5) 

( ) ( ) 

c c c c 

( ) ( ) ( ) 

E 

d d d d d d d 

Επαφή με εξωτερικό περιβάλλον E 

k c : σκληρότητα 

b c : απόσβεση 

x c : θέσης ισορροπίας 

Δυναμική εξίσωση (2) κλειστού βρόχου συστήματος {M, b, k, f u } 

(μηχανική αντίσταση συστήματος που επιτυγχάνεται με τον ελεγκτή f u ): 

m x − x + b x − x + k x − x = F −F 




Δυναμική συμπεριφορά τελικού κλειστού συστήματος {M, b, k, f u } 

σε επαφή με το εξωτερικό περιβάλλον Ε (k c , b c , x c ): 

( ) ( ) ( ) 

( ) 

mx + b + b x + k + k x= mx + bx + bx 

d d c d c d d d d c c 

kd+ kc bd+ bc 

ω′ 0 = m , ζ′ 

= 

d 2 md 

( kd+ kc) 

: ιδιοσυχνότητα : συντελεστής απόσβεσης 

+ kx d d + kx c c −Fd 

Δύναμη πάνω στο σύστημα από το εξωτερικό περιβάλλον εργασίας, 

= →∞ = − 

στην τελική (μόνιμη) κατάσταση (t∞): Fef F( t ) kc( xcxf) όπου xf : τελική θέση συστήματος x 

( kdxd+ kcxc) = x( t→∞ ) = − 1 F 

Άρα: 

( ) 


( kd+ kc) ( kd+ kc) 

15 

f d 

kk k 

ef d 

d c 

c 

F = F t→∞ = 

( ) 

( xc − xd) 

+ F 

kd+ kc ( kd+ kc) 

16

Έλεγχος ενεργούς μηχανικής εμπέδησης 

ρομποτικού χειριστή - Γενικά 

Γενική Περίπτωση (ρομποτικός χειριστής) 

(a) Σύστημα ελέγχου (ροπές τ i ) 

με ανάδραση δύναμης F 

(και θέσης y) 

(b) Μηχανική αντίσταση που 

θέλουμε να επιτύχουμε μέσω 

του ρομποτικού ελεγκτή ενεργούς 

μηχανικής εμπέδησης 


Δυναμική ρομποτικού χειριστή (1) 

Δυναμικό μοντέλο ρομποτικού χειριστή (υπενθύμιση): 

T 

M( q) ⋅ q + h( q, q) = τ + J F 

(στο χώρο των γενικευμένων μεταβλητών q i των αρθρώσεων) 

όπου Μ: μήτρα αδρανείας ρομπότ (συχνά συμβολίζεται και ως D(q)) 

h: όροι Coriolis και φυγοκέντρου δυνάμεως 

τ: ροπές (γενικευμένες μεταβλητές δράσης) στις αρθρώσεις 

F e: εξωτερική δύναμη που ασκείται από το εξωτερικό περιβάλλον 

και J : Ιακωβιανή μήτρα του ρομπότ 

Μοντέλο Lagrange : 

Μοντέλο Newton-Euler : 

τ = 

i 

d 

dt 

⎛ ⎞ 

⎜∂K⎟ K P 

⎜ ⎟− 

∂ + ∂ 

⎜∂qi ⎟ ∂qi ∂q 

⎝ ⎠ 

i 


e 

(Δ1) 

∑ fi = mv i ci 

∑ Ni = I 

ciωi+ ωi× ( Iciωi) 

K: κινητική ενέργεια 

P: δυναμική ενέργεια 

17 

18

Δυναμική ρομποτικού χειριστή (2) 

Δυναμικό μοντέλο ρομποτικού χειριστή στο χώρο εργασίας (task-space): 

* * 

M ⋅ p+ h = F + F (Δ2-α) 

όπου M * : μήτρα αδρανείας ρομπότ στο χώρο εργασίας, 

h * : όροι Coriolis και φυγοκέντρου δυνάμεως στο χώρο εργασίας, 

Fa: (τ=JTFa) γενικευμένη δύναμη στο χώρο εργασίας οφειλόμενη 

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

Fe: εξωτερική δύναμη που ασκείται από το εξωτερικό περιβάλλον. 

Έχουμε: = J⋅ ⇒ = J⋅ J⋅ 

p q p q+ q 

T T 

και (Δ1): M⋅ q + h= J Fa + J Fe −1 −1 

T 

⇔ q + M h= M J Fa + Fe 

⇔ 

−1 −1 

T 

Jq + JM h= JM J F + F 

Επομένως τελικά: 

και 


a 

− − T 

με ( ) 

* −1 

* 

( M J M ) M J 

= = ⋅ ⋅ 

* 

h = ⋅ ⋅ h− ⋅ ⋅q 

* 1 1 

M Q Q J M J 

e 

( a e) 


( ) 

(Δ2-β) 

Δυναμικός έλεγχος ενεργούς μηχανικής 

εμπέδησης ρομποτικού χειριστή (1) 

Επιθυμητή μηχανική εμπέδηση (desired impedance) ρομποτικού χειριστή 

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

M p − p + B p − p + K p − p = F −F 

(Ε1) 

( ) ( ) ( ) 

d d d d d d d e 

Δυναμική εξίσωση ρομποτικού χειριστή στο χώρο εργασίας (Δ2-α): 

* * 

M ⋅ p+ h = F + F 

a 

e 

Δυναμικός ελεγκτής ενεργούς εμπέδησης: 

(βασισμένος στη μεθοδολογία ελέγχου υπολογιζόμενης ροπής (computed-torque control) 

T 

τ 

= J ⋅ Fa 

F = M ⋅ u+ h −F 

* * 

a e 

(Ε2) 

( u= p : εξίσωση κλειστού βρόχου ) 

με το σήμα ελέγχου u να δίνεται από τη σχέση (ώστε στο κλειστό βρόχο να έχουμε την (Ε1)) 

( ) ( ) ( ) 

−1 

u= pd + Md ⎡⎣Bd pd − p + Kd 

pd − p − Fd −Fe 

⎤⎦ 

(Ε3) 

σφάλμα ταχύτητας 

σφάλμα θέσης 

σφάλμα δύναμης 

19 

20

p d 

pd 

p d 

F 



p d 

pd 

p d 

F 

d 

e p 

+ 

x 

- + e p 

x 

- + 

x 

- 

K d 

eF 

B d 

ανάδραση δύναμης 

ανάδραση ταχύτητας 

ανάδραση θέσης 

Δυναμικός 

Ελεγκτής 

(Ε2),(E3) 

τ Ρομποτικός 

Χειριστής 

(Δ2) 




Σχηματικό διάγραμμα (block-diagram) 

δυναμικού ρομποτικού ελεγκτή ενεργούς μηχανικής 

αντίστασης 

(υπολογιζόμενης ροπής) 




d 

e p 

+ 

x 

- + e p 

x 

- + 

x 

- 

(task-space) 

K d 

eF 

B d 




+ 

+ 

- 

J( q ) 

Γ(q) 

Ρομποτικό Μοντέλο: M * ,h * 

* 

h( q, q 

) 

* 

M( q ) 

+ 

+ 

+ 

- 

Ρομποτικός 


(Δ2) 



(joint space) 


Σχηματικό διάγραμμα (block-diagram) 

δυναμικού ρομποτικού ελεγκτή ενεργούς εμπέδησης 

(impedance control) 


* 

MM 

−1 

d 

τ 

p 

p 

F 

21 

22 

e 

q 

q 

F 

e

p 

p 



d 

p d 

F 

d 

d 

(task-space) 

Έλεγχος 

Εμπέδησης 

(Ε3) 




u Αντίστροφη 

Δυναμική 

(Ε2) 

J( q ) 

Γ(q) 

Ευθεία κινηματική 

Ρομποτικό Μοντέλο: M * ,h * 



(Δ2) 



(joint space) 


Γενική δομή ρομποτικού ελεγκτή εμπέδησης 

(impedance control) 


Απλοποιημένος έλεγχος ενεργούς μηχανικής 

ακαμψίας (stiffness) ρομποτικού χειριστή 

Θεωρούμε: M * =I και h * =0 

οπότε ο ελεγκτής (Ε2)-(Ε3) μπορεί να γραφεί: 

τ 

( ) 

J ⎡⎣ p B eK e K e F ⎤⎦ 

T 

= ⋅ d + d p + d p − Fd F − e 

όπου: e = p − p και e = F −F 

p d F d e 


τ 

(Ε4) 

Εαν επιπλέον Fd=0 και KFd = I, καθώς και Βd = 0, pd = 0, 

τότε η (Ε4) γράφεται: 

K qd 

(Ε5) 

τ = J ⋅K ⋅ e = J ⋅K ⋅J⋅e T T 

d p d q 

όπου: 

και ονομάζεται active-stiffness control, όπου: 

eq = qd − q 

K d : μήτρα «ακαμψίας» (stiffness matrix) ορισμένη στο χώρο εργασίας 

K qd = J T K d J : ισοδύναμη μήτρα «ακαμψίας» στο χώρο αρθρώσεων 

q 

q 

F 

23 

24 

e

Έλεγχος δύναμης για ρομποτικό χειριστή 

(Force control) - Εισαγωγή 

ενεργός μηχανική 

αντίσταση ρομπότ 

K d 

B d 

Δύναμη πάνω στο ρομποτικό χειριστή 

από το εξωτερικό περιβάλλον εργασίας, 

στην τελική (μόνιμη) κατάσταση (t∞): 

( t ) K ( ) 

F = F →∞ = p −p 

ef e e e f 

όπου pf : τελική θέση (στη μόνιμη κατάσταση) του στοιχείου δράσης του ρομπότ 

p = p t →∞ = K + K 

−1 

K p + K p −F 

Άρα: 

M d 

Fe pe εξωτερικό περιβάλλον 

p 

p d 

K e 

B e 

( ) [ ] ( ) 

f d e d d e e d 

−1 

( t ) K ( K K ) K ( ) 

Fef = Fe→∞ = e d + e ⎡⎣ d pe− pd+ Fd⎤⎦ 

Βασική αρχή: 

Στον έλεγχο δύναμης θέτουμε: Kd =0 ⇒ Fef = Fe (t→∞) = Fd (δηλαδή δε λαμβάνουμε υπ’όψη στον έλεγχο το σφάλμα θέσης) 

E 

σφάλμα 

θέσης 



επιθυμητή 

δύναμη 

Έλεγχος δύναμης ρομποτικού χειριστή 

Ρομποτικός έλεγχος δύναμης βασισμένος στο μοντέλο (υπολογιζόμενης ροπής): 

T 

τ = J ⋅Fa 

* * 

Fa = M ⋅ u+ h −Fe 

(F1) 

(computed torque) 



( ) ( ) 

u= M ⎡⎣−B p − F − F ⎤⎦=−K 

p −K F −F 

−1 

d d d e Bd Fd d e 

ανάδραση 

ταχύτητας 


δύναμης 

Σε σύγκριση με το δυναμικό ελεγκτή ενεργούς εμπέδησης (impedance control), 

στον έλεγχο δύναμης (force control) θέτουμε Kd =0 , δηλαδή: 

«Ακαμψία» του συστήματος ελέγχου του ρομποτικού χειριστή = μηδέν 

(μηδενική φαινόμενη «ακαμψία» -apparent stiffness- στο χώρο εργασίας) 

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

ανάδραση σφάλματος δύναμης (και ταχύτητας για ευστάθεια), 

οπότε τελικά: ⇒ Fef = Fe (t→∞) → Fd (επιθυμητή δύναμη) 

(F2) 

25 

26

Fd 

x 

+ - 

eF 

Άμεσος Έλεγχος δύναμης (1) 

(direct force control) 

K F 

pF p 

x 

+ 

e 

- 

K P 

- 


u 



−1 

x M 

x 

d ∫ ∫ + 

Bd 

p 

Περιβάλλον 

p 

p p - 

Άμεσος Έλεγχος Δύναμης με εσωτερικό βρόχο θέσης 



Άμεσος Έλεγχος Δύναμης με εσωτερικό βρόχο θέσης (συνέχεια) 

( ) 

( ) 

u = KP⋅ pF − p −Bd ⋅ p 

p = K F −F 

F F d e 

M ⋅ p= u 

d 

( ) 

F = K p− p 

e e e 

( ) 

M d ⋅ p+ Bd ⋅ p+ KP⋅ p= 

= KPKF ( Fd − Fe) 

∧ 

( / ) 

p = F K + p 

e e e 

⇒ M ⋅ F + B ⋅ F + K I + K K F = K K K F 

Δυναμική Εξίσωση κλειστού βρόχου 

d e d e P e F e P e F d 

Proportional (P) Force‐Control: K = K = const. 

F FP 

Proportional‐Integral (PI) Force‐Control: 

F = FP + FI∫( 

) t 

Μηδενικό μόνιμο σφάλμα δύναμης eF K K K . dτ 


Ke 

e 

Fe 

27 

28

Fd 

x 

+ - 



p 

eF F 

K F 

K P 

- 


u 

M 



−1 

x ∫ ∫ x 

d 

Bd 

p 

Περιβάλλον 

p 

p p - 

Άμεσος Έλεγχος Δύναμης με εσωτερικό βρόχο ταχύτητας 

⇒ M ⋅ F + B ⋅ F + K K K F = K K K F 

d e d e P e F e P e F d 

Δυναμική Εξίσωση κλειστού βρόχου 

P και PI force controller: Μηδενικό μόνιμο σφάλμα δύναμης eF Υβριδικός έλεγχος δύναμης/τροχιάς 

ρομποτικού χειριστή (hybrid control) 

Βασική Αρχή 

έλεγχος δύναμης 

έλεγχος θέσης 


+ 

Ke 

e 

Fe 

Διαφορετικός τύπος ελέγχου σε 

διαφορετικές κατευθύνσεις στο 

χώρο εργασίας: 

Έλεγχος θέσης / δύναμης / 

ενεργούς μηχανικής εμπέδησης /... 

κλπ. 

ανάλογα με την υπο-εργασία 

29 

30

Υβριδικός έλεγχος δύναμης/τροχιάς 

ρομποτικού χειριστή (συνέχεια) (1) 

Παραδείγματα μηχανικών εργασιών που απαιτούν την 

εφαρμογή υβριδικού ελέγχου δύναμης/θέσης 

(σε διαφορετικούς άξονες στο χώρο του τελικού στοιχείου δράσης) 




Βασικό σχήμα υβριδικού ελέγχου δύναμης/τροχιάς (1) 

Επιθυμητή 

Τροχιά + 

Εντολή κατεύθυνσης 

ελέγχου {s i } (i=1,..,n) 


Δύναμη 

S=diag[s i ] 

+ 

- 

- 

Μετασχηματισμοί 

Συντεταγμένων 

[S] 

[I] – [S] 



s i =1 έλεγχος τροχιάς 

s i =0 έλεγχος δύναμης 

0 

⎡ c ⎤ 

⎣ 

R 

⎦ 

0 

⎡ c ⎤ 

⎣ 

R 

⎦ 

0 

⎡ c ⎤ 

⎣ 

R 

⎦ 

0 

⎡ c ⎤ 

⎣ 

R 

⎦ 


Τροχιάς 

+ 

+ 


Δύναμης 

Αισθητήρες 

Θέσης 



Αισθητήρας 



T 

T 

Θέση 

31 

Δύναμη 

32



Βασικό σχήμα υβριδικού ελέγχου δύναμης/τροχιάς (2) 


Τροχιά + 

Εντολή κατεύθυνσης 

ελέγχου {s i } (i=1,..,n) 

1 


Δύναμη 

y 0 

S=diag[s i ] 

+ 

- 

- 




Τροχιάς 



u 

u 

( c ) 

p 

( c ) 

F 



s i =1 έλεγχος τροχιάς 

s i =0 έλεγχος δύναμης 

0 

⎡ c ⎤ 

⎣ 

R 

⎦ 

[S] 

[I] – [S] 

0 

⎡ c ⎤ 

⎣ 

R 

⎦ 

Αισθητήρες 

Θέσης 

Αισθητήρας 



T 

T 

0 

⎡ c ⎤ 

⎣ 

R 

⎦ ανάστροφη 

δυναμική 

+ u τ 

D·(.)+h Ρομπότ 

+ 

0 

⎡ c ⎤ 

⎣ 

R 

⎦ 

Ρομποτικός έλεγχος εμπέδησης – 

Παράδειγμα R-R 2D (1) 

2 βαθμοί ελευθ. 

2D, επίπεδο 

l 1 

lc1 m1,I1 O0 y 1 

O 1 

q 1 

m2,I2 lc2 q2 x 0 

2 

yΕ OΕ x 1 

xΕ θ 

Δυναμικό μοντέλο Lagrange : 

τ = 


i 

d 

dt 

⎛ ⎞ 

⎜∂K⎟ K P 

⎜ ⎟− 

∂ + ∂ 

⎜∂qi ⎟ ∂qi ∂q 

⎝ ⎠ 

i 

K = ml q Iq 

1 

1 

2 

2 1 

1 c1 

1 + 

2 

2 

1 1 

= 1 1ˆc1 1 ( s1 sin( 

q1) 

) 

P mgl s = 

K = mvv I q + q 

2 

1 T 

+ 1 ⎛ ⎞ 

2 2 c2 c2 

2⎜ 1 2⎟ 

⎝ ⎠ 

2 2 

P = mgls ˆ + l s s q q 

⎝ 

c 

⎠ 

όπου: vc2= d ( p 

dt c2) 

⎡ lsq l s ⎛qq⎞⎤ ⎢− ⎡ ⎤ 

1 1 − 1 c2 

12⎜ + 1 2⎟⎥ 

⎝ ⎠ 

⎢lc 1 1+ lc2c ⇒ v 

12⎥ 

c2 

= ⎢ ⎥ 

και: pc2 

= 

⎢ 

⎢ ⎥ 

lcq l c ⎛qq⎞ ⎥ 

⎢ 

⎢ls+ l s ⎥ 

1 1 + 1 c2 

12⎜ + 1 2⎟ 

⎥ 

⎣ ⎝ ⎠ ⎦ 

⎣ 1 1 c2 

12⎦ 

⎛ ⎞ 

2 2 ⎜ 1 1 2 12⎟ ( 12 = sin ( 1+ 2 ) ) 

Θέση 

Δύναμη 

33 

K: κινητική ενέργεια 

P: δυναμική ενέργεια 

1ος σύνδεσμος 

2ος σύνδεσμος 

34

Άρα: 



T 

c2c2 ⎡ 

⎢ 

⎣ 

lsq 1 11lc2s ⎛ 

12 ⎜q ⎝ 1 

2 

⎤ q ⎞ 

2 ⎟⎥ ⎠⎦ ⎡ 

⎢lcq 1 1 ⎣ 

1 lc2c ⎛ 

12 ⎜q ⎝ 1 

2 

⎤ q 

⎞ 

2 ⎟⎥ 

⎠⎦ 

T 

c2 2 2 2 2 2 

2 

= lsq l s q q 2llssqq 

q 

c2 1 1 + ⎛ ⎞ ⎛ ⎞ 

1 c2 12⎜ + 1 2⎟ + 1 c2 

1 121⎜ + 1 2⎟ 

⎝ ⎠ ⎝ ⎠ 

2 2 2 2 2 

2 

+ lcq 1 1 1+ lc2c ⎛ 

12 q1 q ⎞ 

⎜ + 2⎟ 

+ 2ll 

⎝ ⎠ 1 c2cc 

1 12q ⎛ 

1⎜q ⎝ 1 q 

⎞ 

2⎟ 

⎠ 

T 

c2 2 2 2 

2 

= lq 2 

c2 1 + l ⎛qq ⎞ llcq ⎛qq ⎞ 

1 c2⎜ + 1 2⎟ + 1 c2 

21⎜ + 1 2⎟ 

⎝ ⎠ ⎝ ⎠ 

vv 

vv 

vv 

=− − + + + + ⇒ 

+ ⇒ 

• K ml q Iq I q q m l q l q q ll c 2q 

q 

q ∂ = + + + + + + + + 

∂ 

1 

2 

1 c1 1 1 1 

⎛ 

2⎜ ⎝ 1 

⎞ 

2⎟ ⎠ 

⎡ 2 

2⎢1 ⎣ 

1 

2 ⎛ 

c2⎜ ⎝ 1 

⎞ 

2⎟ ⎠ 1 c2 

⎛ 

2⎜ ⎝ 1 

⎞⎤ 

2⎟⎥ 

⎠⎦ 

⎛ 

⎜ 

⎜ 

⎜ 

⎝ 

⎞ 

⎟ 

⎟ ml q Iq I ⎛q q ⎞ 

1 c11 

11 2⎜1 2⎟ 

⎟ 

⎝ ⎠ 

1 ⎟ 

⎠ ⎡ 2 2 

⎛ ⎞ ⎛ ⎞ ⎤ 

+ m2 l1q1 l ⎛ 

c2 q1 q ⎞ 

⎢ + ⎜ + 2⎟ + ll 1 c2c2⎜2q1+ q2⎟− ll 1 c2s2⎜2q1+ 

q2⎟q2⎥ ⎣ ⎝ ⎠ ⎝ ⎠ ⎝ ⎠ ⎦ 

d 

dt K q 

• ∂ ∂ = + + + 

• 

∂P∂ ∂q ∂q 

= mgl ˆ s+ mgˆ ls+ l s = mgl ˆ c+ mgˆ lc+ l c 

1 1 

... 

Τελικά: 

⎡ ⎛ ⎞⎤ ⎛ ⎞ 

⎢ 1 c1 1 2 ⎜ 1 1 c2 12⎟⎥ 1 c1 1 2 ⎜ 1 1 c2 

12⎟ 

⎣ ⎝ ⎠⎦ ⎝ ⎠ 




Ρομποτικό Δυναμικό Μοντέλο 2D Χειριστή 2 β.ε. 

• = I + ml + I + m l + l + 2ll 

c q+ I + m l + ll c q 

τ1 ⎡ 

⎢ 

⎣ 

1 

2 

1 c1 2 

⎛ 2 

2⎜ ⎝ 

1 

2 

c2 1 c2 ⎞⎤ 2⎟⎥ ⎠ 

1 

⎦ 

⎡ 

⎢ 

⎣ 

2 

⎛ 2 

2⎜ ⎝ 

c2 1 c2 

⎞⎤ 

2⎟⎥ ⎠ 

2 

⎦ 

⎛ mll s 2qq q2⎞ ⎛ ⎞ 

− mgl 21c22⎜ + 1 2 2⎟+ 

1ˆ c+ mg c112ˆ⎜lc+ l c 

1 1 c212⎟ 

⎝ ⎠ ⎝ ⎠ 

⎡ ⎛ 2 ⎞⎤ ⎡ 2 ⎤ 

τ 2 

2 ⎢I2 ml 2⎜ c2 llc 1 c2 2⎟⎥q1 ⎢I2 ml 2 c2⎥q2 mllsq 2 1 c2 21 mglc 2ˆ ⎝ ⎠ ⎣ ⎦ 

c2 

12 

⎣ ⎦ 

• = + + + + + + 

τ = M q 2 

1 11 + M q 2 

1 12 + h q h qq g 

2 122 + + 

2 112 1 2 

1 

τ = M q 2 

2 21 + M q h q g 

1 22 + + 

2 211 1 2 

όπου: 2 2 2 

M11 = I1+ ml 1 c1+ I2+ m2( l1+ lc2+ 2ll 

1 c2c2) 

2 

M12 = M21 = I2+ m2( lc2+ ll 1 c2c2) 

M22 = I2+ ml 2 

h122 = h112 =− h211 =−mll 

2 1 c2s2 

g1= m1gl ˆ c1c1+ mg 2ˆ( lc 1 1+ lc2c12) g2= m2gl ˆ c2c12 

Δηλαδή: 


2 

c2 

35 

36

Είναι: 

Θέτουμε: 



M= 

M M 

= 

h 

= 

h q + 2h 

qq + g 

⎡ ⎤ ⎡ ⎤ ⎡ 2 

⎤ 

⎢ 11 12 ⎥ ⎢ 1⎥ ⎢ 122 2 112 1 2 1⎥ 

⎢ ⎥ h ⎢ ⎥ ⎢ ⎥ 

⎢M M ⎥ ⎢ 2 

21 22 h ⎥ ⎢ h q g ⎥ 

⎣ ⎦ ⎣ 2⎦ ⎣ 211 + 1 2 ⎦ 

− − T 

με ( ) 

* −1 

* 

( M J M ) M J 

* 1 1 

M = Q Q= J⋅M ⋅J 

* 

h = ⋅ ⋅ h− ⋅ ⋅q 

Ρομποτικός Ελεγκτής Ενεργούς Μηχανικής Εμπέδησης (υπολογιζόμενης ροπής) 

με 

τ = J F 1 11 ax + J F 21 ay και τ = J F 2 12 ax + J F 22 ay 

F = Mu+ Mu+ h−F και F * * * 

= Mu+ Mu+ h−F * * * 

ax 11 1 12 2 1 ex 

όπου, θέτοντας: 

−1 −1 −1 

B′ d = Md Bd, K′ d = Md Kd και Μ′ d = M d 

με: ep= pd − p και eF = Fd −Fe 

έχουμε: 


(J: Ιακωβιανή μήτρα) 

ay 21 1 22 2 2 ey 

[ ′ ′ ′ ′ ′ ′ ] 

[ ′ ′ ′ ′ ′ ′ ] 

u = p + B e+ B e+ K e + K e −M e −M 

e 

1 dx d , xx px d , xy py d , xx px d , xy py d , xx Fx d , xy Fy 

u = p + B e+ B e+ K e + K e −M e −M 

e 

2 dy d , yx px d , yy py d , yx px d , yy py d , yx Fx d , yy Fy 



Ρομποτικός Έλεγχος Δύναμης / 

Εμπέδησης 

(Παράδειγμα Εφαρμογής) 


37 

38

Δυναμικός έλεγχος ενεργούς 

εμπέδησης ρομποτικού χειριστή 

Επιθυμητή μηχανική εμπέδηση (desired impedance) ρομποτικού χειριστή 

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

( ) ( ) ( ) 

M p − p + B p − p + K p − p = F −F 

d d d d d d d e 

Δυναμική εξίσωση ρομποτικού χειριστή στο χώρο εργασίας (Δ2-α): 

⋅ p+ h = F + F 

* * 

M 

a 

e 

Δυναμικός ελεγκτής ενεργούς εμπέδησης: 

(βασισμένος στη μεθοδολογία ελέγχου υπολογισμένης ροπής (computed-torque control) 

T 

τ = J ⋅ Fa 

(Ε2) ( u= p : εξίσωση κλειστού βρόχου ) 

* * 

F = M ⋅ u+ h −F 

a e 


( ) ( ) ( ) 


(Ε1) 

−1 

u= pd + Md ⎡⎣Bd pd − p + Kd 

pd − p − Fd −Fe 

⎤⎦ 

(Ε3) 




Έλεγχος δύναμης ρομποτικού χειριστή 

Ρομποτικός έλεγχος δύναμης βασισμένος στο μοντέλο: 

T 

τ = J ⋅Fa 

F = M ⋅ u+ h −F 

* * 

a e 

(F1) 



( ) ( ) 

u= M ⎡⎣−B p − F − F ⎤⎦=−K 

p −K F −F 

−1 

d d d e Bd Fd d e 


ταχύτητας 


δύναμης 

Σε σύγκριση με το δυναμικό ελεγκτή ενεργούς εμπέδησης (impedance control), 

στον έλεγχο δύναμης (force control) θέτουμε K d =0 , δηλαδή: 

«Ακαμψία» του συστήματος ελέγχου του ρομποτικού χειριστή = μηδέν 

(μηδενική φαινόμενη «ακαμψία» -apparent stiffness- στο χώρο εργασίας) 

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

ανάδραση σφάλματος δύναμης (και ταχύτητας για ευστάθεια), 

οπότε τελικά: ⇒ F ef = F e (t→∞) → F d (επιθυμητή δύναμη) 


(F2) 

39 

40

1 

Έλεγχος δύναμης ρομποτικού χειριστή – 


2 βαθμοί ελευθ. 

2D, επίπεδο 

y 0 

l 1 

lc1 m1,I1 O0 y 1 

O 1 

q 1 

m2,I2 lc2 q2 x 0 

2 

yΕ OΕ x 1 

xΕ θ 

Ρομποτικό δυναμικό μοντέλο: 

τ = M q + M q + h q+ 2h 

qq + g 

2 

1 11 1 12 2 122 2 112 1 2 1 

τ = M q + M q + h q+ g 

2 

2 21 1 22 2 211 1 2 

όπου: 

M = I + ml + I + m l + l + ll c 

M22 = I2+ ml2 

2 c2 

2 

M12 = M21 = I2+ m2( lc2+ ll 1 c2c2) 

h122 = h112 =− h211 =−mll 

2 1 c2s2 

g1= mgl 1ˆ c1c1+ mg 2ˆ( lc 1 1+ lc2c12) g = mgl ˆ c 


( 2 ) 

2 2 2 

11 1 1 c1221c21c22 2 2 c2 

12 

Έλεγχος δύναμης ρομποτικού χειριστή - 


Ρομποτικό Δυναμικό Μοντέλο ρομπότ 2 β.ε. (στο χώρο αρθρώσεων) 

με: 

T 

τ = M( q) ⋅ q + h( q, q) −J 

F 

M= 

M M 

= 

h 

= 

h q + 2h 

qq + g 

⎡ ⎤ ⎡ ⎤ ⎡ 2 

⎤ 

⎢ 11 12 ⎥ ⎢ 1⎥ ⎢ 122 2 112 1 2 1⎥ 

⎢ ⎥ h ⎢ ⎥ ⎢ ⎥ 

⎢M M ⎥ ⎢ 2 

21 22 h ⎥ ⎢ h q g ⎥ 

⎣ ⎦ 2 211 + 

⎣ ⎦ ⎣ 1 2 ⎦ 

Ρομποτικό Δυναμικό Μοντέλο ρομπότ 2 β.ε. στο χώρο δράσης (task-space) 

J Fa 

T 

= ⋅ 

τ και: 

* 

F = M ⋅ p+ h −F 

* 

a e 

− − T 

με ( ) 

* −1 

* 

( M J M ) M J 

* 1 1 

M = Q Q= J⋅M ⋅J 

όπου: 

* 

h = ⋅ ⋅ h− ⋅ ⋅q 


e 

(J: Ιακωβιανή μήτρα) 

41 

42

y (cm) 

Ρομποτικός έλεγχος συμμόρφωσης - 

Παράδειγμα R-R 2dof - Αποτελέσματα (1) 

y (cm) 

10 

8 

6 

4 

2 

0 

-2 

-4 

-6 

-8 

-10 

0 5 10 

x (cm) 

15 20 

-2 

0 0.5 1 

time t (sec) 

1.5 2 


Fex,Fey (grf) 

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

(joint-space trajectory computed-torque controller) 

τ = M( q) ⋅ u+ h( q, q) 

u= qd + ⎡⎣ Bd ( qd − q) + Kd 

( qd −q) 

⎤⎦ 

2.5 

2 

1.5 

1 

0.5 

0 

-0.5 

-1 

-1.5 


x 104 

3 




10 

8 

6 

4 

2 

0 

-2 

-4 

-6 

-8 

-10 

0 5 10 

x (cm) 

15 20 

-6000 

0 0.5 1 

time t (sec) 

1.5 2 

Ελεγκτής ενεργούς μηχανικής εμπέδησης (στο χώρο εργασίας) 

(task-space active impedance controller) 

T 

* * 

τ 

= J ⋅FaFa 

= M ⋅ u+ h −Fe 

−1 

u= pd + Md ⎡⎣Bd ( pd − p) + Kd 

( pd − p) −( Fd −Fe) 

⎤⎦ 



Fex,Fey (grf) 

8000 

6000 

4000 

2000 

0 

-2000 

-4000 



43 

44

y (cm) 



10 

8 

6 

4 

2 

0 

-2 

-4 

-6 

-8 

-10 

O 

y 

x 

x c 

O c 

yc 

0 5 10 

x (cm) 

15 20 

0 

0 0.5 1 

time t (sec) 

1.5 2 

Υβριδικός έλεγχος δύναμης / εμπέδησης (στο χώρο εργασίας) 

(task-space hybrid force/impedance robot manipulation control) 

τ 

Έστω RO-xyz, το γενικό πλαίσιο του χώρου εργασίας, και 

Rc-xcyczc το τοπικό πλαίσιο αναφοράς στο σημείο επαφής 

O 

R c η μήτρα στροφής του τοπικού πλαισίου αναφοράς Rc ως προς το RΟ T 

= ⋅ 


Υβριδικός ρομποτικός έλεγχος 

δύναμης / τροχιάς - Παράδειγμα 

Υβριδικός ρομποτικός έλεγχος δύναμης / τροχιάς βασισμένος 

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

J Fa 

F = M ⋅ u+ h −F 

* * 

a e 

( c) ( c) ( c) −1 

( c) ( c) ( c) ( c) ( c) ( c) 

= d + ⎡Md ⎤ Bd p + Kd 

p − d − e 


Fey (task-space) (grf) 

1200 

1000 

800 

600 

400 

200 

45 

O 

⎡⎣Rc⎤⎦ ( c) 

( Fe O ( c) 

= ⎡ 

⎣Rc⎤ ⎦Fe 

) 

( ) 

u= ⋅u 

u p ⎣ ⎦ 

⎡ 

⎣ 

e e F F ⎤ 

⎦ 

( c) O 

T 

O 

T 

ep = ⎡⎣Rc⎤⎦ ⋅ ep = ⎡⎣Rc⎤⎦ ( pd − p) 

και 

( c) O 

T 

⎡⎣R ⎤⎦ O 

T 

⎡⎣R ⎤⎦ 

( c) 

Για το Kd 

( c) 

μπορούμε να θέσουμε: Kd = S⋅Kd όπου η μήτρα Kd επιλέγεται κανονικά για έλεγχο εμπέδησης, ενώ 

η μήτρα S (selection matrix) είναι στο συγκεκριμένο παράδειγμα S=diag[sx ,sy ] 

όπου ep = c ⋅ ep = c ( pd − p) 

με s x (,s y ) = 0, για έλεγχο δύναμης στον άξονα x c (y c ) 

1, για έλεγχο τροχιάς στον άξονα x c (y c ) 

( ή γενικά s x (,s y ) στο διάστημα [0,1] ) 

46



Ρομποτικός Έλεγχος 

Οπτικής Οδήγησης 

(Robot Visual Servoing) 


Διατάξεις Οπτικού Συστήματος 

Camera Placement Configurations 

R 0 e R c e 

R e t 

R 

“eye-in-hand” (‘hand-eye’) διάταξη “fixed camera” (‘static-eye’) διάταξη 

0 c 

R c R 

t 0 t 

Κάμερα προασαρτημένη στο τελικό Κάμερα σταθερά τοποθετημένη 

εργαλείο δράσης του ρομπότ 

στο περιβάλλον εργασίας 

Πλαίσια Αναφοράς 

(0): βάση ρομπότ, (e): end-effector, (c): camera, (t): target/task (αντικείμενο/εργασία) 


47 

48

Αρχιτεκτονικές Ελέγχου Οπτικής Οδήγησης 

(c) pd + 

1. Οπτική οδήγηση – Έλεγχος θέσης/τροχιάς ρομπότ 

a. Ιεραρχικός έλεγχος (look-and-move) (οπτικό σύστημα 

παρέχει σήματα αναφοράς σε ρομποτικό ελεγκτή αρθρώσεων) 

b. Απ’ευθείας έλεγχος αρθρώσεων (direct visual servo) 

2. Πλαίσιο αναφοράς για σφάλμα ελέγχου 

a. Έλεγχος βασισμένος στη θέση (position-based): 

σφάλμα ορισμένο σε κάποιο Καρτεσιανό πλαίσιο αναφοράς 

b. Έλεγχος βασισμένος στην εικόνα (image-based): 

σφάλμα ορισμένο στο χώρο των οπτικών χαρακτηριστικών 

(feature space) 


Αρχιτεκτονικές Ελέγχου (συνέχεια) (1/4) 

- 

(c) 

pˆ 

Καρτεσιανός 

Νόμος Ελέγχου 

Cartesian-space 

control law 

q d 

Εκτίμηση Θέσης 

(pose estimation) 

Ελεγκτές αρθρώσεων 

(joint controllers) 

Ενισχυτές 

(power amplifiers) 

Ανάδραση q – αισθητήρες αρθρώσεων 

f 

Εξαγωγή Οπτικών 

Χαρακτηριστικών 

(feature extraction) 


Εικόνα (video) 

“Position-based” / “look-and-move” 

Aρχιτεκτονική ‘Βασισμένου στη Θέση’ / ‘Ιεραρχικού’ Ελέγχου 

τ 

49 

50

f d 


+ 

- 

(c) pd + 


Οπτικών Χαρακτηριστικών 

Feature-space 

control law 

q d 





Ανάδραση q – αισθητήρες αρθρώσεων 

f 






“Image-based” / “look-and-move” 

Aρχιτεκτονική ‘Βασισμένου στην Εικόνα’ / ‘Ιεραρχικού’ Ελέγχου 


- 

(c) 

pˆ 

Καρτεσιανός 


Cartesian-space 

control law 

τ 

Εκτίμηση Θέσης 

(pose estimation) 


τ 



f 







“Position-based” / “direct” visual servo 

Aρχιτεκτονική ‘Βασισμένου στη Θέση’ / ‘Αμέσου’ Ελέγχου 

51 

52

f d 


+ 

- 


Οπτικών Χαρακτηριστικών 

Feature-space 

control law 

τ 



f 








“Image-based” / “direct” visual servo 

Aρχιτεκτονική ‘Βασισμένου στην Εικόνα’ / ‘Αμέσου’ Ελέγχου 

Έλεγχος με βάση τη θέση (1/2) 

Position-Based Visual Servo Control 

Οπτικά χαρακτηριστικά εξάγονται από την εικόνα και χρησιμοποιούνται 

για την εκτίμηση της «θέσης» του «στόχου» (target pose) ως προς την 

κάμερα ... 

Συνάρτηση Κινηματικού Σφάλματος : E(p e)=0 

where p e: robot (end-effector) pose 

E(.): T m , where T : robot workspace (robot poses) 

Γενικά: T ⊆ SE3 = 3 ×SO3 Εθνικό Μετσόβιο Πολυτεχνείο, Σχολή ΗΜ&ΜΥ. Μάθημα: Ρομποτική ΙΙ (K.Τζαφέστας) 

53 

54

Έλεγχος με βάση τη θέση (2/2) 

Position-Based Visual Servo Control (cont’d) 

Σημείο-προς-Σημείο (point-to-point positioning): 

Τοποθέτηση σημείου p του ρομποτικού εργαλείου, με 

end-effector coordinates p (e) , σε επιθυμητή θέση pd 0 ( e) 

Epp = Pd−Ae⋅P Έστω: T = 

0 ( ) 0 ( ) 

Static-eye: ˆ ˆ c ˆ e 

u3= K ⋅⎡⎣Ac⋅Pd −Ae⋅P ⎤⎦ 

3 u3=K·Epp • 

Συνάρτηση κινηματικού 

σφάλματος 

• 

Hand-eye: 

ˆ e 

A c 

( ) 

ˆ c 

pd 

βαθμονόμηση κάμερας 

κινηματική ρομπότ 

( e) ˆ e ˆ( c) ( e) ˆ e ˆ( c) ˆ( 

c) 

u3= K⋅⎡⎣Ac⋅Pd − P ⎤⎦= K⋅Ac⋅⎡⎣Pd 

−P 

⎤⎦ 

σφάλμα ανεξάρτητο κινηματικής ρομπότ 

: βαθμονόμηση κάμερας (τοποθέτηση ως προς ρομποτικό εργαλείο) 

: παρατηρήσεις στόχου μέσω της κάμερας (camera observations) 



Σήμα Καρτεσιανού 

ελέγχου (επιθυμητή μεταβολή 

θέσης ρομποτικού εργαλείου) 

Έλεγχος με βάση την εικόνα (1/8) 

Image-Based Visual Servo Control 

Camera Projection Models 

Y 

Μοντέλο Προοπτικής Προβολής 

X 

viewpoint 

f=(u,v) 

λ 

P=(x,y,z) 

Z 

αντικείμενο 

(Perspective Projection) 

⎡u⎤ x 

π ( xyz , , ) 

λ ⎡ ⎤ 

= ⎢ ⎥= v z ⎢ ⎥ 

⎣ ⎦ ⎣y⎦ λ: εστιακή απόσταση 

(focal length) 

image plane 

Μοντέλο Ορθογραφικής Προβολής 

(Orthographic Projection) 

⎡⎤ u ⎡⎤ x 

oxyz ( , , ) = ⎢⎥= σ ⎢⎥ 

⎣⎦ v ⎣⎦ y (σ: fixed scale) 

(ειδική περίπτωση) 

Μοντέλο Αφινικής Προβολής 

(Affine Projection) 

⎡u⎤ u ( c) 

⎡ 0⎤ 

⎢ ⎥= A⋅ P + ⎢ ⎥ (A: 2x3) 

⎣v⎦ ⎣v ⎦ 

0 

55 

56


Image-Based Visual Servo Control (cont’d) 

Image Jacobian («Ιακωβιανή Μήτρα Εικόνας») 

f = J ( p) ⋅p 

J 

c 

c 

f: feature vector (k×1), π.χ.: f=[u 1 ,v 1 , …, u n ,v n ] T 

J: (k×m) Ιακωβιανή Εικόνας 

⎡∂f1 ∂p1 ∂f1 ∂pm⎤ 

⎡∂f ⎢ ⎥ 

= ⎤= 

⎢ 

 

⎢⎣∂p⎥⎦ ⎥ 

⎢ 

⎣∂f∂p ∂f ∂p 

⎥ 

⎦ 

k 1 

k m 



( n=(k/2) σημεία ) 


Image-Based Visual Servo Control / «Eye-in-Hand» 

Παράδειγμα εξαγωγής Ιακωβιανής Μήτρας Εικόνας 

R c 

P 

P 

(c) 

vz 

x = zωy − ωz 

+ Vx 

λ 

uz 

y= ωz − zωx + Vy 

λ 

z 

z= ( vω − uω ) + V 

λ 

x y z 

[ x y z] 

[ x y z] 

• Έστω: 

( c) 

P = 

⇒ P = = × P + V 

∧ 

Τ 

( c) Τ ( c) ( c) ( c) 

Ωe 

e 

⎧ λ ⎫ ⎧ λ λ ⎫ 

⎪ 

u = x⋅ u x x z 2 

z⎪ ⎪ 

= ⋅ − ⋅ ⋅ 

⎪ ⎪ ⎪ z z ⎪ 

⎨ ⎬⇒⎨ ⎬ 

⎪ λ λ λ 

v = y⋅ ⎪ ⎪v= ⋅y− ⋅y⋅z⎪ 2 

⎩⎪z⎭⎪ ⎩⎪ z z ⎭⎪ 

57 

58


Παράδειγμα εξαγωγής Ιακωβιανής Μήτρας Εικόνας (συνέχεια) 

2 2 

λ u uv λ + u 

u= Vx− Vz − ωx + ωy −vωz 

z z λ λ 

2 2 

λ u −λ−v uv 

v= Vy − Vz + ωx + ωy + uωz 

z z λ λ 

⎡Vx ⎤ 

⎢ 2 2 V 

⎥ 

⎡λ u uv λ + u ⎤ y 

0 − − −v 

⎢ ⎥ 

⎡u⎤ ⎢ 

z z λ λ 

⎥⎢Vz⎥ f 

= ⎢ 2 2 

v 

⎥ = ⎢ ⎥⎢⎥ 

⎣⎦ ⎢ λ u −λ −v 

uv ⎥ ωx 

0 

u ⎢ ⎥ 

⎢ 

− 

⎣ z z λ λ ⎥⎦⎢ω⎥ y 

⎢ ⎥ 

⎣ωz ⎦ 



Για κίνηση σε επίπεδο: 

και για περισσότερα (n) χαρακτηριστικά 

(σημεία) στην εικόνα, παίρνουμε για τη 

χρονική μεταβολή του διανύσματος 

χαρακτηριστικών : 

⎡λ⎤ 0 −v 

⎡Vx ⎤ 

⎡u⎤ ⎢Z⎥⎢ V 

⎥ 

⎢ y 

v 

⎥= ⎢ ⎥ 

⎣⎦ λ ⎢ ⎥ 

⎢0u⎥ ⎢ 

⎢ωz ⎥ 

⎣ Z ⎥⎦⎣⎦ 

⎡ λ ⎤ 

0 −v 

⎢ 1 

Z ⎥ 

1 

⎢ ⎥ 

⎡u1⎤ ⎢ λ ⎥ 

0 u1 

v 

1 

1 

⎢ ⎥ ⎢ Z ⎥ Vx 

⎢ ⎥ 

⎡ ⎤ 

⎢ ⎥ 

f 

= ⎢ ⎥ = 

⎢ 

V 

⎥ 

⎢ ⎥ y 

⎢ ⎥ 

⎢ ⎥ 

u 

⎢ 

n λ ⎥ 

⎢ 

 

⎥ 

⎢ωz ⎥ 

⎢ 0 −v 

⎥⎣⎦ 

n 

⎢⎣v⎥ Z 

n ⎦ ⎢ n ⎥ 

⎢ λ ⎥ 

⎢ 0 u ⎥ n 

⎢⎣ Zn 

⎥⎦ 


J 

c 

59 

60



Έλεγχος Οπτικής Οδήγησης Επίλυση Συστήματος: 

(Ιακωβιανή κάμερας J: k × m) 

ΣχετικήΚίνησηΣτόχου– Κάμερας 

Επιθυμητή Κίνηση της Κάμερας (ρομποτικού εργαλείου) 

• Εαν 

−1 

= : =J (c) 

p f 

k m 

Εαν τώρα: k ≠ m, rank( J) 

= min( k, m) 

+ + 

k < m: 

= J + ( I−J J) 

⋅ 

(c) 

• Εαν p f c 

όπου 

+ 

• Εαν k > m: 

=J όπου J = ( J J) J 

(c) 

p f 



= J ⋅ (c) 

f p 

(c) 

p 

J = J ( JJ ) 

+ T T −1 

+ T −1 

T 


Καρτεσιανός Έλεγχος Επιλυμένης Ταχύτητας (resolved rate) 

u =−K ⋅ p 

Έστω: 

(c) (c) 

(1) Σήμα καρτεσιανού ελέγχου ρομπότ 

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

ε =Δ f = ( fd−f ) (2) 

όπου fd 

το επιθυμητό διάνυσμα χαρακτηριστικού 

Στόχος του ελεγκτή ασυμπτωτικήμείωσησφάλματοςε: 

Από (2) ⇒ 

(3) 

ε =−f ⇔ f = K ⋅ε 

(4) 

+ 

Αλλά, ως γνωστόν, έχουμε: = Jc (c) 

p f ⇒ (4) 

(c) 

K + 

(c) 

u = − ⋅J⋅ε Νόμος Ελέγχου: c 

K + 

p = ⋅J⋅ε c 

ε= −K⋅ε 61 

(3) 

62



Πρόβλεψη κίνησης – Παρακολούθηση χαρακτηριστικών στην εικόνα 

(«παράθυρο» πρόβλεψης για αναζήτηση/παρακολούθηση χαρακτηριστικών) 

ft+ h= ft−h⋅Jc( p) ⋅u 

(h=Δt) 

Κατάστρωση εξισώσεων στο χώρο κατάστασης 

Προβλήματα «ιχνηλάτησης» στόχου – 

βέλτιστη εκτίμησης (π.χ. Φίλτρο Kalman, ... ) 


63

Force and Visual Servoing (PDF, 910Kb, 32 σελίδες) - Εθνικό ...

You also want an ePaper? Increase the reach of your titles

Delete template?

Save as template?