Controllo del moto e Controllo del moto e robotica industrialerobotica industriale

Richiami di cinematica

Prof. Paolo Rocco (paolo.rocco@polimi.it)

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: