UNIVERSITA’ DEGLI STUDI DI PARMA
FACOLTA’ DI INGEGNERIA
Corso di
MECCANICA DEI ROBOT
Prof. A. Tasora
Studente
Michele Artoni
125832
Introduzione
Nel documento vengono riportate e spiegate brevemente alcune funzioni
implementate in ambiente MatLab durante il laboratorio del corso di Meccanica
dei robot.
Per lo più le funzioni ed i programmi hanno come riferimento un robot di tipo
S.C.A.R.A.1, significativo per caratteristiche cinematiche e perché tuttora
molto utilizzato (soprattutto in operazioni di pick and place).
Nella trattazione sono riportati i passaggi analitici ed alcune schematizzazioni
ritenuti indispensabili per la comprensione delle funzioni.
Le stesse funzioni sono riportate così come realizzate in ambiente di
programmazione, corredate di commenti iniziali che ne favoriscano un facile
utilizzo.
FK_scara.m
FK_scara è una funzione per il calcolo della cinematica diretta relativamente ad
un robot tipo S.C.A.R.A..
Le equazioni utilizzate per l’implementazione della funzione si rifanno ai
principi ed alle definizioni riportate di seguito.
Per favorire una scrittura delle equazioni più compatta si sono utilizzate le
coordinate omogenee. Un punto dello spazio viene dunque espresso in forma di
vettore tetradimensionale:
P = {Px; Py; Pz; 1}
Essendo Px,Py,Pz le coordinate cartesiane del punto P.
Inoltre vale:
[ ]
Pii = M ji ⋅ P jj
Essendo:
[Mji], matrice di Denavit Hartenberg per passare dal sistema di riferimento j
al sistema i;
Pii, è da intendersi come: vettore delle coordinate omogenee di P rispetto
all’origine i (primo pedice) e alla base i (secondo pedice).
Analogamente per Pjj.
La matrice di Denavit Hartenberg, quadrata (4x4), può essere così costruita:
[M ] = [R0 ]
ji
ji
J ii
1
Dove:
[Rji] è la matrice di rotazione per passare dal sistema di riferimento j al
sistema i;
1
‘Selective Compliance Automatic Robot Arm’
“…compliance, the flexing of an arm to accommodate the small error in position that occur when one object is brought into contact with another.”
Da ‘Introduction to Robotics’ -McKerrow-
2
Jii è un vettore che individua l’origine J (del sistema di riferimento j)
rispetto all’origine i e al sistema i.
0 è un vettore tridimensionale: [0,0,0]
Nel caso le rotazioni avvengono soltanto attorno all’asse z (verticale).
Per la determinazione
particolare:
delle
matrici
FK_scara
richiama
altre
funzioni;
in
- DH_translate(v) che in funzione di un vettore tridimensionale v (significativo
di una traslazione nello spazio), ne calcola la matrice di Denavit Hartenberg
associata (D.H.), cioè tale da rappresentare rotazione nulla e traslazione pari
allo stesso v.
- DH_rotatez(alfa) che in funzione del parametro angolare alfa, espresso in
radianti, calcola la matrice di D.H. rappresentativa della medesima rotazione
attorno all’asse z.
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%
%
%
FK_scara
%
%
%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%
%
%
%
%
%
%
%
%
%
%
%
%
%
%
%
%
%
%
FK_scara: funzione di cinematica diretta relativa ad un robot tipo S.C.A.R.A.
In funzione delle rotazioni impresse ai giunti e delle dimensioni geometriche
del robot, fornisce le coordinate finali dell'end effector e dei punti
necessari per una rappresentazione schematica del robot stesso, siano
A,B,C,D: punti in cui siano localizzati i giunti.
La base del robot ha centro nell'origine del sistema di riferimento: [0,0,0].
Sintassi:
[A,B,C,D,P] = FK_scara(alfa1,alfa2,alfa3,d,h,D1,D2,P44)
alfa1,alfa2,alfa3 sono le rotazioni impresse ai giunti rispettivamente localizzati
in A,B,C.
d è la traslazione verticale impressa , positiva verso il basso.
h,D1,D2 sono rispettivamente le dimensioni di torretta, bicipite,
avambraccio dello S.C.A.R.A.
P44 vettore dello spazio tridimensionale che identifica le
coordinate di P nel sistema relativo '44', solidale alla pinza.
function [A,B,C,D,P] = FK_scara(alfa1,alfa2,alfa3,d,h,D1,D2,P44)
start = [0;0;0;1];
%
%
mij sono le matrici le matrici di Denavit - Hartenberg per 'spostarsi'
tra sistemi di riferimento (da j a i).
m10
m21
m32
m43
=
=
=
=
DH_translate([0, 0, h]) * DH_rotatez(alfa1);
DH_translate([D1, 0, 0]) * DH_rotatez(alfa2);
DH_translate([D2, 0, 0]) * DH_rotatez(alfa3);
DH_translate([0, 0, -d]);
P44omog = [P44(1); P44(2); P44(3);1];
A
B
C
D
P
=
=
=
=
=
m10
m10
m10
m10
m10
*
*
*
*
*
start;
m21 * start;
m21 * m32 * start;
m21 * m32 * m43 * start;
m21 * m32 * m43 * (P44omog);
3
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%
%
%
DH_translate
%
%
%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%
%
%
%
%
%
%
%
%
%
%
%
%
%
%
%
%
DH_translate(T): funzione per la determinazione della matrice di
Denavit-Hartenberg associata alla traslazione 'T'.
In funzione di una generica traslazione viene creata la matrice m=mji
che permette il passaggio di coordinate tra un sistema di riferimento j
traslato di 'T' rispetto ad un sistema i, ed il sistema i
dalla cui origine 'T' viene valutato.
Sintassi:
m = DH_translate(T)
'T' è un vettore tridimensionale, le cui componenti esprimono
rispettivamente gli offset lungo x,y,z presenti
fra i due sistemi di riferimento: i e j.
'm' è la matrice di Denavit-Hartenberg associata a traslazione 'T'
e rotazione nulla.
function m = DH_translate(T)
m = [ 1
0
0
0
0
1
0
0
0
0
1
0
T(1);
T(2);
T(3);
1
];
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%
%
%
m=DH_rotatez
%
%
%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%
%
%
%
%
%
%
%
%
%
%
%
%
%
%
%
%
DH_rotatez(alpha): funzione per la determinazione della matrice di
Denavit-Hartenberg associata alla rotazione 'alpha'.
In funzione di una rotazione attorno all'asse z viene creata la matrice m=mji
che permette il passaggio di coordinate tra un sistema di riferimento j,
ruotato di 'alpha' rispetto ad un sistema i, ed il sistema i
a partire dal quale la rotazione viene valutata.
Le rotazioni sono da considerarsi positive se destrorse.
Sintassi:
m = DH_rotatez(alpha)
'alpha' è l'angolo, espresso in radianti,
fra i due sistemi di riferimento: i e j.
'm' è la matrice di Denavit-Hartenberg associata alla rotazione
'alpha', e ad una traslazione nulla fra le due origini dei sistemi i e j.
function m = DH_rotatez(alpha)
m = [ cos(alpha)
sin(alpha)
0
0
-sin(alpha)
cos(alpha)
0
0
0
0
1
0
0;
0;
0;
1 ];
4
IK_scara.m
IK_scara è una funzione per il calcolo della cinematica inversa relativamente ad
un robot tipo S.C.A.R.A..
Le equazioni utilizzate per l’implementazione della funzione si rifanno ai
principi ed alle definizioni riportate di seguito.
La vista ‘dall’alto’ di un robot S.C.A.R.A. può essere così schematizzata:
Nota: α2 è rappresentativo della rotazione del giunto che collega “a” e “b”. Per
la configurazione riportata, α2 è una rotazione di verso orario (perciò
negativa) di modulo pari a π-δ.
Px e Py sono le coordinate dell’end effector, rot è la rotazione assoluta
dell’end effector rispetto ad un asse di riferimento.
5
Passaggi per la determinazione della funzione di cinematica inversa:
f ik : x → ϑ
α1 = γ + β
γ = atan2(Py , Px )
(Convenzione: angoli positivi antiorari)
Dal teorema di Carnot (applicato al triangolo di lati a,b,d):
a 2 + d 2 − b2
β = ± acos
2⋅a⋅d
Essendo :
d = Px 2 + Py 2
Inoltre:
α 2 = −(π − δ )
a 2 + b2 − d 2
δ = ± a cos
2⋅a⋅b
Determinazione di α3:
α 3 = rot − α1 − α 2
Per la determinazione della traslazione verticale ‘d’ si fa riferimento allo
schema seguente:
d = h − Pz
d è dunque da intendersi positivo verso il basso.
6
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%
%
%
IK_scara
%
%
%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%
%
%
%
%
%
%
%
%
%
%
%
%
%
%
%
IK_scara: funzione di cinematica inversa relativa ad un robot tipo S.C.A.R.A.
In funzione delle coordinate di un punto 'P' dello spazio, dell'orientazione
'rot' dell'end effector e delle dimensioni geometriche del robot
calcola le rotazioni e la traslazione da imprimere ai giunti, perché l'end
effector venga a trovarsi nel punto fornito con l'orientazione data.
Sintassi:
[alfa1,alfa2,alfa3,d]=IK_scara(px,py,pz,rot,a,b,h)
px,py,pz sono le componenti nello spazio cartesiano di 'P';
'rot' è l'orientazione dell'end effector,
che può ruotare attorno all'asse z;
a, b, h sono rispettivamente le dimensioni geometriche di:
bicipite,avambraccio e torretta dello S.C.A.R.A.
function [alfa1,alfa2,alfa3,d] = IK_scara(px,py,pz,rot,a,b,h)
gamma = atan2(py,px);
d = (px^2 + py^2)^(1/2);
beta = acos((a^2 + d^2 - b^2)/(2*a*d));
alfa1 = gamma + beta;
delta = acos((a^2 + b^2 - d^2)/(2*a*b));
alfa2 = - pi + delta;
alfa3 = rot - alfa1 - alfa2;
d = h - pz;
IK_scara_sol.m
La funzione è del tutto analoga a IK_scara ma restituisce un parametro
aggiuntivo: ‘sol’.
‘sol’ è pari a zero qualora non sia possibile calcolare la cinematica inversa,
uno altrimenti.
In particolare sono stati utilizzati blocchi di tipo ‘if’ per valutare le
diverse impossibilità di calcolo; qualora una di queste venga riscontrata si
assegna a sol il valore zero e si esce immediatamente dalla funzione.
Le grandezze in uscita vengono dapprima inizializzate a zero per evitare, nei
casi di cinematica inversa non calcolabile, di restituire delle variabili
indefinite.
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%
%
%
IK_scara_sol
%
%
%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
7
%
%
%
%
%
%
%
%
%
%
%
%
%
%
%
%
%
%
%
%
%
%
%
IK_scara_sol: è un' 'evoluzione' di IK_scara
funzione di cinematica inversa relativa ad un robot tipo S.C.A.R.A.
In funzione delle coordinate di un punto 'P' dello spazio, dell'orientazione
dell'end effector e delle dimensioni geometriche del robot,
calcola le rotazioni e la traslazione da imprimere ai giunti, perché l'end
effector venga a trovarsi nel punto fornito con l'orientazione data.
'rot'
IK_scara_sol restituisce inoltre un indicatore: 'sol', pari a zero nei casi di
impossibilità di calcolo della cinematica inversa (condizioni di
singolarità), pari a uno altrimenti.
Sintassi:
[alfa1,alfa2,alfa3,d_t,sol]=IK_scara_sol(px,py,pz,rot,a,b,h,d_tmin,d_tmax)
px,py,pz sono le componenti nello spazio cartesiano di 'P';
'rot' è l'orientazione dell'end effector rispetto all'asse x;
a,b,h sono rispettivamente le dimensioni geometriche di:
bicipite,avambraccio e torretta dello S.C.A.R.A.
d_tmin,d_tmax sono le escursioni minima e massima lungo z,
positive verso il basso.
function [alfa1,alfa2,alfa3,d_t,sol]=IK_scara_sol(px,py,pz,rot,a,b,h,d_tmin,d_tmax)
alfa1
alfa2
alfa3
d_t =
sol =
%
%
%
= 0;
= 0;
= 0;
0;
1;
Nella funzione si alternano equazioni per il calcolo della cinematica
inversa e blocchi di controllo (if) che permettono di 'uscire' se la
I.K. non è calcolabile, ritornando sol = 0.
gamma = atan2(py, px);
d = (px^2 + py^2) ^ (1/2);
if (d==0)
sol = 0;
return;
end
if (d > (a + b))
sol = 0;
return;
end
if (d < abs(a - b))
sol = 0;
return;
end
sol = 1;
beta = acos((a^2 + d^2 - b^2) / (2*a*d));
alfa1 = gamma + beta;
delta = acos((a^2 + b^2 - d^2)/(2*a*b));
alfa2 = -pi + delta;
alfa3 = rot - alfa1 - alfa2;
d_t = h - pz;
if d_t < d_tmin
sol = 0;
return
end
8
if d_t > d_tmax
sol = 0;
return
end
movescara_IK_FK.m
Si tratta di un programma per la visualizzazione del movimento di un robot tipo
S.C.A.R.A.
La visualizzazione è semplificata ma utile per la verifica della correttezza
delle funzioni richiamate dal programma.
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%
%
%
movescara_IK_FK
%
%
%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%
%
%
%
%
%
%
%
movescara_IK_FK: programma per la visualizzazione del movimento di un robot
S.C.A.R.A.
L'end effector segue un moto rettilineo ad accelerazione costante fra un punto
iniziale ed uno finale (Pstart e Pend) forniti dall'utente.
Le stesse dimensioni geometriche dello S.C.A.R.A. sono richieste
all'utente che dovrà, quindi, avere l'accorgimento di fornire punti interni al
volume di lavoro del robot.
h = input('Inserire la dimensione della torretta h= ');
D1 = input('Inserire la dimensione del bicipite D1= ');
D2 = input('Inserire la dimensione dell'' avambraccio D2= ');
Pstart = input('Inserire le coordinate iniziali di P, es... [2;-2;0] = ');
Pend = input('Inserire le coordinate finali di P, es... [3;4;3] = ');
%
%
%
T è il tempo impiegato dal robot per passare da Pstart a Pend
giocando su tale parametro (e sull'entità della pausa contenuta nel
ciclo 'for') è possibile velocizzare o rallentare la
9
%
visualizzazione del movimento
T=40;
%
Settaggio grafico
figure;
Rag=10
axis([ -Rag Rag -Rag Rag -Rag Rag]);
grid
xlabel('assex')
ylabel('assey')
zlabel('assez')
rot = 0;
P44 = [0 0 0]';
gro_p
=line('xdata',[],'ydata',[],'zdata',[],'linestyle','o','markersize',6,'color','b');
gro_Pto
=line('xdata',[],'ydata',[],'zdata',[],'linewidth',1,'linestyle','o','markersize',6,'color','g');
gro_line=line('xdata',[],'ydata',[],'zdata',[],'linewidth',1,'linestyle','-','color','r');
%
%
Nel ciclo 'for' vengono calcolati e visualizzati i punti dello spazio
occupati dallo S.C.A.R.A. nei diversi istanti di tempo.
for t=0:T/30:T
Punto = traiettrettil(Pstart,Pend,t,T);
[alfa1,alfa2,alfa3,d] = IK_scara(Punto(1), Punto(2), Punto(3), rot, D1, D2, h);
[A,B,C,D,P] = FK_scara(alfa1,alfa2,alfa3,d,h,D1,D2,P44);
set(gro_line,'xdata',[0 A(1) B(1) C(1) D(1) P(1)], 'ydata',[0 A(2) B(2) C(2) D(2) P(2)],
'zdata', [0 A(3) B(3) C(3) D(3) P(3)]);
set(gro_p,
'xdata',[0 A(1) B(1) C(1) D(1) P(1)], 'ydata',[0 A(2) B(2) C(2) D(2) P(2)],
'zdata', [0 A(3) B(3) C(3) D(3) P(3)]);
pause(0.1);
drawnow;
end
%
%
%
Al termine del ciclo viene evidenziato in figura 'Pend' per una
verifica immediata della correttezza di funzionamento del
programma.
pause(1);
set(gro_Pto,'xdata',Pend(1),'ydata',Pend(2),'zdata',Pend(3));
pause(5);
close
movescara_IK_FK.m richiama la funzione traiettrettil.m, che,
richiama acconst.m; queste ultime sono di seguito riportate.
a
sua
volta
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%
%
%
traiettrettil
%
%
%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%
%
%
traiettrettil: funzione per il calcolo dei punti di una traiettoria rettilinea
Sintassi:
10
%
%
%
%
%
%
%
%
%
%
%
P = traiettrettil(PA,PB,t,T)
PA e PB sono rispettivamente il punto iniziale e finale della traiettoria;
t è l'istante 'attuale' nel quale viene calcolato il punto P;
T è il periodo di tempo assegnato per spostarsi con accelerazione costante da PA a PB
P è il punto dello spazio (3D) appartenente alla traiettoria rettilinea
di estremi PA e PB, che compete all'istante 't'.
(Per t = 0 P = PA; PB se t = T; per 0 < t < T si trovano i punti della traiettoria
compresi fra PA e PB)
function P = traiettrettil(PA,PB,t,T)
P(1) = acconst( t, T, PB(1) - PA(1) ) + PA(1);
P(2) = acconst( t, T, PB(2) - PA(2) ) + PA(2);
P(3) = acconst( t, T, PB(3) - PA(3) ) + PA(3);
acconst.m
Le equazioni utilizzate nella funzione fanno riferimento ad una legge di moto ad
accelerazione costante, di periodo ‘T’ e alzata ‘h’.
‘t’ è la variabile temporale, ‘y’ variabile spaziale.
L’accelerazione (y’’) è dunque costante e nota una volta definiti i valori
dell’alzata ‘h’ e del periodo ‘T’. Le espressioni analitiche di velocità y’ e
della variabile spaziale
y possono essere agevolmente ricavate integrando
rispettivamente una e due volte la y’’.
0<t<T/2
→
y = 2⋅h⋅t2/(T2)
11
Per una determinazione ‘rapida’ di y, per valori T/2<t<T, si consideri che
questa è una parabola del tutto analoga a quella determinata per 0<t<T/2, ma con
concavità di segno opposto (la concavità è ora rivolta verso il basso), traslata
di ‘T’ nell’asse dei tempi e con un ‘offset’ di h per quanto riguardo il vertice
(per t=T, y=h): si ritroverà l’espressione riportata nella funzione.
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%
%
%
acconst
%
%
%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%
%
%
%
%
%
%
%
%
%
%
%
%
%
%
%
acconst: funzione per il calcolo dei punti di una legge di moto
ad accelerazione costante.
Sintassi:
y = acconst(t,T,h)
t è l'istante 'attuale' nel quale viene calcolata y;
T è il periodo di tempo assegnato per spostarsi con accelerazione
costante da 0 ad h;
h è l''alzata complessiva' ovvero la distanza che deve essere percorsa nel periodo T;
y è l'alzata che compete all'istante 't';
Per t = 0, y = 0; y = h se t = T; per 0 < t < T si trovano i diversi
parametri y, compresi fra 0 e h, immaginando di percorrere la distanza 0-h ad
accelerazione costante, pari a 4*h / (T^2).
function y = acconst(t,T,h)
if (t < 0)
y = 0;
end
if (t >= 0) & (t <= T/2)
y = 2*h*t*t / (T^2);
end
if (t > T/2) & (t <= T)
y = - (2*h)*((t - T)^2) / (T^2) + h;
end
if (t > T)
y = h;
end
volume_lavoro.m
Funzione per il calcolo del volume di lavoro
proprietà cinematiche di un robot S.C.A.R.A.
e
per
la
valutazione
delle
L’analisi viene effettuata con un metodo esaustivo di ricerca: stabilito un
parallelepipedo entro il quale effettuare l’analisi, questo viene discretizzato
in divisioni e scandito per mezzo di tre cicli ‘for’ annidati.
Richiamando la funzione di cinematica inversa ‘IK_scara_sol.m’ si è in grado di
attribuire, ad ogni punto, un indicatore che ne identifichi l’appartenenza o
meno al volume di lavoro.
Analogamente per la valutazione delle proprietà cinematiche richiamando
‘Jfk_scara.m’ e servendosi dell’operatore di MatLab ‘rcond(X)’ che restituisce
‘1’ se la matrice ‘X’ è ben condizionata, è prossimo a ‘0’ se il condizionamento
12
è pessimo e così via per valori intermedi (rcond è il reciproco della funzione
‘cond2’ di MatLab).
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%
%
%
volume_lavoro
%
%
%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%
%
%
%
%
%
%
%
%
%
%
%
%
%
%
%
%
volume_lavoro: funzione per la determinazione del volume di lavoro 'V' di un robot S.C.A.R.A.
e del condizionamento dello Jacobiano del robot per ogni punto appartenente a 'V'.
%
%
%
%
%
%
%
%
V è una matrice che contiene tanti elementi ('sol') quanti sono i punti dello
spazio con cui si è discretizzato il parallelepipedo di ricerca:
'sol'=1 se il punto è interno al volume di lavoro, 'sol'=0 altrimenti
Sintassi:
[V,Vcond] = volume_lavoro(xmin,xmax, ymin,ymax, zmin,zmax, nx,ny,nz, l1,l2,h, dmin,dmax)
xmin e xmax sono ascissa minima e massima del parallelepipedo che si intende considerare
per un metodo esaustivo di ricerca dei punti appartenenti al volume di lavoro.
Analogamente per y e z.
nx è il numero di divisioni con le quali si intende discretizzare, lungo
x, la dimensione del parallelepipedo di ricerca.
Analogamente per y e z.
l1,l2,h sono rispettivamente le dimensioni di bicipite, avambraccio,
torretta del robot.
dmin,dmax sono rispettivamente le escursioni minima e massima lungo z
consentite.
Vcond è una matrice che contiene tanti elementi ('sol') quanti sono i punti dello
spazio con cui si è discretizzato il parallelepipedo di ricerca:
Se il punto è interno al volume di lavoro sol = 'condizionamento della
matrice Jacobiana calcolata nel punto'; altrimenti 'sol'=0.
function [V,Vcond] = volume_lavoro( xmin,xmax, ymin,ymax, zmin,zmax, nx,ny,nz, l1,l2,h, dmin,dmax)
P44 = [0;0;0];
V =[];
Vcond = [];
dx = (xmax-xmin)/nx;
dy = (ymax-ymin)/ny;
dz = (zmax-zmin)/nz;
ix = 1;
for x = xmin:dx:xmax
iy = 1;
for y=ymin:dy:ymax
iz = 1;
for z=zmin:dz:zmax
[alfa1,alfa2,alfa3,d_t,sol]=IK_scara_sol(x,y,z, 0, l1,l2,h, dmin,dmax);
V(ix,iy,iz) = sol;
if (sol)
J=Jfk_scara(alfa1,alfa2,alfa3,d_t,l1,l2,h,P44);
2
COND Condition number with respect to inversion.
COND(X) returns the 2-norm condition number (the ratio of the
largest singular value of X to the smallest). Large condition
numbers indicate a nearly singular matrix.
COND(X,P) returns the condition number of X in P-norm:
NORM(X,P) * NORM(INV(X),P).
where P = 1, 2, inf, or 'fro'. (MatLab 6.5 help)
13
sol=rcond(J);
Vcond(ix,iy,iz) = sol;
else
Vcond(ix,iy,iz) = 0;
end
iz = iz+1;
end
iy = iy+1;
end
ix = ix+1;
end
Jfk_scara.m
Funzione per il calcolo della matrice Jacobiana della trasformazione cinematica
diretta di un robot S.C.A.R.A.: JFK.
Lo studio della JFK riveste notevole interesse; questa permette infatti di
linearizzare la trasformazione cinematica diretta: fra le altre cose può dunque
essere intesa come un’indicatore di come errori nelle rotazioni dei giunti si
ripercuotono sulla posizione dell’end effector.
Valgono le seguenti considerazioni:
La funzione di cinematica diretta, FFK, e analogamente per la cinematica
inversa, può essere linearizzata in un opportuno intorno del punto, sia x0, in
cui si intende calcolarla.
Si può scrivere:
FFK : ϑ → x
FFK (ϑ 0 ) = x 0
∂x
∆ x ≅ ⋅ ∆ϑ
∂ϑ 0
Con θ, x vettori rispettivamente dello spazio dei
Euclideo, entrambi appartenti a ℜN, con N = d.o.f..
giunti
e
dello
spazio
∆ x = x − x0
∆ϑ = ϑ − θ 0
∂x
∂ϑ = J FK
JFK è proprio lo Jacobiano della trasformazione cinematica diretta, matrice di
ordine NxN (con N = n.ro di gradi di libertà del robot).
14
J FK
∂x1
∂ϑ
1
∂x 2
= ∂ϑ1
∂x
3
∂ϑ1
...
∂x1
∂ϑ2
∂x 2
∂ϑ2
∂x1
∂ϑ3
...
...
...
...
...
...
...
...
...
Dove xi rappresenta l’elemento i-simo del vettore x e θj l’elemento j-simo del
vettor θ.
Un calcolo numerico dello Jacobiano in un determinato punto dello spazio, x0, è
possibile sostituendo, alle derivate parziali, i corrispondenti rapporti
incrementali. Per l’elemento generico, i-sima riga, j-sima colonna, varrà:
( J FK )ij =
xi (ϑ 0 + ∆ϑ j ) − xi (ϑ 0 )
∆ϑ
Si intenda:
-xi, elemento i-simo del vettore x;
-∆θj, è un vettore di dimensione N; di tutti zeri fuorché nella posizione j-sima
in cui assume il valore di ∆θ;
-∆θ, incremento angolare ‘arbitrariamente’ piccolo.
In realtà ∆θ non può essere scelto del tutto arbitrariamente: se troppo piccolo
esalterà errori di troncamento numerico, se troppo elevato può portare i
rapporti incrementali a differire significativamente rispetto alle derivate a
causa della non linearità della trasformazione cinematica diretta.
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%
%
%
Jfk_scara
%
%
%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%
%
%
%
%
%
%
%
%
%
%
%
%
%
%
%
%
Jfk_scara: funzione per il calcolo dello Jacobiano della trasformazione cinematica
diretta per un robot tipo S.C.A.R.A.
Sintassi:
J=Jfk_scara(alfa1,alfa2,alfa3,d,L1,L2,h,P44)
alfa1,alfa2,alfa3 sono le rotazioni impresse ai giunti.
d è la traslazione verticale impressa , positiva verso il basso.
L1,L2,h sono rispettivamente le dimensioni di bicipite,
avambraccio e torretta dello S.C.A.R.A.
P44, vettore dello spazio tridimensionale che identifica le
coordinate di P nel sistema relativo '44', solidale alla pinza, con
origine nel centro del giunto cui è associata la rotazione alfa3.
J è la matrice Jacobiana, di ordine NxN, N = gradi di
libertà del robot.
function J = Jfk_scara(alfa1,alfa2,alfa3,d,L1,L2,h,P44)
delta_alfa = 1e-10;
15
X_0 = FK_scara_bis(alfa1, alfa2, alfa3, d, h, L1, L2, P44);
X_1 = FK_scara_bis(alfa1 + delta_alfa, alfa2, alfa3, d, h, L1, L2, P44);
X_2 = FK_scara_bis(alfa1, alfa2 + delta_alfa, alfa3, d, h, L1, L2, P44);
X_3 = FK_scara_bis(alfa1, alfa2, alfa3 + delta_alfa, d, h, L1, L2, P44);
X_4 = FK_scara_bis(alfa1, alfa2, alfa3, d + delta_alfa, h, L1, L2, P44);
%
%
%
%
NOTA
viene utilizzato per l'incremento della variabile d, lo stesso valore
delta_alfa, pur presentando dimensionalmente incongruenza: l'uno è una
dimensione spaziale, delta_alfa un'incremento angolare.
%
Scrittura
della matrice Jacobiana:
J = [(X_1-X_0)/delta_alfa, (X_2-X_0)/delta_alfa, (X_3-X_0)/delta_alfa, (X_4-X_0)/delta_alfa];
‘Jfk_scara.m’ richiama la funzione FK_scara_bis, di seguito riportata.
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%
%
%
FK_scara_bis
%
%
%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%
%
%
%
%
%
%
%
%
%
%
%
%
%
%
%
%
%
%
%
%
FK_scara_bis: funzione di cinematica diretta relativa ad un robot tipo S.C.A.R.A.
nella sostanza analoga a FK_scara.
In funzione delle rotazioni impresse ai giunti e delle dimensioni geometriche
del robot, fornisce le coordinate e l'orientazione dell'end effector.
La base del robot ha centro nell'origine del sistema di riferimento: [0,0,0].
Sintassi:
Xv=FK_scara_bis(alfa1,alfa2,alfa3,d,h,D1,D2,P44)
alfa1,alfa2,alfa3 sono le rotazioni impresse ai giunti.
d è la traslazione verticale impressa , positiva verso il basso.
h,D1,D2 sono rispettivamente le dimensioni di torretta, bicipite,
avambraccio dello S.C.A.R.A.
P44 vettore dello spazio tridimensionale che identifica le
coordinate di P nel sistema relativo '44', solidale alla pinza, con
origine nel centro del giunto cui è associata la rotazione alfa3..
Xv è un vettore che contiene le coordinate e l'orientazione dell'end
effector.
function Xv = FK_scara_bis(alfa1,alfa2,alfa3,d,h,D1,D2,P44)
m10
m21
m32
m43
=
=
=
=
DH_translate
DH_translate
DH_translate
DH_translate
([0,0,h]) * DH_rotatez(alfa1);
([D1,0,0]) * DH_rotatez(alfa2);
([D2,0,0]) * DH_rotatez(alfa3);
([0,0,d]);
P44omog = [P44(1);P44(2);P44(3);1];
P = m10*m21*m32*m43*(P44omog);
beta = alfa1+alfa2+alfa3;
Xv = [P(1); P(2); P(3); beta];
16
prova_volume_cond.m
E’ un programma per la visualizzazione:
- del volume di lavoro di un robot S.C.A.R.A.
- di superfici di isocondizionamento di JFK.
In sostanza viene richiamata la funzione ‘volume_lavoro.m’ e visualizzati,
tramite opportuni comandi MatLab, i dati contenuti nelle matrici che questa
restituisce.
Un esempio di visualizzazione è di seguito riportato.
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%
%
%
prova_volume_cond
%
%
%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%
%
%
%
prova_volume_cond: programma per la visualizzazione :
- del volume di lavoro di un robot S.C.A.R.A.
- di superfici di 'isocondizionamento' dello Jacobiano Jfk del robot.
disp('PROGRAMMA PER LA VISUALIZZAZIONE DEL VOLUME DI LAVORO DI UN ROBOT S.C.A.R.A')
disp('Oltre al volume di lavoro verranno visualizzate isosuperfici relative ai valori')
disp('di condizionamento dello Jacobiano della trasformazione cinematica diretta.')
17
risp=input('Se si intende utilizzare valori di default si digiti "1",\nse si preferisce inserire i
dati si digiti un altro numero e si prema "invio" ...')
l1=5;
l2=7;
h=30;
dmin=2;
dmax=15;
nx=18;
ny=nx;
nz=nx;
OFFs=4;
xmin=-l1-l2-OFFs;
xmax=l1+l2+OFFs;
ymin=xmin;
ymax=xmax;
zmin=-OFFs;
zmax=h+OFFs;
if risp==1
disp('Ok, valori di default');
else
disp('Verranno di seguito richiesti valori per definire il parallelepipedo da scandire')
disp('per la valutazione del volume di lavoro, e per definire la geometria del robot.')
xmin=input('inserisci i valori del parallelepipedo di "ricerca" , xmin=...')
xmax=input('inserisci i valori del parallelepipedo di "ricerca", xmax=...')
ymin=input('inserisci valori del parallelepipedo di "ricerca", ymin=...')
ymax=input('inserisci valori del parallelepipedo di "ricerca", ymax=...')
zmin=input('inserisci valori del parallelepipedo di "ricerca", zmin=...')
zmax=input('inserisci valori del parallelepipedo di "ricerca", zmax=...')
l1=input('inserisci la lunghezza del bicipite dello scara, l1=...')
l2=input('inserisci il valore del''avambraccio dello scara, l2=...')
h=input('inserisci il valore della torretta dello scara, h=...')
nx=input('inserisci il numero di campionamenti su x, nx=...')
ny=input('inserisci il numero di campionamenti su y, ny=...')
nz=input('inserisci il numero di campionamenti su z, nz=...')
dmin=input('inserisci il valore dell''escursione verticale minima, dmin=...')
dmax=input('inserisci il valore dell''escursione verticale massima, dmax...')
end
[V,Vcond] = volume_lavoro(xmin,xmax, ymin,ymax, zmin,zmax, nx,ny,nz,
l1,l2,h, dmin,dmax);
figure(1)
p = patch(isosurface(V));
set(p,'FaceColor',[1 0 1],'EdgeColor','none');
view(3);
camlight ;
lighting gouraud;
title('VOLUME DI LAVORO');
figure(2);
subplot(2,2,1);
p = patch(isosurface(V));
set(p,'FaceColor','b','EdgeColor','none');
view(3);
axis tight;
camlight ;
lighting gouraud;
title('VOLUME DI LAVORO')
subplot(2,2,2)
title('Condizionamento')
p = patch(isosurface(Vcond,0.01));
set(p,'FaceColor','red','EdgeColor','none');
view(3); axis tight
camlight
lighting gouraud
xlabel('rcond=0.01');
subplot(2,2,3)
18
title('Condizionamento')
p = patch(isosurface(Vcond,0.03));
set(p,'FaceColor','g','EdgeColor','none');
view(3); axis tight
camlight
lighting gouraud
xlabel('rcond=0.04');
subplot(2,2,4)
title('Condizionamento')
p = patch(isosurface(Vcond,0.1));
set(p,'FaceColor',[0 0.9 1],'EdgeColor','none');
view(3); axis tight
camlight
lighting gouraud
xlabel('rcond=0.1');
pause(3)
disp('Se si vuole visualizzare
condizionamento digitarlo ora')
ric_cond=input('\n....')
il
risultato
corrispondente
ad
un
determinato
numero
di
if abs(ric_cond)<=1
ric_cond=abs(ric_cond);
figure(3)
title('Condizionamento richiesto')
p = patch(isosurface(Vcond,ric_cond));
set(p,'FaceColor',[0.1 0.1 0.5],'EdgeColor','none');
view(3); axis tight
camlight
lighting gouraud
xlabel(ric_cond);
else
disp('Il numero digitato doveva essere fra 0 e 1!')
end
matrice.m
Programma semplificato per la generazione di un linguaggio di programmazione per
robot di tipo S.C.A.R.A.
Il linguaggio si serve di pochi comandi base che permettono di generare il
movimento del robot, tenerlo in arresto per pause di entità determinate,
ripetere ciclicamente una serie di istruzioni.
I comandi vengono richiesti all’utente, la sintassi essendo spiegata nei
commenti introduttivi del programma, e permettono di generare e visualizzare una
simulazione di un ciclo di lavoro del robot.
In particolare fra le specifiche che seguono il comando di movimento, è
possibile impostare il periodo T correlato allo spostamento fra punto attuale e
punto di arrivo dell’end effector, Pend.
Si possono così stabilire fasi lente e veloci nel moto del robot, come in una
reale applicazione di pick and place, dove le operazioni di afferraggio e
posizionamento richiedono maggior precisione, e dunque minori velocità, mentre
la movimentazione del pezzo può in genere avvenire a velocità elevata.
Nonostante le specifiche debbano essere fornite sotto forma di matrice, una
volta definito un ciclo è possibile ripeterlo più volte.
19
E’ altresì presente una variabile ‘lim_max’ e un contatore ‘limite’ per evitare
di generare dei cicli infiniti (ogni volta che un ciclo è ripetuto, ‘limite’
viene incrementato, quando limite=lim_max il ‘loop’ viene interrotto).
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%
%
%
matrice
%
%
%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%
%
%
%
%
%
%
%
%
%
%
%
%
%
%
%
%
%
Programma per pilotaggio di un robot S.C.A.R.A.
Si richiede all'utente di inserire, sotto forma di matrice, i dati
necessari a definire un ciclo di lavoro del robot.
Ad ogni riga, di 5 elementi, è associata un'istruzione identificata dal primo elemento;
- '1' implica spostamento; il punto di arrivo (Pend) è definito dagli elementi
2,3,4 ( = x,y,z) della medesima riga; l'elemento 5 definisce il tempo
stabilito per spostare l'end effector dalla posizione attuale a Pend.
Inizialmente l'end effector si trova in [0,0,0].
- '2' implica una pausa, l'entità della pausa in secondi è definita dal
secondo elemento della medesima riga; gli altri elementi benché
ininfluenti devono comunque essere scritti.
- '3' implica di tornare ad eseguire 'matrice' dalla riga identificata
dal secondo parametro della medesima riga.
disp('Inserire la matrice da utilizzare
disp('matrice MxN dove M è il numero di
disp('L''elemento della prima colonna è
disp('..1 per "move to", 2 per "pause",
per il pilotaggio-scara, del tipo [1,2,3,4];')
istruzioni, N di parametri (5).')
relativo al comando:')
3 per ritornare ad una determinata riga di comando.')
matrix=[];
matrix=input('Inserire ora la matrice...matrix=...');
dimensione=size(matrix);
n_righe=dimensione(1);
%prow è un contatore di riga di comando
prow=1;
start=[0;0;0]
%
%
%
%
Inizializzazione di variabili e oggetto grafico
Con una veloce modifica è possibile permettere al''utente di inserire
direttamente le caratteristiche geometriche del robot.
h=6;
D1=5;
D2=5;
d_tmin=0;
d_tmax=10;
Pstart=[0;0;0];
Pend=[];
%
%
%input('Inserire la dimensione della torretta h= ');
%input('Inserire la dimensione del bicipite D1= ');
%input('Inserire la dimensione dell avambraccio D2= ');
%input('Inserire le coordinate iniziali di P,es...
[2;0;0] = ');
limite: variabile che limita il n.ro di cicli, utilizzando la funzionalità '3',
si ricadrebbe, altrimenti, in loop infiniti
lim_max=10;
limite=0;
T=5;
figure;
Rag=10
axis([ -Rag Rag -Rag Rag -Rag Rag]);
grid
xlabel('assex')
20
ylabel('assey')
zlabel('assez')
rot = 0;
P44 = [0 0 0]';
gro_p
=line('xdata',[],'ydata',[],'zdata',[],'linewidth',1.5,'linestyle','o','markersize',6,'color','b');
gro_Pto
=line('xdata',[],'ydata',[],'zdata',[],'linewidth',3,'linestyle','o','markersize',6,'color','g');
gro_line=line('xdata',[],'ydata',[],'zdata',[],'linewidth',1.3,'linestyle','-','color','r');
while prow <= n_righe
cond=matrix(prow,1);
if cond==1
%
Visualizzazione dello spostamento richiesto
T= matrix(prow,5);
Pend=[matrix(prow,2);matrix(prow,3);matrix(prow,4)];
for t=0:T/30:T
Punto=traiettrettil(Pstart,Pend,t,T);
[alfa1,alfa2,alfa3,d,sol]=IK_scara_sol(Punto(1), Punto(2), Punto(3), rot, D1, D2, h,
d_tmin,d_tmax);
[A,B,C,D,P] = FK_scara(alfa1,alfa2,alfa3,d,h,D1,D2,P44);
set(gro_line,'xdata',[0 A(1) B(1) C(1) D(1) P(1)], 'ydata',[0 A(2) B(2) C(2) D(2) P(2)],
'zdata', [0 A(3) B(3) C(3) D(3) P(3)]);
set(gro_p,
'xdata',[0 A(1) B(1) C(1) D(1) P(1)], 'ydata',[0 A(2) B(2) C(2) D(2) P(2)],
'zdata', [0 A(3) B(3) C(3) D(3) P(3)]);
pause(0.02);
drawnow;
end
Pstart = Pend;
end
if cond==2
%
Pausa in secondi di entità uguale al parametro passato.
pause(matrix(prow,2));
end
if cond==3
%
%
Si torna all'esecuzione della riga di matrix identificata
da matrix(prow,2)
prow=matrix(prow,2)-1;
limite=limite+1;
if (limite==lim_max)
prow=n_righe+1;
end
end
prow = prow+1;
end
pause(1)
text(-2,-2,-2, 'FINE PROGRAMMA!','FontName','Book Antiqua','FontSize',14,'color','k');
pause(1)
close
21
© Copyright 2024 Paperzz
