1

Controllo di ManipolatoriControllo di Manipolatori(Calcolo delle Traiettorie)(Calcolo delle Traiettorie)

Ph.D Ing. Folgheraiter MichelePh.D Ing. Folgheraiter Michele

Corso di Robotica Corso di Robotica Prof.Prof. CagliotiCaglioti

Anno Anno AccAcc. 2005/2006. 2005/2006

DEFINIZIONI

• Cammino geometrico (Path) E una curva definita nello spazio Euclideo. L’end-effector del manipolatore deve seguire tale curva (segmento, arco di circonferenza..). E possibile definire il cammino geometrico con l’ascissa curvilinea.

• Traiettoria (legge oraria) indica una funzione dipendente dal tempo (s=s(t))

2

E’ possibile classificare le traiettorie in:

•Stop to Stop: I giunti si muovono fino ad attivare i sensori di fine corsa (attuazione pneumatica).

•Point to Point: Si fornisce al controllore del robot una sequenza di punti da raggiungere (es Pick-and-Place)

•Traiettoria Controllata: E necessario specificare istante per istante lo stato del robot (posizione, velocità, accelerazione), può essere pre-calcolata o calcolata in real time.

•Traiettoria Continua: Si fa memorizzare al robot la traiettoria punto per punto con un campionamento molto fitto (non c’è bisogno di grosse elaborazioni)

3

14

2

Le traiettorie possono essere espresse nello:

• Spazio Cartesiano: Si stabilisce una traiettoria nello spazio cartesiano, poi punto per punto si passa, con la cinematica inversa, ai valori da mandare ai giunti.

- Molto pesante dal punto di vista computazionale- Problemi di controllo (punti singolari)

• Spazio dei Giunti: Si stabilisce la posizione iniziale e finale del giunto, poi si definisce una legge oraria ?i(t) (nello spazio dei giunti) che passa per questi due punti.

- Permette di risparmiare il calcolo della cinematica inversa per ogni punto.- Le traiettorie nello spazio cartesiano sono imprevedibili

3

Ci sono ostacoli da evitare

Non ci sono ostacoli da evitare

C’è un cammino voluto (rettilineo,

ottimo ecc...)

� Pianificazione Fuori linea

+Inseguimento(Ostacoli fissi)

�Inseguimento

Run Time

Non c’è un cammino voluto (ogni cammino è

sicuro)

� Controllo della posizione

Robotica mobile(Ostacoli Variabili)

Metodo dei Potenziali

�Controllo della

posizione

(Il più semplice)

Traiettoria con Ostacoli, casi possibili:

Se gli ostacoli che vogliamo evitare sono fissi, possiamo usare la rappresentazione del manipolatore nel C-Space.

C- Space: Il C-space è uno spazio n-dimensionale, dove n è il numero dei gradi di libertà della struttura.

-Ogni punto del C-space rappresenta una particolare configurazione del robot-Definiti degli ostacoli, vogliamo che la mano del manipolatore e la sua struttura li eviti.-Nel C-Space ogni ostacolo viene rappresentato come una serie di punti (configurazione dei giunti) non ammissibili

OSS: Non c’è corrispondenza biunivoca tra spazio cartesiano e spazio dei giunti.

4

- La generazione del C-Space è molto onerosa, la complessità è esponenziale col il numero di giunti.

- Per velocizzare i calcoli di collisione, è possibile discretizzare il C-Space

- Se m è il numero di intervalli mn la complessità di calcolo del C-Space (esponenziale)

Strategie di Movimento:

Manipolatore a 2 DOF, vogliamo passare dalla posizione di Start a quella di Goal, esistono infinite traiettorie possibili.

Modalità di movimento1) I giunti possono essere mossi alla stessa velocità2) I giunti vengono mossi uno alla volta3) I giunti devono iniziare e finire assieme il movimento

start

10°

20°

60°

90°goal

θ1 si sposta di 50°

θ2 si sposta di 70°

5

goal

In questo tratto i duegiunti si muovonoassieme (θ1 di 50°e θ2 di 50°)

start

Qui c’è un punto didiscontinuità il giunto 1 èarrivato, da questo punto inpoi si muove solo il giunto 2.

goal

Prima il giunto 1 poi il giunto 2

start

goal

Prima il giunto 2 poi il giunto 1

start

1

2

Calcolo delle traiettorie nello spazio dei giunti

1. Si parte con il definire alcuni punti di via nello spazio cartesiano

2. Attraverso la cinematica inversa si calcolano i valori dei giunti per ogni punto di via.

3. Si considera un giunto alla volta θi

4. Si utilizza un algoritmo che determini delle traiettorie θi(t) interpolatrici di questi punti- Le traiettorie devono essere facilmente computabili- Posizione e velocità devono essere funzioni continue (il robot non può

muoversi a scatti)

6

Traiettorie polinomiali :

Come traiettoria si utilizza una funzione polinomiale di grado n, che ha derivate continue.

Dopo di che si fissano dei vincoli quali per esempio:

- Posizione Iniziale- Posizione Finale- Velocità Iniziale- Velocità Finale- Accelerazione Iniziale- Accelerazione Finale

Con c vincoli serve un polinomio di grado c-1.

