Simulatore cinematico di Robot 3R

Simulatore cinematico di Robot 3R
Descrizione generale
Vogliamo simulare il movimento di un manipolatore antropomorfo 3R costituito da tre
giunti rotazionali e tre bracci (o link) da essi connessi. Tale robot è in grado di muoversi nello
spazio grazie ai tre gradi di mobilità forniti dalla struttura mostrata in figura:
ogni giunto conferisce un singolo grado di mobilità in quanto il manipolatore è costituito da una
catena cinematica aperta, con un estremo vincolato alla base e l'altro connesso all'organo terminale,
detto anche end-effector. Questo elemento è tipicamente movimentato attraverso un polso sferico
(altri 3 gradi di mobilità), invece nel nostro caso è semplicemente ancorato all’estremità del robot.
L'asse di rotazione del giunto di base (piantone) è ortogonale agli altri due, tra di loro paralleli. Per
analogia con il braccio umano il secondo giunto è detto giunto di spalla, mentre il terzo di gomito.
Vantaggio di questa struttura è la grande agilità dovuta alla natura rotoidale di tutti i giunti.
D'altro canto la precisione nel posizionamento dell'end-effector varia in tutto lo spazio di lavoro e
viene meno la corrispondenza tra gradi di mobilità e gradi di libertà. Per spazio di lavoro di un robot
si intende l'insieme dei punti (posizioni) dello spazio che il robot può raggiungere con l'organo
terminale. Nella figura sottostante si vede che lo spazio di lavoro per il manipolatore 3R è
approssimativamente una porzione di sfera:
Desideriamo far svolgere al manipolatore due compiti:
1) assegnare all'end effector una traiettoria rettilinea punto-punto con un certo profilo di velocità
2) assegnare al 3R una configurazione di giunti iniziale e fargli raggiungere un'altra configurazione
imponendo particolari condizioni di moto per i giunti.
Prima di implementare i compiti del robot e commentare i risultati spieghiamo le descrizioni
matematiche e le funzioni necessarie a tale scopo:
Descrizione geometrica
Per descrivere il robot utilizziamo la convenzione di Denavit-Hartenberg la quale fornisce
un metodo generale per le catene cinematiche aperte che consente di fissare i sistemi di riferimento
sui giunti (e link) per poterne determinare i parametri caratteristici. Da questa rappresentazione è
possibile, tramite l'uso di matrici di roto-traslazione dei sistemi di riferimento, trovare un legame fra
i parametri dei giunti e la posizione e l'orientamento dell'end-effector.
Riferendoci alla figura assumiamo come asse i l’asse del giunto che connette il braccio i − 1 al
braccio i e per la definizione della terna i (solidale al braccio i ) utilizziamo la
Convenzione di Denavit-Hartenberg
•
•
•
•
si sceglie l’asse zi giacente lungo l’asse del giunto i + 1 ;
si individua Oi all’intersezione dell’asse zi con la normale comune agli assi zi −1 e zi , e con Oi ' si
indica l’intersezione della normale comune con zi −1 ;
si sceglie l’asse xi diretto lungo la normale comune agli assi zi −1 e zi con verso positivo dal giunto
i al giunto i + 1 ;
si sceglie l’asse yi in modo da completare una terna levogira.
ci sono casi (come il 3R) in cui due o più terne del robot non sono univocamente determinate:
o
o
o
o
o
con riferimento alla terna 0, per la quale solo la direzione dell’asse z0 risulta specificata: si
possono quindi scegliere arbitrariamente O0 ed x0 ;
con riferimento alla terna n poiché non vi è giunto n + 1 , zn non è univocamente definito
mentre l’asse xn , deve essere normale all’asse zn−1 : tipicamente, il giunto n è rotoidale, per
cui z n va allineato con z n−1 ;
quando due assi consecutivi sono paralleli, in quanto la normale comune tra di loro non è
univocamente definita;
quando due assi consecutivi si intersecano, in quanto il verso di xi è arbitrario;
quando il giunto i è prismatico, nel qual caso solo la direzione dell’asse zi −1 è determinata.
Il robot antropomorfo 3R è caratterizzato pienamente da queste condizioni: oltre a terna base
e terna solidale all’end-effector ci sono due giunti paralleli e inoltre gli assi consecutivi del
primo e del secondo giunto si intersecano nel secondo giunto. Vedremo in base all’algoritmo
come effettuare le tutte le scelte per la rappresentazione spaziale del manipolatore. Intanto
basti dire che una volta scelte le terne solidali ai bracci, la posizione e l’orientamento della
terna i rispetto alla terna i − 1 risultano completamente specificati dai seguenti parametri:
ai
di
αi
ϑi
distanza di Oi da Oi ' ,
coordinata su zi −1 di Oi '
(magari riscrivere come sta su dispense +chiaro)
angolo intorno all’asse xi tra l’asse zi −1 e l’asse zi valutato positivo in senso
antiorario
angolo intorno all’asse zi tra l’asse xi −1 e l’asse xi valutato positivo in senso
antiorario
I parametri ai e α i sono sempre costanti e dipendono soltanto dalla geometria di
connessione dei giunti consecutivi dettata dalla presenza del braccio i . Degli altri due , uno
soltanto è variabile in dipendenza del tipo di giunto utilizzato per connettere il braccio i − 1
al braccio i ; in particolare:
•
•
se il giunto i è rotoidale la variabile è ϑi
se il giunto i è prismatico la variabile è d i
Pertanto una particolare configurazione del 3R sarà individuata da un vettore q = [ϑ1 ϑ2 ϑ3 ]T ,
mentre i parametri d1 , d 2 , d 3 sono fissi.
Vediamo dunque come mettere in relazione tutti i riferimenti e i parametri del robot per poi
utilizzarli nei calcoli delle simulazioni:
Algoritmo di Denavit-Hartenberg
1. Individuare e numerare consecutivamente gli assi dei giunti; assegnare, rispettivamente, le direzioni
agli assi z0 , ..., zn −1 .
2. Fissare la terna 0 posizionandone l'origine sull'asse z0 ; gli assi x0 e y0 sono scelti in maniera tale da
ottenere una terna levogira.
i passi 3-5 vanno ripetuti per i = 1, ..., n − 1 :
3. Individuare l’origine Oi all’intersezione di zi con la normale comune agli assi zi −1 e zi . Se gli assi
zi −1 e zi sono paralleli e il giunto i è rotoidale, posizionare Oi in modo da annullare d i ; se il
giunto i è prismatico, scegliere Oi in corrispondenza di una posizione di riferimento per la corsa del
giunto (ad esempio un fine-corsa).
4. Fissare l’asse xi diretto lungo la normale comune agli assi zi −1 e zi con verso positivo dal giunto i
al giunto i + 1 .
5. Fissare l’asse yi in modo da ottenere una terna levogira.
6. Fissare la terna n , allineando zn lungo la direzione di zn −1 se il giunto n è rotoidale, ovvero
scegliendo zn in maniera arbitraria se il giunto n è prismatico; fissare l’asse xn in accordo al punto
4.
Gli ultimi due passi dell’algoritmo sono specificamente pensati per la determinazione delle
matrici di trasformazione coinvolte nel problema della
Cinematica diretta
Tenendo presente che, avendo seguito i passi sinora descritti, dalle scelte operate risulta la
seguente assegnazione dei sistemi di riferimento,
vogliamo ora esprimere le relazioni che, a partire dalle configurazioni dei giunti del robot, ci
permetteranno di ricavare le coordinate della terna solidale all'end-effector, quindi la sua posizione,
ossia risolvere il problema della cinematica diretta. Proseguendo con Denavit-Hartenberg
applichiamo l’algoritmo per definire le relazioni del manipolatore antropomorfo 3R:
7. Costruire per i = 1, ... , n la tabella dei parametri ai , d i , α i ,ϑi .
Per il 3R compiliamo la seguente tabella:
Braccio
ai
αi
di
ϑi
1
0
π /2
0
ϑ1
2
a2
0
0
ϑ2
3
a3
0
0
ϑ2
si noti che la terna 0 è stata scelta con origine all’intersezione di z0 e z1 (cioè sul secondo giunto),
da cui d1 = 0 e che z1 e z2 sono paralleli e gli assi x1 e x2 sono scelti lungo gli assi dei relativi
bracci, da cui gli altri valori. In questa assegnazione i valori a2 e a3 corrispondono alle lunghezze
del secondo e del terzo braccio.
8. Calcolare sulla base dei parametri di cui al punto 7 le matrici di trasformazione omogenea Aii −1 (qi )
per i = 1, ... , n . Queste esprimono il legame tra la terna solidale al giunto i e la terna solidale al
giunto i − 1 , contengono le operazioni da effettuare sull’una perché si sovrapponga all’altra. Per il
3R, in base al calcolo delle operazioni si ha:
⎡c1
⎢s
0
A1 (ϑ1 ) = ⎢ 1
⎢0
⎢
⎣0
0 s1
0 − c1
1 0
0 0
0⎤
0⎥⎥
0⎥
⎥
1⎦
⎡ci
⎢s
i −1
A1 (ϑi ) = ⎢ i
⎢0
⎢
⎣0
− si
ci
0
0
0 ai ci ⎤
si ,..., j = sin(ϑi + ... + ϑ j )
0 ai si ⎥⎥
i = 2,3
1 0 ⎥
ci ,..., j = cos(ϑi + ... + ϑ j )
⎥
0 1 ⎦
9. Calcolare la trasformazione omogenea Tn0 ( q ) = A10 ... Ann−1 che fornisce posizione e orientamento
della terna n rispetto alla terna 0.
⎡c1c23
⎢s c
1 23
0
0 1 2
Nel nostro caso T3 (q ) = A1 A2 A3 = ⎢
⎢ s23
⎢
⎣ 0
− c1s23
− s1s23
c23
0
s1 c1 (a2c2 + a3c23 )⎤
− c1 s1 (a2c2 + a3c23 ) ⎥⎥
0
a2 s2 + a3 s23 ⎥
⎥
0
1
⎦
q = [ϑ1 ϑ2 ϑ3 ]T
10. Assegnate T0b e Ten , calcolare la funzione cinematica diretta Teb (q ) = T0bTn0Ten che fornisce
posizione e orientamento della terna utensile rispetto alla terna base. Nel nostro caso esiste solo la
terna n che non coincide con una eventuale terna utensile, dato che non è presente nella nostra
simulazione per cui non consideriamo questo ultimo passo dell’algoritmo.
Segue la realizzazione della funzione cinematica diretta in Matlab, nel riquadro sottostante;
ognuno dei riquadri delle funzioni illustrate sarà sempre preceduto dal nome del file e da
un’insieme di link a forma di icona, facendo clic sui quali saranno intraprese determinate azioni:
Æ
sarà avviato Matlab che aprirà dalla cartella ‘programs’ il file per poterlo editare e/o eseguire;
Æ il file sarà aperto anziché con Matlab con l’editor associato agli m-file (a scelta dell’utente);
questi primi due link funzionano nel caso in cui sia stato installato il simulatore su disco rigido;
Æ
il file sarà aperto con l’editor degli m-file direttamente dalla cartella programmi (Æ )
contenuta nel CD-ROM. A seconda del sistema operativo installato alcuni link potrebbero risultare
non funzionanti per via della configurazione (ad esempio se si legge il CD attraverso un
masterizzatore in ambiente XP con l’opzione di masterizzazione di Windows attivata); sarà
opportuno riferirsi all’Appendice C di questo documento per l’installazione e la disinstallazione del
simulatore cinematico sul proprio computer. Delle funzioni sono riportate le righe principali e di
alcune particolarmente lunghe soltanto i commenti essenziali per una comprensione panoramica.
‘cindir.m’
Æ
Æ
Æ
% Calcola la cinematica diretta del manipolatore antropomorfo 3R,
% con in ingresso il vettore di configurazione dei giunti e restituisce
% il vettore posizione dell'origine della terna solidale all'end-effector
function [p]=cindir(q)
global l2 l3
% Tabella dei parametri di Denavit-Hartenberg robot 3R
a1=0;
d1=0;
tw1=pi/2;
th1=q(1);
a2=l2;
d2=0;
tw2=0;
th2=q(2);
a3=l3;
d3=0;
tw3=0;
th3=q(3);
% posizioni comode per scrivere la matrice completa
c1=cos(q(1));
s1=sin(q(1));
c2=cos(q(2));
s2=sin(q(2));
c23=cos(q(2)+q(3));
s23=sin(q(2)+q(3));
Tdir=
[
c1*c23
-c1*s23
s1*c23
-s1*s23
s23
c23
0
0
p=[Tdir(1,4) Tdir(2,4) Tdir(3,4)]';
s1
-c1
0
0
c1*(a2*c2+a3*c23)
s1*(a2*c2+a3*c23)
a2*s2+a3*s23
1
;
;
;
];
La funzione, per un ingresso q nel formato vettore (ad esempio q = [0.2 − 3.14 2.15]' ),
restituisce la posizione, sempre in formato vettore ( p = [...]' ) della terna solidale all’end-effector in
quella data configurazione utilizzando le informazioni sui parametri di Denavit-Hartenberg forniti
dall’esterno. I parametri che seguono il comando global sono comuni a tutte le funzioni del
workspace di Matlab che riportano gli stessi parametri dopo l’istruzione global, in modo che una
volta forniti i dati dall’esterno (da un m-file o dal prompt di Matlab) e immessi nello spazio globale
tutte le funzioni che useremo faranno uso di tali dati. Se ad esempio si compila il file con i dati, nel
quale è contenuta la stringa ‘global l1 l2 l3’, sarà possibile richiamare cindir che utilizzerà
proprio i valori ivi definiti. Questo vale anche per tutte le funzioni successivamente illustrate.
Cinematica differenziale
Mentre la cinematica diretta è un legame statico tra spazio dei giunti e spazio cartesiano,
così come la cinematica inversa analitica (che vedremo immediatamente dopo), la cinematica
differenziale utilizza il legame dinamico tra la velocità cartesiana dell'end-effector e le velocità dei
giunti. Tali relazioni si basano su un operatore lineare detto Jacobiano che in generale dipende
dalla configurazione in cui si trova il Robot.
Jacobiano
Esistono due tipi di Jacobiano: Jacobiano geometrico e Jacobiano analitico. Il primo
descrive il legame tra le velocità dei giunti e la velocità lineare dell'end-effector tramite una matrice
di trasformazione dipendente dalla configurazione del manipolatore. Il secondo sfrutta una
rappresentazione in forma minima della postura dell'end-effector, in base ad un calcolo diretto,
mediante un'operazione di differenziazione della funzione cinematica diretta rispetto alle variabili di
giunto.
Nel caso del manipolatore antropomorfo 3R (nel nostro caso privato del polso) i due tipi di
Jacobiano concidono. La struttura base dello Jacobiano è la seguente:
⎧⎡ zi−1 ⎤
per un giunto prismatico
⎪
⎡ jPi ⎤ ⎪⎢⎣0 ⎥⎦
⎢ ⎥=⎨
⎢⎣ jOi ⎥⎦ ⎪⎡ zi−1 × ( p − pi−1 )⎤
⎥ per un giunto rotoidale
⎪⎢ z
i
−
1
⎣
⎦
⎩
Nel nostro caso si ha:
⎡ z × ( p − p0 ) z1 × ( p − p1 ) z2 × ( p − p2 )⎤
J =⎢ 0
⎥
z0
z1
z2
⎣
⎦
il calcolo dei vettori di posizione dei bracci fornisce
⎡a2c1c2 ⎤
⎡0 ⎤
⎥
⎢
p0 = p1 ⎢0⎥ p2 = ⎢⎢ a2 s1c2 ⎥⎥
⎢⎣ a2 s2 ⎥⎦
⎢⎣0⎥⎦
⎡c1 (a2c2 + a3c23 )⎤
p = ⎢⎢ s1 (a2c2 + a3c23 ) ⎥⎥
⎢⎣ a2 s2 + a3 s23 ⎥⎦
e quello dei versori degli assi di rotazione dei giunti:
⎡0 ⎤
z0 = ⎢⎢0⎥⎥
⎢⎣1⎥⎦
⎡ s1 ⎤
z1 = z2 = ⎢⎢− c1 ⎥⎥
⎢⎣ 0 ⎥⎦
sostituendo si ottiene lo Jacobiano completo (posizione e orientamento)
⎡− s1 (a2c2 + a3c23 ) − c1 (a2 s2 + a3 s23 ) − a3c1s23 ⎤
⎢ c (a c + a c ) − s (a s + a s ) − a s s ⎥
1
2 2
3 23
3 1 23 ⎥
⎢ 1 2 2 3 23
⎢
a2c2 + a3c23
a3c23 ⎥
0
⎡J P ⎤
J =⎢
⎥ ossia J = ⎢ ⎥
s1
s1 ⎥
0
⎣JO ⎦
⎢
⎢
0
− c1
− c1 ⎥
⎥
⎢
1
0
0 ⎦⎥
⎣⎢
Nel nostro caso, avendo un manipolatore mai ridonante con soli tre gradi di libertà, abbiamo
solo tre righe linearmente indipendenti (le prime tre). Ciò si giustifica intuitivamente con il fatto che
l’end-effector è ancorato al terzo braccio e dunque non è possibile sceglierne arbitrariamente
l’orientamento una volta fissata la posizione. Pertanto lo Jacobiano che noi prenderemo in
considerazione si riduce alla seguente espressione:
⎡− s1 (a2c2 + a3c23 ) − c1 (a2 s2 + a3 s23 ) − a3c1s23 ⎤
J P = ⎢⎢ c1 (a2c2 + a3c23 ) − s1 (a2 s2 + a3 s23 ) − a3 s1s23 ⎥⎥
⎢⎣
0
a2c2 + a3c23
a3c23 ⎥⎦
La funzione che scriviamo in Matlab per calcolare lo Jacobiano del 3R è banalmente
l’applicazione della formula:
Æ
Æ
Æ
‘Jac.m’
%
%
%
La funzione Jac calcola lo Jacobiano del robot 3R che
risulta definito per una determinata configurazione di
giunti in ingresso q.
function ja=Jac(q)
global l2 l3;
t1=q(1);
t2=q(2);
t3=q(3);
%a1=0; l1 è indifferente in quanto il riferimento è sul piantone e a1=0
a2=l2;
a3=l3;
ja=[-sin(t1)*(a2*cos(t2)+a3*cos(t2+t3)) -cos(t1)*(a2*sin(t2)+a3*sin(t2+t3)) -a3*cos(t1)*sin(t2+t3);
cos(t1)*(a2*cos(t2)+a3*cos(t2+t3))
-sin(t1)*(a2*sin(t2)+a3*sin(t2+t3)) -a3*sin(t1)*sin(t2+t3);
0
a2*cos(t2)+a3*cos(t2+t3)
a3*cos(t2+t3)];
Cinematica inversa
Ora la nostra attenzione punta a risolvere il problema cinematico inverso e in seguito
all’implementazione di una funzione che ci faccia sapere non solo quali sono le soluzioni, quando è
possibile trovarle, ma anche più in generale se esistono e se sono in numero finito o infinito. Queste
funzionalità saranno particolarmente utili nelle successive fasi di pianificazione del moto. Tale
analisi è resa possibile a monte dallo studio dello Jacobiano appena esposto. Esattamente il
problema è il seguente: data una determinata posa per l'end-effector, cerchiamo, se possibile, una o
più soluzioni per la corrispondente configurazione dei giunti. A differenza della cinematica diretta,
che ammette sempre un'unica soluzione, possiamo avere:
• nessuna soluzione: se la configurazione non appartiene allo spazio di lavoro del
manipolatore
• un numero finito di soluzioni : quando esistono più modi per posizionare i link, per il 3R
solitamente sono quattro, ma possono degenerare in due coppie di soluzioni coincidenti.
• infinite soluzioni : quando esistono infiniti modi di posizionare i link, questo succede in
particolari condizioni di singolarità.
Quindi cerchiamo un vettore q = [ϑ1 ϑ2 ϑ3 ]T di angoli dei giunti tali che l'organo terminale si
trovi nel punto desiderato. Riprendiamo in considerazione la struttura del robot:
Dal caso del manipolatore planare 3R (tenendo fisso ϑ1 la descrizione per ϑ2 e ϑ3 è analoga):
poniamo
pW x = p x − a3cφ = a1c1 + a2c12
pW y = p y − a3 sφ = a1s1 + a2 s12
Nel caso planare pW x è la proiezione sull' asse x dell'origine della terna solidale al giunto
W mentre nel caso tridimensionale rappresenta, per un valore fissato dell'angolo relativo al primo
giunto, la proiezione sull'asse x dell'origine della terna solidale all'end-effector dal quale vogliamo
ricavare tramite inversione la configurazione dei giunti. Analoghe considerazioni valgono per pW y e
PW z , dato che ragioniamo ora in tre dimensioni . Per il primo giunto otteniamo due soluzioni:
ϑ1 = Atan 2( pW y , pW x )
ϑ1 = π + Atan 2( pW y , pW x )
A differenza della semplice funzione arco-tangente la funzione Atan2, considerando i segni degli
argomenti, restituisce l'arco-tangente nel quadrante corretto.
A questo punto, determinato ϑ1 , per il calcolo di ϑ2 e ϑ3 possiamo considerare il manipolatore
planare:
pW2 x + pW2 y + pW2 z − a22 − a32
c3 =
s3 = ± 1 − c32
2a2 a3
dove con pW z si intende la proiezione del vettore posizione sull'asse z del riferimento, quindi:
ϑ3 = Atan 2( s3 , c3 )
per ϑ2 si ha:
s2 =
(a2 + a3c3 ) pW z − a3 s3 pW2 x + pW2 y
pW2 x + pW2 y + pW2 z
c2 =
(a2 + a3c3 ) pW2 x + pW2 y + a3 s3 pW z
pW2 x + pW2 y + pW2 z
ϑ2 = Atan 2( s2 , c2 )
Dai precedenti passaggi si riconosce l'esistenza di quattro soluzioni al variare dei valori di
ϑ1 , ϑ2 e ϑ3 : spalla destra-gomito alto, spalla sinistra-gomito alto, spalla destra-gomito basso, spalla
sinistra-gomito basso. Queste configurazioni sono illustrate nella successiva figura.
Si può osservare che al problema cinematico inverso esiste un'unica soluzione solo se
pW x ≠ 0 e pW y ≠ 0 ; infatti in caso contrario si possono ottenere infinite soluzioni a prescindere dal
valore di ϑ1 ; Situazioni del genere vengono definite singolarità cinematiche. Questo non significa
che in tutte le condizioni di singolarità le soluzioni sono sempre infinite: ad esempio per tutte le
configurazioni del tipo ϑ3 = 0 ma con pW x ≠ 0 e pW y ≠ 0 esistono due soluzioni; o meglio, le
quattro soluzioni relative a spalla e gomito si riducono a due coppie di soluzioni coincidenti. Infatti
in tale situazione il gomito è steso, per cui la configurazione spalla-destra gomito alto corrisponde a
spalla-destra gomito basso e spalla sinistra gomito-alto è identica a spalla sinistra-gomito basso.
Questa è un’altra situazione di singolarità. È evidente dunque la necessità di una caratterizzazione
di tutte queste configurazioni.
Classificazione delle singolarità cinematiche
Le singolarità o configurazioni singolari si hanno quando la matrice che esprime lo
Jacobiano perde di rango (determinante nullo); in tali configurazioni del manipolatore può accadere
che:
• alcune direzioni di moto non siano realizzabili
• basse velocità dello spazio operativo corrispondono a velocità molto elevate ai giunti
• non si ha una ben definita soluzione del problema cinematico inverso.
È chiaro dunque che tali punti (o zone) vanno evitati oppure affrontati con opportuni metodi.
Esistono due classi di singolarità:
•
ai confini dello spazio di lavoro: il manipolatore è completamente esteso o retratto; sono
facilmente aggirabili evitando di portare il manipolatore ai confini dello spazio di lavoro.
Per il manipolatore 3R se le lunghezze degli ultimi due bracci non sono uguali si crea una
zona di non raggiungibilità sferica e centrata nell’origine avente come raggio la differenza
del più lungo con il più corto. È facile vederlo sul caso del 2R planare, l’estensione al caso
spaziale è poi immediata aggiungendo nello spazio il primo link:
Spazio di lavoro: 2R planare
Robot 3R con l2>l3
Inoltre se si vogliono considerare i fine-corsa dei giunti lo spazio di lavoro è ancor più
ristretto:
esempio di workspace di 2R planare con
ϑ1 ∈ [−120°,120°] ; ϑ2 ∈ [−90°,90°] .
Per il 3R si trovano risultati simili
Nel modello cinematico del simulatore noi ignoriamo questa possibilità lasciando tutti i
giunti liberi di ruotare nell’intervallo [ 0 , 2π ] compiendo anche più giri (nella realtà questo
è piuttosto raro…). I contorni verdi descrivono le zone di singolarità gomito steso/retratto.
•
All’interno dello spazio di lavoro: allineamento di assi del moto o configurazioni particolari
dell’organo terminale; sono in generale da evitare in fase di pianificazione della traiettoria.
Analizziamo ora matematicamente le singolarità del nostro robot: non essendo dotato di polso può
imbattersi soltanto in singolarità di struttura portante.
Il determinante dello Jacobiano da noi considerato è det( J P ) = −a2 a3 s3 (a2c2 + a3c23 ) . Nell'ipotesi di
a2 , a3 ≠ 0 il determinante si annulla per:
• s3 = 0 si ha quando ϑ3 = 0 o ϑ3 = π , cioè quando il robot ha il gomito tutto steso (come
nella figura sottostante) o ripiegato su se stesso. Queste vengono chiamate singolarità di
gomito.
•
(a2c2 + a3c23 ) = 0 si ha quando l'origine del sistema di riferimento solidale all'end-effector
si trova lungo l'asse di rotazione di base z0 (infinite soluzioni), cioè quando p x = p y = 0 .
Questa viene detta singolarità di spalla. In particolare nell’origine del sistema di riferimento
da noi adottato, se essa è raggiungibile, cioè se l2 = l3 , si hanno addirittura ∞ 2 soluzioni.
Infatti un modo di descriverle è per esempio fissare ϑ3 a π oppure a −π e far variare
indipendentemente gli altri due parametri ϑ2 in [ −π / 2, +π / 2] e ϑ1 in [ 0, 2π ] .
Aggiungiamo infine che nel caso di singolarità sul confine interno dello spazo di lavoro,
qualora i bracci l2 ed l3 fossero di lunghezza diversa, si trovano sempre quattro soluzioni distinte (a
meno che il punto non sia su z0 ) sebbene l’unico orientamento possibile per l’end-effector, che
essendo ancorato al terzo braccio è esattamente ϑ3 , è comune a tutte e quattro le soluzioni.
Presentiamo ora l’implementazione della cinematica inversa che tiene conto di tutte le
considerazioni fatte finora. In più premettiamo che, per ragioni che saranno chiarite quando
esporremo la pianificazione dei compiti, laddove analiticamente troviamo infinite soluzioni, la
funzione effettua una ricerca semplificata di valori ammissibili per le configurazioni dei giunti e
restituisce, oltre a un insieme di informazioni sulle singolarità individuate, l’insieme di valori
trovati. Di questa funzione è riportato il commento, che ne costituisce anche l’help, in quanto il
codice è puramente tecnico, non è molto significativo il modo in cui è implementato:
‘invanalitica.m’
%
%
%
%
%
%
%
%
%
%
%
Æ
Æ
Æ
Inversa analitica: data una posizione per il centro dell'end-effector del 3R
analizza l'esistenza di soluzioni e le calcola quando esistono. Se il manipolatore
si trova in singolarità viene specificato il tipo di singolarità e vengono
calcolate, tra le infinite soluzioni, tante a seconda della specifica di precisione
in ingresso; il motivo è che faranno comodo per la pianificazione
di traiettorie avvicinarsi in tutte le situazioni possibili di partenza e arrivo
a quelle che rendono migliore il moto sotto vari aspetti (ad esempio minimizzano
gli spostamenti dei giunti), anche qualora il punto di partenza e/o arrivo
fossero in singolarità. I commenti sottostanti favoriranno la comprensione
dei codici scritti nella funzione:
% Ingressi: 2
%
p = posizione cartesiana del centro del polso
%
accuracy=livello di precisione con cui esplorare le possibili soluzioni in particolari
%
condizioni di singolarità; in pratica è uno scalare che indica il numero di parti
%
cui viene divisa una semicirconferenza e serve a far variare un angolo tra 0 e pi/2
%
in (piccoli) passi. Tale valore sarà utilizzato dalla funzione 'invnormaminima'
%
Non è comunque necessario specificare tale ingresso. Se l'ingresso non esiste
%
la funzione utilizza un valore interno predefinito (volendo può essere cambiato)
%
Notare che questo metodo è considerato SOLO in alcune singolarità. In tutti gli
%
altri casi l'inversione è puramente analitica.
% Uscite:
5
%
msg = è un messaggio che può essere indirizzato all'utente, indica a parole la situazione
%
analizzata dalla funzione relativamente al punto e anche il tipo di singolarità;
%
è particolarmente utile se si vuole utilizzare temporaneamente la funzione da sola
%
senza far girare il simulatore, ai fini di valutare rapidamente molti punti di fila
%
per la pianificazione da inserire successivamente nella simulazione.
%
Esempio: digitando al prompt di Matlab >>invanalitica([0 0 3]')
%
anzichè un comando completo del tipo >>[msg,M,insing,sit,nsol]=invanalitica(p,accuracy)
%
la funzione restituisce solo la prima delle uscite, cioè il messaggio che potrebbe essere
%
ad esempio (se l1=l2=l3=5)'Il manipolatore si trova in singolarità lungo l'asse del piantone
%
(infinite soluzioni per theta1)', e usa la precisione interna dato che accuracy non è fornito
%
M = Se esistono soluzioni in forma chiusa sono 2 doppie o 4, riportate a 4 righe (matrice 4x3).
%
Qualora però le soluzioni risultassero analiticamente infinite, come suddetto la matrice assume
%
diverse dimensioni secondo il tipo di singolarità e in base al livello di accuratezza;
%
Le singolarità in cui si hanno analiticamente infinite soluzioni sono qui divise in due insiemi
%
1) singolarità lungo l'asse del piantone ma non nell'origine (ammesso che sia raggiungibile)
%
in questo caso theta2 e theta3 hanno determinate soluzioni mentre theta1 varia in [0,2pi]
%
Poichè come specificato su bisogna far variare un angolo (theta1) tra 0 e 2pi
%
saranno generati 2*accuracy valori per theta1 essendo accuracy il numero di divisioni
%
da effettuare su una semicirconferenza. Pertanto M avrà dimensioni (2*accuracy x 3)
%
2) singolarità nell'origine (possibile solo se l2=l3 altrimenti non è neppure raggiungibile)
%
in questo caso theta1 varia sempre in [0,2pi], ma in più varia anche theta2 in [-pi,pi].
%
Il discorso sulla dimensione di M è analogo solo che ora si fanno variare due angoli.
%
Dunque la matrice sarà composta da più gruppi di colonne tipo quelle viste per il caso 1)
%
L'algoritmo estrae le soluzioni in questo modo: parte da theta1=0 e theta2=-pi (th3=pi)
%
poi fa variare theta1 fino a 2pi ottenendo una colonna di (2*accuracy x 3) elementi;
%
dopodichè incrementa theta2 di un passo di accuracy (cioè pi/accuracy) e ripete il primo
%
loop. Questo finchè theta2 è arrivato a pi. Così a ogni loop di theta 1 si genera una
%
colonna del tipo [(theta1 variabile) (theta2 fisso) (th3 sempre fisso)]
%
La matrice ha così (accuracy+1)*3 colonne (+1 perchè estremi inclusi) e 2*accuracy righe.
%
In tal modo saranno esplorate molte soluzioni nell'intorno sferico dell'origine
%
per il posizionamento del gomito. Ricordiamo che questo servirà ad altre funzioni
%
a monte della pianificazione di traiettorie per la scelta ottimale dell'inizializzazione.
%
E' chiaro che più è grande accuracy e più l'insieme di soluzioni si avvicinerà a oo
%
ma per generare tante soluzioni occorrerà anche maggior tempo. Di default accuracy vale12
%
dunque saranno generate 24 soluzioni nel caso 1 M(24x3) e 312 per il caso 2 [M(24x39)]
%
Esistono poi le singolarità di braccio steso (ma non lungo l'asse z0) e, se l2<>l3, retratto:
%
nel primo caso le soluzioni degenerano da 4 a 2 (coppie di soluzioni coincidenti); nel secondo
%
sono invece sempre 4 soluzioni distinte in quanto la singolarità implica solo mobilità ridotta
%
M ha sempre dimensioni 4x3 anche se le righe indipendenti sono soltanto 2 (codice più comodo)
%
Infine se il punto si trova fuori dello spazio di lavoro la matrice è posta uguale a [](vuoto).
%
insing = variabile booleana. Vale TRUE se il manipolatore si trova in singolarità altrimenti FALSE;
%
sit = è un semplice scalare che vale 1 se la singolarità è lungo l'asse z0, 2 se è di gomito steso,
%
3 se il punto si trova sul confine interno dello spazio di lavoro (solo nel caso l2<>l3);
%
in tutti gli altri casi è posto a -1.
%
nsol = numero di soluzioni trovate, può essere 0 (oltre WS), oppure oo (singolarità z0/origine)
%
2 per le singolarità ai confini esterni dello spazio di lavoro,4 nel WS o ai confini interni
Riteniamo opportuno, piuttosto che riportare qui anche l’intero codice della funzione, che il lettore
consulti direttamente il file nel caso sia interessato ai dettagli del codice, peraltro completamente
commentato a fianco.
Implementazione della cinematica differenziale
Sia data la relazione υ = J (q )q che coinvolge lo Jacobiano geometrico. Se lo Jacobiano è
quadrato e non singolare, l’equazione della cinematica differenziale inversa è q = J −1 (q)υ che
consente, assegnata la velocità della terna utensile e la configurazione del manipolatore, di
determinare le velocità dei giunti. Tale relazione consente anche di risolvere il problema della
cinematica inversa.
Assegnata la traiettoria della terna utensile e la velocità ad essa associata υ (t ) è possibile
determinare le variabili di giunto corrispondenti integrando la relazione q (t ) = J −1 (q(t ))υ (t ) a
partire da una condizione iniziale nota q (0) ; questa condizione deve essere tale che la terna utensile
sia nella posizione iniziale desiderata. L’integrazione può essere eseguita per via numerica: è
esattamente quello che faremo nelle simulazioni attraverso simulink. Tale metodo è in alternativa
alla risoluzione analitica del problema cinematico inverso.
Vantaggi: l’integrale numerico può sempre essere calcolato;
Svantaggi: sono richieste risorse in termini di tempo di calcolo.
.
Trattamento delle singolarità cinematiche
Le soluzioni della cinematica differenziale inversa possono essere calcolate solo nell'ipotesi
in cui lo Jacobiano sia di rango pieno. Infatti nelle configurazioni singolari il sistema associato
contiene equazioni linearmente dipendenti. È possibile comunque determinare una soluzione di q
estraendo tutte le equazioni linearmente indipendenti, quando il percorso assegnato nello spazio
operativo è fisicamente eseguibile da parte del manipolatore.
Bisogna considerare che lo Jacobiano può dar luogo a una serie di problemi che riguardano
essenzialmente le velocità imposte ai giunti. Infatti il determinante assume, man mano che ci si
avvicina ad una singolarità valori tendenti allo zero, il che significa imporre grandi velocità ai
giunti, cosa da evitare perchè generalmente irrealizzabile nella pratica. Questo è il frutto di
un'inversione semplice (o elementare) dello Jacobiano.
Per ovviare a questo problema allora utilizziamo l'inversa ai minimi quadrati smorzata:
J * = J T ( JJ T + λ2 I ) −1
questo significa aggiungere un fattore di correzione che modifica il comportamento delle inversioni
proprio seguendo l'avvicinamento alle zone di singolarità. Il fattore λ , dunque, se opportunamente
predisposto, produce uno smorzamento che ammorbidisce l'inversione e dunque migliora il
comportamento a livello dei giunti.
Metodi per l'inversione della cinematica differenziale
Poichè effettueremo la simulazione su un calcolatore dovremo considerare che la
discretizzazione delle funzioni per l'inversione, rispetto al caso continuo, comporta fenomeni di
deriva per la soluzione: non si ha una corrispondenza esatta tra configurazione di giunti calcolate e
postura effettivamente raggiunta dall'organo terminale nello spazio operativo. Si può ovviare a
questo problema tenendo conto dell'errore tra la posizione desiderata e la posizione calcolata:
e = xd − x , da cui, derivando si ha e = xd − x . Applicandolo alla cinematica differenziale si ha
e = xd − J A (q)q .
Per legare q ad e è dunque possibile sfruttare il:
Metodo di Newton (e inversione robusta)
Il metodo di Newton è un'algoritmo generale di approssimazione per le soluzioni di
problemi numerici (non lineari) e si riassume nella seguente espressione:
2
f (q ) = f (q k ) + J r (q k )[q − q k ] + o[ q − q k ] ;
se identifichiamo la f (q ) come la cinematica diretta del nostro manipolatore e scartiamo
l' o piccolo allora possiamo riscrivere tale espressione come:
qk +1 = qk + J −1 (qk )[r − f (qk )] .
Ipotizzando J quadrata e non singolare e applicando il metodo di Newton si giunge a:
q = J A−1 (q )( xd + Ke) .
Il parametro K è una matrice definita positiva e (tipicamente) diagonale che fa tendere
l'errore a zero lungo la traiettoria desiderata con una velocità di convergenza che dipende dai suoi
autovalori: più sono grandi, più è veloce la convergenza. L'inversa J −1 può essere adattata in base
alle esigenze specifiche. Se il percorso passa vicino ad una zona di singolarità la normale inversione
dello Jacobiano porta ad una "esplosione" dell'algoritmo verso valori sempre più elevati. È pertanto
opportuno modificare il metodo per l'inversione definendo una nuova inversione "robusta", secondo
il metodo già visto dei minimi quadrati smorzati. La nuova espressione diventa:
q = J * (q )( xd + Ke) con J * = J T ( JJ T + λ2 I ) −1
Nel nostro caso, esaminiamo dapprima una configurazione in cui l'end-effector risulta
prossimo ad una singolarità. Più precisamente consideriamo una posizione in cui la configurazione
di giunti risulta singolare per via dell’annullamento del determinante. La lontananza dalla
singolarità è come un ε che rappresenta la distanza del punto dal “centro” della singolarità. Il
manipolatore è prossimo ad una zona di singolarità, come l’asse z0 , piuttosto che ad un punto,
allora ε sarà semplicemente la distanza dell’end-effector dall’asse. Infatti si trovano infiniti
“centri” di singolarità vicini al manipolatore e l’ ε scelto è per l’appunto quello relativo al più
vicino.
ε
Per variori inferiori a ε1 , cioè da un certo ε fissato in poi (in direzione dello zero), applichiamo un
profilo triangolare al parametro λ in funzione di ε :
λ = λ (ε )
λmax
ε
− ε1
ε1
L'espressione di tale funzione risulta analiticamente:
⎛
λ (ε ) = λmax ⎜⎜1 −
⎝
|ε |⎞
⎟
ε1 ⎟⎠
ε ≤ ε1
nell'implementazione di tale profilo in Matlab però sceglieremo di utilizzare la funzione solo nel
primo quadrante (positivo) in quanto Matlab non permette di gestire vettori con coordinate negative,
senza peraltro alterare effettivamente il comportamento della funzione: al centro della singolarità la
correzione λ è massima, mentre, man mano che l’end-effector si allontanana dal centro, λ
decresce sempre più per ogni direzione in modo lineare. Con la funzione ‘lam’ implementiamo tale
profilo:
‘lam.m’
%
%
%
%
%
%
Æ
Æ
Æ
Questa funzione costruisce un profilo triangolare che viene utilizzato
nella cinematica differenziale per ammorbidire il calcolo dello
Jacobiano e fare subentrare gradualmente l'inversione robusta
quando il manipolatore va verso condizioni di singolarità.
Altrimenti il passaggio sarebbe brusco con probabili discontinuità
dannose per i giunti
function [l]=lam(err,eps,lmax);
l=lmax*(1-err/eps);
Gli argomenti in ingresso sono
err = ε , è l’indice delle ascisse, variabile tra 0 ed ε1 ;
eps = ε1 , è il raggio massimo entro cui si considera il robot in configurazione di singolarità;
lmax = λmax , è il valore massimo di correzione che si ha in piena singolarità.
In uscita è restituito il valore λ di correzione dipendente da ε .
Fatte queste considerazioni commentiamo la funzione che in Matlab implementa il calcolo
dell’inversione della cinematica differenziale trattando le singolarità:
‘cindiffinv.m’
%
%
%
%
%
%
%
%
%
%
%
%
%
%
%
%
%
%
%
%
%
%
%
%
%
%
%
%
%
%
%
%
Æ
Æ
Æ
cindiffinv: è una funzione real time che calcola, secondo lo scorrere del clock di Simulink (implicito)
la cinematica differenziale secondo le equazioni e i metodi numerici adottati nella tesina.
ad ogni passo di campionamento dell'ambiente Simulink avviene il seguente percorso:
la funzione calcola lo jacobiano del manipolatore in base alla configurazione dei giunti più recente;
calcola poi con la cinematica diretta la posizione attuale del manipolatore in base a q del passo precedente
passo per passo; questo gli servirà per applicare gli algoritmi del gradiente e di Newton. Quindi risolve.
Per la scelta della risoluzione sono esaminate le seguenti condizioni:
1) se il punto è in prossimo ad una singolarità allora è utilizzata l'inversione ai minimi quadrati smorzati
con un fattore di correzione a profilo triangolare (relativo alla vicinanza della singolarità)
tale profilo è realizzato richiamando la funzione lam opportunamente progettata.
2) se il punto piuttosto lontano dal punto di arrivo parte l'algoritmo con il metodo del gradiente
3) se il punto è abbastanza vicino al punto di arrivo parte l'algoritmo con il metodo di Newton.
Durante le simulazioni sono attivi dei contatori che al termine riportano i passi effettuati per ognuno dei
metodi: ngrad (gradiente), nnewt (newton), sing (minimi quadrati). digitando tali nomi compare il valore dei
passi. Per vedere come sono inizializzati q0 e qf riferirsi alla Appendice A.
Intanto basti dire che se l'utente specifica punto iniziale e finale anzichè configurazione iniziale e finale,
per entrambi i punti se esistono soluzioni ne esistono almeno due. Pertanto esiste un'altra funzione
che sceglie tra le soluzioni quelle che minimizzano la distanza tra le configurazioni nello spazio dei giunti.
E' chiaramente un criterio molto approssimativo, se ne potrebbero utilizzare di assai più sofisticati,
ma già permette di migliorare notevolmente le prestazioni globali rispetto alla scelta della
"prima soluzione che capita", specialmente quando uno o entrambi i punti sono in zona di singolarità
Una precisazione: in ambiente Simulink come in molti risolutori matematici, sono "vietati" i loop algebrici
e in generale non si possono utilizzare funzioni di Matlab con più ingressi per i blocchi delle simulazioni,
allora sfruttiamo la potenzialità di Matlab che ci permette di calcolare un parametro interno alla funzione
in tempo reale e di mandarlo nello spazio globale, avendolo dichiarato global, e quindi di riutilizzarlo
per i passi successivi sempre richiamandolo dallo spazio globale. Questo vale anche per combinazioni di più
parametri. Il risultato è che ad ogni passo si ha il valore attuale calcolato internamente alla funzione
e il valore tenuto in memoria per un passo di campionamento nel workspace di Matlab. Questa è esattamente
la procedura per implementare il nostro simulatore a tempo discreto quando si lavora nello spazio cartesiano.
Dunque nel codice che segue sono contenuti implicitamente dei loop non visibili nello schema simulink,
che potrebbe trarre in inganno facendo pensare ad un semplice schema in catena diretta.
Tali loop sono proprio quelli presenti nelle espressioni degli algoritmi di inversione riportati nella tesina.
function [qd]= cindiffinv(point)
global l1 l2 l3 pf q0 qf eps lmax sing nnewt ngrad K Ks alpha Id qd sp alpha;
% il parametro globale sp sarà la velocità cartesiana imposta all’end-effector
Lq=dist2p(q0,qf);% distanza nello spazio dei giunti tra configurazione iniziale e configurazione finale
% viene qui richiamata la funzione dist2p – viene spiegata nella sezione “funzioni appoggio”
sceltaerr=Lq/0.5;% questo è un parametro, funzione della suddetta distanza, che serve a scegliere quando
% va usato il gradiente, e quando l'algoritmo di Newton (se il robot è fuori da singolarità)
% calcolo dello Jacobiano nella configurazione attuale qd – richiama la funzione 'Jac.m'
Jact=Jac(qd);
% calcolo della posizione attuale secondo l'ultima configurazione calcolata (inizia il loop)
p=cindir(qd);
% richiama la funzione della cinematica diretta 'cindir.m'
if test(point)
% richiama la funzione 'test.m' che verifica se un punto è nello spazio di lavoro, v. avanti
error('End effector oltre i confini dello spazio di lavoro! Rivedere la pianificazione: quasi
certamente perchè l2<>l3')
end
a1=l1; PWx=p(1);
a2=l2; PWy=p(2);
a3=l3; PWz=p(3);
% distanza dall'asse delle singorità di spalla z0
distz0=sqrt(PWx^2+PWy^2);
distconfest=abs((a2+a3)-sqrt(PWx^2+PWy^2+(abs(PWz)^2)));
% distanza dal confine esterno del WS
if l2~=l3
distconfint=sqrt(PWx^2+PWy^2+PWz^2)-(a2-a3);
% distanza dal confine interno del WS (solo se l2<>l3)
% ANALISI DELLE SINGOLARITA'
end
if ((sqrt(PWx^2+PWy^2)<=eps)... % di spalla: il centro del polso giace sull'asse z0 -> infinite singolarità
| (abs((a2+a3)-sqrt(PWx^2+PWy^2+(abs(PWz)^2)))<=eps))... % gomito steso: polso ai confini esterni
| (a3<a2 & (sqrt(PWx^2+PWy^2+PWz^2)-(a2-a3)<=eps)) | (a3>a2 & (sqrt(PWx^2+PWy^2+PWz^2)-(a3a2)<=eps)) % gomito tutto ripiegato con l2 diverso da l3: singolarità nel confine interno del WS
% viene qui scelto l'epsilon per il profilo triangolare di correzione dell'inversa
if (distz0<=eps)
distsing=distz0;
elseif (distconf<=eps)
distsing=distconfest;
elseif ((a3<a2 & (sqrt(PWx^2+PWy^2+PWz^2)-(a2-a3)<=eps)) | (a3>a2 & (sqrt(PWx^2+PWy^2+PWz^2)(a3-a2)<=eps)))
distsing=distconfint;
end
Js=Jact'*(inv((Jact*Jact')+((lam(distsing,eps,lmax)))*Id)); % INVERSA AI MINIMI QUADRATI SMORZATI
qd=qd+(Js*(sp+(Ks*(point-p))))*0.005; % aggiorna la configurazione dei giunti attuale
sing=sing+1; % conta i passi in singolarità
elseif dist2p(p,pf)>=sceltaerr
qd=qd+(alpha*(Jact')*(point-p))*0.005; %METODO DEL GRADIENTE
ngrad=ngrad+1; % conta i passi lontano da singolarità quando è usato il gradiente
elseif dist2p(p,pf)<sceltaerr
qd=qd+(inv(Jact)*(sp+(K*(point-p)))*0.005);
% METODO DI NEWTON
nnewt=nnewt+1; % conta i passi lontano da singolarità quando è usato Newton
end
Pianificazione delle traiettorie
Abbiamo ora tutti gli strumenti per impostare e risolvere il compito del manipolatore. L’obiettivo
della pianificazione di traiettorie è quello di produrre i riferimenti che assicurano l'esecuzione da
parte dell'end-effector delle traiettorie specificate. Questi riferimenti consistono in una sequenza
temporale dei valori assunti dalla funzione scelta come traiettoria. La traiettoria può essere
convenientemente scomposta nei due ingressi
•
percorso: luogo dei punti dello spazio dei giunti o dello spazio operativo che il manipolatore
deve descrivere per eseguire il movimento assegnato
•
legge oraria: funzione che esprime lo spazio percorso dal manipolatore al variare del tempo
(legge lineare, cubica, polinomiale di grado n, spline...) a partire dal tempo iniziale.
La pianificazione può essere eseguita in due modi:
1)
2)
nello spazio operativo (rappresentazione cartesiana) – percorso assegnato all’end-effector
nello spazio dei giunti – percorso assegnato ai giunti
a seconda della scelta si possono avere pro e contro:
1)
per la pianificazione nello spazio operativo, si possono specificare un certo numero di
parametri quali ad esempio punti estremi, punti intermedi e cammino da eseguire. Per
quanto riguarda la legge di moto, possono essere specificati il tempo di percorrenza, velocità
e accelerazione massima. Quello che verrà poi generato dall'algoritmo dovrà essere
riportato, per il controllo del robot, nello spazio dei giunti tramite le inversioni. In questo
contesto possono essere affrontati e risolti meglio problemi quali la presenza di ostacoli che
altrimenti sarebbero difficili da definire (wandering) e soprattutto visualizzare;
2)
per la pianificazione nello spazio dei giunti si ha una maggiore facilità nel riconoscere e
trattare le singolarità, in base ad un ridotto numero di parametri.
Analizziamo dunque in dettaglio il moto "punto-punto" in entrambi i casi:
1) nel caso dello spazio operativo si vuole che l'end-effector segua un percorso predefinito in
linea retta portandosi da un punto iniziale p0 ad un punto finale p f entro un intervallo di
tempo prefissato. Consideriamo allora il segmento che congiunge i due punti nello spazio e
che avrà come rappresentazione parametrica:
p ( s ) = pi +
s
( p f − pi )
p f − pi
la legge oraria utilizzata s = s (t ) è la legge oraria trapezoidale (in velocità ) o "bang-coastbang" (in accelerazione) composta da tre fasi di moto, insieme alla quale viene imposto che
le velocità iniziali e finali devono essere nulle, i tratti di accelerazione e decelerazioni
costanti e in modulo uguali, e il tratto intermedio deve avere velocità costante (figura
successiva).
⎧
t2
⎪amax 2 t ∈ [0, Ts ]
s (t )
⎪
⎪
2
⎪
vmax
v
t
t ∈ [Ts , T − Ts ]
−
⎪ max
s(t )
s (t ) = ⎨
2amax
⎪
⎪
2
2
s (t )
⎪− a (t − T ) + v T − vmax t ∈ [T − T , T ] s
max
max
⎪
amax
2
⎪
⎩
Questo tipo di legge viene usata nella pratica industriale perché permette di poter imporre
velocità e accelerazioni in modo compatibile con le caratteristiche fisiche del manipolatore
(nelle simulazioni allegate alla tesina è possibile scegliere anche un semplice profilo
lineare). Nello schema a blocchi utilizzato per la simulazione la s sarà imposta in velocità,
dunque attraverso la sua derivata analitica. Ecco l’implementazione dei blocchi relativi alla
pianificazione nello spazio cartesiano. Ricordiamo che la traiettoria è scomposta in ingressi:
‘trapezio.m’
%
%
%
%
Æ
Æ
Æ
Realizza il profilo di velocità trapezoidale per la
pianificazione di traiettorie nello spazio cartesiano.
In ingresso prende "cont"=tempo dato dal clock in Simulink;
in uscita sp=spazio percorso in ascissa derivato al tempo t=cont
function [sp]=trapezio(cont);
global vmax amax p0 pf time sp % sp è globale perchè utilizzato non solo come uscita
diretta verso il blocco percorso, ma anche come parametro nella cinematica differenziale
% time=tempo totale del profilo
Ts=tempo di salita
cont è la variabile tempo
Ts=vmax/amax;
% Intervallo di accelerazione/decelerazione=Tempo di salita
if ((cont>=0) & (cont<=Ts))
% FASE DI ACCELERAZIONE
sp=amax*cont;
% derivata analitica di s=(amax*(cont^2))/2;
end
if ((cont>=Ts) & (cont<=(time-Ts)))
% FASE A VELOCITA' COSTANTE
% derivata analitica di s=(vmax*cont)-((vmax^2)/(2*amax));
sp=vmax;
end
if ((cont>=(time-Ts)) & (cont<=time))
% FASE DI DECELERAZIONE
sp=-amax*(cont-time); % derivata analitica di s=-(amax*((cont-time)^2)/2)+(vmax*time)
end
-((vmax^2)/amax);
‘lineare.m’
Æ
Æ
Æ
% Realizza il profilo lineare per la pianificazione
% di traiettorie nello spazio cartesiano
% in ingresso prende "cont"=tempo dato dal clock
% in uscita sp=spazio percorso in ascissa derivato al tempo t=cont
function [sp]=lineare(cont);
global time L sp;
if cont<=time
sp=L/time; %L=distanza tra i due punti calcolata nel main program
else
sp=0; end
‘percorso.m’
%
%
%
%
%
%
%
%
%
%
%
Æ
Æ
Æ
Questa funzione calcola la traiettoria di movimento del 3R tra
configurazione iniziale e finale in linea retta secondo il
profilo fornito dalla legge oraria (si può scegliere un qualsiasi
profilo scrivendo una funzione apposita che vada in ingresso a percorso)
In ingresso prende "s"=profilo fornito dalla funzione trapezio o altra
dopo essere stato integrato (infatti il profilo fornisce sp);
in uscita uscita fornisce "point"=punto calcolato in coordinate cartesiane;
notare che la funzione percorso implicitamente dipende dal tempo
attraverso l'ingresso s (dipendente da cont nella simulazione fatta in simulink)
per cui calcola le coordinate dell'end-effector al tempo t=cont
in base al profilo assegnato.
function [point]=percorso(s)
global p0 pf time point;
norma=dist2p(p0,pf);
point=p0+(s*(pf-p0)/norma);
2) In questo caso si vuole che il manipolatore si muova da una configurazione iniziale ad una
finale entro un tempo prefissato. il percorso cartesiano seguito dall' end-effector non è di
interesse primario, mentre è fondamentale che giunti partano e raggiungano le
configurazioni richieste. Nella nostra tesina è richiesta l’applicazione di un profilo di
velocità quintica ai giunti, espressa dunque dalla seguente funzione polinomiale:
q (t ) = a5t 5 + a4t 4 + a3t 3 + a2t 2 + a1t + a0 .
Per i parametri ai scegliamo i valori tipicamente utilizzati nelle applicazioni della robotica
in base al procedimento illustrato di seguito:
1) imponiamo le sei condizioni relative ai due punti q0 e q1
q (0) = q0
q (1) = q1
q (0) = v0
q (1) = v1
q(0) = a0
q(1) = a1
2) scaliamo l’argomento della funzione q , cioè t , tramite una variabile τ := t / T in modo
che la variazione di t ∈ [0, T ] sia proporzionale a quella di τ ∈ [0,1] ; questa scalatura sarà
Æ
Æ
Æ
in modo elementare;
svolta dalla funzione ‘convtime.m’
3) possiamo dunque scrivere la q in funzione di τ , considerando le sei condizioni imposte
al punto 2 e quindi riportare il tutto in un’unica espressione completa
q (τ ) = (1 − τ ) [q0 + (3q0 + v0 )τ + (a0 + 6v0 + 12q0 )
τ2
]+
2
(1 − τ ) 2
]
+ τ 3[q1 + (3q1 − v1 )(1 − τ ) + (a1 − 6v1 + 12q1 )
2
3
4) particolarizziamo l’espressione scegliendo il caso notevole v0 = v1 = a0 = a1 = 0
da cui, per ∆q = q1 − q0 , otteniamo l’espressione risultante:
q(τ ) − q0
= 6τ 5 − 15τ 4 + 10τ 3
∆q
∈[ 0 ,1]
Possiamo finalmente introdurre la funzione che implementa la quintica. È ovvio a questo
punto che il tempo in ingresso non sarà direttamente il tempo di Simulink ma lo stesso
scalato attraverso la precedente funzione ‘convtime’:
‘quintica.m’
Æ
Æ
Æ
% Realizza il profilo con legge quintica per il comando a livello dei giunti.
% L'ingresso lambda è il tempo scalato a seconda del tempo totale dell'operazione
function [Q]=quintica(lambda)
global q0 qf Q; % configurazione di partenza e d'arrivo
if lambda<=1
q=q0+((qf-q0)*(6*((lambda)^5)-(15*(lambda)^4)+(10*(lambda)^3)));
else q=Q; end
Q=[q(1) q(2) q(3)]'; % posizioni dei giunti calcolate al tempo lambda
Poiché lo schema che useremo per l’inversione nello spazio dei giunti prevede di imporre il
profilo in velocità, oltre ad una correzione sulla posizione, introduciamo anche la quartica,
che è la derivata analitica della quintica (che calcoliamo piuttosto che integrare la quartica):
‘quartica.m’
Æ
Æ
Æ
% quartica: è la derivata analitica della quintica per il profilo
% da assegnare nello spazio dei giunti.
function [Qd]=quartica(lambda)
global q0 qf Qd;
qd=(((qf-q0)*(30*((lambda)^4)-(60*(lambda)^3)+(30*(lambda)^2)))); % qd=quartica al tempo
t (d=dot)
Qd=[qd(1) qd(2) qd(3)]';
% Qd=vettore q punto (dot) delle velocità dei giunti.
Ecco dunque come incastoniamo i blocchi-funzione visti sinora riassumendo il tutto nei due
schemi Simulink relativi ai due compiti del manipolatore (i blocchi richiamano le funzioni Matlab):
1) Assegnazione di traiettoria rettilinea nello spazio cartesiano (con inversione
differenziale)
Viene generato, attraverso un clock, il tempo utilizzato dalla funzione ‘trapezio.m’ (giallo)
che genera la legge oraria s (t ) la quale è elaborata dalla funzione ‘percorso.m’ (rosa) dando
luogo ai riferimenti p ( s ) per l’end-effector. Poi i valori vengono invertiti con la ‘cindiffinv’
(verde) e passati alla s-function di animazione ‘anima’ che ricostruisce l’immagine del robot
in base alla configurazione istantanea dei giunti. I quadratini all’estrema destra sono gli
‘scope’ di Simulink, visualizzano in forma di grafico bidimensionale le proiezioni dei valori
vettoriali ricevuti: facendo doppio clic sopra ciascuno di essi, sia durante, sia alla fine della
simulazione, si apre una finestra apposita che mostra al passare del tempo l’andamento delle
variabili osservate. Di default sono aperti, in finestre intorno allo schema Simulink, quelli
colorati a sfondo nero. Il blocco ‘integrator’ integra l’ingresso nel tempo a partire dalla
condizione iniziale specificata (zero).
È possibile scegliere qualsiasi profilo
di velocità per un percorso rettilineo
semplicemente facendo doppio clic sul
blocco giallo (legge oraria) e
scrivendo ad esempio al posto di
trapezio, il nome della funzione che
implementa il profilo desiderato: noi
disponiamo dei profili trapezoidale e
lineare ma è possibile scrivere altri
profili da utilizzare all’interno della
cartella del simulatore e utilizzarli nel
modo spiegato (le funzioni sono *.m).
Lo schema Simulink
con il profilo lineare.
Adesso presentiamo un filmato in cui è stato imposto il profilo di velocità cartesiana
trapezoidale lungo un percorso in linea retta che mostra cosa fa il simulatore avviato con i seguenti
dati: l1 = l2 = l3 = 5 , punto iniziale p0 = [4 3 8.5]T , p f = [0 −7 3]T , t = 4s , amax = 5 :
Æ
viene aperto un breve filmato avi compresso con codec XviD MPEG4, che è
possibile installare all’avvio dell’autorun del CD-ROM oppure da qui Æ
XviD_Install.exe
(fare doppio
clic). Una volta installato il codec sarà possibile visualizzare il filmato.
Tale installatore si trova sotto la cartella “Installer” del CD-ROM e può essere avviato
sempre facendovi sopra doppio clic. Se si preferisce non installare questo componente (freeware
sotto licenza GNU) è possibile installare altri codec compatibili a piacere, altrimenti il filmato non
sarà visibile a meno che un codec compatibile con il formato XviD non sia già installato sul sistema
operativo in uso. Riportiamo sotto una sequenza relativa ad un’altra simulazione nel caso non sia
possibile visualizzare il filmato, per avere un’idea delle simulazioni:
La finestra con il robot è animata dalla funzione anima presente nello schema Simulink di
cui sopra. Lo schema Simulink è stato creato con risoluzione video 1024x768 in modo da riempire
quasi tutto lo schermo una volta avviato, per cui è possibile che usando differenti risoluzioni video
le finestre risultino spostate rispetto alle locazioni predefinite e che si coprano a vicenda, mentre
alla risoluzione 1024x768 ogni volta che il simulatore sarà avviato le finestre prenderanno il posto
giusto massimizzando la leggibilità.
È inoltre possibile eseguire cinque demo immediate nel cartesiano con tutti i dati già pronti:
Æ
Æ
Æ
1) percorso lungo
2) ripiegamento nell’origine
3) lungo
Æ
z0
4) attraverso
Æ
z0
5) errore oltre WS
2) Assegnazione di traiettoria quintica nello spazio dei giunti
Viene generato, attraverso un clock, il tempo t , che viene scalato tramite la funzione
‘convtime’ a seconda dell’intervallo time specificato come la durata totale del compito.
Dunque il nuovo tempo λ = t ' = t / time viene utilizzato dalla funzione ‘quintica’ e
‘quartica.m’ (quest’ultima è la derivata analitica della quintica). Queste due generano i
riferimenti rispettivamente in posizione e velocità per i giunti. È inoltre presente un
sottrattore, che fornisce in uscita lo scarto tra la configurazione qd desiderata al tempo t e la
configurazione qact istantanea del manipolatore. Il motivo è che per via dell’integrazione
numerica si ottengono effetti di deriva che producono un errore e tale errore viene
recuperato attraverso un semplice guadagno posto in ingresso ad un sommatore, insieme alla
velocità istantanea qd imposta dalla quartica. Facendo doppio clic sullo scope “errore di
posizione dei giunti” è possibile osservare come durante la simulazione l’errore aumenti
man mano che t si avvicina a t / 2 (valore per cui tale errore ha valore massimo), per poi
decrescere fino allo zero al fine della simulazione.
È infine possibile eseguire altre tre demo nello spazio dei giunti con tutti i dati già pronti:
Æ
Æ
Æ
1) estensione dall’origine
2) elevazione rotante
3) nuove dimensioni
Il problema dell’inizializzazione
Come vedremo nell’Appendice B, l’interfaccia del simulatore permette, sia nel caso della
pianificazione di traiettorie nello spazio operativo, sia quando si lavori nello spazio dei giunti, di
assegnare la coppia ( p0 , p f ) - posizione iniziale e posizione finale – oppure la coppia (q0 , q f ) ,
cioè configurazioni iniziale e finale dei giunti. Al di là dello spazio in cui è definita la traiettoria, se
è assegnata la prima coppia si avrà il seguente problema: con quale delle possibili soluzioni
inizializzare la configurazione del robot?
Infatti si è visto che utilizzando l’inversa analitica (‘invanalitica.m’) si hanno in uscita più
valori per sia per p0 sia per p f , dato che se esistono soluzioni sono almeno due, per cui serve
un’altra funzione che selezioni tra le varie soluzioni quella migliore secondo un criterio. Abbiamo
ragionato così: prendiamo tra le possibili coppie (q0 , q f ) quella che minimizza la distanza nello
spazio dei giunti d (q0 , q f ) , calcolata secondo la norma euclidea. Chiaramente è un metodo non
molto sofisticato, ma produce sicuramente risultati buoni rispetto allo scegliere la prima soluzione
che capita per ognuno dei due punti: lo abbiamo constatato sperimentalmente per molti casi. Tale
calcolo è svolto dalla funzione ‘invsceltanormaminima’ che sfrutta anche il fatto che la
‘invanalitica’ ricerca soluzioni anche in caso di singolarità, selezionando le migliori tra le infinite.
Raggruppiamo ora nell’Appendice A questa funzione con altre funzioni di appoggio del simulatore.
Appendice A: Funzioni di appoggio del simulatore
Æ
‘invsceltanormaminima.m’
%
%
%
%
%
%
%
%
%
%
%
%
%
%
%
%
%
%
%
%
%
Æ
Æ
Questa funzione non appartiene all'interfaccia con l'utente, in quanto l'utente non
la utilizza direttamente. Viene richiamata solo se l'utente specifica
posizione iniziale e finale, anzichè configurazioni dei giunti iniziale e finale.
In generale serve a migliorare la scelta per l'inizializzazione del robot
in quanto trova tra le soluzioni (che possono essere infinite in singolarità)
per i due punti, fornite dalla invanalitica attraverso il main, quelle migliori
secondo il criterio che minimizza la distanza delle due configurazioni corrispondenti.
Per fare ciò prende in ingresso p0 e pf, ed inoltre accuracy che è il valore di precisione
da passare alla funzione perchè a sua volta lo passerà alla funzione invanalitica.
La spiegazione di questo parametro può essere ottenuta digitando 'help invanalitica'.
Vengono fornite 4 uscite:
message = è una stringa di testo che riporta a parole ciò che la funzione analizza
riguardo a esistenza, numero, tipo di soluzioni. In taluni casi esso è
indirizzato dal 'main' (o dalla 'simulazione' a seconda di come si avvia)
all'utente per spiegare ad esempio che i dati immessi non sono validi.
vettorenorme = è un vettore di dimensione variabile a seconda del tipo e del numero
di soluzioni trovate per i due punti p0 e pf, e riporta
tutte le distanze nello spazio dei giunti relative alle coppie possibili.
chiaramente la continuità delle infinite soluzioni in alcune singolarità
è discretizzata proprio attraverso il parametro accuracy (v.invanalitica)
q0,qf = sono le due configurazioni "migliori" calcolate alla fine.
function [message,vettorenorme,q0,qf]=invsceltanormaminima(p0,pf,accuracy)
%
%
%
%
il codice della funzione è strettamente legato al codice della invanalitica, in quanto la forma
delle matrici delle soluzioni che deve la prima deve gestire dipende proprio da come vengono
fornite dalla seconda, per cui si consiglia di aprire entrambe le funzioni e consultarle
insieme.
%
%
%
%
questa funzione controlla se il punto in ingresso
si trova o meno all'interno dello spazio di lavoro
del robot 3R e pone l'uscita uguale a 1 in caso affermativo
altrimenti a 0.
‘test.m’
Æ
Æ
Æ
function controllapunto=test(p)
global l1 l2 l3;
controllapunto=0;
PWx=p(1);
PWy=p(2);
PWz=p(3);
if sqrt(PWx^2+PWy^2+PWz^2)>l2+l3 |... se il punto è troppo lontano dall'origine e supera la somma
dei bracci l2,l3; oppure se
(l3<l2 & (sqrt(PWx^2+PWy^2+PWz^2)<l2-l3)) | (l3>l2 & (sqrt(PWx^2+PWy^2+PWz^2)<l3-l2))
% punto oltre il confine interno del WS (l2<>l3)
controllapunto=1;
else
controllapunto=0;
end;
‘dist2p.m’
Æ
Æ
Æ
% dist2p = calcola la distanza tra due punti in uno spazio a tre dimensioni (norma euclidea)
function [d]=dist2p(p1,p2)
d=sqrt(((p1(1)-p2(1))^2)+((p1(2)-p2(2))^2)+((p1(3)-p2(3))^2));
‘gra2rad.m’
Æ
Æ
Æ
% Conversione da gradi a radianti: dato in ingresso uno scalare o un vettore
% contenente valori in gradi, la funzione restituisce i valori in radianti
function radianti=gra2rad(gradi);
radianti=gradi*pi/180;
Segue la funzione che disegna in 3D il manipolatore istante per istante, in base alle configurazioni
istantanee q (t ) ottenute dai calcoli, con le modifiche che abbiamo apportato rispetto all'originale (di
Flavio Cappelli) evidenziate e commentate in arancio:
‘anima.m’
Æ
Æ
Æ
function [sys,x0]=anima(t,x,u,flag,ts); % Animazione del robot 3R antropomorfo.
global Robot Animato VisualizzaCampione CampioniTraiettoria PosizioneEndEffector l1 l2 l3 p0 pf;
NomeFigura='Animazione Robot 3R';
NumeroCampioniDaSaltare=1; %(MaxStepSize=.005)
% posto a 1 per rappresentare tutti i campioni
ax=max(abs(p0)+abs(pf)); % ax è un fattore di correzione che tiene conto della distanza tra punto
if flag==2,
% Visualizzazione grafica.
iniziale e punto finale
if any(get(0,'Children')==RobotAnimato),
if strcmp(get(RobotAnimato,'Name'),NomeFigura),
set(0,'currentfigure',RobotAnimato);
hndl=get(gca,'UserData');
% Ora calcola la cinematica diretta (u=variabili di giunto). Li = Lunghezza del braccio i-esimo
(i=1,2,3; L0=0). Oxi,Oyi,Ozi = Coordinate dell'origine di SRi (i=0,1,2,3).
L1=l1/ax; L2=l2/ax; L3=l3/ax; % le lunghezze dei bracci vengono ridotte o amplificate
Ox0=0; Oy0=0; Oz0=0;
in proporzione alla distanza dei due punti
Ox1=0; Oy1=0; Oz1=L1;
s1=sin(u(1)); c1=cos(u(1));
a2=L2*cos(u(2)); a3=a2+L3*cos(u(2)+u(3));
Ox2=a2*c1; Oy2=a2*s1; Oz2=L1+L2*sin(u(2));
Ox3=a3*c1; Oy3=a3*s1; Oz3=Oz2+L3*sin(u(2)+u(3));
% Adesso visualizza il robot nella configurazione attuale.
x=ax*[Ox0 Ox1 NaN Ox1 Ox2 NaN Ox2 Ox3 NaN -.2 .2 NaN
0 0];
y=ax*[Oy0 Oy1 NaN Oy1 Oy2 NaN Oy2 Oy3 NaN
0 0 NaN -.2 .2];
z=ax*[Oz0 Oz1 NaN Oz1 Oz2 NaN Oz2 Oz3 NaN
0 0 NaN
0 0];
set(hndl(1),'XData',x,'YData',y,'ZData',z-l1); % la quota è corretta considerando che
% Quindi visualizza un campione della traiettoria.
la terna di riferimento è sul piantone
VisualizzaCampione=rem(VisualizzaCampione+1,NumeroCampioniDaSaltare);
if (VisualizzaCampione==0),
CampioniTraiettoria=[CampioniTraiettoria [(Ox3)*ax; (Oy3)*ax; (Oz3)*ax-l1]];
set(hndl(2),'XData',CampioniTraiettoria(1,:),... % anche la traiettoria viene
'YData',CampioniTraiettoria(2,:),... % scalata in proporzione alle
'ZData',CampioniTraiettoria(3,:));
% nuove dimensioni del robot
end
drawnow; % Aggiorna la figura
end
end
PosizioneEndEffector=[Ox3 Oy3 Oz3];
sys=[];
elseif flag==3
sys=PosizioneEndEffector;
elseif flag==4
% Ritorna istante prossimo campione.
ns = t/ts; sys = (1 + floor(ns + 1e-13*(1+ns)))*ts;
elseif flag==0,
% Inizializza la figura.
[exist,RobotAnimato]=figflag(NomeFigura);
if ~exist,
position=get(0,'DefaultFigurePosition');
position(3:4)=[350 301]; position(1:2)=[666 59]; - ottimizza la leggibilità a 1024x768
RobotAnimato=figure( ...
'Name',NomeFigura, ...
'NumberTitle','off', ...
'BackingStore','off', ...
'Position',position);
axes( ...
'Units','normalized', ...
'Position',[0.05 0.08 .95 .98], ...
'Visible','on', ...
'DrawMode','fast');
end;
cla reset; set(gca,'DrawMode','fast');
VisualizzaCampione=-1; CampioniTraiettoria=[];
axis([-(ax+l1) (ax+l1) -(ax+l2) (ax+l2) -(ax+l3) (ax+l3)]); % gli assi sono scalati secondo
% Fissa gli assi e li mantiene attivi.
% la porzione di spazio esplorato
view(90-37.5,30);
% durante il percorso
hold on;
% Chiama procedure grafiche per inizializzazione oggetti (robot e traiettoria).
hndl1=plot3([0],[0],[0],'EraseMode','background','LineWidth',3);
hndl2=plot3([0],[0],[0],'EraseMode','background','LineWidth',1,'Color','magenta');% distingue
set(gca,'AspectRatio',[1 1]);
la traiettoria dal robot
set(gca,'UserData',[hndl1 hndl2]);
sys=[0 0 3 3 0 0];
x0=[];
xlabel('x');
ylabel('y');
zlabel('z');
grid on; % mostra una griglia che facilita la visualizzazione dei punti nello spazio cartesiano
end;
Appendice B: L’interfaccia con l’utente e l’avvio del simulatore
Il Simulatore può essere avviato dal seguente link:
START HERE!
Æ
Å
Visto che potrebbe non funzionare per motivi già spiegati, sfruttare questo collegamento non
è l’unico modo di avviare il simulatore, dato che può essere installato e avviato manualmente da
Matlab indipendentemente da questo documento. Il CD infatti dispone di un autorun che se attivato
permette con una presentazione in Power Point varie opzioni tra cui l’installazione/rimozione del
simulatore e l’avvio diretto sia da CD sia da hard-disk una volta installato. È possibile consultare il
file leggimi se avete aperto questo documento senza essere passati per la schermata di autorun.
Diciamo che una volta installato, sia manualmente sia automaticamente, il simulatore su disco
rigido, sarà sufficiente digitare “main” al prompt per fare partire la simulazione assistita.
Il file main è il programma principale ma non rappresenta l’unica possibile modalità di
immissione dei dati. Infatti sono predisposti altri due file dove l’utente può caricare tutti i dati
specifici in modalità non interattiva, poi salvare e quindi avviare la simulazione. I due file sono
‘dati3R.m’ e ‘simulazione.m’. Il primo contiene le caratteristiche descrittive del manipolatore,
mentre il secondo i dati specifici sul compito da eseguire. Caricati questi dati e salvati i file le
possibilità sono 2: avviare il ‘main’ e specificare che siano utilizzati quei file di dati (è una tra le
opzioni proposte dal ‘main’), oppure nessuno o uno solo dei due a piacere e caricare gli altri dati
manualmente in modo interattivo. È un modo per rendere molto elastica l’interfaccia con l’utente
che utilizzerà il simulatore come preferisce non appena acquisita un po’ di familiarità.
Un’ultima possibilità è quella di “by-passare” completamente il ‘main’ rendendo eseguibile
il file simulazione.m. Matlab infatti permette di mettere a commento alcune parti di programma che
non vengono lette se c’è il simbolo ‘%’ davanti (in tutte le funzioni viste sinora le parti descrittive
in verde erano precedute da tale simbolo). Noi abbiamo scritto in fondo al file ‘simulazione.m’ delle
righe di codice, che di default sono messe a commento e che, se rimesse da commenti a istruzioni,
costituiranno il codice eseguibile simile a quello del file main, mentre la prima parte descrive il
compito. In questo caso verranno richiamate le informazioni sul robot contenute nel file dati3R,
visto che non possono essere immesse in modo interattivo. Per togliere il commento alle righe è
sufficiente selezionarle con il mouse, fare un clic con il destro sulla selezione e scegliere la voce
“uncomment”, quindi salvare. Per eseguire un file matlab (che sia eseguibile) si può premere F5 una
volta aperto, oppure dal prompt della command window si può digitarne il nome. Per tutte le
funzioni del simulatore sono predisposti degli help che compaiono digitando ‘help nome’ dove
nome è la parte del nome del file che precede l’estensione ‘.m’. Esempio: ‘help main’ (main.m).
Vediamo ora come sono costituiti i file che costituiscono l’interfaccia del simulatore,
riportandone i commenti e le righe principali:
‘dati3R.m’
%
%
%
%
%
%
%
%
%
%
%
%
%
%
%
%
Æ
Æ
Æ
Questo è il file dove l'utente scrive i dati del manipolatore 3R per
utilizzarli nelle simulazioni;
E' stata scelta la convenzione di Denavit Hartenberg; dapprima
si richiedono solo le lunghezze dei tre bracci, tutti gli altri
parametri e le altre scelte per l'esecuzione del compito sono
decisi nel file "simulazione.m" che può essere avviato
direttamente dal programma main anzichè indicare online le scelte
durante l'esecuzione dello stesso
Per ulteriori spiegazioni digitare 'help simulazione' e 'help main'.
La prima volta che si eseguirà il main i file "dati3R.m" e "simulazione.m" e "paramdiff.m"
conterranno dei valori di default che possono essere considerati come
"demo" del funzionamento del simulatore, e si possono modificare a piacimento;
per editare un file e scrivervi i dati digitare 'edit NOMEFILE.m'.
global l1 l2 l3; % questi sono i parametri che vengono passati in comune alle
global accuracy; % varie funzioni utilizzate nella simulazione
% lunghezze dei 3 bracci:
% se si sceglie di utilizzare tre valori diversi per i tre bracci
% si modifica la forma dello spazio di lavoro del robot
% e in generale si altera la destrezza del manipolatore
% pertanto di default sono scelti tre valori simmetrici per i link:
l1=5;
l2=5;
l3=5;
% tali valori possono essere cambiati a piacimento pertanto è possibile vedere
% come si comporta un 3R con dimensioni diverse o proporzioni più grandi/più piccole
% come succede ad esempio nella 'democ5.m' in cui viene violato il confine interno del WS
accuracy=12;
% è il valore di accuratezza con cui sono rappresentati internamente gli angoli per le inversioni.
% Consultare la tesina oppure l'help delle funzioni "invanalitica" e “invsceltanormaminima"
% per una spiegazione dettagliata del significato di questo parametro. Se non lo si conosce
% si consiglia di lasciare il valore di default (12) che fornisce buoni risultati.
‘simulazione.m’
%
%
%
%
%
%
%
%
%
%
%
%
%
%
%
%
%
%
%
%
%
%
%
%
%
%
%
%
%
%
Æ
Æ
Æ
Questo file contiene le informazioni che il programma principale main può
utilizzare in alternativa all'immissione "online" dei dati e delle scelte.
Non è propriamente una funzione ma è semplicemente un file che se richiamato
(cioè compilato) dal main manda nel workspace di Matlab tutti i parametri
utilizzati dalle funzioni della tesina
Qui l'utente può scrivere nelle sezioni apposite i parametri e le stringhe
per operare le scelte: se si preferisce immettere i dati "offline" anzichè
direttamente dal main basta dunque seguire le istruzioni riportate a commento
in questo file a fianco alle sezioni e compilare le varie parti dello stesso
con i valori desiderati; (digitare "edit simulazione" per accedervi);
quindi basta riavviare il main digitando al prompt "run main" o "main"
e tra le opzioni offerte dal programma scegliere di utilizzare "simulazione.m".
Ogni volta che si esegue il main sarà possibile scegliere se immettere
i dati uno alla volta con delle domande oppure se utilizzare i dati
compilati in questo file.
Se si desidera evitare il main completamente - utenti con una minima esperienza è possibile modificare la riga 35 e le righe da 217 a 267 di questo file (che
contengono anche i controlli normalmente previsti nel main), trasformando i
commenti in istruzioni (è sufficiente rimuovere '%' da davanti a ciascuna riga)
quindi salvare ed eseguire (F5). Per velocizzare tale operazione, selezionare
con il mouse l'insieme delle righe, quindi fare clic con il destro e selezionare
la voce "uncomment". Si tenga presente però che se, alla fine della simulazione,
non si rimettono tali righe a commento ("comment"), quando si dovesse scegliere di
riutilizzare il main e da esso richiamare il file 'simulazione', allora il file
'simulazione.m' sovrascriverebbe le operazioni del main non appena eseguito! ATTENZIONE!
------------------------------------------------------------------------------------
% NELLA PARTE OMESSA C’E’ L’INSIEME DEI VALORI CHE L’UTENTE PUO’ EDITARE (aprire uno dei link sopra per vedere)
%
( . . . )
Se ‘simulazione.m’ è reso eseguibile, richiamerà automaticamente ‘dati3R’ all’esecuzione.
Esiste inoltre un file che viene richiamato automaticamente dal file simulazione o anche dal main,
qualora l’utente, durante l’esecuzione del main, specifichi di richiamare tale file piuttosto che
immetterne i valori interattivamente:
‘paramdiff.m’
%
%
%
%
%
%
%
%
%
%
%
%
Æ
Æ
Æ
Questo file contiene i valori dei parametri utilizzati nei calcoli della cinematica
differenziale.
I valori di default sono relativi alla simulazione di default che corrisponde alla prima demo
nello spazio operativo (file 'democ1.m')e sono stati ricercati pazientemente
per ottenere buoni risultati. In generale non vanno sempre bene, perchè
le inversioni sono soggette a fenomeni di deriva e instabilità. Infatti la stabilità asintotica
nello schema di inversione adottato è garantita solo per riferimenti costanti, mentre
i compiti che sono pianificati con il simulatore prevedono riferimenti variabili, anche se
lineari a tratti nello spazio operativo. A seconda del compito specificato vanno opportunamente
ritoccati, come è stato fatto per le demo avviabili dal CD-ROM della tesina. Inoltre sono
relativi alle dimensioni di default del Robot 3R (lunghezze dei bracci tutte uguali a 5)
global eps lmax K Ks alpha dTG dTN dTNS;
eps=0.1;
% misura entro la quale si considera di stare in zona di singolarità
% (fare riferimento alla tesina scritta per maggiori dettagli)
lmax=0.1;
% valore massimo di correzione del fattore lambda in zona di singolarità
% (con riferimento alla tesina è il vertice del profilo triangolare)
K=150*Id;
% valore della matrice diagonale di correzione dello Jacobiano 250
% in inversione cinematica quando si utilizza il metodo di Newton;
% tale valore è moltiplicando per l'identità 3x3.
Ks=250*Id;
% valore della matrice diagonale di correzione dello Jacobiano 8
% in inversione cinematica quando si utilizza il metodo dell'inversa robusta;
% tale valore è moltiplicando per l'identità 3x3.
alpha=3*Id;
% valore della matrice diagonale di correzione dello Jacobiano 10
% in inversione cinematica quando si utilizza il metodoto del gradiente;
% tale valore è moltiplicando per l'identità 3x3.
dTN=0.005;
% passo di campionamento relativo al metodo di Newton
dTNS=0.005;
% passo di campionamento relativo al metodo dell'inversione robusta
dTG=0.005;
% passo di campionamento relativo al metodo del Gradiente
% scegliamo sempre uguali i passi di campionamento dei tre metodi
Riportiamo infine l’help del programma principale che dà il benvenuto all’utente:
‘main.m’
%
%
%
%
%
%
%
%
%
%
%
%
%
%
%
%
%
%
%
%
%
%
%
%
%
%
Æ
Æ
Æ
Questo è il programma principale della tesina di Robotica I sul robot
antropomorfo 3R, realizzato da Stefano Pizzarotti e Luigi Russo.
Si richiedono posizione iniziale e finale dell'end-effector
quindi il programma calcola, in base ai parametri scelti per il
manipolatore, una traiettoria su percorso rettilineo tra i due punti,
imponendo un profilo di velocità lineare o trapezoidale, entro
l'intervallo di tempo specificato.
Oppure si richiedono configurazione iniziale e finale dei giunti,
quindi il programma calcola, in base ai parametri scelti, una
traiettoria con profilo di velocità quintica nello spazio dei giunti
(comprese le correzioni per effetti di deriva durante le inversioni)
Quindi vengono rappresentati i movimenti del robot animandolo in
3D con grafici relativi a posizioni (e velocità) cartesiane e di giunto.
funzioni richiamate durante la simulazione (digitare l'help relativo):
dati3R.m
simulazione.m
cindir.m
invanalitica.m
cindiffinv.m
invsceltanormaminima
%
%
%
%
%
%
lineare.m
trapezio.m
percorso.m
quintica.m
quartica.m
convtime.m
%
%
%
%
%
%
paramdiff.m
Jac.m
lam.m
dist2p.m
gra2rad.m
test.m
% anima.m
Blocchi simulink utilizzati per simulare i compiti scelti:
- cartelineare – cartetrapezio - giunti
Appendice C: Installazione e rimozione del simulatore cinematico
Installazione
L’installazione prevede che Matlab sia già installato. È richiesta la versione 6.5 (release 13)
o superiore, dato che i blocchi Simulink sono stati disegnati nella versione di Simulink relativa a
tale release per cui non possono essere avviati su versioni di Matlab meno recenti. Tuttavia avendo
una versione vecchia di Matlab non è escluso che se si aggiorna solo Simulink alla versione recente
senza aggiornare tutto Matlab il simulatore funzioni. Infatti il problema della versione riguarda
esclusivamente Simulink, mentre è molto probabile che le funzioni e in generale gli m-file girino
perfettamente su release di Matlab come la 6 r12 o la 6.1 e forse anche meno recenti. Non abbiamo
testato questa possibilità però! Cliccare su questo link
per aprire il file con i requisiti di sistema
per l’installazione di Matlab (fonte: www.mathworks.com). Infatti il simulatore è stato progettato e
testato con successo su Windows XP con Matlab 6.5 r13 e relativo Simulink
L’installazione può essere fatta in tre modi:
1) lasciando partire l’autorun del cd in modo automatico e all’avvio della presentazione
powerpoint cliccare su “Installare il simulatore cinematico su hard-disk”. Se non si può o
non si vuole attivare l’autorun si può fare doppio clic sul file “present.pps”. Sarà avviato un
file batch che copierà i programmi del simulatore dal CD sul disco rigido nella locazione
‘c:\programmi\3RbotSim’. Inoltre sarà aggiundo in fondo alla lista delle directory di Matlab
tale percorso, in modo che digitando il nome degli m-file al prompt (ad esempio ‘main’) essi
saranno ricercati in tale percorso, dato che non saranno trovati altrove.
2) Copiando manualmente i file dal cd su un percorso a piacere, come c:\programmi\3RbotSim
o altro, e aggiungendo tale percorso alla lista delle directory di ricerca in Matlab. Se si usa
un percorso diverso da c:\programmi\3RbotSim I link del tipo
presenti in questo
documento non funzioneranno. Per installare manualmente procedere nel modo seguente:
1. Creare un percorso su disco rigido per il simulatore, ad esempio andare sotto c:\ con
eplora risorse o risorse del computer, poi sotto programmi e creare “3RbotSim” o
qualsiasi nome a piacere purchè senza spazi o caratteri speciali (consultare la guida di
Matlab per sapere quali caratteri e tipi di nomi saranno scartati).
2. Esplorare il CD tramite esplora risorse oppure risorse del computer
3. individuare la cartella “programs” e selezionarla con il mouse. Fare un clic con il
pulsante secondario del mouse e scegliere la voce “copia”.
4. esplorare il disco rigido e recarsi sotto il percorso specificato appositamente al passo 1
(ad esempio c:\3RbotSim). Andare sul menu modifica e selezionare “incolla”. Verranno
ora copiati i file dalla cartella programs del CD alla cartella programs sotto il percorso
scelto (c:\programmi\3RbotSim\programs\ file del simulatore)
5. Avviare Matlab; comparirà il prompt in attesa di comandi.
6. Digitare ‘pathtool’ oppure puntare il mouse su File, cliccare e scegliere la voce “Set
Path…”. Si apre una finestra dove sono specificate le directory in cui Matlab cerca le
funzioni e i file richiamati al prompt.
7. Fare clic su “Add Folder…”. Sfogliando è necessario trovare la cartella “programs” del
simulatore appena copiato; trovata la cartella selezionarla e premere OK.
8. Fare clic su “Save” e quindi un altro clic su “Close”.
9. È ora possibile digitare al prompt il comando ‘main’ per avviare il simulatore oppure
‘help main’ se si desidera dapprima avere maggiori spiegazioni.
3) È infine possibile non copiare la cartella del CD-ROM ma semplicemente aggiungere al
path di Matlab la cartella del CD come al passo 6. In tal caso però dovrà essere inserito il
CD-ROM per avviare il simulatore digitando ‘main’ e tutti i file delle simulazioni e dei dati
non saranno modificabili. Inoltre non funzioneranno i link presenti in questo documento.
Rimozione
Se si è effettuata l’installazione automatica da CD o se comunque si è installato il simulatore
manualmetne con il percorso di installazione è predefinito c:\programmi\3RbotSim\ è possibile
procedere con la disinstallazione in modo automatico facendo partire nuovamente l’autorun del CD
oppure facendo doppio clic sul file “present.pps” e scegliendo quindi la voce “Dis” prima di
“Installare il simulatore…”. Partirà un file batch che rimuoverà il percorso del simulatore dal path
di Matlab e poi cancellerà dal disco rigido file copiati in precedenza.
Se invece si è scelto un percorso differente bisogna procedere come segue:
1. Esplorare il disco rigido con esplora risorse o risorse del computer. Recarsi al percorso
scelto per l’installazione del simulatore. Cancellare la cartella scelta per l’installazione.
2. Avviare Matlab
3. Digitare ‘pathtool’ oppure puntare il mouse su File, cliccare e scegliere la voce “Set
Path…”. Si apre una finestra dove sono specificate le directory in cui Matlab cerca le
funzioni e i file richiamati al prompt.
4. Selezionare dall’elenco la cartella che si era scelta per l’installazione e fare clic su
“remove”.
5. fare clic su “save” e poi su “close”. La rimozione è così completata.
Stefano Pizzarotti, Luigi Russo