Introduzione

Vorremmo introdurre questa relazione partendo da una considerazione che riteniamo

dal nostro modesto punto di vista essere molto importante.
Constatando quella che oggi è l’offerta formativa delle università italiane, non
sempre rivolta in modo del tutto accurato alla formazione professionale degli studenti
ma più in generale a una formazione più superficiale, spesso vista solamente come
fine a se stessa, o al massimo come preparazione rivolta solamente al conseguimento
degli esami; vogliamo ringraziare il professore Luca Zaccarian per aver organizzato
uno dei pochi corsi della nostra università che anche se purtroppo di breve durata
guarda quella che è l’attività lavorativa di un ingegnere.
Tale corso pone lo studente in un contesto lavorativo di gruppo capace di fargli
acquisire le qualità di relazione con gli altri e di organizzazione del lavoro che in
generale non sono intrinseche nel carattere di ognuno di noi.
Ci auguriamo quindi che questo corso sia di esempio ad altri docenti e che essi si
adoperino per attivarne di simili.
Tali considerazione non hanno alcuna pretesa di essere prese in considerazione dal
docente o da chiunque leggerà questa relazione e non vogliono avere alcuna valenza
critica nei confronti di nessuno. Vogliono invece comunicare che spesso non servono
grandi riforme universitarie che stravolgano i sistemi delle nostre università ma,
partendo da un buon lavoro dei docenti, dall’incentivazione di quelli meritevoli e
dalla volontà degli studenti che spesso non manca, si possono ottenere buoni risultati
che possono essere di grande aiuto alla collettività.
Lo scopo di questa relazione è quello di riassumere le varie fasi che ci hanno portato
ad affrontare l’esperienza del laboratorio e alla realizzazione della dimostrazione
finale.
Oltre ad illustrare gli aspetti pratici, verranno presi in considerazione anche alcuni di
quelli teorici che ci hanno guidato nel portare a termine l’esperienza di laboratorio.

1
La prima parte della relazione mette in luce gli aspetti che ci consentono di ricavare
tutti i parametri del robot al fine di poterli utilizzare nella risoluzione della cinematica
diretta ed inversa. La seconda parte invece riguarda la descrizione della
dimostrazione e tutti gli aspetti legati alla programmazione in linguaggio C.
Il robot che verrà considerato è lo SCORBOT III che come vedremo più in dettaglio
in seguito è un robot a cinque gradi di libertà, pilotato da dei motori in CC
opportunamente interfacciati con un calcolatore tramite software dedicato e hardware
di diverso tipo.
Anche se in generale il sistema è abbastanza complicato il computer lo gestisce come
un insieme di più motori da movimentare, sui quali effettua delle acquisizioni di
informazioni (posizione angolare) tramite degli encoder di cui si parlerà più in
dettaglio nel seguito della trattazione.
Per avere una visione generale si ricorre al seguente schema che non ha alcuna
pretesa di voler dare una rappresentazione esaustiva del sistema calcolatore-
interfaccia-robot. SCORBOT V

Data bus Power Unit Power signal

Fig. 1 Schema a blocchi del sistema PC-Power Unit-Scorbot

2
Come si può vedere dalla figura 1 esiste una differenza sostanziale tra i segnali che
vanno dal PC alla PU che sono segnali che vanno a pilotare l’elettronica di potenza
contenuta all’interno della PU e i segnali che vanno da quest’ultima al robot che sono
dei segnali di potenza e vanno direttamente a pilotare i motori.
E’ da notare che comunque esistono dei segnali che attraversando la PU vanno dallo
scorbot verso il PC, che sono i segnali provenienti dagli encoder fissati sull’albero di
ogni motore e servono per conoscere la posizione angolare dei motori istante per
istante.

3
Capitolo 1
Parametri del robot e loro determinazione

1.1Introduzione
Per definire i parametri di un robot è importante in primo luogo fornire una breve
presentazione di questi ultimi in modo tale da comprendere in maniera più
approfondita quali sono le problematiche da affrontare.
Inoltre è necessario tener conto che oltre alla determinazione dei parametri di giunto e
di link, si ha la necessità di stabilire la relazione tra variazione della posizione
angolare del motore e variazione dei passi encoder corrispondente.

1.2 Link e giunti.

Un generico manipolatore meccanico può essere visto come una sequenza di

