Sei sulla pagina 1di 11

STUDENT PROJECT for the course MECHANICS OF ROBOTS

Assigned by G. Gatti in A.Y. 2016/2017


University of Calabria, Rende, Itaty

Sviluppo di un modello cinematico del manipolatore a 6 assi KUKA


Agilus KR 6 R900 via software MATLAB

Eugenio Romeo, Simone Rugna, Umberto Severino


Dipartimento di ingegneria meccanica energetica e gestionale, Universit della Calabria, Rende, Italia
{eugenioromeo, u.severino}@hotmail.it, simone.rugna@gmail.com

Keyword: Kuka, Matlab, Cinematica, Adams

Abstract: Questo studio riguarda lo sviluppo di un modello cinematico del manipolatore a 6 assi Kuka Agilus KR 6
R900. Sono stati impiegati i software MATLAB e ADAMS per lelaborazione dei dati e per la verifica
cinematica. E stata analizzata la cinematica per la ricerca delle condizioni di singolarit e per la scelta di una
soluzione ottima fra le possibili al fine di ottimizzare lo svolgimento di un determinato task assegnato
dallutente. Particolare riguardo e attenzione stata data alla stabilit e la personalizzazione dei dati in input
sviluppando un modello capace di adattarsi a diverse geometrie e condizioni di vincolo.

INTRODUZIONE
stato preso in esame un manipolatore seriale a 6 gradi di libert, il Kuka KR 6 R900, al fine di studiarne la
cinematica e di fornire un task compatibile con il robot stesso.
Allinterno del capitolo della cinematica sono stati trattati i parametri di Denavit-Hartenberg e i metodi
sviluppati per la descrizione di un modello matematico che permetta di risolvere, in via generale, la cinematica
diretta ed inversa al variare delle caratteristiche geometriche del robot, con particolare attenzione alla
discriminazione dei punti di singolarit.
Successivamente viene trattata la formulazione della traiettoria, che pu avvenire tramite assegnazione di un
numero definito di punti oppure mediante la discretizzazione di una curva parametrica. Per ciascuna definizione
sono state implementate due distinte leggi del moto, un polinomio di terzo grado oppure con una curva descritta
da un andamento lineare unito a due tratti di parabola agli estremi. Il primo caso pone una condizione sul tempo
che intercorre fra due posizioni successive, mentre il secondo vincola la velocit e accelerazione massima.
Tutti i metodi sono gestiti da un unico metodo principale che permette tramite inserimento da console di
personalizzare gli input da fornire al modello.
La rappresentazione multibody stata eseguita tramite il software MSC ADAMS nel quale vengono importate
le leggi del moto ottenute da MATLAB, questo ha permesso un confronto tra gli output dei due software.

1 CINEMATICA

1.1 Schema cinematico e parametri di Denavit-Hartenberg


Lo schema cinematico riportato in Fig. 1 rappresenta la configurazione zero del manipolatore. Il robot
costituito da 6 giunti rotoidali (numerati da 1 a 6) e 7 link (numerati da 0 a 6).

Seguendo la convenzione di Denavit-Hartenberg (in seguito come DH), a ciascun link stato associato un
sistema di riferimento; sono state inoltre definite le 6 matrici di trasformazione utilizzando i parametri DH, come
riportato nella tabella Tabella 1.
Figura 1: Schema cinematico del manipolatore

Tabella 1: Parametri DH per il KUKA KR6 R900

Rz Dz Dx Rx
T01 q1 a0 + a 1 0 /2
T12 q2 + /2 0 a2 0
T23 q3 0 0 /2
T34 q3 a3 + a 4 0 - /2
T45 q5 0 0 /2
T56 q6 a5 + a 6 0 0

Sfruttando le propriet delle matrici di trasformazione stato scritto il metodo DH_Kuka() sul software
MATLAB, che riceve in input un vettore contenente le dimensioni dei link del manipolatore, e restituisce in
output la matrice T06 in forma simbolica. Nello script sono richiamati inoltre i metodi Rz(), Dz(), Dx() e Rx(), i
quali permettono di calcolare le matrici di rotazione elementari fornendo in input un dato parametro.

1.2 Cinematica diretta


