richiami di cinematica - intranet deibhome.deib.polimi.it/rocco/cmri/cinematica_2p.pdf · controllo...
TRANSCRIPT
Controllo del moto e Controllo del moto e robotica industrialerobotica industriale
Richiami di cinematica
Prof. Paolo Rocco ([email protected])
Nota:I disegni riportati in queste slide sono tratti dal testo:L.Sciavicco, B.SicilianoRobotica industriale –Modellistica e controllo di robot manipolatori(2a ed.) Mc Graw-Hill, 2000
Controllo del moto e robotica industriale - Richiami di cinematica - P. Rocco [2]
Il sistema meccanicoIl sistema meccanicoCome è noto il manipolatore è costituito da una serie di corpi rigidi (link) connessi da giunti.
Un’estremità della catena è costituita dalla BASE, di norma fissata terra.
All’altra estremità è presente l’ENDEFFECTOR (pinza, strumento di lavoro).
Nel manipolatore si individua una struttura portante che garantisce il posizionamento ed un POLSO che conferisce destrezza, dando i gradi di libertà di orientamento all’organo terminale.
Controllo del moto e robotica industriale - Richiami di cinematica - P. Rocco [3]
I giuntiI giunti
Ciascun giunto consente uno (e uno solo) grado di mobilità tra due bracci. Chiamiamo variabile di giunto la coordinata associata a questo grado di mobilità.Schematizzazione dei giunti:
GIUNTI ROTOIDALI GIUNTI PRISMATICI
Controllo del moto e robotica industriale - Richiami di cinematica - P. Rocco [4]
Terna base e terna utensileTerna base e terna utensile
Definiamo una terna di base ed una terna solidale con l’utensile.
La terna utensile èdefinita per mezzo di 3 versori:
ae (approccio): direzione di avvicinamento al pezzo;se (scivolamento): ortogonale ad ae nel piano di scorrimento della presa;ne (normale): ortogonale agli altri due.
pe punta all’origine della terna utensile (punto centrale dell’organo di presa).
Controllo del moto e robotica industriale - Richiami di cinematica - P. Rocco [5]
Cinematica direttaCinematica diretta
L’equazione cinematica diretta determina posizione ed orientamento della terna utensile rispetto alla terna base, in funzione delle variabili di giunto.
( ) ( ) ( ) ( ) ( )⎥⎥⎦
⎤
⎢⎢⎣
⎡=
1000qpqaqsqnqT
be
be
be
beb
e
Esempio: manipolatore planare a due bracci
( )⎥⎥⎥⎥
⎦
⎤
⎢⎢⎢⎢
⎣
⎡+−+
=
10000001
00
122111212
122111212
sasasccacacs
be qT
(matrice di trasformazione omogenea)
Controllo del moto e robotica industriale - Richiami di cinematica - P. Rocco [6]
Cinematica direttaCinematica diretta
Per procedere in modo sistematico nella determinazione della cinematica diretta, occorre definire una terna solidale con ciascun braccio:
( ) ( ) ( ) ( )nnnn qqq 1
2121
01
0 −= AAAqT K
braccio 0 a terra
n ultimo braccio
ogni giunto dà un grado di mobilità
Procedendo iterativamente:
( ) ( ) nen
bbe TqTTqT 0
0=
Controllo del moto e robotica industriale - Richiami di cinematica - P. Rocco [7]
Convenzione di Convenzione di DenavitDenavit--HartenbergHartenberg
È una convenzione per la scelta delle terne solidali ai bracci:
• zi giace lungo l’asse del giunto i+1• Oi è all’intersezione dell’asse zi con la normale comune agli assi zi e zi−1;
si indica con Oi’ l’intersezione della normale comune con zi−1• xi è diretto lungo la normale comune agli assi zi e zi−1, con verso positivo
dal giunto i al giunto i+1• yi completa una terna destra
terna i solidale con il braccio i
Controllo del moto e robotica industriale - Richiami di cinematica - P. Rocco [8]
Definizione non univoca della ternaDefinizione non univoca della ternaVi sono alcuni casi in cui la terna non è univocamente definita:
• Nella terna 0 solo la direzione di z0 è specificata: si possono scegliere arbitrariamente origine ed asse x0
• Nella terna n è specificato solo l’asse xn che deve essere normale a zn−1• Quando due assi consecutivi sono paralleli non è univocamente definita la
normale comune• Quando due assi consecutivi si intersecano, non è definito il verso di xi• Quando il giunto i è prismatico solo la direzione di zi−1 è determinata
L’indeterminazione può essere usata per semplificare la procedura.
Controllo del moto e robotica industriale - Richiami di cinematica - P. Rocco [9]
Parametri di Parametri di DenavitDenavit--HartenbergHartenberg
Per definire una terna rispetto alla precedente sono sufficienti 4 parametri:
• ai distanza di Oi da Oi’• di coordinata su zi−1 di Oi’• αi angolo intorno all’asse xi tra l’asse zi−1 e l’asse zi valutato positivo in
senso antiorario• ϑi angolo intorno all’asse zi-1 tra l’asse xi-1 e l’asse xi valutato positivo in
senso antiorario
terna i solidale con il braccio i
ai e αi sono sempre costantiϑi o di è variabile
Controllo del moto e robotica industriale - Richiami di cinematica - P. Rocco [10]
Matrice di trasformazione omogeneaMatrice di trasformazione omogenea
Costruzione della matrice di trasformazione dalla terna i−1 alla terna i:
I) Per sovrapporre la terna i−1 alla terna i’si trasla la terna lungo l’asse zi-1 di una lunghezza diruotandola di un angolo ϑi intorno a zi-1: ⎥
⎥⎥⎥
⎦
⎤
⎢⎢⎢⎢
⎣
⎡ −
= ϑϑ
ϑϑ
−′
1000100
0000
1
i
ii d
cssc
ii
ii
A
II) Per sovrapporre la terna i’ alla terna i si trasla la terna lungo l’asse xi’ per una lunghezza ai, ruotandola di un angolo αi intorno a xi’:
⎥⎥⎥⎥
⎦
⎤
⎢⎢⎢⎢
⎣
⎡−
=αα
αα′
10000000
001
ii
ii
cssc
ai
iiA
( )⎥⎥⎥⎥
⎦
⎤
⎢⎢⎢⎢
⎣
⎡−
−
==αα
ϑαϑαϑϑ
ϑαϑαϑϑ
′−′
−
10000
11
i
i
i
ii
iii
ii dcs
sascccscasscsc
qii
iiiiii
iiiiii
AAA
Controllo del moto e robotica industriale - Richiami di cinematica - P. Rocco [11]
Manipolatore planare a tre bracciManipolatore planare a tre bracci
La terna utensile non coincide con la terna 3
33
22
11
003002001
ϑϑϑϑα
aaa
da iiii
⎥⎥⎥⎥
⎦
⎤
⎢⎢⎢⎢
⎣
⎡++++−
==
10000100
00
123312211123123
123312211123123
23
12
01
03
sasasacscacacasc
AAAT
Controllo del moto e robotica industriale - Richiami di cinematica - P. Rocco [12]
Manipolatore sfericoManipolatore sferico
La terna utensile coincide con la terna 3
0003202
0201
3
22
1
dd
da iiii
ϑπϑπ−ϑα
⎥⎥⎥⎥
⎦
⎤
⎢⎢⎢⎢
⎣
⎡
−+−−
==
10000 3222
2132121121
2132121121
23
12
01
03 dccs
dcdssssccsdsdscscscc
AAAT
Controllo del moto e robotica industriale - Richiami di cinematica - P. Rocco [13]
Manipolatore antropomorfoManipolatore antropomorfo
La terna utensile non coincide con la terna 3
33
22
1
0030020201
ϑϑϑπϑα
aa
da iiii
( )( )
⎥⎥⎥⎥
⎦
⎤
⎢⎢⎢⎢
⎣
⎡
++−−+−
==
10000 233222323
2332211231231
2332211231231
23
12
01
03 sasacs
cacascsscscacacssccc
AAAT
Controllo del moto e robotica industriale - Richiami di cinematica - P. Rocco [14]
Polso sfericoPolso sferico
La terna utensile coincide con la terna 6
66
5
4
00602050204
ϑϑπϑπ−ϑα
d
da iiii
⎥⎥⎥⎥
⎦
⎤
⎢⎢⎢⎢
⎣
⎡
−+−+−−−
==
10006556565
654546465464654
654546465464654
56
45
34
36 dccsscs
dssssccscsscccsdscsccssccssccc
AAAT
ϑ4, ϑ5, ϑ6 sono gli angoli di Eulero ZYZ della terna 6 rispetto alla 3
Controllo del moto e robotica industriale - Richiami di cinematica - P. Rocco [15]
Manipolatore di Manipolatore di StanfordStanford
66
5
4
3
22
1
00602050204
0003202
0201
ϑϑπϑπ−
ϑπϑπ−ϑα
d
dd
da iiii
Il manipolatore di Stanford è un manipolatore sferico con polso sferico
36
03
06 TTT =
Già calcolata per il polso sfericoGià calcolata per il manipolatore sferico
Controllo del moto e robotica industriale - Richiami di cinematica - P. Rocco [16]
Manipolatore antropomorfo con polso sfericoManipolatore antropomorfo con polso sferico
66
5
44
3
22
1
0060205
20402030020201
ϑϑπϑπ−ϑπϑϑπϑα
d
d
a
da iiii
Montiamo un polso sferico sul manipolatore antropomorfo
La terna 3 del manipolatore antropomorfo non era orientata correttamente per il successivo polso sferico, per cui per calcolare la cinematica diretta occorre rifare i conti (non basta semplicemente moltiplicare le due matrici di trasformazione parziali)
Controllo del moto e robotica industriale - Richiami di cinematica - P. Rocco [17]
Spazio dei giunti e spazio operativoSpazio dei giunti e spazio operativo
Lo spazio dei giunti è definito dal vettore delle variabili di giunto:
Equazione cinematica diretta:
⎥⎥⎥
⎦
⎤
⎢⎢⎢
⎣
⎡=
nq
qM1
qqi = ϑi (giunto rotoidale)
qi = di (giunto prismatico)
Lo spazio operativo è lo spazio in cui è specificata l’operazione che il manipolatore deve compiere. È definito dalla postura x :
⎥⎦
⎤⎢⎣
⎡φ
=p
x p (posizione)φ (rappresentazione minima dell’orientamento)
m componenti
( )qkx =
Controllo del moto e robotica industriale - Richiami di cinematica - P. Rocco [18]
Problema Problema cinematicocinematico inversoinverso
Assegnati posizione ed orientamento della terna utensile, trovare le corrispondenti variabili di giunto.qx
qT⇒⇒
• Il problema può non ammettere soluzione (se posizione ed orientamento non appartengono allo spazio di lavoro destro)
• La soluzione analitica (in forma chiusa) può non esistere, nel qual caso si ricorre a tecniche numeriche
• Possono esserci soluzioni multiple• Possono esserci infinite soluzioni
In generale la soluzione si ricava senza un procedimento sistematico, ma sulla base di intuizione nella manipolazione delle equazioni.
Controllo del moto e robotica industriale - Richiami di cinematica - P. Rocco [19]
Manipolatore con polso sfericoManipolatore con polso sfericoSe il manipolatore ha polso sferico, si può disaccoppiare la soluzione del problema cinematico inverso per la posizione da quella per l’orientamento
Posizione del centro polso:
Procedura operativa• si calcola la posizione del centro polso pW• si risolve la cinematica inversa per (q1, q2, q3)• si calcola la matrice R3
0(q1, q2, q3)• si calcola R6
3(ϑ4, ϑ5, ϑ6) = R30TR
• si risolve la cinematica inversa per l’orientamento, ricavando (ϑ4, ϑ5, ϑ6)
app 6dW −=
Controllo del moto e robotica industriale - Richiami di cinematica - P. Rocco [20]
Manipolatore antropomorfoManipolatore antropomorfo
Esistono quattro configurazioni ammissibili:
spalla destra,gomito alto
spalla sinistra,gomito alto
spalla destra,gomito basso
spalla sinistra,gomito basso
Controllo del moto e robotica industriale - Richiami di cinematica - P. Rocco [21]
Polso sfericoPolso sferico
Il dato del problema è la matrice:
⎥⎥⎥
⎦
⎤
⎢⎢⎢
⎣
⎡
=333
333
333
36
zzz
yyy
xxx
asnasnasn
R
( )( )
( )336
3232355
334
,2Atan
,)()(2Atan:,0
,2Atan
zz
zyx
xy
ns
aaa
aa
−=ϑ
⎟⎠⎞⎜
⎝⎛ +=ϑπ∈ϑ
=ϑ
Per la soluzione, si utilizzano le formule note per gli angoli di Eulero ZYZ:
( )( )
( )336
3232355
334
,2Atan
,)()(2Atan:0,
,2Atan
zz
zyx
xy
ns
aaa
aa
−=ϑ
⎟⎠⎞⎜
⎝⎛ +−=ϑπ−∈ϑ
−−=ϑ
Controllo del moto e robotica industriale - Richiami di cinematica - P. Rocco [22]
Jacobiano geometricoJacobiano geometricoAnalizziamo ora il moto della terna utensile (solidale con l’organo terminale) per un manipolatore robotico . Possiamo definire la velocità lineare e la velocità angolare di tale terna: p e ω.
Obiettivo della cinematica differenziale è di esprimere queste velocità in funzione delle velocità delle variabili di giunto.
( )( )qqJ
qqJp&
&&
O
P
==
ω
.
In forma compatta possiamo scrivere:
( )qqJp
v &&
=⎥⎦
⎤⎢⎣
⎡=
ω
La matrice (6×n): ( ) ( )( )⎥⎦
⎤⎢⎣
⎡=
qJqJ
qJO
P
Prende il nome di Jacobiano geometrico del manipolatore.
Controllo del moto e robotica industriale - Richiami di cinematica - P. Rocco [23]
Calcolo dello Jacobiano Calcolo dello Jacobiano
Partizioniamo lo Jacobiano in n colonne, ciascuna delle quali, a sua volta, partizionata in due:
⎥⎥⎥
⎦
⎤
⎢⎢⎢
⎣
⎡=
OnO
PnP
jj
jjJ
1
1
L
qi jPi: contributo del giunto i alla velocità lineare
qi jOi: contributo del giunto i alla velocità angolare
.
.
( )⎪⎪⎪
⎩
⎪⎪⎪
⎨
⎧
⎥⎦
⎤⎢⎣
⎡ −×
⎥⎦
⎤⎢⎣
⎡
=⎥⎦
⎤⎢⎣
⎡
−
−−
−
rotoidale giunto
prismatico giunto
1
11
1
i
ii
i
Oi
Pi
zppz
z
jj 0
Risulta:
Gli elementi per il calcolo di queste colonne si ricavano da relazioni cinematiche dirette.
Controllo del moto e robotica industriale - Richiami di cinematica - P. Rocco [24]
Manipolatore planare a tre bracciManipolatore planare a tre bracci
( ) ( ) ( ) ( )⎥⎦
⎤⎢⎣
⎡ −×−×−×=
210
221100
zzzppzppzppz
qJ
⎥⎥⎥
⎦
⎤
⎢⎢⎢
⎣
⎡===
⎥⎥⎥
⎦
⎤
⎢⎢⎢
⎣
⎡++++
=
⎥⎥⎥
⎦
⎤
⎢⎢⎢
⎣
⎡++
=⎥⎥⎥
⎦
⎤
⎢⎢⎢
⎣
⎡=
⎥⎥⎥
⎦
⎤
⎢⎢⎢
⎣
⎡=
100
0
00000
210123312211
123312211
12211
12211
211
11
10
zzzp
ppp
sasasacacaca
sasacaca
saca
⎥⎥⎥⎥⎥
⎦
⎤
⎢⎢⎢⎢⎢
⎣
⎡+++
−−−−−−
=
11100000000012331233122123312211
12331233122123312211cacacacacacasasasasasasa
J
N.B.:
⎥⎥⎥
⎦
⎤
⎢⎢⎢
⎣
⎡
−−−
=×
1221
3113
2332
babababababa
ba
Controllo del moto e robotica industriale - Richiami di cinematica - P. Rocco [25]
Manipolatore antropomorfoManipolatore antropomorfo
( ) ( ) ( ) ( )⎥⎦
⎤⎢⎣
⎡ −×−×−×=
210
221100
zzzppzppzppz
qJ
( )( )
⎥⎥⎥
⎦
⎤
⎢⎢⎢
⎣
⎡−==
⎥⎥⎥
⎦
⎤
⎢⎢⎢
⎣
⎡=
⎥⎥⎥
⎦
⎤
⎢⎢⎢
⎣
⎡
+++
=
⎥⎥⎥
⎦
⎤
⎢⎢⎢
⎣
⎡=
⎥⎥⎥
⎦
⎤
⎢⎢⎢
⎣
⎡==
0100
000
1
1
210
23322
233221
233221
22
212
212
210
cs
sasacacascacac
sacsacca
zzzp
ppp
( ) ( )( ) ( )
⎥⎥⎥⎥⎥⎥
⎦
⎤
⎢⎢⎢⎢⎢⎢
⎣
⎡
−−
+−+−+−+−+−
=
001000
11
11
23323322
2313233221233221
2313233221233221
ccsscacaca
ssasasascacacscasasaccacas
J
Controllo del moto e robotica industriale - Richiami di cinematica - P. Rocco [26]
Manipolatore di Manipolatore di StanfordStanford
( ) ( ) ( )
( ) ( ) ( )⎥⎦⎤−×−×−×
⎢⎣⎡ −×−×=
543
554433
10
21100
zzzppzppzppz
zzzppzppzqJ 0
( )( )
( ) ⎥⎥
⎦
⎤
⎢⎢
⎣
⎡
−+++++−++−
=
⎥⎥
⎦
⎤
⎢⎢
⎣
⎡+−
===⎥⎥⎦
⎤
⎢⎢⎣
⎡==
52452632
2155142541621321
5412515421621321
32
21321
21321
54310000
sscccddcsscssccsscddcdssssssccscccddsdsc
dcdcdssdsdsc
p
ppppp
⎥⎥
⎦
⎤
⎢⎢
⎣
⎡
−++−+
=⎥⎥
⎦
⎤
⎢⎢
⎣
⎡+−−−
=⎥⎥
⎦
⎤
⎢⎢
⎣
⎡==
⎥⎥
⎦
⎤
⎢⎢
⎣
⎡−=
⎥⎥⎦
⎤
⎢⎢⎣
⎡=
52452
2155142541
5412515421
5
42
41421
41421
4
2
21
21
32
1
1
1001
00
sscccsscssccsscssssccsccc
ssccscscsscc
csssc
cs
zzzz
zz
Controllo del moto e robotica industriale - Richiami di cinematica - P. Rocco [27]
Jacobiano analiticoJacobiano analiticoConsideriamo l’equazione cinematica diretta di un manipolatore:
La matrice: ( ) ( )( )⎥⎦
⎤⎢⎣
⎡=
φ qJqJ
qJ PA
Prende il nome di Jacobiano analitico del manipolatore.
( ) ( )( )⎥⎦
⎤⎢⎣
⎡==
qqp
qkxφ
dove φ costituisce una rappresentazione minima dell’orientamento. Derivando rispetto al tempo otteniamo:
( ) ( )qqJqqqkx &&& A=
∂∂
=
D’altra parte:
( )( )( )( )
( )( ) qqJqJ
qqqqqqpp
x &&
&
&
&& ⎥
⎦
⎤⎢⎣
⎡=⎥
⎦
⎤⎢⎣
⎡∂∂∂∂
=⎥⎦
⎤⎢⎣
⎡=
φ
P
φφ
Controllo del moto e robotica industriale - Richiami di cinematica - P. Rocco [28]
JacobianoJacobiano analitico e analitico e JacobianoJacobiano geometricogeometrico
Esprimiamo quindi la velocità (lineare ed angolare) della terna utensile in termini delle derivate di p e φ:
( ) ( ) ( ) qJTxTxT0
0Ipv &&&
&AAA φφ
φω==⎥
⎦
⎤⎢⎣
⎡=⎥
⎦
⎤⎢⎣
⎡=
Ne consegue:
( ) AA JTJ φ=
( )φφω &T=
Il legame tra la velocità angolare ω e la derivata del vettore φ che esprime l’orientamento è il seguente:
dove T è una matrice dipendente dalla rappresentazione utilizzata per l’orientamento.
Controllo del moto e robotica industriale - Richiami di cinematica - P. Rocco [29]
SingolaritSingolaritàà cinematichecinematicheRiprendiamo la relazione che definisce lo Jacobiano geometrico:
( )qqJv &=
I valori di q per cui la matrice J diminuisce di rango prendono il nome di singolarità cinematiche. In presenza di singolarità cinematiche si ha:
1. Perdita di mobilità (non è possibile imporre leggi di moto arbitrarie)2. Possibilità di infinite soluzioni al problema cinematico inverso3. Velocità elevate nello spazio dei giunti (nell’intorno di singolarità)
Le singolarità possono essere:
1. Ai confini dello spazio di lavoro raggiungibile2. All’interno dello spazio di lavoro raggiungibile
Queste ultime sono più problematiche, perché ci si può incorrere con traiettorie pianificate nello spazio operativo.
Controllo del moto e robotica industriale - Richiami di cinematica - P. Rocco [30]
SingolaritSingolaritàà cinematiche: esempiocinematiche: esempioConsideriamo un manipolatore planare a due bracci:
Si tratta di singolarità ai confini dello spazio di lavoro.
In queste configurazioni, le due colonne dello Jacobiano non sono indipendenti.
⎥⎦
⎤⎢⎣
⎡+
−−−=
12212211
12212211
cacacasasasa
J
Determiniamone le singolarità:
( )⎩⎨⎧π
=ϑ⇔==0
0det 2221 saaJ
Controllo del moto e robotica industriale - Richiami di cinematica - P. Rocco [31]
DisaccoppiamentoDisaccoppiamento di singolaritdi singolaritààConsideriamo un manipolatore con polso sferico:
Si può articolare la ricerca delle singolarità in due sottoproblemi:
1. Calcolo delle singolarità della struttura portante2. Calcolo delle singolarità del polso
( ) ( ) ( ) ( )( )⎩
⎨⎧
==
⇔==polso di àsingolarit0det
portante struttura di àsingolarit0det0detdetdet
22
112211 J
JJJJ
Controllo del moto e robotica industriale - Richiami di cinematica - P. Rocco [32]
SingolaritSingolaritàà di polsodi polso
[ ]54322 zzzJ =
Lo Jacobiano è singolare se z3 è parallelo a z5 (ϑ5=0, ϑ5=π ):
Rotazioni uguali e opposte di ϑ4 e ϑ6 non producono alcuna rotazione dell’organo terminale. Inoltre il polso non è in grado di effettuare rotazioni attorno all’asse ortogonale a z3 e z4.
È una singolarità difficile da individuare in una pianificazione del moto nello spazio operativo.
Controllo del moto e robotica industriale - Richiami di cinematica - P. Rocco [33]
SingolaritSingolaritàà di struttura portantedi struttura portanteIn un manipolatore antropomorfo:
Singolarità di gomito Singolarità di spalla
Sono singolarità che si caratterizzano facilmente nello spazio operativo.
Controllo del moto e robotica industriale - Richiami di cinematica - P. Rocco [34]
Inversione della cinematica differenzialeInversione della cinematica differenzialeL’equazione cinematica differenziale è lineare per un dato valore di q:
( )qqJv &=
Data la velocità v nello spazio operativo e una condizione iniziale su qpossiamo allora pensare di risolvere il problema della cinematica inversa invertendo la cinematica differenziale ed integrando. Se lo Jacobiano èquadrato (n = r, numero di coordinate dello spazio operativo necessarie per descrivere il compito):
( ) ( ) ( ) ( )00
1 qqqvqJq +σσ=⇒= ∫−t
dt &&
Utilizzando direttamente questa formula si incorre in derive della soluzione.
Si introduce allora l’errore, nello spazio operativo, commesso dall’algoritmo di inversione cinematica:
xxe −= d
Controllo del moto e robotica industriale - Richiami di cinematica - P. Rocco [35]
Inversa dello Inversa dello JacobianoJacobianoSe adottiamo la seguente legge di dipendenza di q da e:
( )( )KexqJq += −dA && 1
otteniamo:0=+ Kee&
e lo schema:
.
Controllo del moto e robotica industriale - Richiami di cinematica - P. Rocco [36]
Trasposta dello Trasposta dello JacobianoJacobianoSe adottiamo la seguente legge di dipendenza (più semplice):
( )KeqJq TA=&
otteniamo lo schema: