M. De Cecco - Lucidi del corso di Robotica e Sensor Fusion Se i parametri di Denavit-Hartemberg non corrispondono con quelli di progetto a causa di tolleranze.

Presentazione sul tema: "M. De Cecco - Lucidi del corso di Robotica e Sensor Fusion Se i parametri di Denavit-Hartemberg non corrispondono con quelli di progetto a causa di tolleranze."— Transcript della presentazione:

1 M. De Cecco - Lucidi del corso di Robotica e Sensor Fusion Se i parametri di Denavit-Hartemberg non corrispondono con quelli di progetto a causa di tolleranze meccaniche, si può avere una errata corrispondenza tra la posizione realmente ottenuta imponendo dei valori alle variabili di giunto e la posizione calcolata tramite cinematica diretta. Tale accuratezza di posizionamento può dipendere dalla posizione nello spazio di lavoro. L’effetto appena descritto causa effetti sistematici. Altra cosa è la ripetibilità di posizionamento, essa dipende dalla struttura meccanica, dai trasduttori e dal controllo NOTA: Tramite taratura si possono compensare gli effetti sistematici ma non quelli casuali connessi con la ripetibilità Taratura Cinematica

2 M. De Cecco - Lucidi del corso di Robotica e Sensor Fusion Taratura Cinematica La procedura di taratura ha sempre come obiettivo: 1.determinare stime accurate dei parametri del modello matematico 2.Effettuare una stima dell’accuratezza del sistema in funzione del campo di lavoro La procedura si basa sulla raccolta di dati sperimentali ingresso-uscita confrontati con un sistema più accurato almeno di un ordine di grandezza Nel nostro caso è necessario un sistema di misura della posizione dell’organo terminale che abbia un volume comparabile ed un’accuratezza di un ordine di grandezza migliore rispetto all’applicazione cui il robot è demandato (la procedura non si basa sulla misura diretta dei parametri geometrici – del resto notevolmente più complessa) Nel nostro caso i parametri sono quelli di Denavit-Hartemberg ovvero della cinematica diretta

3 M. De Cecco - Lucidi del corso di Robotica e Sensor Fusion Taratura Cinematica – stima dell’incertezza In ipotesi di distribuzione gaussiana degli effetti interferenti, l’accuratezza si stima tramite varianza definita come: Nel caso di un robot si ha: che il problema non è scalare: la postura è un vettore in generale di 6 componenti che i valori dei residui [  x 1 …  x l ], se la procedura di taratura è arrivata al punto di compensare gli effetti sistematici, hanno valor medio circa zero Dunque si può stimare la matrice di covarianza come:

4 M. De Cecco - Lucidi del corso di Robotica e Sensor Fusion Taratura Cinematica – stima dell’incertezza Da cui scaturisce una matrice di dimensione m x m: Come si impiega tale matrice per la stima dell’incertezza? … ripassiamo il caso scalare l il numero di ripetizioni

5 M. De Cecco - Lucidi del corso di Robotica e Sensor Fusion Valori notevoli: px px px (). (). ().       0680 20950 30997 5 -3-20123 0 0.1 0.2 0.3 0.4 0.5 0.6 0.7 0.8 0.9 1 z p(z i <z) = F(z) p(z i >z) Taratura Cinematica – stima dell’incertezza Probability Density Function PDF

6 M. De Cecco - Lucidi del corso di Robotica e Sensor Fusion Taratura Cinematica – stima dell’incertezza scalare x: posizione Intervallo pari a ±2  x 0 : posizione attesa (media) Intervallo pari a ±2  … quindi possiamo dire che la posizione del manipolatore è pari a: x 0 ± 2  con il 95% di probabilità Ovvero che c’è una probabilità del 95% che la vera posizione del manipolatore cada all’interno dell’intervallo [x-2  x+2  ]

7 M. De Cecco - Lucidi del corso di Robotica e Sensor Fusion Taratura Cinematica – stima dell’incertezza vettoriale Densità di probabilità bidimensionale: Le due coordinate possono rappresentare la posizione nel piano della pinza di un robot planare

8 M. De Cecco - Lucidi del corso di Robotica e Sensor Fusion Taratura Cinematica – stima dell’incertezza vettoriale La funzione vista dall’alto rappresenta i punti di equiprobabilità che uniti tracciano delle ellissi che coincidono con la forma quadratica all’esponente della formula della gaussiana … nel caso scalare i punti di equiprobabilità erano sempre due equispaziati dalla media