nntatataat ++++= ...)( 2

210θ

Polinomio Cubico: solitamente è un buon compromesso in termini qualità della traiettoria e tempo di calcolo per determinarla.Per definire un polinomio cubico servono 4 vincoli:

Es: si consideri il giunto di un robot, supponiamo di muoverlo tra due posizioni :? t1=10°, ? t2=60°, in un tempo pari a 10s, il giunto deve partire fermo e arrivare nella posizione finale fermo. Determinare i parametri della cubica.

33

2210)( tatataat +++=θ

7

1

2

8

Grafici:θ (0) = 10°θ (tf) = 60°.θ(0) = 0 °/s.θ(tf) = 0 °/stf=10s

32

3

2

1

0

1.05.110)(

1.01000

50*2

5.1100

50*3010

ttt

a

a

aa

−+=

−=−

=

==

==

ϑ

)(tϑ )(tϑ&

30

3

20

2

1

00

)(2

)(30

f

f

f

f

ta

ta

aa

ϑϑ

ϑϑ

ϑ

−−=

−=

==

La forma parametrica della soluzione è:

taat

tataat

tatataat

32

2321

33

2210

62)(

32)(

)(

+=

++=

+++=

ϑ

ϑ

θ

&&

&

θ (0) = θ0θ (tf) = θf.θ(0) = 0.θ(tf) = 0

Angolo pari a = θ0 per t=0Angolo pari a = θf per t=tf

Velocità nulla iniziale

Velocità nulla finale

9

ES2: Passaggio per punti di via senza arrestarsi: - abbiamo più punti di via- vogliamo che il manipolatore non si arresti nei punti di via P0 e Pf

θ (0) = θ0θ (tf) = θf. θ (0) = θD0. θ (tf) = θDf

1

10

2

3

11

Forma parametrica della soluzione:

θ (0) = θ0θ (tf) = θf. θ (0) = θD0. θ (tf) = θDf 2

321

10

33

2210

00

32 ffDf

D

ffff

tataa

a

tatataa

a

++=

=

+++=

=

θ

θ

θ

θ

Parametri Cubica:

20

30

3

02

02

01

00

)()(2

2)(3

f

DDf

f

f

f

Df

f

D

f

f

D

tta

ttta

a

a

θθϑϑ

θθθθθθ

++

−−=

−−−

=

==

ESEMPIO: Traiettoria rettilinea nello spazio cartesiano

1. Fissare il numero dei punti di via

2. Per ogni punto applicare la cinematica inversa

3. Per ogni coppia di valori per il singolo giunto calcolare la cubica

4. Supponiamo che le velocità dei giunti nei punti di via siano nulle

x

y

1ϑ

2ϑ

s g

0)(

0)0(

=

=

ftθ

θ&

&

12

Robot Planare

Cinematica Diretta

x

y

1ϑ

2ϑ

l1

l2

12211

12211

slslyclclx

+=+=

( )

)cll,satan2(l-x)atan2(y,

),(2atan 1 , 2

x

)()(

221221

222222

21

22

21

22

2

212211

212211

22

+=

=⇒−=

−−+=

+++=+

θ

θ CSCSll

llyC

slslclclyx

Cinematica Inversa βαβαβαβαβαβα

sensensensensen

⋅⋅=±⋅±⋅=±

mcoscos)cos(coscos)(

y

x

0°

90°

+/-180°

13

322

35241

0053.0079.06.2)(

105.1103.225.0)(

ttt

ttt

+−=

⋅−⋅−= −−

θ

θ

1) N=0 (solo s e g)

s=(0,5), g=(20,5)

La retta che passa per questi punti è y=ax+c

y=5

CinInv(0,5)=(14°,151°)

CinInv(20,5)=(14°,0°)

a0=0.25 b0=2.6

a1=0 b1=0

a2=-2.3*10-4 b2=-0.0791

a3=-1.54*10-5 b3= 0.0053

2

1

2

1

2

1

θ

θ

θ

θ

θθ

&&

&&

&

&

14

2) N=1

3) N=2

Scelta della velocità nei punti di via: ci sono vari metodi:

1) L’utente sceglie le velocità nello spazio cartesiano e utilizzando lo jacobiano inverso vengono calcolate le velocità dei giunti (pesante computazionalmente, al variare di θ va calcolato nuovamente).

Lo jacobiano J è una matrice. Supponiamo di conoscere la cinematica diretta del robot, F.

Xr&

r& 1−= Jϑϑ

r&

r& ⋅= JX

Per variazioni piccolepossiamo approssimare la F con la derivata

⋅

∂∂∂∂∂∂

∂∂∂∂∂∂∂∂∂∂∂∂

=

nnnnn

n

n

n ??

????

?/f?/f?/f

?/f?/f?/f?/f?/f?/f

?X

?X?X 1

1

1

11111

......

..................

...2

2

2222

2

2

J

),...,(

),...,(

)]([)(

1

111

nfX

nfX

tFtX

nn ϑϑ

ϑϑ

ϑ

=

=

=

M

15

=

z

y

x

z

y

x

VVV

X

ωωω

&

Se dividiamo per ∆t, e facciamo tendere l’intervallo di tempo a zero:

Normalmente il vettore è formato da sei componenti, le prime tre sono le velocità lineari dell’EndEffector e le altre tre le sue velocità angolari.

X&

Esempio) Calcolo jacobiano (Robot planare 2DOF)

x

y

1ϑ

2ϑ

(x,y)

l1

l2

16

Jacobiano (Robot planare 2DOF)

++−−−

=

+=+=

12212211

1221221121

12211

12211

),(clclclslslsl

J

slslyclclx

ϑϑ

Oss) L’inversa di una matrice esiste solo se il determinante è diverso da zero.

)()()()( 1221112212212211 clclslclslslJ +⋅−−⋅−−=

Il determinante si annulla per: 180 e 0 21 == ϑϑ