4.3 Flusso e scambio delle informazioni
4.3.2 Calibrazione del sensore e validazione
Nel capitolo precedente è stata illustrata la procedura eseguita dall’applicazione sul PC Windows per ricavare la posizione e l’orientamento della terna utensile H rispetto alla terna sensore S, cioè come si ottengono oS
H e RHS, mentre nel capitolo
2 è spiegato il procedimento per valutare la posa ergonomica a partire dai punti scheletrici riferiti rispetto alla terna W.
C’è bisogno di conoscere la posizione e l’orientamento della terna sensore S rispet- to alla terna base W, per poter riferire la posa dell’utensile e i punti scheletrici rispetto alla base del robot W, poiché, riprendendo la legge di controllo (1.1) de- scritta nel capitolo 1, si ha che tutte le quantità specificate sono riferite rispetto alla terna mondo W, infatti:
vO = ˙rO = KRv(rE− rH)
ωO = KRω∆EH
vOe ωO sono rispettivamente la velocià lineare e angolare della terna oggetto O (e
quindi le velocità dell’end effector del manipolatore), rH e rE sono rispettivamente
le coordinate delle origini delle terne di riferimento utensile ed ergonomica; tutte queste quantità sono espresse rispetto alla terna di riferimento assoluta mondo W (base del manipolatore robotico).
Kv
r e KRω sono le matrici di guadagno della legge di controllo, e sono definite
positive, mentre ∆EH è la parte vettoriale del quaternione unitario associato alla
matrice di rotazione
che rappresenta l’orientamento della terna ergonomica rispetto alla terna utensi- le.
Sapendo che rH ≡ oWH e RHW sono la posizione e l’orientamento della terna H
rispetto alla terna W, è necessario trovare il vettore oW
S e la matrice RSW che per-
mettono di trasformare le posizioni e gli orientamenti dal riferimento sensore S al riferimento W, così da ottenere:
RHW = RSWRHS oWH = oWS + RWS · oSH Inoltre utilizzare oW
S e RSW per trasformare le coordinate dei punti scheletrici, tro-
vati dal Kinect, dal riferimento del sensore al riferimento del robot, consente di ottenere, dopo le operazioni illustrate nell’ultima sezione del capitolo 2, la posizio- ne e l’orientamento della terna ergonomica E rispetto alla terna W direttamente, cioè rE ≡ oWE e REW rispettivamente.
Una volta stabilita la comunicazione tra i due computer, è stato possibile effet- tuare la calibrazione del sensore visivo rispetto al sistema di riferimento del robot. Modificando l’applicazione sul PC Windows perchè riportasse le coordinate 3D di una sola sfera anzichè tre, la calibrazione è stata effettuata attaccando una sfera arancione all’estremità del braccio del Comau, in maniera che fosse visibile dal sensore Kinect, quindi, spostando l’end effector del robot manualmente, sono state registrate 20 posizioni della sfera, rispetto alla base del manipolatore (terna W ) e rispetto al sensore (terna S).
Siano pS
i e pWi (con i = 1, . . . , 20) le coordinate delle posizioni della sfera nella
terna S e nella terna W rispettivamente.
Partendo dai valori assunti dalle variabili di giunto qj(mostrate a video dal WiTP)
per ogni spostamento manuale del robot, la posizione i-esima dell’end effector pWend−i è stata calcolata utilizzando il software Matlab, in cui era stata previa- mente implementata la funzione della cinematica diretta del robot sulla base dei parametri descritti nella sezione precedente. Le posizioni della sfera pW
i rispetto
alla terna W sono state ricavate dalle posizioni dell’end effector pW
end−i aggiungen-
do un offset pari al raggio della sfera (0, 02 m) alla coordinata z del terminale. pWi = pWend−i+ 0 0 0, 02
Le posizioni della sfera pS
i rispetto alla terna S invece sono state trovate e ripor-
tate dall’applicazione presente sul PC Windows.
Avendo a disposizione questi due insiemi di 20 misure, è stato possibile applicare la formula di Least squares fitting, per trovare la posizione e l’orientamento della terna del sensore rispetto alla base del robot.
Dalla geometria 3D si sa che pW
i = RSWpSi + oWS , dove oWS e RSW sono rispetti-
4.3 Flusso e scambio delle informazioni 71
valutare oW
S e RSW, utilizzando il metodo di Least squares fitting, bisogna risolvere
il seguente problema di ottimizzazione:
min RS W,oWS i=20 X i=1 kpW i − (R S Wp S i + o W S )k 2
Tale formula può essere riscritta in questa maniera: min RS W,oWS i=20 X i=1 kqiW − RSWqSik2 dove qiS = pSi − 1 20 i=20 X i=1 pSi qiW = pWi − 1 20 i=20 X i=1 pWi Quindi il problema viene scomposto in due parti:
1. Trovare la matrice di rotazione RS
W che minimizza la funzione riscritta.
2. Calcolare la posizione del centro della terna S rispetto al robot con la formula oWS = qWi − RS
WqiS.
Di seguito si riportano i passaggi, che sono stati eseguiti utilizzando il software Matlab, per trovare RS
W e oWS :
1. E’ stata trovata la matrice 3x3 H = Piq W
i · (qiS)T
2. E’ stata eseguita la scomposizione SVD di tale matrice, ottenendo H = U SVT, in cui U = u1 u2 u3 e V = v1 v2 v3 sono matrici 3x3 3. Si è trovata la matrice X = V UT
4. Siccome nel nostro caso è risultato det(X) = 1 allora RS
W = X = V UT
5. Infine è stato calcolato oW S = 201 Pi=20 i=1 p W i − 201R S W Pi=20 i=1 p S i .
Da queste operazioni risulta che: RSW = −0.0557 0.2268 0.9723 0.9982 0.0314 0.0450 −0.0192 0.9734 −0.2283 oWS = −0.426 −0.032 1.860
trovare la corrispondente posizione nella terna base del robot rW applicando la
seguente rototraslazione:
rW = oWS + RWS rS
Per valutare la bontà della trasformazione di coordinate che è stata ricavata, è stata applicata una procedura di validazione.
Tale procedura consiste in 3 prove in cui sono state confrontate le posizioni della sfera all’estremità del robot ottenute dal PC Linux (sulla base dei valori delle variabili di giunto qj) con quelle risultanti dalla rototraslazione delle misure del
sensore Kinect. Le posizioni della sfera sono state registrate durante un moto imposto all’end effector.
In ogni prova è stata impostata una velocità dell’end effector con profilo sinu- soidale, lungo xW, yW e zW; ciò ha determinato un moto sinusoidale della sfera
attaccata all’estremità del robot.
Perché il PC Linux ricostruisse le posizioni dell’end effector durante il moto, è stata implementata, all’interno della sua applicazione, la funzione di cinematica diretta, in grado di stimare la posizione e l’orientamento del terminale del braccio robotico a partire dai valori assunti dalle variabili di giunto qj, che vengono tra-
smessi dal C4G al PC Linux ogni 2 ms. Sia quindi pW
endla posizione dell’end effector (rispetto alla terna di riferimento W ),
si ha che:
pWend = f (q1, . . . , q6)
dove la f(q1, . . . , q6)implementa la cinematica diretta del manipolatore in funzione
delle 6 variabili di giunto. Dalla posizione pW
end si ricava la posizione della sfera pWsf era aggiungendo l’offset
corrispondente al raggio della sfera (in metri) lungo l’asse zW:
pWsf era = pWend+ 0 0 0, 02
Il sensore Kinect trova le posizioni del centro della sfera pS
sf era rispetto al proprio
sistema di riferimento S, a tali posizioni viene però applicata la trasformazione che rototrasla le coordinate della sfera dalla terna S alla terna W, da tale tra- sformazione si ottiene pW 1
sf era = oWS + RSWpSsf era, dove il riferimento W1 indica la
terna di riferimento ottenuta dalla rototraslazione della terna S. Lo scopo della validazione è determinare se la terna W1 sia assimilabile alla terna W.
La trasformazione da S a W1 è stata implementata già nel processo del PC Win- dows, il quale inviava le posizioni rototraslate al PC Linux.
Durante ogni prova il PC Linux ha collezionato dati a una frequenza di 500 Hz, tali dati erano le posizioni pW
sf erae pW 1sf era. Siccome il Kinect lavora a una frequenza
4.3 Flusso e scambio delle informazioni 73
posizioni pW 1
sf era sono state ripetute, ma ciò non inficia la valutazione della prova.
Le velocità imposte all’end effector nelle 3 prove effettuate non sono state troppo elevate, per sicurezza, e anche per emulare le possibili velocità di un operatore umano.
v1 = 0, 1 · sin(2πf1t) m/s f1 = 0, 1 Hz
v1 = 0, 2 · sin(2πf2t) m/s f2 = 0, 2 Hz
v3 = 0, 3 · sin(2πf3t) m/s f3 = 0, 3 Hz
Vengono riportati i grafici dei dati raccolti con queste 3 prove; sono stati re- gistrati gli andamenti del moto della sfera lungo i 3 assi coordinati applicando le velocità citate lungo i 3 assi. La linea blu riporta le componenti di pW
sf era mentre la
linea arancione riporta le componenti di pW 1
sf era. Dopo ogni immagine è riportato
anche l’andamento dell’errore sulle 3 coordinate.
Si può notare che il segnale arancione ha un offset variabile rispetto al segnale blu, ma comunque contenuto entro 1, 5 cm quando la sfera è ferma, c’è del rumore (dovuto alle interferenze del sensore visivo), e quando invece la sfera è in moto l’errore sulla posizione è tanto più grande quanto più è elevata la velocità, ma ciò è dovuto a un ritardo medio nel segnale di 0, 2 s. La forma frastagliata del segnale proveniente dal PC Windows (quello arancione) è dovuta alla ridotta frequenza di campionamento del sensore Kinect rispetto alla frequenza di campionamento del C4G collegato al PC Linux.
Alla luce dei risultati delle prove si può assumere che, per i nostri scopi, la trasformazione di coordinate che è stata trovata sia valida.
Grazie alla procedura di calibrazione il PC Windows può quindi inviare a PC Linux tutte le informazioni necessarie alla legge di controllo.