Vincoli naturali e vincoli artificiali

Transcript

1 CONTROLLO DI FORZA Interazione del manipolatore con l ambiente Controllo di cedevolezza Controllo di impedenza Controllo di forza Moto vincolato Vincoli naturali e vincoli artificiali Controllo ibrido forza/moto

2 INTERAZIONE DEL MANIPOLATORE CON L AMBIENTE Moto vincolato Controllo posizionale (?) errori di posizionamento incertezza sull ambiente Contatto instabile Misura di forza Strategie di controllo indirette dirette

3 CONTROLLO DI CEDEVOLEZZA Effetti della forza di contatto su schemi di controllo posizionale (nello spazio operativo) B(q) q + C(q, q) q + F q + g(q) = u J T (q)h e all equilibrio J T A(q)K P x = J T (q)h e x = K 1 P T T A (x)h e = K 1 P h A K 1 P cedevolezza attiva del controllo lineare nei riguardi di f torsionale nei riguardi di µ Se h e N(J T ) = x = 0 h e 0

4 Cedevolezza passiva spostamento elementare tra due corpi rigidi R e S accoppiati elasticamente (terna di riferimento indifferente) dx r,s = [ ] dor,s ω r,s dt = v r,s dt h s = [ fs µ s ] = [ Kf K c K T c K µ ] [ ] dor,s ω r,s dt = Kdx r,s h r = h s Matrice di rigidezza K (simmetrica e semi-definita positiva) K f rigidezza traslazionale K µ rigidezza rotazionale K c rigidezza di accoppiamento Matrice di cedevolezza C dx r,s = Ch s Remote Center of Compliance (RCC)

5 Cedevolezza attiva posizione e orientamento terna utensile rispetto a terna desiderata T d e = (T d) 1 T e = [ R d e o d d,e 0 T 1 ] R d e = R T d R e o d d,e = R T d (o e o d ) x = [ o d d,e φ d,e ] x = J Ad (q, x) q J Ad (q, x) = T 1 A (φ d,e) [ R T d O O R T d ] J(q) Controllo PD con compensazione di gravità u = g(q) + J T A d (q, x)k P x J T A d K D J Ad (q, x) q

6 Analisi di stabilità V = 1 2 qt B(q) q xt K P x > 0 q, x 0, all equilibrio J T A d (q)k P x = J T (q)h e h d e = T T A (φ d,e)k P x in termini di spostamenti elementari d x = x 1 dt = TA x=0 (0)(vd d ve)dt d = T 1 A (0)dx e,d h e = K P dx e,d dx e,d = K 1 P h e Modello dell ambiente elasticamente cedevole h e = Kdx r,e K rigidezza dell ambiente semi-definita positiva (solo per le direzioni di moto vincolato)

7 Cedevolezza attiva vs. cedevolezza passiva... all equilibrio h e = ( I 6 + KK 1 ) 1Kdxr,d P dx e,d = K 1 P ( I6 + KK 1 ) 1Kdxr,d P manipolatore ± cedevole ambiente ± cedevole

8 Esempio K = K f = diag{k x, 0} K P = diag{k Px, k Py } All equilibrio k Px k x f e = (x d x r ) k Px + k x o e = 0 [ kpx x d + k x x r k Px + k x y d ] se k Px /k x 1 se k Px /k x 1 x e x d f x k x (x d x r ) x e x r f x k Px (x d x r )

9 Cedevolezza attiva nello spazio dei giunti all equilibrio K P q = J T (q)h e q = K 1 P JT (q)h e d x = J(q)K 1 P JT (q)h e

10 CONTROLLO DI IMPEDENZA Controllo a dinamica inversa nello spazio operativo u = B(q)y + n(q, q) q = y B 1 (q)j T (q)h e scelta del nuovo ingresso di controllo y = J 1 A ( 1 (q)md Md ẍ d + K D x + KP x M d JA (q, q) q ) M d x + KD x + KP x = M d B 1 A (q)h A B A (q) = J T A 1 (q)b(q)ja (q) Impedenza meccanica massa M d smorzamento K D rigidezza K P

11 Integrazione della misura della forza di contatto u = B(q)y + n(q, q) + J T (q)h e y = J 1 A ( ) 1 (q)md Md ẍ d +K D x+kp x M d JA (q, q) q h A M d x + KD x + KP x = h A impedenza lineare attiva

12 Per evitare problemi di singolarità di rappresentazione... terna desiderata tempo variante x = J Ad (q, x) q + b( x, R d, ȯ d, ω d ) [ R T b( x, R d, ȯ d, ω d ) = d ȯ d + S(ωd d ] )od d,e T 1 (φ d,e )ωd d x = J Ad q J Ad q + ḃ nuovo ingresso di controllo y = J 1 A d M 1 ( d KD x + KP x M d JAd q + M d ḃ h d e) M d x + KD x + KP x = h d e

13 Dispositivi meccanici di impedenza passiva