segmenti rigidi chiamati link connessi da giunti.
I link sono dei segmenti rigidi che collegano due giunti, invece i giunti sono delle
strutture meccaniche che consentono uno spostamento relativo fra i due link adiacenti
connessi al giunto stesso.
L’asse di giunto si definisce come l’asse di rotazione o traslazione individuato dal
movimento del giunto.
I giunti possono essere divisi in due categorie: giunti rotazionali che consentono un
movimento di tipo rotativo intorno all’ asse del giunto stesso e prismatici anche detti
lineari e che consentono un movimento di tipo traslatorio dei link adiacenti.

4
Ogni coppia giunto link rappresenta un grado di libertà del manipolatore, quindi un
manipolatore a N gradi di libertà ha N coppie giunto-link con il link 0 ancorato alla
base di appoggio del robot e non considerato parte del robot.
Ad ogni link vengono associati due parametri ai e ai detti appunto parametri di link,
il parametro ai chiamato lunghezza di giunto rappresenta la distanza più breve
misurata lungo la normale comune tra gli assi dei giunti i e i+1 e ai detto angolo di
twist che è l’angolo compreso tra gli assi dei giunti su un piano perpendicolare ad ai.
Allo stesso modo ad ogni giunto vengono associati i parametri di giunto di e Ji detti
rispettivamente distanza e angolo di giunto. La distanza di rappresenta la distanza
misurata lungo l’asse del giunto fra la normale proveniente dall’asse del giunto i-1 e
la normale che va al giunto i+1. L’angolo Ji è l’angolo formato dalle due normali,
misurato su un piano ortogonale all’asse del giunto.Tali parametri possono essere
visualizzati nella fig.2.

Link i-1
Link i

Fig. 2 Parametri di giunto e parametri di link

5
Stabiliti per ogni coppia giunto-link i quattro parametri sopra elencati e una
convenzione di segno per ognuno di essi è possibile determinare la configurazione
cinematica del robot.
Per ricavare tali parametri è necessario associare ad ogni coppia giunto-link un
sistema di riferimento opportunamente scelto in una moltitudine di possibili
soluzioni.
Definiti i sistemi di riferimento di ogni coppia giunto-link e i parametri ai, ai, di, e Ji
sarà possibile rappresentare le coordinate di effettore o di un generico sistema di
riferimento associato alla i-esima coppia giunto-link rispetto a un sistema di
riferimento inerziale che come vedremo in dettaglio più avanti è il sistema di
riferimento di base.

1.3 Determinazione dei parametri del robot

Per determinare i parametri dello SCORBOT ci serviamo di semplici attrezzi da
misura e più in particolare: di un metro, un goniometro a pendolo, di una livella e di
un filo a piombo di quelli utilizzati normalmente in edilizia.
Fissati i sistemi di riferimento tramite la procedura definita da Denavit e Hartenberg
come in figura 2 si vanno a determinare i parametri lineari del robot (di e ai) che
vengono riportati nella seguente tabella:

6
 Link/giunto di (mm) Ai (mm)
1 352 18
2 0 223
3 0 223
4 0 0
5 141 0

Tab.1 Parametri di giunto dello SCORBOT V

1.4 Conversione da angoli a passi encoder.

Come accennato in precedenza è importante stabilire una relazione univoca tra
variazione della posizione angolare degli alberi dei motori e numero di impulsi degli
encoder fissati su di essi .
E’ necessario quindi individuare dei coefficienti Ki tali da soddisfare la relazione
(1.1.3).
Angolo
Ki  (1.1.3)
Passiencoder (i)

Tali Ki sono diversi dall’unità in quanto all’albero vengono applicati dei

motoriduttori necessari ad abbassare la velocità di rotazione dei motori, in modo tale
che la velocità con cui si muovono i link sia minore di quella con la quale ruotano i
motori stessi.
Tali riduttori fanno si che la rotazione e quindi la posizione angolare dell’albero a
valle del “carico” sia diversa da quella misurata dagli encoder.
Per il calcolo del coefficiente K1 dell’encoder del motore di base, sono state
considerate le proiezioni di più punti dell’effettore sul piano di lavoro, e per mezzo
delle regole trigonometriche sono stati calcolati gli angoli di base.

7
Per quanto riguarda le costanti dei encoder associati agli altri motori (gomito, spalla,
pitch e roll), tramite l’utilizzo di una livella sono state identificate posizioni dei
motori a 0°, 45° e 90° e valutati i passi eseguiti dagli encoder.
Nella tabella 2 sono riportati i dati ottenuti dalle misure:

Motore 1 K1 0.046554
Motore 2 K2 0.059135
Motore 3 K3 0.059371
Pitch K4 0.242677
Roll K5 0.229088

