ANNO ACCADEMICO

UNIVERSIT À

Corso di Laurea Magistrale in Ingegneria dell’Automazione

Allievi:

Roberto Rabbeni

Stefania Maria Collura

ANNO ACCADEMICO 2012/2013

À DEGLI STUDI DI PALERMO

Facoltà di Ingegneria

Corso di Laurea Magistrale in Ingegneria dell’Automazione

Tesina di Robotica Industriale

Docente:

Prof. Ing. Filippo D’Ippolito

DEGLI STUDI DI PALERMO

Corso di Laurea Magistrale in Ingegneria dell’Automazione

Tesina di Robotica Industriale

Docente:

Filippo D’Ippolito

2

3

Introduzione

Nel seguente elaborato verrà affrontato lo studio del manipolatore robotico di Fig.1.

In un primo momento si effettuerà l'analisi cinematica, dalla quale verranno evidenziate le

singolarità del robot; si passerà poi allo studio della dinamica, volta a definire il modello

matematico della struttura robotica, ed infine basandosi sul modello ottenuto, si procederà con una

particolare tecnica di controllo nello spazio dei giunti. Verranno quindi mostrati i risultati ottenuti in

simulazione.

Fig. 1: Manipolatore robotico

4

1. Cinematica

1.2 Cinematica Diretta Manipolatore

Per analizzare il problema della cinematica diretta del manipolatore è stata utilizzata la convenzione

di Denavit-Hartenberg:

Figura 2: Schema cinematico del manipolatore

Una volta fissati i sistemi di riferimento, seguendo la procedura DH, è possibile definire la seguente

tabella:

Link ai αi di θi

1 2 3

0 0 0

0 -�

�

0

d1

0 d3

0 θ2

0

Figura 3: Tabella di Denavit-Hartenberg manipolatore

5

Si calcolano le matrici di trasformazione omogenea parziali �����, con i= 1,…,4:

��� = �1 0 0 0

0 1 0 0

0 0 1 ��

0 0 0 1

�

��� = ��� 0 −�� 0�� 0 �� 0

0 −1 0 0

0 0 0 1

�

��� = �1 0 0 0

0 1 0 0

0 0 1 ��

0 0 0 1

�

La matrice di trasformazione omogenea tra il sistema 4 e il sistema 0 è quindi data da:

���� = ��

������

� = ��� 0 −�� −������ 0 �� ����0 −1 0 ��

0 0 0 1

� L'equazione della cinematica diretta del manipolatore è:

���� =����−�3�2�3�2�1�

�

1.3 Cinematica Inversa Manipolatore

Il problema della cinematica inversa consiste nel calcolo delle variabili di giunto ��, � e ��.

La posizione dell’organo terminale è individuata a mezzo di tre coordinate ��, ��e��che

corrispondono agli elementi della quarta colonna della matrice di trasformazione e l’angolo di

orientamento è individuato dall’angolo � .

6

� � ��� = −������ = ������ = ��� = �� + �

2

� Quadrando e sommando le prime due equazioni si ottiene: �3 = ��

2 + �

2

�2 = −��3�2 = �

�3

da cui, applicando la funzione Atan2: �� = ����2(��, ��) 1.4 Cinematica Differenziale

Lo scopo della cinematica differenziale diretta è quello di calcolare velocità lineari e angolari dei