14 Esempio Controllo di impedenza con misura di forza M d = diag{m dx, m dy } K D = diag{k Dx, k Dy } K P = diag{k Px, k Py } dinamica del sistema manipolatore ambiente lungo le due direzioni dello spazio operativo (x d costante) m dx ẍ e + k Dx ẋ e + (k Px + k x )x e = k x x r + k Px x d m dy ÿ e + k Dy ẏ e + k Py y e = k Py y d

15 Ambiente (x r = 1) più cedevole k x = 10 3 N/m meno cedevole k x = 10 4 N/m Controllo m dx = m dy = 100 k Dx = k Dy = 500 k Px = k Py = 2500 Manipolatore x e = [ 1 0 ] T x d = [ ] T pos organo terminale y forza organo terminale x [m] [N] [s] [s]

16 Controllo di ammettenza Impedenza meccanica tra terna desiderata e terna cedevole t M t z + KDt z + KPt z = h d e comportamento cedevole durante l interazione con l ambiente buona robustezza dell anello interno di controllo del moto (maggiore banda passante)

17 CONTROLLO DI FORZA Controllo di cedevolezza o impedenza = controllo indiretto di forza Controllo diretto azione di controllo PD sulla forza (misura corrotta da rumore) chiusura di un anello esterno di regolazione della forza = ingresso di riferimento per uno schema di controllo posizionale a dinamica inversa Ipotesi: sole variabili di posizione modello di ambiente elastico f e = K(x e x r )

18 Controllo di forza con anello interno di posizione u = B(q)y + n(q, q) + J T (q)h e y = J 1 (q)m 1 ( ) d KD ẋ e + K P (x F x e ) M d J(q, q) q M d ẍ e + K D ẋ e + K P x e = K P x F Scelta del controllo x F = C F (f d f e ) M d ẍ e + K D ẋ e + K P (I 3 + C F K)x e = K P C F (Kx r + f d )

19 Azione PI C F = K F + K I t ( ) dς scelta di K D, K P, K F, K I = margini di stabilità + banda passante All equilibrio (stabile) f e = f d Kx e = Kx r + f d

20 Controllo di forza con anello interno di velocità y = J 1 (q)m 1 ( ) d KD ẋ e + K P x F M d J(q, q) q x F = K F (f d f e ) M d ẍ e + K D ẋ e + K P K F Kx e = K P K F (Kx r + f d ) All equilibrio (stabile) f e = f d Kx e = Kx r + f d attenzione dinamiche non modellate (assenza azione integrale)

21 Controllo parallelo forza/posizione se f d / R(K) = deriva di x e se f d R(K) = x e (o ẋ e ) 0 Aggiunta di x d y = J 1 (q)m 1 ( d KD ẋ e + K P ( x + x F ) M d JA (q, q) q ) All equilibrio f e = f d x e = x d + C F ( K(xr x e ) + f d )

22 Esempio Impedenza (M d, K D, K P ) come sopra Riferimenti f d = [ 10 0 ] T x d = [ ] T Controllo di forza C F = diag{c Fx, 0} 1. controllo con anello interno di posizione k Fx = k Ix = controllo con anello interno di velocità k Fx = controllo parallelo k Fx = k Ix =

23 1.02 pos organo terminale x 50 forza organo terminale x [m] [s] [N] [s] 1.02 pos organo terminale x 50 forza organo terminale x [m] [s] [N] [s] 1.02 pos organo terminale x 50 forza organo terminale x [m] [s] [N] [s]

24 MOTO VINCOLATO Situazione di contatto reale vincoli cinematici imposti dall ambiente sul moto dell organo terminale durante il moto vincolato, l organo terminale può esercitare forze e momenti di tipo dinamico la forza e il momento di contatto possono dipendere dalla cedevolezza strutturale del manipolatore, a quella del sensore di forza o di dispositivi deformazioni locali delle parti a contatto (aree di contatto distribuito), attrito statico e dinamico Ipotesi semplificative per il controllo manipolatore + ambiente perfettamente rigidi, vincoli puramente cinematici manipolatore perfettamente rigido + cedevolezza localizzata nell ambiente

25 Ambiente rigido Equazioni di vincolo (vincoli olonomi) ϕ(q) = 0 J ϕ (q) q = 0 J ϕ (q) = ϕ/ q: Jacobiano di vincolo (m 6) In assenza di attrito J ϕ (q)δq = 0 principio dei lavori virtuali τ = J T ϕ (q)λ (ipotesi di contatto mantenuto) h e = J T (q)τ = S f (q)λ λ = S f (q)h e S f = (ST f WS f) 1 Sf T W Problema invarianza della soluzione

26 J ϕ (q) q = 0 S f = J T (q)jϕ T (q) J ϕ (q)j 1 (q)j(q) q = Sf T v e = 0 Reciprocità h T e v e = 0 R(S f ) sottospazio di controllo di forza R(S v ) sottospazio di controllo di velocità v e = S v (q)ν S T f (q)s v(q) = O ν = S v (q)v e Matrici di selezione: S f, S v, S f, S v proiettore in R(S f ): P f = S f S f proiettore in R(S v ): P v = S v S v