Tab.2 Fattori di conversione da angoli a passi encoder.

Capitolo 2
Rappresentazione di Denavit-Hartenberg

8
2.1 Introduzione
Tramite le linee guida date dalla rappresentazione di Denavit e Hartenberg è possibile
stabilire in modo sistematico un sistema di coordinate per ogni link di un sistema
articolato come il nostro robot.
Definiti i vari sistemi di riferimento è possibile ricavare la rappresentazione di
Denavit – Hartenberg.
i 1
Tale rappresentazione consiste in una matrice di trasformazione omogenea Ti  M 44

che rappresenta il sistema di coordinate del link i rispetto al riferimento del link
precedente i-1. Pertanto, individuate tutte le matrici di trasformazione intermedie è

possibile ricavare la matrice omogenea 0Te che lega le coordinate di effettore a

quelle di base tramite la relazione (2.1.1) riportata di seguito:

p 0 Te pe (2.1.1)
0
Dove p0 e pe sono i vettori contenenti le coordinate di un generico punto riferite

rispettivamente al sistema di riferimento inerziale e a quello di effettore e 0Te è data

dalla relazione (2.1.2):

0T 0 T 1 T 2 T 3 T 4 T (2.1.2)
e 1 2 3 4 e
Dove la generica matrice i 1Ti è quella associata alla trasformazione di coordinate

dal sistema di riferimento della coppia giunto-link (i-1)-esima al sistema di

riferimento della coppia giunto-link i-esima.

2.2 Scelta dei riferimenti

Per il posizionamento degli assi cartesiani associati al sistema, si ricorre alla
convenzione di Denavit-Hartenberg, secondo la quale gli assi sono presi come
mostrato nella .

9
 Figura 3. Convenzione di Denavit-Hartenberg .

In particolare:
• Zi giacente lungo l’asse del giunto i+1;
• Oi sull’asse Zi con l’intersezione dell’asse Zi con la normale comune agli assi
Zi–1 e Zi ;
• O’i sull’asse Zi–1 con l’intersezione dell’asse Zi con la normale comune agli assi
Zi–1 e Zi;
• Xi diretto lungo la normale comune agli assi Zi–1 e Zi con verso positivo dal
giunto i al giunto i+1;
• Yi si ricava dalle precedenti seguendo la regola della mano destra;
Si può notare che in un certo numero di situazioni, la procedura appena descritta non
dà indicazioni precise sul posizionamento dei sistemi di riferimento.
In particolare, ciò accade nei seguenti casi:

10
Con riferimento alla terna 0 , per la quale la sola direzione dell’asse Z0 risulta
specificata: si possono quindi scegliere arbitrariamente O0 ed X0
Con riferimento alla terna n, cioè quella relativa all’effettore, per la quale il solo asse
Xn risulta soggetto a vincolo (deve essere normale all’asse Zn-1): infatti non vi è giunto
n+1 per cui non è definito Zn e lo si può scegliere arbitrariamente
Quando due assi consecutivi sono paralleli, in quanto la normale comune tra di essi
non è univocamente definita
Quando due assi consecutivi si intersecano, in quanto il verso di Xi è arbitrario
Quando il giunto i è prismatico, nel qual caso la sola direzione dell’asse Zi–1 è
determinata.
Una volta posizionati tutti i sistemi di riferimento, è possibile ricavare i parametri di
link e di giunto.
• ai è definito come la distanza tra Oi e O’i;
• αi è l'angolo intorno l’asse Xi tra gli assi dei giunti Zi–1 e Zi valutato positivo in
senso antiorario;
• di è la coordinata di O’i su Zi–1;
• θi è l’angolo intorno all’asse Zi–1 tra l’asse Xi–1 e Xi valutato positivo in senso
antiorario;
Da qui è possibile individuare le variabili di giunto
θi se il giunto è rotoidale
di se il giunto è prismatico;
Dopo aver individuato i vari parametri del sistema si può facilmente
individuare la serie di trasformazioni che correla il sistema i-esimo con quello (i–1)-
esimo. In particolare si effettueranno le seguenti operazioni:
1. traslare lungo l’asse Zi–1 di una lunghezza pari a di per portare l'origine Oi sul
punto di intersezione tra il segmento ai e l'asse Zi–1;
2. ruotare intorno all'asse Zi–1 di un angolo θi per allineare l’asse Xi con il
segmento ai;

