Controllo ibrido di forza/velocità

Transcript

1 Corso di Robotica 2 Controllo ibrido di forza/velocità Prof. Alessandro De Luca A. De Luca

2 Vincoli naturali e artificiali il contatto fra robot e ambiente (rigido e privo di attrito = puramente geometrico) vincola in modo naturale il moto dell E-E in condizioni ideali (robot e ambiente perfettamente rigidi, assenza di attrito), nello spazio del compito (task space) si possono individuare due set di direzioni generalizzate selezionate in modo che è possibile il moto dell E-E nel primo set di k direzioni (lungo cui l ambiente non può opporre alcuna reazione) esistono reazioni vincolari nel secondo set di 6-k direzioni (lungo cui l ambiente impedisce il moto dell E-E) vincoli naturali su forza e moto imposti dal compito queste direzioni sono ortogonali tra loro (in realtà complementari, ossia complessivamente coprono tutto lo spazio cartesiano) e definiscono un opportuno task frame SR t (tipicamente associato poi all E-E del robot) per compiti di interazione generali, posizione e orientamento del task frame dipendono dal tempo le specifiche di esecuzione del compito impongono poi vincoli artificiali in termini di valori desiderati (e imposti dal controllo) per le velocità, nelle k direzioni ammissibili di moto, e per le forze di contatto, nelle 6-k direzioni ammissibili di forza A. De Luca 2

3 SR t Task frame e vincoli - esempio 1 z compito: far slittare il cubo lungo il binario y vincoli naturali (geometrici) v y = v z = 0 6-k=4 ω x = ω z = 0 F x = M y = 0 k=2 x v = velocità lineare ω = velocità angolare F = forza M = momento 6-k=4 k=2 vincoli artificiali (imposti dal controllore) F y = F y,des = 0 M x = M x,des = 0, M z = M z,des = 0 F z = F z,des ω y = ω y,des = 0 v x = v x,des A. De Luca 3

4 Selezione delle direzioni - esempio 1 SR t z # ( ( # v " ( = 0 0( # 0 0 ( ) ( 0 1( 0 0 ( qui, costanti y v x " y ( = T ) # v x " y ( x " " " F # M = ( # # F y F z M x M z " = Y ( # A. De Luca 4 F y F z M x M z T T Y = 0 reazioni vincolari non possono compiere lavoro

5 Task frame e vincoli - esempio 2 x y z SR t (α) z 0 α SR0 y 0 x 0 compito: girare la manovella (manicotto folle) vincoli naturali v x = v z = 0 ω x = ω y = 0 F y = M z = 0 S 0 R z (α) S t (α) vincoli artificiali F x = F x,des = 0, F z = F z,des = 0 M x = M x,des = 0, M y = M y,des = 0 v y = v y,des ω z = ω z,des = 0 A. De Luca 5

6 Selezione delle direzioni - esempio 2 x y z SR t (α) z 0 α SR0 y 0 x 0 # 0 v 0 " # ( ( = # R ( T ()) 0 0 0( # 0 R T ( ()) 0 0 ( * ( 0 0( 0 1 ( v y " z ( = T()) * # v y " ( z " # 0 F 0 M " " = " R T (() # 0 R T (() ) # # F x F z M x M y " = Y(() ) # A. De Luca 6 F x F z M x M y T T (α) Y(α) = 0

7 Task frame e vincoli - esempio 3 compito: avvitare una vite in un foro filettato y SR t la vite si muove sia lungo che intorno l asse z, ma non in modo indipendente (1 dof) z x p = passo della vite vincoli naturali (parziali ) v x = v y = 0 ω x = ω y = 0 vincoli artificiali (abbondanti ) F x = F x,des = 0, F y = F y,des = 0 M x = M x,des = 0, M y = M y,des = 0 analogamente, F z ed M z non possono essere indipendenti v z = v z,des, ω z = ω z,des = (2π/p) v z,des F z = F z,des, M z = M z,des (F z,des ) le direzioni di forza sono ortogonali a quelle di moto A. De Luca 7

8 Selezione delle direzioni - esempio 3 # v " ( = ) p ( ) T * v z = T * v z k=1 y SR t z p le colonne di T ed Y non coincidono necessariamente con direzioni cartesiane (colonne della matrice identità) direzioni generalizzate x " F # M = Y: tale che T T Y = 0 " ( 2) p # F z = " 2# p M z " * # F x F y M x M y M z " = Y * # 6-k=5 F x F y M x M y M z A. De Luca 8

