Taratura Cinematica – stima dell`incertezza vettoriale
by user
Comments
Transcript
Taratura Cinematica – stima dell`incertezza vettoriale
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à 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 M. De Cecco - Lucidi del corso di Robotica e Sensor Fusion Taratura Cinematica In ipotesi di distribuzione gaussiana degli effetti interferenti, l’accuratezza si stima tramite varianza definita come: { 1 n 2 2 s = (xi - m ) º E (x - m ) å n - 1 i =1 2 } 1 n m = å xi = E{xi } n i =1 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 [x1 … xl], 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: 1 l C º å Dxi × Dxi T = E {Dx × DxT } l i =1 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: 2 é ù ( ( ) ) ( ) ( ) D x D x i ... D x i × D x i é ù 1 1 1 m ú 1 l ê 1 l ê ú C = å ... × [Dx1 ... Dxm ]= å ê ... ... ... ú ú l i =1 ê l i =1 2 êDx1 (i )× Dxm (i ) ... ú ( ( ) ) êëDxm úû D x i m ë û l il numero di ripetizioni Come si impiega tale matrice per la stima dell’incertezza? … ripassiamo il caso scalare M. De Cecco - Lucidi del corso di Robotica e Sensor Fusion Taratura Cinematica – stima dell’incertezza 1 Probability Density Function PDF p(zi >z) 0.9 0.8 0.7 0.6 0.5 0.4 p(zi<z) = F(z) 0.3 0.2 5 0.1 (x - x0 ) - 2 P (x ) = 1 2p × s P ( x - m < s )= m +s ò m s - e 0 -3 2×s 2 z -2 -1 0 1 2 3 Valori notevoli: 1 2p × s e - (x - x0 )2 2×s 2 × dx p( x ) 0.680 p( x 2 ) 0.950 p( x 3 ) 0.997 M. De Cecco - Lucidi del corso di Robotica e Sensor Fusion Taratura Cinematica – stima dell’incertezza Intervallo pari a ±2 x: posizione Intervallo pari a ±2 x0: posizione attesa (media) … quindi possiamo dire che la posizione del manipolatore è pari a: • x0 ± 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-2e Sensor x+2] M. De Cecco - Lucidi del corso di Robotica Fusion Taratura Cinematica – stima dell’incertezza scalare Densità di probabilità bidimensionale: Le due coordinate possono rappresentare la posizione nel piano della pinza di un robot planare (x - x 0 ) - T P (x ) = 1 (2p ) n e ×C -1 ×(x - x 0 ) 2 C 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 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? P (x ) = e - 1 2p × s (x - x0 )2 2×s 2 e 2 x - x0 ) ( - 2×s 2 =k = k × 2p × s k x0 (x - x0 ) 2 ( ) = - Ln k × 2p × s × 2 × s 2 º k ' x = x0 ± k ' M. De Cecco - Lucidi del corso di Robotica e Sensor Fusion Taratura Cinematica – stima dell’incertezza vettoriale Analogamente nel caso vettoriale: (x - x0 ) - T 1 P (x ) = (2p ) n (x - x0 ) T e - e ×C -1 ×(x - x0 ) 2 C ×C ×(x - x0 ) 2 =k -1 =k× (2p ) C n T x x ( 0 ) × C -1 × (x - x0 ) = -2 × Ln æç k × è 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! n 2 p ( ) C ö÷ º k ' > 0 ø M. De Cecco - Lucidi del corso di Robotica e Sensor Fusion Taratura Cinematica – stima dell’incertezza vettoriale (x - x0 ) × C -1 × (x - x0 ) = k ' T S1 v Se opero la seguente trasformazione: x x ® A × x + x0 x0 S0 xT × AT × C -1 × A × x = k ' é - a1 x ×ê ë- a2 T é| - ù -1 ê × C × a1 ú ê -û êë | |ù é| A = ê a1 a 2 ú ê ú | úû êë | | ù a 2 ú × x = xT ú | úû matrice degli autovettori di C é - a1 ×ê ë- a2 é | - ù êê 1 × × a1 ú - û ê a1 ê ë | ù ú 1 × a2 ú × x = k ' ú a2 ú | û | M. De Cecco - Lucidi del corso di Robotica e Sensor Fusion Taratura Cinematica – stima dell’incertezza vettoriale é - a1 x ×ê ë- a2 T [x é | - ù êê 1 × × a1 ú - û ê a1 ê ë | é1 êa y ]× ê 1 ê ê0 ë ù ú 1 × a 2 ú × x = xT ú a2 ú | û | é1 êa 1 ×ê ê ê0 ë ù 0ú ú× x = k' 1ú NOTA: la matrice C-1 è a 2 úû simmetrica e quindi gli autovettori sono ortogonali ù 0ú 1 ed 2 sono gli é x ù x2 y2 ú×ê ú = + =k' autovalori di C 1 ú ë y û a1 a 2 a 2 úû Equazione dell’ellisse nelle coordinate S1 x ® A × x + x0 Cosa rappresenta A??? Cosa rappresentano 1 2 ??? M. De Cecco - Lucidi del corso di Robotica e Sensor Fusion Taratura Cinematica – stima dell’incertezza vettoriale Nota sugli autovalori ed autovettori della matrice inversa é - a1 x ×ê ë- a2 T é | -ù ê 1 × ê × a1 ú - û ê a1 ê ë | ù ú 1 × a 2 ú × x = xT ú a2 ú | û | C ×a =a ×a Þ 1 C -1 × a = × a a é1 êa 1 ×ê ê ê0 ë ù 0ú ú× x = k' 1ú a 2 úû 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 M. De Cecco - Lucidi del corso di Robotica e Sensor Fusion Taratura Cinematica – stima dell’incertezza vettoriale Cosa rappresenta A: x ® A× x + x 0 S1 x 0 x1 x00 S0 T x x ( 0 ) × C -1 × (x - x0 ) = k ' 1 0 0 x0 x1 x00 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: x2 a1 + y2 a2 =k' 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% M. De Cecco - Lucidi del corso di Robotica e Sensor Fusion Taratura Cinematica – stima dell’incertezza vettoriale % 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 M. De Cecco - Lucidi del corso di Robotica e Sensor Fusion Esempio - Stima dell’incertezza di posizionamento % 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 M. De Cecco - Lucidi del corso di Robotica e Sensor Fusion Esempio - Stima dell’incertezza di posizionamento % 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 M. De Cecco - Lucidi del corso di Robotica e Sensor Fusion Esempio - Stima dell’incertezza di posizionamento % 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 M. De Cecco - Lucidi del corso di Robotica e Sensor Fusion Esempio - Stima dell’incertezza di posizionamento Se andiamo ad ingrandire la zona della posizione nominale dell’organo finale: - 0.00001ù é 0.0003 C=ê ú ë- 0.00001 0.00022 û v2 Che restituisce i seguenti autovalori ed autovettori: v1 s 12 = 0.00019 s 22 = 0.00030 v1 = [0.074 - 0.99] v2 = [- 0.99 - 0.074] s 1 = 0.014 s 22 = 0.017 M. De Cecco - Lucidi del corso di Robotica e Sensor Fusion Esempio - Stima dell’incertezza di posizionamento % 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 Come sarà disposto adesso l’ellisse? Che autovalori ed autovettori avrà? M. De Cecco - Lucidi del corso di Robotica e Sensor Fusion Esempio - Stima dell’incertezza di posizionamento % 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 M. De Cecco - Lucidi del corso di Robotica e Sensor Fusion Esempio - Stima dell’incertezza di posizionamento Se andiamo ad ingrandire la zona della posizione nominale dell’organo finale: é 0.00023 - 0.00003ù C=ê ú ë- 0.00003 0.00042 û v2 v1 s 12 = 0.00023 s 22 = 0.00042 v1 = [- 0.98 0.14] v2 = [0.14 0.98] s 1 = 0.015 s 22 = 0.020 M. De Cecco - Lucidi del corso di Robotica e Sensor Fusion Esempio - Stima dell’incertezza di posizionamento % 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 M. De Cecco - Lucidi del corso di Robotica e Sensor Fusion Esempio - Stima dell’incertezza di posizionamento Se andiamo ad ingrandire la zona della posizione nominale dell’organo finale: é0.00148 0.00126ù C=ê ú 0.00126 0.00173 ë û v1 s = 0.00033 2 1 v2 s 22 = 0.0029 v1 = [-0.74 0.67] v2 = [0.67 0.74 ] Notare la corrispondenza tra autovettori ed autovalori associati s 1 = 0.018 s 22 = 0.053 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 !!! % 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 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) M. De Cecco - Lucidi del corso di Robotica e Sensor Fusion Esempio - Stima dell’incertezza di posizionamento 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 M. De Cecco - Lucidi del corso di Robotica e Sensor Fusion Taratura Cinematica – Stima esplicita dell’incertezza Abbiamo a disposizione un modello (espresso attraverso una generica funzione f ) che lega una variabile y ai parametri xi (supponiamo i = 2 per semplicità) y = f (x1, x2 ) vogliamo stimare l’incertezza su y,conoscendo le incertezze sulle singole xi. Calcoliamo lo sviluppo in serie di Taylor troncato al primo ordine: ¶f ¶f y » f (x1 , x2 ) + d x1 + d x2 ¶x1 ¶x2 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: 1 n 2 s º å ( yi - m ) º E n i =1 2 y {( y - m ) } 2 1 n m = å yi º E {y} n i =1 nel caso in questione (ipotizzando densità di probabilità gaussiana a media nulla per ciascun parametro): ì ü ¶f ¶f m = E {y}= E í f (x1 , x2 ) + d x1 + d x2 ý = f (x1 , x2 ) ¶x1 ¶x2 î þ Ricordando: y » f (x1 , x2 ) + { s y2 = E ( y - m ) 2 ¶f ¶f d x1 + d x2 ¶x1 ¶x2 2 ìïæ ¶f ö üï ¶f = E íç d x1 + d x2 ÷ ý ¶x2 ø ïþ ïîè ¶x1 } M. De Cecco - Lucidi del corso di Robotica e Sensor Fusion Taratura Cinematica – Stima esplicita dell’incertezza E quindi: Nel caso di non correlazione tra i due parametri =0 2 ìïæ ¶f ö2 üï æ ¶f ö ¶f ¶f 2 2 2 s y = E íç d x1 × d x2 ý ÷ × (d x1 ) + ç ÷ × (d x2 ) + 2 ¶x1 ¶x2 è ¶x2 ø ïîè ¶x1 ø ïþ 2 { 2 } { } 2 æ ¶f ö æ ¶f ö æ ¶f ö 2 2 =ç ÷ × E (d x1 ) + ç ÷ × E (d x2 ) = ç ÷ × s x1 è ¶x1 ø è ¶x2 ø è ¶x1 ø ( ) 2 æ ¶f ö +ç ÷ × s x2 è ¶x2 ø Che fornisce la classica formula di propagazione dell’incertezza combinata (di Kline Mc Klintoc): 2 æ ¶f ö sy = ç ÷ × s x1 è ¶x1 ø ( ) 2 2 æ ¶f ö +ç ÷ × s x2 è ¶x2 ø 2 ( ) 2 M. De Cecco - Lucidi del corso di Robotica e Sensor Fusion Taratura Cinematica – Stima esplicita dell’incertezza ( ) 2 Dunque nel caso vettoriale (supponiamo per semplicità un vettore a due componenti): é y1 ù ê y ú = f (x1 , x2 ) ë 2û d y = Áf ×d x { } C y = E ( y - m )× ( y - m ) = E {d y × d y T } T dove f è lo Jacobiano della funzione f E quindi: C y = E {d y × d y T }= E {Á f × d x × d xT × ÁTf } = Á f × E {d x × d xT }× ÁTf = Á f × Cx × ÁTf C y = Á f × Cx × ÁTf M. De Cecco - Lucidi del corso di Robotica e Sensor Fusion Taratura Cinematica – Stima esplicita dell’incertezza C y = Á f × Cx × ÁTf non è altro se non una mera generalizzazione vettoriale della formula di propagazione dell’incertezza combinata Verifichiamolo: é ¶f1 ê ¶x Cy = ê 1 ê ¶f 2 ê ¶x ë 1 é ¶f1 2 ê ¶x s x1 1 =ê ê ¶f 2 2 ê ¶x s x1 ë 1 ì é (d x )2 d x × d x ù ü ì ü d x é ù ï ï ï 1 1 2 ï 1 úý E {d x × d xT }= E í ê × [d x1 d x2 ]ý = E í ê ú 2 d x îï ë 2 û þï ïî êëd x2 × d x1 (d x2 ) úû ïþ ¶f1 ù é ¶f1 ¶f 2 ù ¶x2 ú és x21 0 ù ê ¶x1 ¶x1 ú ú×ê ú ×ê 2 ú ¶f 2 ú êë 0 s x2 úû ê ¶f1 ¶f 2 ú ê ¶x ¶x ú ¶x2 úû 2û ë 2 = s 2f1 ¶f1 2 ù é ¶f1 ¶f 2 ù 2 éæ ¶f ö2 s x2 ú ê æ ¶f1 ö 2 2 ú 1 ¶x2 ¶x ¶x1 s + s ê ú×ê 1 ú = êç ¶x ÷ x1 ç ¶x ÷ x2 è 2ø ¶f 2 2 ú ê ¶f1 ¶f 2 ú è 1 ø ê s x2 ú ê ... ú ë ¶x2 ¶ x ¶ x 2 2 û ë û ù ...ú ú ...úû M. De Cecco - Lucidi del corso di Robotica e Sensor Fusion Taratura Cinematica – Stima esplicita dell’incertezza Torniamo al caso del manipolatore planare: é p1 ù éa1 × cos (J1 ) + a2 × cos (J1 + J2 )ù p =ê ú=ê ú ë p2 û êëa1 × sin (J1 ) + a2 × sin (J1 + J2 ) úû b e Considerando solo i parametri angolo (non i parametri lunghezze dei bracci): [1 2] é-a1 × sin (J1 ) - a2 × sin (J1 + J2 ) -a2 × sin (J1 + J2 )ù Áp = ê ú a × cos J + a × cos J + J a × cos J + J ( 1) 2 ( 1 2) 2 ( 1 2)û ë 1 Lo Jacobiano è funzione della postura del robot M. De Cecco - Lucidi del corso di Robotica e Sensor Fusion Esempio – Stima esplicita dell’incertezza Andiamo a verificare un esempio: % 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 J1 = 90° J2 = -180° Þ é -a1 + a2 Áp = ê ë 0 s J1 = s J 2 = p 180 s J2 = 0.0003 a2 ù é 0 1 ù =ê ú 0 û ë0 0úû = 0.017 s x = 0.0003 = 0.017 Þ ±3ó x = 0.105 intervallo 0 ù é 0 0ù é0 1ù é0.0003 Cp = ê ×ê ×ê ú ú 0.0003û ë1 0úû ë0 0û ë 0 é0 0.0003ù é0 0ù é0.0003 0ù =ê ×ê =ê ú ú ú 0 0 1 0 0 0 ë û ë û ë û M. De Cecco - Lucidi del corso di Robotica e Sensor Fusion Esempio – Stima esplicita dell’incertezza Si risolva il caso di incertezza anche sui parametri di lunghezza dei bracci (considerandoli scorrelati, ovvero ipotizzando alcune correlazioni): é p1 ù éa1 × cos (J1 ) + a2 × cos (J1 + J2 )ù p (J1 ,J2 , a1 , a2 ) = ê ú = ê ú p a × sin J + a × sin J + J ( 1 ) 2 ( 1 2 ) úû ë 2 û êë 1 b e é-a × sin (J1 ) - a2 × sin (J1 + J2 ) -a2 × sin (J1 + J2 ) cos (J1 ) cos (J1 + J2 )ù Áp = ê 1 ú a × cos J + a × cos J + J a × cos J + J sin J sin J + J ( ) ( ) ( ) ( ) ( ) 1 2 1 2 2 1 2 1 1 2 û ë 1 é-a1 × sin (J1 ) - a2 × sin (J1 + J2 ) -a2 × sin (J1 + J2 ) cos (J1 ) Cp = ê ë a1 × cos (J1 ) + a2 × cos (J1 + J2 ) a2 × cos (J1 + J2 ) sin (J1 ) és J21 ê cos (J1 + J2 )ù ê 0 ú× sin (J1 + J2 )û ê 0 ê ëê 0 0 sJ 2 0 0 0 ù é - a1 × sin (J1 ) - a2 × sin (J1 + J2 ) a1 × cos (J1 ) + a2 × cos (J1 + J2 )ù ú ê ú 0 0 ú ê -a2 × sin (J1 + J2 ) a2 × cos (J1 + J2 ) ú × ú s a21 ¹ 0 ú ê cos (J1 ) sin (J1 ) ú 2 ú ê ¹ 0 s a2 ûú êë cos (J1 + J2 ) sin (J1 + J2 ) úû 0 2 Se entrambi i bracci sono stati realizzati mediante la stessa macchina utensile M. De Cecco - Lucidi del corso di Robotica e Sensor Fusion Esempio – Stima esplicita dell’incertezza