11
 3. traslare lungo l’asse Xi di una lunghezza pari ad ai per portare Oi nella
posizione originaria;
4. ruotare intorno all’asse Xi di un angolo pari ad αi per portare l’asse Zi nella
posizione originaria.
Per facilitare l’individuazione dei vari parametri di giunto del sistema, si
preferisce utilizzare una tabella detta appunto Tabella di Denavit-Hartenberg in cui ad
ogni coppia link-giunto corrisponde una riga che mostra i parametri relativi a quella
coppia (ai αi di θi).

Figura 4 Tabella di Denavit-Hartenberg.

In formule matematiche e utilizzando le matrici di trasformazione in coordinate

omogenee ciò si traduce nelle relazioni:
i −1
Ti = Tz ,θi ⋅ Ttr ,[ 0, 0,d i ] ⋅ Ttr ,[ ai , 0, 0 ] ⋅ Tx ,α i
(2.2.1)

 cos  i   cos   i  sin  i  sin   i  sin  i  ai cos  i  

 
sin  i  cos   i  cos  i   sin   i  cos  i  a sin  i  
i 1
Ti   (2.2.2)
 0 sin   i  cos   i  di 
 

 0 0 0 1 

12
2.3 Sistemi di riferimento dello SCORBOT-ER V
In questa sezione si andranno a stabilire i sistemi di riferimento per lo SCORBOT in
accordo con le convenzioni di Denavit e Hartenberg viste nella sezione 2.2.
Seguendo tali regole, è possibile individuare i vari sistemi di riferimento
rappresentato in figura 5

Fig. 5 Sistemi di riferimento dello SCORBOT V

2.4 Compilazione della tabella di Denavit-Hartenberg

Definiti i sistemi di riferimento dello SCORBOT, e individuati i parametri di giunto e

quelli di link, è possibile realizzare la tabella di Denavit e Hartenberg riportata di
seguito.

13
 Parametri

Link/Giunt Di Ji ai ai Matrice
o associata
1 D1 J1 a1 -p/2 0T
1
2 0 J2 a2 0 1T
2
3 0 J3 a3 0 2T
3
4 0 J4 0 p/2 3T
4
5 D5 J5 0 0 4T
e

Tab. 3 Parametri dello SCORBOT V

Capitolo 3
Differenza tra angoli di Denavit e Hartenberg e angoli
considerati per gli spostamenti.

3.1 Introduzione

14
Come accennato nella precedente sezione una situazione che si è dovuta risolvere è
stata quella derivante dalla scelta di riferire tutti gli angoli necessari per spostare il
robot a una configurazione del robot a nostro avviso ritenuta più semplice.

3.2 Convenzioni adottate

Si è scelto di riferire gli angoli alla posizione riportata in figura 6.

Figura 6: posizione a cui vengono riferiti gli angoli per la movimentazione

E’ da tenere inoltre conto che nella movimentazione del robot la variazione di un

angolo non è indipendente dalla variazione degli altri.
Considerando questo aspetto e quello della scelta di riferire gli angoli alla posizione
riportata in figura 6 si trovano le seguenti relazioni:

   
 2( DH ) 2h

     (3.2.1)
 3( DH ) 2h 3h

      
 4( DH ) 3h 2

15
Per quanto riguarda gli angoli 1 e 5  roll non si hanno particolari relazioni in
quanto vengono riferiti alla convenzione di Denavit e Hartenberg e inoltre sono
indipendenti dagli altri angoli.

3.3 Angoli di pitch e roll

Un’altra problematica di cui si è dovuto tener conto è stata quella relativa alla
relazione tra angoli di pitch e roll e la loro misura da parte degli encoder.
Questo problema sorge dal fatto che i movimenti di pitch e di roll vengono effettuati
tramite un differenziale (figura 7) ovvero un meccanismo costituito da tre ingranaggi,
due laterali detti pignoni e rotazionati da due motori indipendenti e uno centrale detto
satellite che si muove sotto l’effetto della rotazione dei due pignoni.

Figura 7: differenziale

Tramite questo meccanismo è possibile effettuare i movimenti di roll facendo ruotare

i due motori nello stesso verso, ovvero applicandogli una tensione dello stesso segno.
I movimenti di pitch vengono effettuati facendo ruotare i due motori in verso
opposto, ovvero alimentandoli con tensioni di segno opposto.
Questo meccanismo rende necessario stabilire due relazioni tra gli angoli di pitch e
roll e i passi degli encoder accoppiati ai motori che muovono i pignoni.
Tali relazioni riportate di seguito sono state trovate facendo effettuare all’effettore
diversi movimenti e misurandone gli angoli relativi.