9 M. De Cecco - Lucidi del corso di Robotica e Sensor Fusion Taratura Cinematica – stima dell’incertezza vettoriale Come si perviene all’intervallo nel caso scalare? x0x0 k

10 M. De Cecco - Lucidi del corso di Robotica e Sensor Fusion Taratura Cinematica – stima dell’incertezza vettoriale Analogamente nel caso vettoriale: Che è una forma quadratica associata ad una matrice definita positiva (tale matrice è proprio la matrice di Covarianza! Calcolata dagli scarti al quadrato) e quindi un ellisse!

11 M. De Cecco - Lucidi del corso di Robotica e Sensor Fusion x0x0 00 11 x v matrice degli autovettori di C Se opero la seguente trasformazione: Taratura Cinematica – stima dell’incertezza vettoriale

12 M. De Cecco - Lucidi del corso di Robotica e Sensor Fusion  1 ed  2 sono gli autovalori di C Equazione dell’ellisse nelle coordinate  1 Cosa rappresenta A??? Cosa rappresentano  1  2 ??? Taratura Cinematica – stima dell’incertezza vettoriale NOTA: la matrice C -1 è simmetrica e quindi gli autovettori sono ortogonali

13 M. De Cecco - Lucidi del corso di Robotica e Sensor Fusion Quindi gli autovettori di C e C -1 sono gli stessi, gli autovalori sono inversi, per questo  1 ed  2 sono gli autovalori di C Taratura Cinematica – stima dell’incertezza vettoriale Nota sugli autovalori ed autovettori della matrice inversa

14 M. De Cecco - Lucidi del corso di Robotica e Sensor Fusion 00 11 la trasformazione A rappresenta una trasformazione di coordinate: la matrice C è simmetrica e quindi gli autovettori sono tra loro ortogonali. Verosimilmente è dal sistema 1 allo 0. In questo modo l’equazione è un ellisse in forma canonica: Taratura Cinematica – stima dell’incertezza vettoriale Cosa rappresenta A:

15 M. De Cecco - Lucidi del corso di Robotica e Sensor Fusion Taratura Cinematica – stima dell’incertezza vettoriale La probabilità che la vera posizione del manipolatore sia all’interno di un ellisse di equiprobabilità è pari all’integrale della funzione all’interno dell’ellisse stesso Analogamente al caso scalare si ha che integrando all’interno dell’ellisse associato alla matrice con elementi pari a circa due volte gli autovalori la probabilità è 95% … in questo caso la probabilità che la vera posizione del manipolatore cada all’interno dell’ellisse tracciato in figura è del 95%

16 M. De Cecco - Lucidi del corso di Robotica e Sensor Fusion % Generazione stocastica delle variabili: Niter = 5000; a1 = normrnd(1, 10e-3, Niter, 1); a2 = normrnd(1, 10e-3, Niter, 1); teta1 = normrnd(pi/2, 1*pi/180, Niter, 1); teta2 = normrnd(-pi, 1*pi/180, Niter, 1); % Cinematica diretta: Px = a1.*cos(teta1) + a2.*cos(teta1+teta2); Py = a1.*sin(teta1) + a2.*sin(teta1+teta2); plot(Px, Py, '.'), axis([-1 1 -0.50 1]), grid on Esempio - Stima dell’incertezza di posizionamento

17 M. De Cecco - Lucidi del corso di Robotica e Sensor Fusion % Generazione stocastica delle variabili: Niter = 1000; a1 = normrnd(1, 10e-3, Niter, 1); a2 = normrnd(1, 10e-3, Niter, 1); teta1 = pi/2; %gli angoli sono conosciuti teta2 = -pi; % Cinematica diretta: Px = a1.*cos(teta1)+a2.*cos(teta1+teta2); Py = a1.*sin(teta1)+a2.*sin(teta1+teta2); plot(Px, Py, '.'), axis([-1 1 -0.50 1]), grid on Esempio - Stima dell’incertezza di posizionamento

18 M. De Cecco - Lucidi del corso di Robotica e Sensor Fusion % Generazione stocastica delle variabili: Niter = 1000; a1 = 1; a2 = 1; teta1 = normrnd(pi/2, 1*pi/180, Niter, 1); teta2 = normrnd(-pi, 1*pi/180, Niter, 1); % Cinematica Diretta: Px = a1.*cos(teta1)+a2.*cos(teta1+teta2); Py = a1.*sin(teta1)+a2.*sin(teta1+teta2); plot(Px, Py, '.'), axis([-1 1 -0.50 1]), grid on Esempio - Stima dell’incertezza di posizionamento

19 M. De Cecco - Lucidi del corso di Robotica e Sensor Fusion % Generazione stocastica delle variabili: Niter = 1000; a1 = normrnd(1, 10e-3, Niter, 1); a2 = normrnd(1, 10e-3, Niter, 1); teta1 = normrnd(pi/2, 1*pi/180, Niter, 1); teta2 = normrnd(-pi, 1*pi/180, Niter, 1); % Cinematica Diretta: Px = a1.*cos(teta1)+a2.*cos(teta1+teta2); Py = a1.*sin(teta1)+a2.*sin(teta1+teta2); plot(Px, Py, '.'), axis([-1 1 -0.50 1]), grid on Esempio - Stima dell’incertezza di posizionamento

20 M. De Cecco - Lucidi del corso di Robotica e Sensor Fusion Se andiamo ad ingrandire la zona della posizione nominale dell’organo finale: Che restituisce i seguenti autovalori ed autovettori: v1v1 v2v2 Esempio - Stima dell’incertezza di posizionamento

21 M. De Cecco - Lucidi del corso di Robotica e Sensor Fusion % Generazione stocastica delle variabili: Niter = 5000; a1 = normrnd(1, 10e-3, Niter, 1); a2 = normrnd(1, 10e-3, Niter, 1); teta1 = normrnd(pi/2, 1*pi/180, Niter, 1); teta2 = normrnd(-135*pi/180, 1*pi/180, Niter, 1); % Cinematica diretta: Px = a1.*cos(teta1)+a2.*cos(teta1+teta2); Py = a1.*sin(teta1)+a2.*sin(teta1+teta2); plot(Px, Py, '.'), axis([-1 1 -0.50 1]), grid on Esempio - Stima dell’incertezza di posizionamento Come sarà disposto adesso l’ellisse? Che autovalori ed autovettori avrà?

22 M. De Cecco - Lucidi del corso di Robotica e Sensor Fusion % Generazione stocastica delle variabili: Niter = 5000; a1 = normrnd(1, 10e-3, Niter, 1); a2 = normrnd(1, 10e-3, Niter, 1); teta1 = normrnd(pi/2, 1*pi/180, Niter, 1); teta2 = normrnd(-135*pi/180, 1*pi/180, Niter, 1); % Cinematica diretta: Px = a1.*cos(teta1)+a2.*cos(teta1+teta2); Py = a1.*sin(teta1)+a2.*sin(teta1+teta2); plot(Px, Py, '.'), axis([-1 1 -0.50 1]), grid on Esempio - Stima dell’incertezza di posizionamento

23 M. De Cecco - Lucidi del corso di Robotica e Sensor Fusion Se andiamo ad ingrandire la zona della posizione nominale dell’organo finale: Esempio - Stima dell’incertezza di posizionamento v2v2 v1v1

24 M. De Cecco - Lucidi del corso di Robotica e Sensor Fusion % Generazione stocastica delle variabili: Niter = 1000; a1 = normrnd(1, 10e-3, Niter, 1); a2 = normrnd(1, 10e-3, Niter, 1); teta1 = normrnd(pi/2, 1*pi/180, Niter, 1); teta2 = normrnd(-135*pi/180, 3*pi/180, Niter, 1); % Cinematica Diretta: Px = a1.*cos(teta1)+a2.*cos(teta1+teta2); Py = a1.*sin(teta1)+a2.*sin(teta1+teta2); plot(Px, Py, '.'), axis([-1 1 -0.50 1]), grid on Esempio - Stima dell’incertezza di posizionamento

25 M. De Cecco - Lucidi del corso di Robotica e Sensor Fusion Se andiamo ad ingrandire la zona della posizione nominale dell’organo finale: v2v2 v1v1 Notare la corrispondenza tra autovettori ed autovalori associati Esempio - Stima dell’incertezza di posizionamento