9 Terne di interesse - esempio 4 moto planare di un robot 2R in contatto con superficie (M=2) y 0 y t y e x t superficie ambientale x 0 x e terna SR t di task, per la definizione indipendente delle specifiche ibride (qui: t v x,des [k=1] e t F y,des [M-k=1]) e il calcolo degli errori per il controllo terna SR e del sensore (qui = SR 2 ) dove si misura la forza e F = ( e F x, e F y ) terna SR 0 di base nella quale è espressa la velocità dell organo terminale (qui, 0 v = ( 0 v x, 0 v y ) di O 2 ), ottenuta dal calcolo dello Jacobiano e dalle velocità di giunto servirà poi esprimere (ruotare) tutte le grandezze nella stessa terna di riferimento: quella di task A. De Luca 9

10 descrizione del contatto robot-ambiente: definisce il task frame dinamica robot Formalismo per il controllo ibrido # v " ( = T(s) ) s " F # M = Y(s) () parametrizza nei primi tre esempi precedenti e in generale M=6 parametrizza il moto libero dell E-E " # IR Mk le forze di contatto le forze di contatto non compiono lavoro sugli spostamenti dell E-E T T (s) Y(s) = 0 le direzioni generalizzate del task frame dipendono in generale da s (dalla posa dell E-E rispetto all ambiente) A. De Luca 10 + " B(q) q + S(q, q ) q + g(q) = u + J T (q) F # M cinematica robot s " IR k # v " ( = J(q) q

11 Controllo ibrido di forza/velocità obiettivo del controllo: imporre andamenti desiderati ai parametri di moto s e a quelli di forza λ s(t) s d (t), λ(t) λ d (t) legge di controllo progettata in due passi 1. disaccoppiamento e linearizzazione via feedback nel task frame modello ad anello chiuso # s " ( = # a s a ( " 2. sintesi (lineare) di a s e a λ per imporre la dinamica desiderata agli errori e s = s d s, ed e λ = λ d λ assunzioni: N = M (= 6, di solito), J(q) non singolare (+ T T Y = 0) A. De Luca 11

12 Feedback linearizzazione nel task space J(q) " q = T(s) " s J" q + J " q = T " s + T " s ( ) q = J "1 T # s + T # s " J # q " B(q) q + S(q, q ) q + g(q) = u + J T (q) F # M = u + JT (q)y(s) ( ) ( B(q)J "1 (q)t(s) "J T (q)y(s)) s # ) + B(q)J"1 (q) T (s) s " J (q) q ( matrice N x N non singolare sotto le ipotesi fatte u = ( BJ "1 T "J T Y) a s a ) # ( + BJ"1 T s " J q # s ( = # a s ( ( ) + S(q, ( ) + S * q + g q ) q + g(q) = u controllo disaccoppiante e linearizzante " a " M k λ ha grado relativo 0 A. De Luca 12 k s ha grado relativo 2