corpi che costituiscono il manipolatore (in particolare dell'end-effector) in funzione delle velocità

dei giunti.

La cinematica differenziale si basa sul calcolo dello Jacobiano geometrico, il quale si effettua

considerando il contributo della velocità di ogni singolo giunto al moto dell’organo terminale,

considerando tutti gli altri giunti bloccati, e sommando insieme tutti i contributi.

Ognuno dei contributi dei giunti al moto dell’organo terminale avrà un’espressione del tipo �(�)�� , dove � `e la colonna i-esima dello Jacobiano.

Per calcolare le colonne dello jacobiano, calcoliamo adesso i vari contributi �(�)�� , distinguendo il

caso in cui il giunto �� e rotoidale dal caso in cui il giunto `e prismatico:

• Giunto rotoidale(�� = �):

���()��()� = �� �� × (�� − � ��)� �� � • Giunto prismatico(�� = ��):

���()��()� = �� ���

7

Nel nostro caso lo Jacobiano geometrico vale:

� =������0 −���� −��0 −���� ��1 0 0

0 0 0

0 0 0

0 1 0 � ∈ ℜ

6�3

Eliminando le righe nulle (4° e 5°) si ottiene lo Jacobiano analitico:

�� = �0 −���� −��0 −���� ��1 0 0

0 1 0

� ∈ ℜ4�3

Si osserva che sono nulle le velocità angolari rispetto a x e y, mentre sono tutte non nulle le altre

velocità (lineari e angolare rispetto a z). Attraverso JA è possibile effettuare l'analisi delle

singolarità, in particolare calcolando il determinante dei seguenti minori:

det������,∶� = −�3, det������,∶

� = 0,det������,∶� = �2.

Si conclude allora che non ci sono singolarità complessive del manipolatore, ma soltanto una

singolarità relativa alla sola parte posizionale (prime tre righe) data da �� = 0.

8

2. Dinamica

La deduzione del modello dinamico di un manipolatore gioca un ruolo di estrema importanza in

relazione a problemi di simulazione del moto, di analisi di strutture di manipolazione e di

determinazione di algoritmi di controllo.

La simulazione del moto di un manipolatore consente di provare strategie di controllo e tecniche di

pianificazione di traiettoria senza la necessità di riferirsi a una struttura di manipolazione

fisicamente disponibile.

L’obiettivo della dinamica è pertanto la derivazione delle equazioni del moto di un manipolatore

nello spazio dei giunti.

Utilizzando la formulazione di Lagrange si ottiene il seguente modello (trascurando le masse dei

motori): ����� + ���,�� ��� + ���� + ���� = �

dove

• B è la matrice d’inerzia;

• C è la matrice delle azioni centrifughe e di Coriolis;

• g è il vettore delle azioni di gravità;

• � è la forza/coppia ai giunti.

2.1 Determinazione del modello

� ��� = ∑ (���� ���(�)��(�) + ��(�)������ �����(�))

Dove:

• ��� è la massa del braccio;

• � � = ���������è il tensore d’inerzia;

• �� è la matrice di rotazione della terna solidale al braccio i alla terna base.

• ���(��) = � �������� × (��� − ����)� • ���

(��) = � ���� �

per un giunto prismatico

per un giunto rotoidale

per un giunto prismatico

per un giunto rotoidale

9

Jacobiano traslazionale:

Giunto 1:

��(��) = !001

0

0

0

0

0

0

"

Giunto 2: ��(��) = ��(��) Giunto 3:

��(��) = !001

−����� − #��−����� − #��

0

−����0"

Jacobiano rotazionale:

Giunto 1:

��(��) = !000

0

0

0

0

0

0

"

Giunto 2:

��(��) = !000

0

0

1

0

0

0

"

Giunto 3: ��(��) = ��(��)

��� =$(

�

���%����(��)���(��) + ��(��)���&��������(��) =

= %����(��)���(��) + ��(��)������� �����(��) +%����(��)���(��) + ��(��)������� �����(��) +%����(��)���(��) + ��(��)������� �����(��)

10

�� = !1 0 0

0 1 0

0 0 1

" ; �� = !�� 0 −���� 0 ��0 −1 0

" ; �� = �� ��(��)������� �����(��) =

��(��)������� �����(��) = !0 0 0

0 &�� 0

0 0 0

"

��(��)������� �����(��) = !0 0 0

0 &�� 0

0 0 0

"

�� = % 1 +% 2 +% 3 0 0

0 % 3�3− #3�2+& 2 + & 3 0

0 0 % 3

�

� '�(, (� � = ∑ ∑ ℎ���(�)����

���� (��(��

dove:

• ℎ��� =�� )�������

+�������

−�������

* Sono i simboli di Christoffel.

I termini non nulli sono:

ℎ��� = %����� − #�� = ℎ

ℎ��� = ℎ���

ℎ��� = −ℎ���

'�(, (� � = +0 0 0

0 ℎ��� ℎ,��0 −ℎ,�� 0

-

11

� .���� = −∑ %������ ������(��)

Per il calcolo dei termini gravitazionali, essendo:

�� = /0 0 −9,810

1��� = !%�� +%�� +%��0

0

"

� La matrice dei coefficienti di attrito viscoso è la seguente:

�� = !2�� 0 0

0 2�� 0

0 0 2��"

� = !��3���" , � = !4�5�4�"

12

3. Pianificazione della traiettoria

Con la pianificazione della traiettoria s'intende stabilire la modalità con cui si vuole che evolva il

movimento del manipolatore, da una postura iniziale ad una postura finale. Si tratta di definire sia il

percorso geometrico sia la legge di moto da realizzare (ossia la dipendenza temporale di posizioni,

velocità ed accelerazioni) .

La corretta pianificazione della traiettoria ha estrema importanza, perché comporta che la traiettoria

possa essere eseguita da parte del sistema di controllo del moto in anello chiuso, senza incorrere nei

limiti di saturazione degli attuatori o provocare dannose sollecitazioni meccaniche alla struttura.

In generale ci si pone l’obiettivo di pianificare traiettorie a curvatura quanto più possibile regolare.

Nelle simulazioni è stato implementato una pianificazione della traiettoria nello spazio dei giunti di

tipo punto-punto, dove vengono specificate solo le condizioni iniziali e finali sulla posizione, sulla

velocità, sull' accelerazione e sul tempo di percorrenza.

Si possono prendere in considerazione funzioni polinomiali del tipo: (�6� = 7� + 7�6 + 7�6� +⋯+ 7�6�

Più alto è il grado n del polinomio, più condizioni al contorno si possono soddisfare e più regolare

sarà la traiettoria.

Nello specifico si è scelto, per imporre la legge di moto su un giunto, il polinomio cubico (terzo

grado) poiché minimizza l'indice di qualità: (�6� = 7�6� + 7�6� + 7�6 + 7�

A cui sono associate una velocità con andamento parabolico: (� �6� = 37�6� + 27�6 + 7�

E un’accelerazione con andamento lineare: (� �6� = 67�6 + 27�

La disponibilità di quattro coefficienti consente di imporre:

1. Valori iniziale �� e finale �� della variabile di giunto;

2. Velocità iniziale ��� e finale ���.

La determinazione della traiettoria è fornita dal seguente sistema di equazioni: 7� = (� 7� = (�� 7�6�� + 7�6�� + 7�6� + 7� = (�

37�6�� + 27�6� + 7� = (��

13

Da cui si ricavano i coefficienti: 7� = (� 7� = (�� 7� = 3

(� − (��6� − (�6�� −(�� − (��6�

7� = −2(� − (��6� − (�6�� +

(�� − (��6��

Una volta ricavati i coefficienti che permettono di generare la traiettoria desiderata q(t) quest’ultima

sarà inviata come riferimento al sistema di controllo che provvederà a fare inseguire al manipolatore

tale ingresso.

14

4. Controllo del moto

Il problema del controllo consiste nella determinazione dell’andamento di segnali di ingresso

(coppie oppure tensioni di alimentazione) per ottenere un comportamento desiderato dal

manipolatore.

Solitamente si desidera controllare la posizione (ed eventualmente l’orientamento) dell’end-effector

visto che è la parte del robot che fisicamente esegue il lavoro.

4.1 Controllo adattativo

Di particolare interesse è il controllo adattativo quando la struttura del modello dinamico è

ragionevolmente nota ma c’è incertezza sui parametri del modello stesso.

Il controllo adattativo si basa sulla proprietà di linearità del modello dinamico del manipolatore nei

parametri dinamici. Infatti il modello dinamico del robot può scriversi nella forma:

����� + ���,�� ��� + ���� + ���� = 8��,�� ,�� �9 = :

dove � è un opportuno vettore costante di parametri.

Apparentemente sembrerebbe che per il calcolo del regressore Y occorre conoscere l'accelerazione

dei giunti attraverso misure o stime, il che naturalmente costituirebbe una notevole limitazione al

suo utilizzo pratico. In realtà la tecnica che verrà presentata nel seguito, dovuta a Slotine e Li,

risolve il problema senza la necessità di informazioni sulle accelerazioni. Si consideri dapprima la

seguente legge di controllo, in cui si suppone noto senza incertezza il modello dinamico: � = ����� � + ���,�� ��� � + ����+; <

con KD matrice definita positiva. Si ponga: �� � = �� ! + =�>�� � = �� ! + =�>� con � definita positiva (si può prendere diagonale) che consente di esprimere i termini di

compensazione non lineare e di disaccoppiamento in funzione di velocità e accelerazioni desiderate

aggiornate sulla base dello stato corrente del manipolatore. Se ora poniamo: < = �� � − �� = �>� + =�>

l’azione ��� equivale ad un’azione PD sull’errore.

Complessivamente l’adozione della legge di controllo comporta: ���<� + ���,�� �<+; < =

15

Si assuma come funzione di Lyapunov candidata la seguente espressione:

?�<,�>� = @A<� ���<+@A�>�B�> > 0∀C,�> ≠

con M matrice definita positiva. Derivando, inoltre si dimostra che �� < 0. Quindi lo stato

����,<��� = 0 risulta globalmente asintoticamente stabile.

Consideriamo ora una legge di controllo basata su stime dei parametri: � = D����� � + �D��,�� ��� � + �E���+; < = 8��,�� ,�� �,�� ��9E +; <

Si osservi che � non dipende dalle accelerazioni dei giunti. Sostituendo nel modello dinamico: ���<� + ���,�� �<+; < = − F����� � − �F��,�� ��� � − �>��� = 8��,�� ,�� �,�� ��9>

dove: F = D − �F = �D − ��> = �E − �9> = 9E − 9

Modifichiamo ora la funzione candidata di Lyapunov:

G�<,�>� = @A<� ���<+ �>�=; �> +@A9>�;"9> > 0∀C,�>,9> ≠

con �� matrice definita positiva. Derivando e adottando come legge dei parametri: 9E� = ;"��8���,�� ,�� �,�� ��<

risulta, tenendo conto che � è costante: G� = −�>� �; �>� − �>�=; �>

Pertanto �� e ��� convergono asintoticamente (e globalmente) a zero.

Dalla relazione: ���<� + ���,�� �<+; < = 8��,�� ,�� �,�� ��9>

si ottiene quindi che la quantità: 8��,�� ,�� �,�� ���9E − 9� →

Questo non implica necessariamente che �� tenda a �, la qual cosa dipende dalla struttura della

matrice Y. Si tratta in effetti di un controllo adattativo diretto, mirato all’individuazione di una

legge di controllo efficiente, piuttosto che alla identificazione dei parametri del modello.

16

Lo schema di controllo risultante è il seguente:

Figura 4: Schema di controllo adattativo nello spazio dei giunti

Nella legge di controllo si individuano quindi tre elementi:

1. ��� riconducibile ad un’azione a dinamica inversa approssimata;

2. ��� azione stabilizzante di tipo PD sull’errore;

3. Il vettore di stima dei parametri � viene aggiornato secondo una tecnica a gradiente. La

matrice �� determina la velocità di convergenza delle stime.

Per il manipolatore in esame le matrici ��,�� ,�� � e � valgono:

���,�� ,�� � = ���� + 0 0 0 0 0

0 ��� ��� ������� + ��� ���� + ����� −2���� � − ��� ���� + ���� ���0 0 0 ��� − ������ ���� 0

�

9 =

������%� +%� +%�&�&�%�%�#�%�#�� �

Quindi la matrice ���,�� ,�� ,�� � necessaria per il controllo adattativo è:

17

#$%,%& ,%& �,%' �( =)**+,'�,� + 0 0 0 0 0

0 ,'�, ,'�, �3-�

3,'�, + �

3& $,&�, + ,&�,(. −2,'�,�3− ,&�,$,&�,� + ,&�,( ,'�,

0 0 0 ,'�, − �3�2& ,&�, �

2& ,&�, 0 /

001

5. Applicazione Matlab/Simulink

Nelle figure successive vengono riportati i grafici della posizione, velocità ed accelerazione dei

giunti per il controllo adattativo.

Per il controllo adattativo i parametri dati al controllore sono:

H2 = !500 0 0

0 1000 0

0 0 400

"

H3 = !100 0 0

0 100 0

0 0 100

" Λ = !300 0 0

0 100 0

0 0 100

"

Lo schema realizzato in ambiente Matlab/Simulink è il seguente:

Figura 5: Schema di controllo adattativo nello spazio dei giunti in ambiente simulink

Xd

qd

retroazione negativa-C-

vel_in

-C-

vel_fin

1

t_fin

0

orientamento_in

0

orientamento_fin

Pos_in

Pos_f in

v el_in

v el_f in

orientamento_in

orientamento_f in

t_f in

q(t)

inversione cinematica

acc_1

vel_3

vel_2

vel_1

pos_3

t

To Workspace12

acc_3

acc_2

pos_2

pos_1

Scope3

Scope2

Scope1

-C-

Pos_in

-C-

Pos_fin

tau

q_bipunto

q_punto

q

Manipolatore

[Z]

[Y]

[X]

[Z]

[Y]

[X]

In Out

Filter

du/dt

Derivative1

du/dt

Derivative

q

q_punto

qd

qd_punto

qd_bipunto

u

Controllo

Clock

18

Figura 6: Blocco di inversione cinematica e di pianificazione della traiettoria

Figura 7: Blocco che implementa il controllo adattativo

q(t)

q(t)=a3*t^3+a2*t^2+a1*t+a0

1

q(t)

Pos

orientamento

d1

theta2

d3

cinematica_inversaper posa iniziale

Pos

orientamento

d1

theta2

d3

cinematica_inversaper posa finale

up

u

lo

y

SaturationDynamic

Ramp

Product3

Product2

Product1

Product

u2

MathFunction0

Constant

Pos_in

Pos_f in

v el_in

v el_f in

t_f in

a0

a1

a2

a3

Calcolo_coeff_polinomiocubico

pianificazione punto-punto

Add

7

t_fin

6

orientamento_fin

5

orientamento_in

4

vel_fin

3

vel_in

2

Pos_fin

1

Pos_in

1

u

uT

Transpose

q

q_punto

qr_punto

qr_bipunto

Y

Subsystem2

q

q_punto

qr_punto

qr_bipunto

Y

Subsystem1

MatrixMultiply

Product5

MatrixMultiply

Product4

MatrixMultiply

Product3

MatrixMultiply

Product2

MatrixMultiply

Product1

MatrixMultiply

Product

1s

Integrator

delta

Constant3

delta

Constant2

Kd

Constant1

inv_K_pi

Constant

5

qd_bipunto

4

qd_punto

3

qd

2

q_punto

1

q

19

Figura 8: Blocco che implementa il modello del manipolatore

1) [B +B(q)]*q_bipunto + C(q,q_punto)*q_punto+Fv*q_punto+G(q)=Tau

Modello manipolatore

2) q_bipunto=B^-1[Tau - B(q)*q_bipunto-C(q,q_punto)*q_punto-Fv*q_punto-G(q)]

3

q

2

q_punto

1

q_bipunto

Fv

attrito

q_punto

q

C(q,q)q_punto

q_bipunto

q

B(q)

MatrixMultiply

Product1

MatrixMultiply

Product

1s

Integrator1

1s

Integrator

G

Constant2

9.81*(m1+m2+m3)

Constant1

inv(B)

Constant

Add1

1

tau

20

Figura 9: Posizione dei giunti 1,2,3. In verde il riferimento desiderato in blu invece l'andamento effettivo