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-2e 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