13 Stabilizzazione con a s e a λ al solito, sufficienti tecniche di controllo lineare (sui singoli canali ingressouscita scalari) a s = s d + K D ( s d " s ) + K P s d " s ( ) e s + K D e s + K P e s = 0 e s = s d " s # 0 a " = " d + K I (" d # ")d " # + K I " # = 0 " # = (# d #)d 0 a " = " d N.B. qui basterebbe ma non avremmo un errore di forza (scarsa robustezza). servono misure di s, s e λ A. De Luca 13

14 Filtraggio misure di posizione e forza.. s, s ricavate dalle misure di q e q, uguagliando le descrizioni di posa e velocità dell E-E dal punto di vista del robot (cinematica diretta e differenziale) e dal punto di vista dell ambiente (funzioni di s) esempio L r z 0 " L cos s 0 r = 0 f(q) = L sin s y 0 # 0 s = α x 0 J(q) " q = T(s) " s s = ATAN2( 0 f y (q), 0 f x (q)) s = T # (s)j(q) " q λ ricavato dalle misure delle forze/momenti all end-effector " F # M = Y(s) ( ) " = Y # # (s) F M ( pseudoinverse di matrici strette a rango pieno, ad es. (T T T) -1 T T (o anche pesate) A. De Luca 14

15 Schema a blocchi complessivo s d + K D s d + K P s d + λ d _ + _ + _ K I s + a s + a λ q, q feedback linearizzazione nel task space u robot in contatto ambiente q q " F # M M-k anelli di forza k anelli di moto K D s, s K P λ s s filtraggio misure casi limite k=m (moto nello spazio libero): nessun controllo di forza, solo di moto k=0 (E-E del robot bloccato ): nessun controllo di moto, solo di forza A. De Luca 15

16 Fonti di incongruenze fra misure 1. presenza di attrito al contatto c è una componente di forza nelle direzioni libere di velocità che si oppone al moto (se attrito di Coulomb, dipendente anche dalla forza normale applicata ) 2. cedevolezza nella struttura robotica e/o al contatto ci può essere uno spostamento nelle direzioni vincolate dalla reazione ambientale N.B. se però la geometria dell ambiente è nota con precisione, gli errori causati da 1. e 2. sulle misure di s e λ sono filtrati automaticamente dalla pseudo-inversione di T ed Y 3. incertezze sulla geometria dell ambiente nel contatto (da stimare real-time con sensori esterni: visione, ma anche forza) A. De Luca 16

17 Identificazione di un profilo incognito si può identificare un profilo incognito della superficie di contatto, a partire dalle misure di velocità e forza? 1. normale = direzione della forza misurata... ma la presenza di attrito provoca una rotazione della forza rispetto alla normale effettiva di un angolo γ (incognito) 2. tangente = direzione della velocità misurata... ma l elasticità nella struttura robotica e/o al contatto possono indurre una componente di velocità lungo la normale 3. metodo misto (fusione sensoriale) con RLS a. tangente identificata in modo ricorsivo da misure di posizione b. angolo di attrito identificato in modo ricorsivo dalla tangente trovata in a. e dalle misure di forza A. De Luca 17 F n F F t = µ " F n = tg# " F n v t v n v il robot è mantenuto in moto di contatto in qualche modo

18 Identificazione e controllo ibrido effettuati in modo simultaneo robot MIMO-CRF (DIS, Laboratorio di Robotica, 1992) A. De Luca 18

19 Tangente stimata da misure di posizione (superficie circolare percorsa a velocità costante) [gradi] tempo [intervalli di campionamento] A. De Luca 19

20 Tangente stimata da misure di forza (stessa superficie circolare percorsa a velocità costante) [gradi] tempo [intervalli di campionamento] A. De Luca 20

21 Differenza fra le tangenti stimate [gradi] differenze dell ordine dei ma qual è quella giusta? meglio una fusione sensoriale tempo [intervalli di campionamento] A. De Luca 21

22 Profilo superficie riconosciuta identificazione con metodo RLS (Recursive Least Squares), fondendo sia misure di forza che di velocità [m] era la pizza di un film (raggio = 17 cm) [m] A. De Luca 22

23 Forza normale regolata a 20 N durante il moto [N] i picchi corrispondono a scanalature sulla superficie tempo [intervalli di campionamento] A. De Luca 23

24 Sbavatura robotizzata di parabrezza parabrezza con spigoli vivi e tolleranze dimensionali dovute alla lavorazione e all eccesso di materiale collante (PVB) tra strati vetrosi del parabrezza il robot segue il percorso prefissato, nonostante gli errori sulla forma nominale, grazie alla cedevolezza passiva nella testa di sbavatura la forza di contatto fra lama e vetro può essere controllata, in modo indipendente dal moto, grazie a un cilindro pneumatico nella testa di sbavatura la testa di sbavatura è dotata di due lame per tagliare il PVB cella di carico per misurare la forza applicata sistema di controllo A. De Luca 24

25 Modello testa di sbavatura fisico matematico per l analisi della stabilità (con luogo delle radici) del controllo di forza in una singola direzione in presenza di masse/elasticità multiple come in questo caso, vedi Eppinger Seering, IEEE CSM, 1987 (materiale didattico) A. De Luca 25

26 Video di ricapitolazione compliance control (controllo attivo di cedevolezza senza sensore di forza) impedance control (controllo di impedenza) force control (controllo di forza come anello esterno ad un controllore di posizione) hybrid force/position control robot COMAU c/o Università di Napoli A. De Luca (controllo ibrido) 26