27 Ambiente cedevole Moto parzialmente vincolato ambiente modellato come coppia di corpi rigidi S e R h e = S f λ dx r,s = Ch e spostamento elementare rispetto alla posa di equilibrio dx r,e = dx v + dx f dx v = P v dx r,e dx f = (I 6 P v )dx r,e = (I 6 P v )dx r,s Sf T dx r,e = Sf T dx r,s = Sf T CS fλ h e = K dx r,e K = S f (Sf T CS f ) 1 Sf T dx f = C h e C = (I 6 P v )C possibilità di contatto rigido in alcune direzioni

28 VINCOLI NATURALI E VINCOLI ARTIFICIALI Riferimenti di forza e velocità scelti in maniera congruente all ambiente non è possibile imporre sia una forza che una velocità lungo la stessa direzione Analisi cineto-statica dell interazione vincoli naturali imposti dall ambiente lungo ciascun grado di libertà del compito (velocità o forza) vincoli artificiali soggetti al controllo lungo ciascun grado di libertà del compito (forza o velocità) Terna di vincolo O c x c y c z c descrizione semplificata del compito in termini di vincoli naturali e vincoli artificiali

29 Analisi di casi Scivolamento su superficie piana Vincoli Naturali ȯ c z ωx c ωy c fx c fy c Vincoli Artificiali fz c µ c x µ c y ȯ c x ȯ c y µ c z ω c z S f = S v =

30 in caso di piano elasticamente cedevole (lungo z c, intorno a x c, y c ) K m = c 3,3 c 3,4 c 3,5 c 4,3 c 4,4 c 4,5 c 5,3 c 5,4 c 5,5 1

31 Inserimento in foro cilindrico Vincoli Naturali ȯ c x ȯ c y ωx c ωy c fz c Vincoli Artificiali fx c fy c µ c x µ c y ȯ c z µ c z ω c z S f = S v =

32 Rotazione di una manovella Vincoli Naturali ȯ c x ȯ c z ωx c ωy c fy c Vincoli Artificiali fx c fz c µ c x µ c y ȯ c y µ c z ω c z S f = S v =

33 CONTROLLO IBRIDO FORZA/MOTO Modello dinamico nello spazio operativo v e = J(q) q + J(q) q B e (q) v e + n e (q, q) = γ e h e B e = J T BJ 1 n e = J T (C q + g) B e J q Ambiente cedevole dx r,e = P v dx r,e + C S f λ. (terna r fissa) v e = S v ν + C S f λ ν = S v(q)v e C = (I 6 P v )C terna comune terna base, Ṡv = O, Ṡf = O, Ċ = O v e = S v ν + C S f λ controllo a dinamica inversa γ e = B e (q)α + n e (q, q) + h e

34 v e = α Controllo ibrido forza/velocità α = S v α ν + C S f f λ ν = α ν λ = f λ controllo di velocità t α ν = ν d + K Pν (ν d ν) + K Iν (ν d (ς) ν(ς))dς 0 controllo di forza f λ = λ d + K Dλ ( λ d λ) + K Pλ (λ d λ) λ = S fḣe... λ = S f K J(q) q

35 funziona anche in caso di stima disponibile Ĉ

36 Esempio ] ] [ Sf c 0 = 1 [ ] Pf c 0 0 = 0 1 [ Sv c 1 = 0 [ ] Pv c 1 0 = 0 0 [ ] [ K c 0 0 = 0 c 1 C c 0 0 = 2,2 0 c 2,2 [ ] 1/ 2 1/ 2 R c = 1/ 2 1/ 2 v c e = Sc v ν + C c S c f λ ]

37 se f c e = [ f c x f c y ] T, v c e = [ ȯ c x ȯ c y ] T ν = ȯ c x, λ = fc y in terna base S f = R c S c f = [ 1/ 2 1/ 2 ] [ ] S v = R c Sv c 1/ 2 = 1/ 2 C = R c C c R T c = c 2,2 controllo ibrido forza/velocità [ 1/2 1/2 1/2 1/2 ] ν = ö c x = α ν λ = f c y = f λ

38 Ambiente rigido B e (q) v e + n e (q, q) = γ e h e h e = S f λ ( ) λ = B f (q) Sf T Be 1 (q)(γ e n e (q, q)) + ṠT f v e B f = (Sf T B 1 e S f) 1 B e (q) v e + S f B f (q)ṡt f v e = P(q)(γ e n e (q, q)) P = I 6 S f B f S T f B 1 e modello dinamico di ordine ridotto v e = S v ν + Ṡvν 6 m equazioni indipendenti B v (q) ν = S T v ( ) γ e n e (q, q) B e (q)ṡvν ( ) λ = B f (q)sf T Be 1 (q) γ e n e (q, q) B e (q)ṡvν

39 Controllo ibrido forza/velocità γ e = B e (q)s v α v + S f f λ + n e (q, q) + B e (q)ṡvν ν = α ν λ = f λ controllo di forza f λ = λ d + K Pλ (λ d λ) controllo di posizione t f λ = λ d + K Iλ (λ d (ς) λ(ς))dς 0 α ν = r d + K Dr (ṙ d ν) + K Pr (r d r)