26 M. De Cecco - Lucidi del corso di Robotica e Sensor Fusion Esempio - Stima dell’incertezza di posizionamento Esempi di applicazione della stima dell’incertezza: - robot di misura (ovvio) - inserimento verticale di un oggetto !!! In questo caso in quale postura programmereste il robot per una operazione di inserimento verticale? (una applicazione in cui è importante centrare il foro ma meno la profondità di inserimento) % Generazione stocastica delle variabili: Niter = 1000; a1 = normrnd(1, 10e-3, Niter, 1); a2 = normrnd(1, 10e-3, Niter, 1); teta1 = normrnd( ?, 1*pi/180, Niter, 1); teta2 = normrnd( ?, 3*pi/180, Niter, 1); % Cinematica Diretta: Px = a1.*cos(teta1)+a2.*cos(teta1+teta2); Py = a1.*sin(teta1)+a2.*sin(teta1+teta2); plot(Px, Py, '.'), axis([-1 1 -0.50 1]), grid on

27 M. De Cecco - Lucidi del corso di Robotica e Sensor Fusion Fino adesso abbiamo usato la conoscenza del modello matematico (cinematica diretta) e della densità di probabilità (supposta gaussiana) associata con i suoi parametri per determinare la densità di probabilità associata con il vettore posizione impiegando la simulazione Montecarlo Se però si vuole la stima dell’incertezza in tempo reale è necessario determinare formule chiuse di propagazione per l’espressione della densità di probabilità, ovvero della matrice di covarianza … partiamo di nuovo dal caso scalare Taratura Cinematica – Stima esplicita dell’incertezza

28 M. De Cecco - Lucidi del corso di Robotica e Sensor Fusion Abbiamo a disposizione un modello (espresso attraverso una generica funzione f ) che lega una variabile y ai parametri x i (supponiamo i = 2 per semplicità) vogliamo stimare l’incertezza su y,conoscendo le incertezze sulle singole x i. Calcoliamo lo sviluppo in serie di Taylor troncato al primo ordine: Taratura Cinematica – Stima esplicita dell’incertezza

29 M. De Cecco - Lucidi del corso di Robotica e Sensor Fusion Taratura Cinematica – Stima esplicita dell’incertezza Determiniamo la varianza secondo la classica definizione, impiegando l’operatore E{.} definito valore atteso (Expected value) a cui sappiamo corrispondere una operazione di media: nel caso in questione (ipotizzando densità di probabilità gaussiana a media nulla per ciascun parametro): Ricordando:

30 M. De Cecco - Lucidi del corso di Robotica e Sensor Fusion E quindi: Che fornisce la classica formula di propagazione dell’incertezza combinata (di Kline Mc Klintoc): Nel caso di non correlazione tra i due parametri = 0 Taratura Cinematica – Stima esplicita dell’incertezza

31 M. De Cecco - Lucidi del corso di Robotica e Sensor Fusion Dunque nel caso vettoriale (supponiamo per semplicità un vettore a due componenti): Taratura Cinematica – Stima esplicita dell’incertezza dove  f è lo Jacobiano della funzione f E quindi:

32 M. De Cecco - Lucidi del corso di Robotica e Sensor Fusion non è altro se non una mera generalizzazione vettoriale della formula di propagazione dell’incertezza combinata Verifichiamolo: Taratura Cinematica – Stima esplicita dell’incertezza

33 M. De Cecco - Lucidi del corso di Robotica e Sensor Fusion Esempio – Stima esplicita dell’incertezza Torniamo al caso del manipolatore planare: Considerando solo i parametri angolo (non i parametri lunghezze dei bracci): [  1  2 ] Lo Jacobiano è funzione della postura del robot

34 M. De Cecco - Lucidi del corso di Robotica e Sensor Fusion Esempio – Stima esplicita dell’incertezza % Generazione stocastica delle variabili: Niter = 1000; a1 = 1; a2 = 1; teta1 = normrnd(pi/2, 1*pi/180, Niter, 1); teta2 = normrnd(-pi, 1*pi/180, Niter, 1); % Cinematica Diretta: Px = a1.*cos(teta1)+a2.*cos(teta1+teta2); Py = a1.*sin(teta1)+a2.*sin(teta1+teta2); plot(Px, Py, '.'), axis([-1 1 -0.50 1]), grid on Andiamo a verificare un esempio: intervallo

35 M. De Cecco - Lucidi del corso di Robotica e Sensor Fusion Si risolva il caso di incertezza anche sui parametri di lunghezza dei bracci (considerandoli scorrelati, ovvero ipotizzando alcune correlazioni): Esempio – Stima esplicita dell’incertezza Se entrambi i bracci sono stati realizzati mediante la stessa macchina utensile