Con lausilio dei risultati precedenti stato costrito uno script DK_Kuka() per la cinematica diretta. Partendo
dalle dimensioni dei link, e fornendo in input le variabili di giunto desiderate, si ottiene in uscita la matrice di
trasformazione T06 corrispondente.
Il metodo genera inoltre unanimazione in cui viene riprodotto il movimento del manipolatore per raggiungere,
partendo dalla configurazione zero, la configurazione finale imposta dalle variabili di giunto.
In seguito allesecuzione dello script viene mostrata la traiettoria dellend-effector nello spazio, nel piano X-
Z e X-Y.

1.3 Cinematica inversa


Il manipolatore oggetto di studio, come evidente dallo schema cinematico, pu essere scomposto in due
sotto-strutture risolvibili singolarmente che semplificano la trattazione del problema di cinematica inversa.
Questo metodo di risoluzione che considera il manipolatore come costituito da una struttura portante (spalla) e
da un polso sferico detto Approccio alla Pieper. Il problema si riduce alla risoluzione di due sotto-problemi,
uno riguardante la posizione e uno riguardante lorientamento. Il polso sferico individuato dallintersezione dei
tre assi di rotazione terminali della struttura. Tale punto subito noto risalendo dalla posizione e orientamento
dellend-effector secondo lequazione [1]:

,1
= [,2 ] = 6 (1)
,3

Dove:
- , vettore colonna delle coordinate del polso;
- , vettore colonna delle coordinate dellend-effector;
- 6 , distanza fra lorigine del sistema di riferimento dellend-effector e quella del polso sferico;
- , terza colonna della matrice di rotazione.

Dove tutte le quantit impiegate sono degli input del problema.


La soluzione al problema di cinematica inversa, nel caso in questione di manipolatore a sei gradi di libert con
polso sferico, si riduce allo svolgimento dei seguenti punti [1]:

1. Calcolare la posizione del polso (q1, q2, q3) secondo la (1);


2. Risolvere la cinematica inversa per (q1, q2, q3);
3. Calcolare R03(q1, q2, q3);
4. Calcolare la R36(q4, q5, q6) = R03T * R06;
5. Risolvere la cinematica inversa per lorientamento (q4, q5, q6).

Imponendo luguaglianza fra la posizione del polso vista dalla struttura portante con quella ottenuta dalla
relazione (1):

1 [(3 + 4 )23 2 2 ] = ,1 (2)

1 [(3 + 4 )23 2 2 ] = ,2 (3)

(3 + 4 )23 + 2 2 + (1 + 0 ) = ,3 (4)

Le soluzioni alla cinematica inversa della struttura portante sono quindi, considerando le possibili
soluzioni ottenibili [1]:

,2
1, = tan1 ( ) (5)
,1
sin(2, )
2, = tan1 ( ) , = , , , (6)
cos(2, )

sin(3 )
3, = tan1 ( ) (7)
cos(3 )