16
 Passi  Passi
•    (encoder 4) (encoder3) (3.2.1)
4 pitch 2 K
4
Passi  Passi
•    (encoder 4) (encoder3) (3.2.2)
5 roll 2 K
5

Capitolo 4
Cinematica diretta

4.1 Introduzione
Misurati i parametri del robot è possibile ricavare la tabella di Denavit-Hartenberg e
procedere alla determinazione della cinematica diretta.

17
4.2 Cinematica diretta dello SCORBOT
La cinematica diretta consente di esprimere le coordinate di un punto rispetto alla
terna inerziale di base in funzione delle coordinate dello stesso punto riferite alla
terna di riferimento di effettore e delle variabili di giunto e di link, tramite la
relazione (2.1.1) introdotta nella sezione 2.1 e di seguito riportata:

p 0 Te pe
0
A partire dai sistemi di riferimento fissati sul robot come in figura 5 e dalla tabella n°
3, che riporta i parametri di giunto e i parametri di link dello Scorbot V è possibile

ricavare le matrici i 1Ti che compongono la matrice di trasformazione 0Te riportata

nella relazione (2.1.2).

Per completezza vengono riportate di seguito le matrici i 1Ti (con 1  i  5 ).

 cos

 1  0 sin  1  a1 cos  1  

0
T1  
 sin  1  0 cos  1  a1sin  1  
 (4.2.1)
 0 1 0 0 
 

 0 0 0 1 

 cos

 2   sin( 2 ) 0 a2 cos  2  

T2  
1
 sin  2  cos( 2 ) 0 a2 sin  2  
 (4.2.2)
 0 0 1 0 
 

 0 0 0 1 

 cos

 3   sin(3 ) 0 a3 cos  3  

2
T3  
 sin  3  cos(3 ) 0 a3sin  3  
 (4.2.3)
 0 0 1 0 
 
0 0 0 1 
 

18
  cos

 4  0 sin( 4 ) 0

T4  
3
 sin  4  0  cos( 4 ) 0 
(4.2.4)
 0 1 0 0
 
 0 0 0 1 

 cos

 5   sin(5 ) 0 0 

T5  
4
 sin  5  cos(5 ) 0 0 
(4.2.5)
 0 0 1 d5 
 
 0 0 0 1 

Capitolo 5
Implementazione software della cinematica diretta.

Si riporta di seguito l’implementazione software utilizzata per realizzare la

