Μάθημα: Ρομποτική Ι - Εθνικό Μετσόβιο Πολυτεχνείο

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

Ε.Μ.Π., Ακαδημαϊκό Έτος 2007-08, 7ο Εξάμηνο 

Μάθημα: Ρομποτική Ι 

Αυτόματος Έλεγχος Ρομπότ –3 

(Έλεγχος Δύναμης) 

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

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

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

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

E-mail: ktzaf@softlab.ntua.gr 

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

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

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

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

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

Μηχανικής Αντίστασης 

(Force / Impedance Robot Manipulation Control) 


1 

2

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

Εισαγωγή 

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

– Passive impedance – Remote center of compliance (RCC) 

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

– Active Impedance – Active stiffness control 

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

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

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

(hybrid force/impedance control) 

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


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

(passive compliance) 

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

Remote Center of Compliance (RCC) 

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

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

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


3 

4


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

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

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

f = Κ · ε 

f = [fx , fy , fz , Nx , Ny , Nz ,] : εξωτερική δύναμη/ροπή 

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

K = 

Κsoft 0 

Κsoft Κhard Κsoft Κsoft 0 Κ soft 

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

(strain) 

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

(stiffness matrix) 




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

Remote Center of Compliance (RCC) 


5 

6



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

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


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

(force sensors) 

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

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

(strain gauges – strain/deformation transducers) 


7 

8

Αισθητήρες ελαστικής παραμόρφωσης 

(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 Fd 

ή πληρέστερα: 

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

k d 

b d 

-Fd md F 

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

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

0 

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

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

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


9 

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

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

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

συστήματος 

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

10

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

Απλό παράδειγμα 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 K F 

( ) ( ) ( ) ( 1) 

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

u a d a a V d P d F 

με 

K 

m 

a 

F = , KV = KFbd και KP = KFkd md 

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

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

(όταν F d=0) 

11 

12



k d 

b d 

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 ): 

(2) 

⇒ 

(5) 

( ) ( ) ( ) 

( ) 

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) 


13 

f d 

kk k 

ef d 

d c 

c 

F = F t→∞ = 

( ) 

( xc − xd) 

+ F 

kd+ kc ( kd+ kc) 

14

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

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

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

(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: δυναμική ενέργεια 

15 

16

Δυναμική ρομποτικού χειριστή (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 

* * 

Fa = M ⋅ u+ h −Fe 

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

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

u= pd −1 

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

( pd − p) −( Fd −Fe) 

⎤⎦ 

(Ε3) 

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

σφάλμα θέσης 

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

17 

18

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) δυναμικού 

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

(υπολογισμένης ροπής) 


* 

MM 

−1 

d 

τ 

p 

p 

F 

19 

20 

e 

q 

q 

F 

e

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

αντίστασης ρομποτικού χειριστή 

Θεωρούμε: 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) γράφεται: 

(Ε5) 

K qd 

τ = 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 : ισοδύναμη μήτρα «σκληρότητας» στο χώρο αρθρώσεων 

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

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

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

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

K d 

B d 


21 

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

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

στην τελική (μόνιμη) κατάσταση (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 

σφάλμα 

θέσης 

επιθυμητή 

δύναμη 

22

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

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

T 

τ = J ⋅Fa 

* * 

Fa = M ⋅ u+ h −Fe 

(F1) 

(computed torque) 

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


( ) ( ) 

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

p −K F −F 

−1 

d d d e Bd Fd d e 

ανάδραση 

ταχύτητας 


δύναμης 


(F2) 

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

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

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

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

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

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

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

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

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

Βασική Αρχή 

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

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

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

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

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


23 

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

ενεργούς μηχανικής-αντίστασης /... 

κλπ. 

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

24


ρομποτικού χειριστή (συνέχεια) (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 

Θέση 

25 

Δύναμη 

26



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


Τροχιά + 

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

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


Δύναμη 

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, επίπεδο 

1 

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Ε θ 

Δυναμικό μοντέλο 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 ) ) 

Θέση 

Δύναμη 

27 

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

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

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

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

28



Άρα: 

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 

29 

30



Είναι: 

Θέτουμε: 

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 


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

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

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

Μηχανικής Αντίστασης 

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


31 

32


αντίστασης ρομποτικού χειριστή 

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

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

( ) ( ) ( ) 

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

d d d d d d d e 

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

* * 

M 

⋅ p+ h = F + F 

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 

(F1) 

* * 

a e 

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


( ) ( ) 

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

p −K F −F 

−1 

d d d e Bd Fd d e 


ταχύτητας 


δύναμης 


(F2) 

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

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

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

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

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

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

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

33 

34

1 

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

Παράδειγμα R-R 2D (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 

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

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

Ρομποτικό Δυναμικό Μοντέλο ρομπότ 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) 

T 

= ⋅ 

J Fa 

τ και: 

* 

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: Ιακωβιανή μήτρα) 

35 

36

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 

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

(joint-space trajectory computed-torque controller) 

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

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

( qd −q) 

⎤⎦ 


Fex,Fey (grf) 

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 

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

(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 

37 

-6000 

0 0.5 1 

time t (sec) 

1.5 2 



38

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 

39 

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] ) 

40

Μάθημα: Ρομποτική Ι - Εθνικό Μετσόβιο Πολυτεχνείο

Create successful ePaper yourself

Delete template?

Save as template?