3, = 3, + (8)
Ottenendo quattro combinazioni di soluzioni:
1, , 2, , 3,
1, , 2, , 3,
= { , , (9)
1, 2, 3,
1, , 2, , 3,

Cos facendo, sfruttando la propriet del polso sferico, per ognuna delle possibili soluzioni, resta da
determinare lorientamento del sistema terminale sfruttando la relazione:

= ( )1 (10)

Ovvero:
4 5 6 4 6 4 5 6 4 6 4 5
= [ 4 5 6 + 4 6 4 5 6 + 4 6 4 5 ]
5 6 5 6 5 (11)

Con = cos( ) , = sin( ).

La soluzione della cinematica inversa del polso sferico [1]:

(2,3)
4 = tan1 ( ) (12)
(1,3)
5 = cos 1 ( (3,3)) (13)
(3,2)
6 = tan1 ( ) (14)
(3,1)

Ovvero:

4, , 5 , 6, , 5 (0, )
= { (15)
4, , 5, , 6, , 5 (, 0)

Si ottengono quindi quattro possibili soluzioni fra cui quelle relative alla orientazione dipendenti dal valore
di q5. Il metodo MATLAB impiegato per la risoluzione della cinematica inversa il metodo IK_Kuka() i cui input
sono le dimensioni dei link e la matrice TBG e il cui output il set di soluzioni.

1.4 Cinematica differenziale


Attraverso lanalisi cinematica differenziale si stabiliscono quali sono i legami tra velocit ai giunti e velocit
lineare ed angolare dellorgano terminale. Le relazioni vengono descritte da una matrice di trasformazione, detta
Jacobiano geometrico, che dipende dalla configurazione zero del robot.
Lobiettivo quello di determinare la velocit lineare e quella angolare attraverso una relazione del tipo [1]:


= [ ] = () (16)

che rappresenta lequazione cinematica differenziale del manipolatore.


Per calcolare lo Jacobiano risulta conveniente procedere separatamente per la velocit lineare e quella
angolare.
Riguardo il contributo di velocit lineare vale la seguente relazione:


= = (17)

=1 =1

mentre per quello di velocit angolare:


= = 1, = (18)
=1 =1

dove qi sar uno spostamento lineare o angolare a seconda della tipologia di giunto, mentre i termini JPi e JOi
sono i pesi associati ad ogni variabile di giunto. In seguito si pu esprimere lo Jacobiano in forma partizionata
nel modo seguente:

1
=[ ] (19)
1

dove ogni vettore colonna [6x1] dato dalla concatenazione dei due vettori [3x1] JPi e JOi. Questi ultimi vengono
determinati a seconda della tipologia di giunto:


[ 1 ]

[ ] = { ( ) (20)
[ 1 1
]
1

Per semplificare lonero computazionale nel determinare le singolarit cinematiche, conveniente


semplificare la struttura dello Jacobiano disaccoppiando il problema, analizzando quindi, spalla e polso
separatamente. La matrice Jacobiana risulter composta da quattro blocchi, il primo (11 ) associato alle
singolarit della struttura portante del manipolatore (spalla), il quarto (22 ) associato alle singolarit di polso:

11 12 [ (4 0 ) 1 (4 1 ) 2 (4 2 )] [ ]
=[ ]=[ 0 ] (21)
21 22 [0 1 2 ] [3 4 5 ]

Le singolarit vengono determinate, quindi, calcolando gli zeri del determinante dello Jacobiano:

det() = det( ) det( )

det( ) = 0 (22)
det() = 0 {
det( ) = 0

Implementando lalgoritmo per il calcolo del determinante in MATLAB, fornendo in input le dimensioni dei
link e la matrice di trasformazione T06 in forma simbolica, sono stati ottenute le seguenti configurazioni di
singolarit:

3 = 0 3 =

5 = 0 5 = (23)

06,1 = 06,2 = 0
2 GENERAZIONE TRAIETTORIA
Essenziale, prima di fare considerazioni riguardo la legge del moto pi adatta per una data traiettoria, la
scelta della soluzione ottima per ogni punto della primitiva di moto tale che minimizzi la variazione delle variabili
di giunto fra una configurazione e la successiva. Questa discriminazione viene fatta al netto dei vincoli geometrici
del robot e dei punti di singolarit e viene svolta dal metodo SoluzioneOttima(). Questo riceve in input le variabili
di giunto corrispondenti alla configurazione precedente (di default la configurazione zero), la matrice P06, la
matrice R06, la dimensione dei link e il numero di punti di passaggio assegnati Npv.
La matrice P06 contiene in ogni riga le coordinate della posizione dellend-effector (xE, yE, zE), rispetto al
sistema di riferimento {B}, per ogni istante di tempo:
,1 ,1
= [ ] (24)
, ,

La matrice R06 che contiene gli elementi delle matrici di rotazione dellend-effector per ogni istante di tempo,
disposti in riga:

, (1,1) , (1,2) , (1,3) , (2,1) , (2,2) , (2,3) , (3,1) , (3,2) , (3,3)


= [ ] (25)
, (1,1) , (1,2) , (1,3) , (2,1) , (2,2) , (2,3) , (3,1) , (3,2) , (3,3)

Il metodo restituisce in output una matrice in cui ogni riga contiene la soluzione ottima alla cinematica inversa
per un dato punto di discretizzazione della primitiva di moto.

2.1 Leggi del moto


Considerando una configurazione di partenza del manipolatore, al fine di raggiungere una nuova posa
dellend-effector necessario definire una legge del moto per ciascun giunto.
Attraverso la cinematica inversa otteniamo le variabili di giunto per una configurazione desiderata, sono
quindi definite le condizioni iniziali (t = t0) e finali (t = tf) per ciascun giunto:

(0 ) = 0
(26)
( ) =

La prima legge del moto presa in considerazione un polinomio di terzo grado, in quanto permette la
continuit della velocit, per cui le velocit iniziali e finali sono definite come:

(0 ) = 0 = 0
(27)
( ) = = 0

La primitiva di moto viene espressa come:

() = 0 + 1 + 2 2 + 3 3 (28)

E conseguentemente la velocit e laccelerazione:

() = 1 + 22 + 33 2 (29)

() = 22 + 63 (30)
Imponendo le 4 condizioni iniziali si ottiene quindi [2]:

0 = 0

1 = 0

3
2 = ( 0 ) (31)
2

3
3 = ( 0 )
3

Utilizzando i coefficienti cos ottenuti quindi possibile ottenere un polinomio di terzo grado per qualsiasi
0 e f con velocit iniziale e finale nulla.
In forma matriciale la relazione pu essere scritta come segue:

1 0 0 2 0 3 0 0
1 1 1 2 1 3 1
[ ] = [ ] (32)
0 1 20 30 2 2 0
4
[0 1 21 31 2 ]

La seconda legge del moto composta da un tratto lineare caratterizzato da una velocit costante e due tratti
estremi parabolici caratterizzati invece da unaccelerazione costante. La formulazione di una legge cos fatta
risulta interessante e vantaggiosa in alcune applicazioni poich permette di limitare la velocit e laccelerazione
rendendo lintervallo di tempo di percorrenza dipendente da queste grandezze.
Analogamente alla legge del moto precedentemente trattata si impongono come condizioni al contorno quelle
relative alle variabili di giunto e alle velocit agli estremi associate:

(0 ) = 0

( ) =
(33)
(0 ) = 0 = 0

( ) = = 0

Inoltre, si definiscono gli intervalli temporali in cui sono compresi i tre intervalli della legge del moto:
0
= | |

0
0 = | |

(34)

= | |

0
= 0 + + +
2 2
Dove:

- , intervallo temporale individuato dai punti di intersezione fra il prolungamento del segmento
lineare e lasse parallelo a quello temporale e passante per i valori delle variabili di giunto iniziale e
finale;
- 0 , intervallo temporale in cui compreso il tratto parabolico iniziale;
- , intervallo temporale in cui compreso il tratto parabolico finale;
- , intervallo temporale necessario a permettere la variazione dal valore iniziale a quello finale;
- , velocit massima imposta;
- , accelerazione massima imposta;

I tre tratti sono descritti dalle seguenti leggi del moto:

Per: t0 t tB0 :

() = 0 + 0 ( 0 ) + ( 0 )2 (35)
2
() = 0 + ( 0 ) (36)
() = (37)

Per: tB0 t tf - tBf :

0
() = 0 + ( ) (38)
2
() = (39)
() = 0 (40)

Per: tf - tBf t tf :

() = + ( ) ( )2 (41)
2
() = ( ) (42)
() = (43)

Per la generazione della traiettoria si impiegano due metodi, il metodo time_law() e il metodo motion2().
Il primo riceve in input il valore iniziale e finale della variabile di giunto considerata, il numero di discretizzazioni
dellintervallo temporale fra due punti, la velocit e laccelerazione massima e la variabile che definisce la scelta
del moto e restituisce le matrici che descrivono la legge del moto fra i due punti di estremo.
Il secondo metodo invece riceve in input la matrice delle soluzioni ottime generate dalla cinematica
inversa, il numero di punti di passaggio, le dimensioni dei link, la velocit e laccelerazione massima e restituisce
la collezione dei punti che descrivono la legge del moto per ogni variabile in forma matriciale , , generando
inoltre i file in formato .dat per la rappresentazione della cinematica in ADAMS.
Di fatto quindi il metodo motion2() risulta una estensione a n = Npv punti del metodo time_law().

1,1 1,6
= [ ] (44)
1, 6,
1,1 1,6

= [ ]
1, 6,

1,1 1,6

= [ ]
1, 6,

Il metodo time_law() incorporato nel metodo motion2() e insieme completano la parte di script relativo alla
formulazione della traiettoria.

3 IDENTIFICAZIONE TASK E VERIFICA


Il task pu essere assegnato al modello MATLAB sia tramite console, assegnando punti manualmente o
descrivendo una curva parametrica, oppure in modo esterno fornendo un file .xls importato dallutente nel metodo
Task(). Questa metodologia permette di personalizzare e fornire in modo rapido nuovi task da elaborare. Gli
output del metodo Task(), cos come quelli elaborati dalla traiettoria vengono poi forniti ad Adams per la verifica
cinematica.
In termini rappresentativi, la traiettoria scelta quella costituita da un tratto di discensione verticale lungo
lasse z della base e il successivo moto con percorso a evolvente su un piano parallelo al piano XY dopo aver
raggiunto un punto di via intermedio fra i due tracciati. La legge del moto assegnata a ciascun giunto, da punto a
punto, una polinomiale di terzo grado, ottenuta come descritto nel paragrafo 2.3.

Il task pu essere descritto come:

Tabella 2: Disceretizzazione raettoria task

x y z TBE(1,1) TBE(1,2) TBE(1,3) TBE(2,1) TBE(2,2) TBE(2,3) TBE(3,1) TBE(3,2) TBE(3,3) t


0 450 500 1 0 0 0 1 0 0 0 -1 t1
0 450 400 1 0 0 0 1 0 0 0 -1 t2
200 0 600 0 -1 0 1 0 0 0 0 -1 t3
450 0 400 0 -1 0 1 0 0 0 0 -1 t4
450+70*t*sin(t) 70*t*cos(t) 400 0 -1 0 1 0 0 0 0 -1 t4<t< t4+

Al fine di verificare il modello cinematico sviluppato su MATLAB stato quindi creato un modello
multibody semplificato del manipolatore sul software ADAMS. Essendo la verifica puramente cinematica, sono
stati trascurati gli effetti della massa e della gravit sul modello. Le variabili di giunto per ciascun istante di
tempo, per il task sopra definito, sono state esportate in formato .dat da MATLAB. Il centro di massa (CM) del
link 6 su ADAMS stato spostato sul sistema di riferimento dellend-effector, come considerato nello schema
cinematico in Fig.1, al fine di agevolare lanalisi dei risultati.
Eseguendo la simulazione su ADAMS, impostando come end time 380s e come step size 0.1s, sono state
rappresentate le posizioni dellend-effector, rispetto agli assi x,y e z, per ogni istante di tempo considerato (Fig.
2). Confrontando i risultati con quanto ottenuto da MATLAB (Fig. 3) possibile osservare la corrispondenza tra
i due grafici.
Figura 2: Posizione CM dell'end-effector da ADAMS

Figura 3: Posizione dell'end-effector da MATLAB

CONCLUSIONI
Laccurata analisi cinematica condotta sul manipolatore KUKA KR6 R900 ha reso possibile realizzare una
robusta modellazione in MATLAB. Attraverso il metodo Main_KukaAgilus.m lutente ha la possibilit di
configurare le caratteristiche geometriche del manipolatore, selezionare il tipo di analisi cinematica da eseguire,
effettuare lo svolgimento di un task oppure di una traiettoria personalizzata. Il modello MATLAB inoltre, durante
lesecuzione di una traiettoria, restituisce il numero di configurazioni singolari concedendo allutente la
possibilit di correggerla. Infine, unulteriore verifica dei risultati ottenuti dal suddetto modello stata effettuata
attraverso il software ADAMS che restituisce la posizione finale dellend-effector, confrontabile, quindi, con
quella raggiunta dallo stesso in ambiente MATLAB.

BIBLIOGRAFIA
[1] Siciliano B, Sciavicco L, Villani L, Oliolo G, (2008). Robotica: Modellistica, pianificazione e controllo, The McGraw-
Hill Companies. Milano, 3a edizione
[2] Craig JJ, (1986). Introdution to robotics: mechanics and control, Addison-Wesley Publishing Company. 2
edizione.