cinematica doretta dello SCORBOT-ER V, evidenziando in particolare i controlli che
vengono effettuati in modo da rendere il più possibile sicura la movimentazione del
robot nel caso in cui vengano inseriti dall’utente gli angoli di Denavit e Hartenberg.
19
In primo luogo viene presentata la funzione “cinematica_diretta”.
void cinematica_diretta(double angolo_di_DH[], double ce[],double phi)
{
double pwx=0;
double pwy=0;
double pwz=0;

phi=conversione_in_radianti(phi);

angolo_di_DH[0]=conversione_in_radianti(angolo_di_DH[0]);
angolo_di_DH[1]=conversione_in_radianti(angolo_di_DH[1]);
angolo_di_DH[2]=conversione_in_radianti(angolo_di_DH[2]);
angolo_di_DH[3]=conversione_in_radianti(angolo_di_DH[3]);
angolo_di_DH[3]=conversione_in_radianti(angolo_di_DH[4]);

pwx=cos(angolo_di_DH[0])*(a3*cos(angolo_di_DH[1]+angolo_di_DH[2])+a2*cos(angol
o_di_DH[1])+a1);
pwy=sin(angolo_di_DH[0])*(a3*cos(angolo_di_DH[1]+angolo_di_DH[2])+a2*cos(angol
o_di_DH[1])+a1);
pwz=-a3*sin(angolo_di_DH[1]+angolo_di_DH[2])-a2*sin(angolo_di_DH[1])+d1;

ce[0]=pwx+d5*cos(phi)*cos(angolo_di_DH[0]);
ce[1]=pwy+d5*cos(phi)*sin(angolo_di_DH[0]);
ce[2]=pwz+d5*sin(phi);

Viene riportata di seguito l’implementazione dei controlli.

 Il primo controllo consente di evitare la situazione riportata in figura 8, ovvero
quella in cui l’effettore collide con il piano di lavoro,

20
 Figura 8: posizione non consentita

e viene implementato con la seguente procedura software :

if(controllo[1]<=0 && controllo[2]<=-90 && controllo[3]<=-80)
{cprintf("posizione pericolsa");

break;
}

Dove controllo[1]  2 , controllo[2]  3 , controllo[3]   .

 Il secondo controllo evita la situazione riportata in figura 9, ovvero quella in

cui l’effettore collide con la base e viene implementato nel seguente modo:
if(controllo[1]<=0 && controllo[2]<=-90 && controllo[3]<=-80)
{cprintf("posizione pericolsa");

break;
}

21
 Figura 9: posizione non consentita

Capitolo 6
Cinematica inversa

6.1 Introduzione
22
Tramite la cinematica inversa è possibile esprimere gli angoli di giunto in funzione
delle coordinate di effettore.
In tal modo sarà possibile implementare delle procedure software che consentono di
calcolare i vari angoli di giunto in funzione della posizione e dell’orientamento della
terna di effettore, in modo tale da portare l’effettore in un qualsiasi punto dello spazio
operativo.

6.2 Calcolo della cinematica inversa

Il calcolo della cinematica inversa viene effettuato in due fasi, in una prima fase
viene posizionato il robot in un punto qualsiasi nello spazio di lavoro e vengono
calcolate le coordinate dell’origine della terna di effettore in funzione degli angoli di
Denavit e Hartenberg .
Tali coordinate vengono definite dalla relazione (6.2.1) espressa di seguito:

 Pwx   0
   
 Pwy   0
  0 T  
  3  0 (6.2.1)
 Pwz   
   1
 1   


Con 0T3 0 T1 1 T2 2 T3 .

Come si può notare dalla figura 8 l’ angolo 1 è banalmente calcolabile tramite la

relazione (6.2.2):

1  arc tan 2( Pwx , Pwy ) (6.2.2)

23
 Figura 10: rappresentazione del robot per il calcolo della cinematica inversa.

Nella seconda fase il vettore di coordinate espresso nella relazione (6.2.1) viene
calcolato tramite delle considerazioni di tipo geometrico sulla figura 10 nel seguente
modo:
• Il segmento indicato in figura 10 come S4 altro non è che:
S  d cos() (6.2.3)
4 5
Stabilito chi è tale segmento si possono individuare i segmenti S1 S2 ed S3 che
sono:

• S  S cos( )  d cos() cos( ) (6.2.4)

1 4 1 5 1
• S2  S sin( )  d cos() sin( ) (6.2.5)
4 1 5 1
• S3  d sin() (6.2.6)
5

24
 T
A questo punto è possibile calcolare il vettore Pwx  Pwy 
Pwz 1 che è:



Pwx 

X e  S1 
 
 Pwy   Y S 
   e 2
  
 Pwz   Ze  S3 
  

 1   1 
Ovvero:
 Pwx   cos(sin( ) 
 
Xe   1 
   
 Pwy   Y  cos( ) cos( )

   e  d  1  (6.2.7)
   5
 Pwz   Ze   sin() 
    
  1   
 1   
 1 

T
Dove il vettore  Xe Ye Ze 1 è il vettore che rappresenta le coordinate di

effettore rispetto alla terna di riferimento di base, d5 è la lunghezza del link n° 5

come si può notare dalla figura 10 F è l’orientamento dell’effettore riferito al piano

orizzontale e funzione degli angoli 2 ,3,4 come si evince pure dalla relazione
(6.2.8).
E’ da notare che tutti questi parametri sono noti in quanto essi rappresentano o la
posizione desiderata per l’effettore rispetto alla terna di riferimento inerziale, oppure
parametri del robot già misurati.

       (6.2.8)
4 3 2 2

A questo punto eguagliando le relazioni (6.2.1) e (6.2.7):

25
  cos(sin( ) 
 0  Xe   1 
     
  
0T  0    Ye   cos() cos( ) 
 d  1  (6.2.9)
3  0  Ze  5  
   
sin() 
 1  1   
   1
 

Considerando anche la relazione (6.2.8), si ottengono quattro equazioni con tre

incognite che sono:
 xe  d 5 cos( ) sin(1 )  cos(1 ) [cos( 2 ) cos(3 )  a3 sin( 2 ) sin(3 )  a2 cos( 2 )  a1 ]
 y  d cos( ) cos( )  sin( ) [cos( ) cos( )  a sin( ) sin( )  a cos( )  a ]
  e 5 1 1 2 3 3 2 3 2 2 1

 ze  d5 sin( )  a3 [sin( 2 ) cos(3 )  sin(3 ) cos( 2 )]  a2 sin( 2 )  d1


       
 4 3 2
2

(6.2.10)
Da queste relazioni, e considerando 1 noto, è possibile tramite operazioni algebriche

e trigonometriche ricavare cos(2 ),cos(3 ),sin(2 ),sin(3 ) e tramite la funzione

arctan 2(sin(i ),cos(i )) , è possibile ricavare i i con i  (2,3) in funzione delle

coordinate di effettore espresse rispetto il sistema di riferimento inerziale, e

all’orientamento della terna di effettore.
In particolare 4 è esprimibile in funzione di  2 ,3 e  semplicemente come:


      (6.2.11)
4 2 3 2
Per quanto riguarda 2 ,3 , dal sistema di equazioni (6.2.10) si ha:

a2  c2  a 2  a 2
cos( )  3 2 (6.2.12)
3 2 a a
2 3

sin( )   1 cos2 ( ) (6.2.13)

3 3

26
si tenga presente che nell’implementazione software la relazione 6.2.13 è stata
considerata con il segno positivo per movimentare il robot con il gomito alto, al fine
di ottenere uno spazio di lavoro più ampio.
Dalla relazione 6.2.13 si ottiene:
  arctan 2[sin(3 ),cos(3 )] (6.2.14)
3

Per quanto riguarda invece 2 2 si ha:

b a cos( )  b a  a sin( ) c
cos( )  3 3 2 3 3
2 a 2 cos2 ( )  2 a a cos( )  a 2  a 2 sin 2 ( ) (6.2.15)
3 3 3 2 3 2 3 3

a sin( ) b  c a cos( )  c a
sin( )   3 3 3 3 2
2 (6.2.16)
(a cos( ))2  2 a a cos( )  (a sin( ))2  a 2
3 3 3 2 3 3 3 2

Da cui:
  arctan 2[sin( ),cos( )] (6.2.17)
2 2 2
Le costanti a1, a2 , a3 sono i parametri di link riportati nella tabella n°3 invece a,b,c
provengono dal sistema (6.2.10) e sono:

a  xe  d cos() sin( ) (6.2.18)

5 1

b  ye  d cos() cos( ) (6.2.19)

5 1

c  ze  d sin() (6.2.20)
5

27
Definita la cinematica inversa, nel prossimo capitolo si illustreranno i problemi
derivanti dal fatto che gli angoli di Denavit e Hartenberg non coincidono con gli
angoli che verranno imposti nella movimentazione dal software che verrà utilizzato.

Capitolo 7
Implementazione software della cinematica inversa.

28
Si riporta di seguito l’implementazione software utilizzata per realizzare la
cinematica inversa dello SCORBOT-ER V, evidenziando in particolare i controlli che
vengono effettuati in modo da rendere il più possibile sicura la movimentazione del
robot nel caso in cui vengano inseriti dall’utente le coordinate di effettore.
Viene riportata prima di tutto la funzione “cinematica_inversa”.

void cinematica_inversa (double coord_eff[], double angoli_Denavit[], double

phi1, double roll)
{
double xe=0;
double ye=0;
double ze=0;
double c3=0;
double c2=0;
double s3=0;
double s2=0;
double a=0;
double b=0;
double c=0;

xe=coord_eff[0];
ye=coord_eff[1];
ze=coord_eff[2];

phi1=conversione_in_radianti(phi1);

angoli_Denavit[0]=atan2(ye,xe);

a=(xe-(d5*cos(phi1)+a1)*cos(angoli_Denavit[0]))/(cos(angoli_Denavit[0]));
b=(ye-(d5*cos(phi1)+a1)*sin(angoli_Denavit[0]))/(sin(angoli_Denavit[0]));
c=ze-d1-d5*sin(phi1);

c3=((a*a)+(c*c)-(a3*a3)-(a2*a2))/(2*a2*a3);
if(c3>1||c3<-1)
{ clrscr();
textcolor(RED+BLINK);
cprintf("errore nel dominio di cos(theta3)");
getch();
clrscr();

29
 }

s3=sqrt(1-(c3*c3)); // sto prendendo gomito alto

angoli_Denavit[2]=atan2(s3,c3);

c2= (b*a3*c3+b*a2-a3*s3*c)/(a3*a3+2*a3*c3*a2+a2*a2);
if(c2>1||c2<-1)
{
clrscr();
textcolor(RED+BLINK);
cprintf("errore nel dominio di cos(theta2)");
getch();
clrscr();
}
s2= -(a3*s3*b+c*a3*c3+c*a2)/(a3*a3+2*a3*c3*a2+a2*a2);
if(s2>1||s2<-1)
{
clrscr();
textcolor(RED+BLINK);
cprintf("errore nel dominio di sin(theta2)");
getch();
clrscr();
}

angoli_Denavit[1]=atan2(s2,c2);

angoli_Denavit[3]=phi1+angoli_Denavit[1]+angoli_Denavit[2]+M_PI_2;
angoli_Denavit[4]=conversione_in_radianti(roll);

Il controllo riportato di seguito è necessario per evitare le situazioni riportate in figura

6 e 7 che vengono distinte a seconda che F sia compreso tra 0° e -90° (come figura 8)
o tra -90° e -180° (come figura 9).
if(ze>0.5)
{
if((phi<=-90) && (xe>ye))
{
if(xe<14)

30
{
printf("\n\rnon ok xe");
getch();
}
Else
{
printf("\n\r ok per xe");
getch();
}
}
if((phi<=-90) && (xe<=ye))
{
if(ye<14)
{
printf("\n\rnon ok per ye")
getch();
}
else
{printf("\n\r ok per ye");
getch();
}
}
if((phi>-88) &&((sqrt(xe*xe+ye*ye)-(d5*cos(phi))-4)<13))
{
printf("\n\rnon ok sqrt ");
getch();
}
else
{
printf("\n\r ok sqrt ");
getch();
}
}
Else
{
printf("\n\rnon ok per ze");
getch();
}

31
Capitolo 8
Comando del robot da file

8.1 Pseudolinguaggio

32
Nei paragrafi precedenti è stato illustrato il modo in cui a partire dalla scelta e dal
posizionamento dei sistemi di riferimento del robot, si è arrivati prima
all’implementazione della cinematica diretta e poi di quella inversa.
Queste hanno rappresentato il punto di partenza per realizzare l’automatizzazione del
robot, ossia il suo asservimento mediante la lettura di un file contenente un certo
numero di istruzioni e di funzioni.
Nel nostro caso si è scelto di implementare sei funzioni riconoscibili dal robot e
identificati con le seguenti sigle:

1. Muoviti
2. Diretta
3. DDH
4. Pausa
5. Pinza
6. Home
7. Fine
La funzione ”muoviti” consente all’utente di specificare la posizione dell’effettore
rispetto al sistema di riferimento inerziale e il suo orientamento facendo
riferimento all’implementazione della cinematica inversa.
Tramite la funzione “diretta” è permesso all’utente di specificare gli angoli di
giunto rispetto alla convenzione specificata nella sezione 3.2.
Per permettere la movimentazione del robot in base agli angoli di Denavit e
Hartenberg è stata realizzata la funzione DDH.
Nell’automatizzazione del robot è sempre necessario tener conto delle varie
temporizzazioni tra i movimenti originati dal file, per risolvere tale problema è
stata introdotta la funzione “Pausa” che in particolare tiene conto anche delle
temporizzazioni opportune che devono essere realizzate nell’esecuzione del
movimento di roll e di pitch.

33
Per i movimenti di apertura e chiusura della pinza è stata utilizzata la funzione
“Pinza”.
Molto utile per risparmiare tempo è la funzione “Home” la quale permette di
riportare il robot in una posizione molto vicina a quella di home in modo tale che
in movimentazioni future il robot si trovi in una posizione “predisposta” alla
home.
Concludendo si è deciso di implementare una funzione di controllo che ci
restituisse sia la lettura del file sia la sua esatta conclusione: tale funzione è quella
di “Fine”.
Per una più chiara comprensione di come realizzare il file di movimentazione
automatica si riporta di seguito un esempio.
Muoviti *23.75*37.866*-12.4*3.6*0
Pausa *2
Muoviti *-0.59*34.09*25*-90.3*1.1
Pausa *2
Muoviti *-0.59*34.09*14.4*-90.3*1.1
Pausa *2
Pinza *c
Pausa *2
Muoviti *-0.59*34.09*45*-90.3*1.1
Pausa *2
Muoviti *32.38*9.88*15.37*-95*1.8
Pausa *2
Pinza *o
Pausa *1
Home
Fine

Si noti che per una corretta lettura dei parametri delle funzioni sia necessario
inserire dopo la sigla della funzione stessa il simbolo ‘*’, fatto eccezione per la
funzione di Home e di Fine.

34
35