Indice
Introduzione
3
1 Leggi anti-windup per manipolatori robotici
6
1.1
Definizione del problema . . . . . . . . . . . . . . . . . . . . . . . .
6
1.2
Soluzione anti-windup non lineare . . . . . . . . . . . . . . . . . . .
9
1.3
Considerazioni sulla soluzione anti-windup
1.4
non lineare . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .
11
Prestazione anti-windup . . . . . . . . . . . . . . . . . . . . . . . .
12
2 Calcolo delle equazioni dinamiche del moto di un manipolatore
17
2.1
Introduzione . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .
17
2.2
Formulazione di Eulero-Lagrange . . . . . . . . . . . . . . . . . . .
18
2.3
Cinematica e dinamica di un manipolatore . . . . . . . . . . . . . .
19
2.3.1
Coordinate omogenee . . . . . . . . . . . . . . . . . . . . . .
19
2.3.2
Link, giunti e loro parametri . . . . . . . . . . . . . . . . . .
21
2.3.3
Rappresentazione di Denavit-Hartenberg . . . . . . . . . . .
23
2.3.4
Trasformazione omogenea associata ad un link . . . . . . . .
24
2.3.5
Jacobiano geometrico . . . . . . . . . . . . . . . . . . . . . .
26
2.3.6
Derivata di una matrice di rotazione . . . . . . . . . . . . .
27
2.3.7
Calcolo dello Jacobiano geometrico . . . . . . . . . . . . . .
28
2.3.8
Energia cinetica di un corpo rigido tridimensionale che si
2.3.9
muove nello spazio . . . . . . . . . . . . . . . . . . . . . . .
29
Energia potenziale di un manipolatore . . . . . . . . . . . .
31
1
INDICE
2.4
Procedura e per il calcolo automatico dei ’coefficienti dinamici’
. .
3 SimMechanics
32
49
3.1
Introduzione . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .
49
3.2
Che cos’è SimMechanics . . . . . . . . . . . . . . . . . . . . . . . .
49
3.3
Creare un modello SimMechanics . . . . . . . . . . . . . . . . . . .
50
3.4
Librerie . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .
52
3.5
Blocchi . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .
53
3.5.1
Body . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .
53
3.5.2
Ground . . . . . . . . . . . . . . . . . . . . . . . . . . . . .
55
3.5.3
Revolute . . . . . . . . . . . . . . . . . . . . . . . . . . . . .
56
3.5.4
Joint Actuator . . . . . . . . . . . . . . . . . . . . . . . . .
58
3.5.5
Joint Sensor . . . . . . . . . . . . . . . . . . . . . . . . . . .
60
3.5.6
Joint Initial Condition Actuator . . . . . . . . . . . . . . . .
62
3.5.7
Continuous Angle . . . . . . . . . . . . . . . . . . . . . . . .
63
Modello SimMechanics del robot Siemens-Manutec r3 . . . . . . . .
64
3.6
4 Simulazioni
71
4.1
Introduzione . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .
71
4.2
Congruenza dei modelli del robot . . . . . . . . . . . . . . . . . . .
71
4.3
Anti-windup applicato al robot SM-r3 . . . . . . . . . . . . . . . . .
78
4.3.1
Soluzione non lineare . . . . . . . . . . . . . . . . . . . . . .
78
4.3.2
Soluzione anti-windup generallizzata . . . . . . . . . . . . .
83
5 Conclusioni
87
Bibliografia
89
INDICE
2
Introduzione
L’automazione è un complesso di tecniche volte a sostituire l’intervento umano,
o a migliorarne l’efficienza, nell’esercizio di dispositivi e impianti. Un importante settore della scienza dell’automazione o automatica è costituito dalla disciplina
denominata controlli automatici. Esso si riferisce allo studio dei dispositivi (detti
regolatori o dispositivi di controllo), mediante i quali si fanno variare automaticamente le grandezze liberamente manipolabili di un sistema (detto sistema controllato) in modo che subisca la migliore possibile evoluzione nel tempo: la maggiore
o minore “bontà” dell’evoluzione nel tempo viene valutata con criteri che sono di
volta in volta specificati e sui quali si basa il progetto dei dispositivi di controllo
medesimi. Nell’ambito della progettazione dei dispositivi di controllo può accadere che un controllore funzioni ottimamente per una certa gamma di valori del
segnale di riferimento, ma abbia una notevole perdita di prestazioni nel caso in
cui il suddetto segnale sia anche di poco al di fuori di tale gamma. Ciò si verifica
perché l’uscità del controllore è applicata a dispositivi di attuazione che presentano
saturazione, cioè ammettono segnali di controllo contenuti in un intervallo di valori
compresi tra un massimo ed un minimo. E’ opportuno evitare che, nel caso in cui
la variabile di controllo saturi, si verfichi il cosiddetto fenomeno del “windup”. Il
windup è particolarmente evidente se nel controllore è presente un’azione di tipo
integrale, ciò è dovuto al fatto che nel controllore si continua ad integrare l’errore
anche quando questo è molto grande per via della saturazione, facendo cosı̀ crescere eccessivamente i valori numerici assunti dalle variabili di stato. Il termine
windup si riferisce proprio a questo comportamento delle variabili di stato, che
crescono spropositatamente verso valori troppo elevati. Il windup provoca spesso
3
Introduzione
l’insorgenza di fenomeni oscillatori molto marcati o, a volte, di risposte divergenti
che possono presentarsi anche in assenza di azione integrale. Negli anni passati,
particolare attenzione è stata rivolta allo studio del problama windup per processi
lineari, in cui è nota la struttura del controllore che garantisce le proprietà di stabilità, asintotica globale ed esponenziale locale, del processo del sistema privo di
saturazione. In questo tipo di problemi si cerca di eliminare la perdita di prestazioni e spesso l’instabilità che si manifestano non appena connettiamo il dispositivo
di controllo in controreazione al processo lineare, che risente, in questo caso, della
saturazione sugli ingressi. Sostanzialmente la soluzione consiste nella progettazione
di un compensatore anti-windup che esplichi un’azione che sommata a quella del
controllore consenta di:
1. preservare la risposta lineare del sistema lineare a ciclo chiuso senza saturazione finché non vengono raggiunti i limiti di saturazione;
2. garantire quanto più possibile il recupero della risposta del sistema lineare a
ciclo chiuso qualora vengano raggiunti i limiti di saturazione.
Un risultato simile a quello appena visto vale anche per i sistemi di controllo non
lineari per processi non lineari, come ad esempio i manipolatori robotici. In questo
caso anche se non consideriamo la saturazione abbiamo comunque a che fare con
un processo non lineare. Tuttavia il sistema è spesso caratterizzato da alcune
proprietà notevoli di linearizzabilità che suggeriscono delle interessanti tecniche
per il progetto delle leggi di controllo non lineari, necessarie come prima per:
1. preservare la risposta detta unconstrained (del sistema non soggetto a saturazione), relativa al sistema a ciclo chiuso ottenuto dall’interconnessione del
controllore e del processo non lineari, fintantoché non vengano raggiunti i
limiti di saturazione imposti dagli attuatori dei dispositivi di controllo;
2. garantire quanto più possibile il recupero della risposta unconstrained del
sistema non lineare a ciclo chiuso non appena vengano raggiunti i limiti di
saturazione.
Introduzione
4
Introduzione
L’oggetto del presente lavoro di tesi è proprio lo studio simulativo di leggi antiwindup per manipolatori robotici, in particolare per il robot industriale SiemensManutec r3. Nel seguito il robot verrà descritto mediante un sistema di equazioni
dinamiche ricavate tramite una procedura automatica di calcolo, implementata con
il software per l’analisi simbolica e numerica, M aple 8.0. La scelta di questo particolare robot è legata alla possibilità di verificare la correttezza dei risultati forniti
dalla procedura di calcolo automatico dei coefficienti dinamici delle equazioni di
moto del manipolatore. All’interno di un file dimostrativo del toolbox MATLAB
SimMechanics, si trova infatti un modello molto dettagliato del robot SiemensManutec r3. Inoltre, il fatto di poter verificare la “fedeltà” del modello ottenuto
permette anche di garantire la validità e l’efficienza delle leggi anti-windup per
i manipolatori robotici, le cui equazioni sono il risultato dell’applicazione diretta
della formulazione di Eulero-Lagrange per sistemi non conservativi.
Gli argomenti trattati nella tesi sono organizzati in quattro capitoli. Nel primo
capitolo viene considerato in dettaglio il problema del windup per i manipolatori robotici e vengono forniti i risultati generali per la progettazione ed il tuning
dei parametri caratteristici del compensatore anti-windup. Nel capitolo due viene
invece richiamta la teoria della cinematica e dinamica di un manipolatore, per spiegare come è stata implementata la procedura di calcolo automatico dei coefficienti
dinamici delle equazioni di moto di un manipolatore. Il terzo capitolo costituisce una breve guida introduttiva all’uso del toolbox MATLAB SimMechanics. Lo
studio delle principali caratteristiche di questo programma è fondamentale per la
comprensione del modello SimMechanics del robot Siemens-Manutec r3. L’ultimo
capitolo è dedicato alle simulazioni delle leggi anti-windup applicate ai modelli del
robot SM r3 e al commento dei risultati forniti dal simulatore Simulink 5.0, del
M AT LAB R13.
Introduzione
5
Capitolo 1
Leggi anti-windup per
manipolatori robotici
1.1
Definizione del problema
Si considera il problema della saturazione degli attuatori per i sistemi di controllo
dei manipolatori robotici rigidi. Dato un manipolatore appartenente a questa
categoria, se indichiamo con q ∈ Rn il vettore delle variabili di giunto e con q̇ ∈
Rn il vettore delle corrispondenti velocità di giunto, possiamo scrivere l’equazione
dinamica che descrive il moto del robot come:
B(q)q̈ + C(q, q̇)q̇ + R(q)q̇ + h(q) = τp
(1.1)
dove B(q) è la matrice generalizzata d’inerzia, C(q, q̇) rappresenta la matrice dei
termini centrifughi e di Coriolis, h(q) è il vettore delle forze gravitazionali, τ p il
vettore delle forze/coppie esterne applicate ai giunti del robot.
Per dimostrare la soluzione del problema in esame, sono necessarie delle assunzioni sulla regolarità delle matrici che caretterizzano l’equazione (1.1). Le seguenti
assunzioni derivano dai principi classici che governano i sistemi meccanici.
Assunzione 1:
1. la matrice generalizzata d’inerzia q 7→ B(q) è simmetrica e differenziabile
con derivata continua, inoltre esistono dei numeri positivi λM e λm tali che
λm I ≤ B(q) ≤ λM I per ogni q ∈ Rn (I è la matrice identità);
2. la funzione (q, q̇) 7→ C(q, q̇) è continua;
6
§ 1.1 Definizione del problema
3. il vettore delle forze gravitazionali q 7→ h(q) è localmente Lipschitziano;
4. la funzione q 7→ R(q) è localmente Lipschitziana e definita positiva.
Per un manipolatore robotico descritto dalla (1.1), per il quale valgano le Assunzioni 1, il controllore, che d’ora in poi chiameremo ’unconstrained controller’ dovrà
garantire, una volta connesso in controreazione al robot senza saturazione sugli ingressi, la stabilità asintotica globale (GAS) e la stabilità esponenziale locale (LES)
del sistema a ciclo chiuso. L’unconstrained controller (1.2) è un controllore PID a
controreazione linearizzante, che garantisce, quando non interviene la saturazione,
il desiderato comportamento a ciclo chiuso (GAS,LES), e regola lo stato nel punto
richiesto dal riferimento:
ẋc = q − r
τp = B(q) (−Kp (q − r) − Kd q̇ − Ki xc )
(1.2)
+C(q, q̇)q̇ + R(q)(q̇) + h(q)
dove xc ∈ Rn è lo stato del controllore e Kp , Kd , Ki sono delle matrici quadrate
(tipicamente diagonali) scelte in maniera tale che la matrice:


0
I
0
 0
0
I 
−Kp −Kd −Ki
(1.3)
che descrive il sistema (lineare) a ciclo chiuso (1.1), (1.2), sia Hurwitz. Quindi,
sulla base del valore fornito in ingresso, dal riferimento di posizione r ∈ R n , il
controllore (1.2) è in grado di garantire la stabilità asintotica globale del punto
(q, q̇) = (r, 0) quando è interconnesso al robot (1.1).
La scelta del tipo di controllore non è univoca, molte sono le alternative possibili. Effettivamente, l’unica condizione che il controllore deve soddisfare, è quella
di garantire, quando non interviene la saturazione, la stabilità asintotica globale
e la stabilità esponenziale locale. In alternativa, quindi, potremmo scegliere un
controllore PD con compensazione della gravità, particolarmente adatto alla soluzione del problema di regolazione dei processi in un punto. Tuttavia in seguito ci
Capitolo 1 Leggi anti-windup per manipolatori robotici
7
§ 1.1 Definizione del problema
riferiremo sempre alla struttuta del controllore (1.2), perchè è quello che permette
di illustrare, nel modo migliore, le proprietà locali delle leggi di compensazione
anti-windup.
In questo contesto, per caratterizzare le nonlinearità dell’ingresso del manipolatore
(1.1), viene usata la funzione vettoriale di saturazione simmetrica decentralizzata.
La funzione di saturazione sat(.) : Rn → Rn :
sat(u) = [σ1 (u1 ), · · · , σn (un )]T
dove:

ui ≥ m i
 mi
−mi ui ≤ −mi
σi (ui ) :=

ui
−mi < ui < mi
(1.4)
descrive, per ogni attuatore dei giunti del robot, il valore massimo mi della coppia/forza associato alla corrispondente ’coppia motore’/’unità di potenza’.
E’ necessario che i valori massimi, mi , delle coppie/forze applicate ai giunti del
manipolatore, siano sufficienti per garantire agli attuatori un’adeguata potenza,
che permetta di compensare le forze gravitazionali agenti sui link in ogni configurazione finale assunta dal robot (corrispondente a qualsiasi q ∈ Rn con q̇ = 0).
Formalizziamo quanto appena detto con la seguente assunzione:
Assunzione 2 :Dato il vettore delle forze gravitazionali h(·) del sistema robotico (1.1), e noti i limiti di saturazione mi , i = 1, ..., n, deve essere soddisfatta la
condizione:
hMi := sup |h(q)| < mi ,
i = 1, ..., n
(1.5)
q∈Rn
In conclusione gli effetti tipici della saturazione sul comportamento del sistema a
ciclo chiuso, sono due:
1. il sistema preserva il suo comportamento originale se non vengono raggiunti
i limiti di saturazione dai segnali d’ingresso;
2. perdita di prestazione e spesso anche di stabilità quando i segnali diventano
abbastanza grandi da far intervenire la saturazione.
Capitolo 1 Leggi anti-windup per manipolatori robotici
8
§ 1.2 Soluzione anti-windup non lineare
1.2
Soluzione anti-windup non lineare
r
uc
Unconstrained yc
Controller
+ +
Saturation
Nonlinearity
up
Robot
x
v1
Anti-windup
Compensator
+
x
v2
+
Figura 1.1: Schema anti-windup per un manipolatore robotico.
Con riferimento allo schema in figura 1.1, la soluzione al problema introdotto
nel precedente paragrafo, consiste nell’inserzione di un ’compensatore anti-windup’
(AW ), che costituisce un’estensione dell’originale legge di controllo (1.2).
In accordo con la figura 1.1, denotiamo con x := (q, q̇) ∈ R2n lo stato del robot,
con yc ∈ Rn la coppia/forza comandata dal controllore e con uc = x + v2 ∈ R2n
l’ingresso misurabile del controllore. Ricevuti in input l’uscita del controllore,
lo stato e l’ingresso del processo, il compensatore anti-windup fornisce in output
due segnali v1 , v2 che vengono rispettivamente immessi in ingresso ed in uscita
al controllore. Se indichiamo con xe := (qe , q̇e ) ∈ R2n lo stato del compensatore
anti-windup, possiamo scrivere la sua equazione dinamica come:
q̈e = B −1 (q)(sat(yc + v1 ) − C(q, q̇)q̇ − R(q)q̇ − h(q))
− B −1 (q − qe )(yc − C(q − qe , q̇ − q̇e )(q̇ − q̇e )
(1.6)
− R(q − qe )(q̇ − q̇e ) − h(q − qe ))
Per i due segnali d’uscita dal blocco ’anti-windup compensator ’ si ha invece:
v1 = β(x, xe ) := h(q) − h(q − qe ) − Kg sat(Kg−1 qe ) − K0 q̇e
v2 = −xe = −(qe , q̇e )
(1.7)
dove K0 e Kg sono due matrici diagonali e definite positive che rappresentano il
cosiddetto ’tuning’ dei parametri della legge anti-windup. Gli elementi, k gi , che
Capitolo 1 Leggi anti-windup per manipolatori robotici
9
§ 1.2 Soluzione anti-windup non lineare
sono sulla diagonale della matrice Kg , devono verificare la condizione:
h Mi + k gi mi < m i ,
i = 1, ..., n
(1.8)
Si può facilmente notare, che se vale la condizione (1.5) dell’Assunzione 2, è sempre possibile trovare una matrice diagonale e definita positiva, Kg , che soddisfi la
(4.1). Ricapitoliamo quanto finora detto scrivendo le equazioni dell’intero schema
di controllo:
ẋc = q − qe − r
yc = B(q − qe ) (−Kp (q − qe − r) − Kd (q̇ − q̇e ) − Ki xc )
+ C(q − qe , q̇ − q̇e )(q̇ − q̇e ) + R(q − qe )(q̇ − q̇e ) + h(q − qe )
q̈e = B −1 (q)((τ + v1 ) − C(q, q̇)q̇ − R(q)q̇ − h(q))
− B −1 (q − qe )(yc − C(q − qe , q̇ − q̇e )(q̇ − q̇e )
(1.9)
− R(q − qe )(q̇ − q̇e ) − h(q − qe ))
τ = sat(yc + v1 )
dove v1 è dato dall’espressione (1.7). Se si sceglie di usare un controllore diverso dall”unconstrained controller ’, sarà necessario sostituire le prime due equazioni
della (1.9), relative alla dinamica stessa del controllore.
Per concludere e meglio formalizzare l’effetto prodotto dall’inserzione del compensatore anti-windup sullo schema di controllo, commentiamo il seguente teorema (di
cui non viene riportata la dimostrazione):
Teorema 1. Supponiamo che valgano le Assunzioni 1,2 e che la ’legge antiwindup’ (1.7) soddisfi la condizione (4.1). Dato un segnale di riferimento costante
r, indichiamo con (xl (t), xcl (t)) la risposta del sistema a ciclo chiuso, in assenza
di saturazione e con condizioni iniziali (xl (0), xcl (0)). Indichiamo poi con ul (t) la
corrispondente uscita del controllore. Con queste ipotesi il sistema a ciclo chiuso
(1.1), (1.9), (4.1), risultante dall’inserzione del compensaore anti-windup, è tale
che:
1. se ul (t) = sat(ul (t)),
∀t ∈ R e se (x(0), xc (0), xe (0)) = (xl (0), xcl (0), 0)
Capitolo 1 Leggi anti-windup per manipolatori robotici
10
§ 1.3 Considerazioni sulla soluzione anti-windup
non lineare
allora (x(t), xc (t), xe (t) = (xl (t), xcl (t), 0)),
∀t ∈ R, vale a dire il sistema
preserva il suo comportamento originale;
2. se definiamo x∗ := (r, 0), esiste un vettore x∗c ∈ Rn tale che il punto (x∗ , x∗c , 0)
è sia globalmente asintoticamente stabile che localmente esponenzialmente
stabile.
Il Teorema 1. stabilisce due importanti proprietà. La prima deriva dalle specifiche di progetto che definiscono lo stesso compensatore: sostanzialmente,la legge di
compensazione anti-windup deve preservare la risposta originale del sistema ogni
volta che non vengono raggiunti i limiti di saturazione imposti agli attuatori. La seconda proprietà indica che, il sistema a ciclo chiuso completato del compensatore
anti-windup, deve essere globalmente asintoticamnte (e localmente esponenzialmente) stabile, cosı̀ da eliminare gli effetti di instabilità che spesso intervengono
quando vengono raggiunti i limiti di saturazione.
1.3
Considerazioni sulla soluzione anti-windup
non lineare
Il paragrafo precedente garantisce interessanti proprietà per il sistema a ciclo chiuso
comprensivo del compensatore anti-windup, ma non fornisce alcuna indicazione
utile riguardo al comportamento transitorio del sistema, quando vengono raggiunti
i limiti di saturazione. In questo caso, infatti, il Teorema 1 assicura solo che la
triettoria del sistema a ciclo chiuso converga al punto di equilibrio desiderato, cioè
q = r, q̇ = 0. Per avere informazioni più precise sul comportamento transitorio è
conveniente modificare, come segue, il segnale v1 della legge anti-windup proposta
precedentemente:
v1 = sat(yc ) − yc + h(q) − h(q − qe ) − Kg sat(Kg−1 Kq qe ) − Kqd (qe , q̇e )q̇e
(1.10)
dove Kg è una matrice diagonale i cui elementi verificano la condizione (4.1), Kq è
una matrice diagonale e definita positiva, Kqd (·, ·) è una matrice diagonale di funzioni , i cui elementi diagonali kqdi (·, ·), i = 1, ..., n sono funzioni scalari localmente
Capitolo 1 Leggi anti-windup per manipolatori robotici
11
§ 1.4 Prestazione anti-windup
Lipschitziane tali che è possibile trovare delle opportune costanti positive, k qd , per
cui vale la seguente espressione:
kqdi (qe , q̇e ) ≥ k qd ,
∀qe , q̇e ∈ Rn , ∀i ∈ {1, ..., n}
(1.11)
Con le modifiche appena introdotte generalizziamo il risultato fornito dal Teorema
1. Con riferimento al sistema a ciclo chiuso che deriva dall’interconnessione tra le
(1.1), (1.9) e il nuovo segnale di compensazione (1.10), possiamo scrivere:
Teorema 2. Se valgono le Assunzioni 1,2 e se il segnale 1.10 verifica le condizioni 4.1 e 1.11, allora per il sistema a ciclo chiuso (1.1,1.9, 1.10), completo del
compensatore anti-windup, sono ancora valide le proprietà garantite dal Teorema 1.
Per meglio compredere quanto finora detto, nel prossimo paragrafo, verrà fornita una formalizzazione matematica dei vantaggi introdotti dal compensatore antiwindup e verrà descritto come scegliere in modo opportuno i parametri Kg , Kq e
Kqd .
1.4
Prestazione anti-windup
La qualità della risposta del sistema a ciclo chiuso, può essere misurata in base alla
deviazione che la traiettoria attuale x, presenta rispetto alla traiettoria (ideale) x l ,
del sistema a ciclo chiuso non soggetto agli effetti della saturazione.
In particolare, siamo interessati al valore assunto, in ogni istante, dall’ampiezza
del segnale (x(t) − xl (t)). In queste circonstanze, il Teorema 1 ci garantisce che,
quando il segnale di controllo, τl , non raggiunge i limiti di saturazione imposti,
l’ampiezza del segnale (x(t) − xl (t)) è identicamente uguale a zero. Tuttavia nessuna informazione viene fornita circa il possibile comportamento transitorio del
segnale (x(t) − xl (t)) di una qualsiasi altra traiettoria. D’altra parte, in base alla
continuità delle traiettorie rispetto alle condizioni iniziali su un intervallo di tempo compatto, e se consideriamo la proprietà 1 (GAS) garantita dal Teorema 1,
possiamo ragionevolmente aspettarci che la traiettoria del sistema non soggetto a
Capitolo 1 Leggi anti-windup per manipolatori robotici
12
§ 1.4 Prestazione anti-windup
saturazione, corrisponda alla traiettoria, del sistema a ciclo chiuso completo del
compensatore anti-windup, che presenta piccoli valori del segnale (x(t) − xl (t)).
Ancora una volta, però, il Teorema 1 garantisce solo delle proprietà a regime permanente. In questi casi il prossimo risultato costituisce un buon punto di partenza
per avere informazioni circa il comportamento transitorio delle altre traiettorie, e
per minimizzare in qualche modo il segnale (x(t) − xl (t)):
Teorema 3. A prescindere dalla scelta di v1 , dato un qualunque segnale di riferimento r(t), t ≥ 0, se indichiamo con (xl (t), xcl (t)) la risposta del sistema a ciclo
chiuso non soggetto alla saturazione, a partire dalle condizioni iniziali (x l (0), xcl (0))
e se indichiamo con (x(t), xc (t), xe (t)) la risposta del sistema a ciclo chiuso (1.1),
(1.9), (1.7)), a partire dalle condizioni iniziali (x(0), xc (0), xe (0)) = (xl (0), xcl (0), 0)
si ha che:
xe (t) = xl (t) − x(t),
∀t ≥ 0.
(1.12)
Dimostrazione. Si considera il sistema a ciclo chiuso (1.1), (1.9) e si opera il
cambiamento di coordinate (x, xc , xe ) → (x̃, xc , xe ), dove x̃ := x − xe . Dopodiché
˙ := x̃. Dopo aver eseguito le dovute semplificazioni si ottengono
si definisce (q̃, q̃)
le seguenti equazioni:
˙ q̃˙ + R(q̃)q̃˙ + h(q̃) − yc )
q̃¨ = −B −1 (q̃)(C(q̃, q̃)
ẋc = q̃ − r
(1.13)
˙ q̃˙ + R(q̃)q̃˙ + h(q̃)
yc = B(q̃)(−Kp (q̃ − r) − Kd q̃˙ − Ki xc ) + C(q̃, q̃)
q̈e = B −1 (q̃ + qe )(τ − C(q̃ + qe , q̃˙ + q̇e )(q̃˙ + q̇e )
− R(q̃ + qe )(q̃˙ + q̇e ) − h(q̃ + qe ))
(1.14)
˙ q̃˙ + R(q̃)q̃˙ + h(q̃) − yc )
B −1 (q̃)(C(q̃, q̃)
τ = sat(yc + v1 )
Capitolo 1 Leggi anti-windup per manipolatori robotici
13
§ 1.4 Prestazione anti-windup
I sistemi di equazioni (1.13), (1.14) rappresentano il sistema a ciclo chiuso come
l’insieme di due sottosistemi. Il primo (corrispondente alla (1.13)) di coordinate
(x̃, xc ) ’guida’ il secondo (corrispondente alla (1.14)) di coordinate xe . Si può notare che la dinamica del primo sottosistema coincide con quella del sistema a ciclo
chiuso (1.1), (1.2) e che dato xe (0) = 0 si ha (x̃(0), xc (0)) = (x(0), xc (0)). Di conseguenza se la dinamica e le condizioni iniziali sono le stesse, si ha (x̃(t), xc (t)) =
(xl (t), xcl (t)) ∀t ≥ 0. Dunque, dalla definizione, xl (t) = x̃(t) = x(t) − xe (t), ∀t ≥
0, segue il risultato da dimostrare.
L’importanza del Teorema 3, sta nel fatto che esso chiarisce il significato della
scelta del segnale v1 , rispetto alla misura dell’errore espressa mediante la variabile
xe = x − x̃. Infatti, se concentriamo la nostra attenzione sul sottosistema 1.14,
possiamo notare che la scelta di v1 , secondo l’espressione (1.10), è particolarmente adatta a mantenere qe piccolo, e quindi a garantire che l’effettiva traiettoria
del sistema q si avvicini il più possibile all’andamento della traiettoria ’ideale’ q̃.
Prima di procedere oltre, ricordiamo che per il Teorema 2, se i parametri scelti
soddisfano le condizioni (4.1), (1.11), è comunque garantita la stabilità asintotica
globale (ed esponenziale locale) del sottosistema descritto dalla (1.14). Quindi,
d’ora in poi, concentreremo tutta l’attenzione solo sullo studio dell’efficacia delle
leggi anti-windup precedentemente introdotte, senza preoccuparci della stabilità,
già garantita.
Cominciamo con il precisare che, con riferimento alle equazioni (1.14), il termine
yc costituisce un disturbo per la dinamica qe . Il segnale yc infatti, produce spesso,
specialmente nei sistemi di controllo aggressivi, degli indesiderati picchi di sovraelongazione, che vanno ad interessare la risposta transitoria del sistema. Quindi
per ridurre significativamente questo disturbo, all’interno dell’equazione (1.10), è
stato introdotto il termine che trasforma il disturbo yc in sat(yc ). Ora, per capire
meglio, che tipo di influenza ha la scelta del segnale v1 sulla dinamica dell’errore
associata al sottosistema (1.14), sostituiamo i segnali v1 e τ all’interno della prima
Capitolo 1 Leggi anti-windup per manipolatori robotici
14
§ 1.4 Prestazione anti-windup
equazione delle (1.14). Dalla sostituzione si ottiene:
q̈e = B −1 (q̃ + qe )(−Kg sat(Kg−1 Kq qe ) − Kqd (qe )q̇e )
B −1 (q̃ + qe )(sat(yc ) − C(q̃ + qe , q̃˙ + q̃˙e )(q̃˙ + q̃˙e )
(1.15)
− R(q̃ + qe )(q̃˙ + q̃˙e ) − h(q̃) − q̃¨
con q̃ = q − qe . E’ interessante notare che, quando (qe , q̇e ) è piccolo e sat(yc ) = yc ,
per continuità, se la saturazione (sulla prima riga della (1.15)) non è attiva, la
seconda riga dell’equazione (1.15) è circa zero e si ha:
B(q)q̈e ≈ −Kg sat(Kg−1 Kq qe ) − Kqd (qe , q̇e )q˙e
(1.16)
L’equazione (1.16) descrive la dinamica del sistema a ciclo chiuso come un doppio
integratore controllato da un’azione proporzionale saturata e da un’azione derivativa, i cui guadagni sono associati alla scelta dei parametri Kq e Kqd (·, ·) (per
costruzione, la matrice Kqd (·, ·) è diagonale e strettamente positiva, per qualsiasi
valore del suo argomento).
Data una matrice K0 diagonale e definita positiva, la funzione Kqd (·, ·) sarà data
dalla seguente espressione:
Kqd (qe , q̇e ) := ((I − D(qe , q̇e ))γE (qe ) + D(qe , q̇e ))K0
(1.17)
dove γE (qe ) è diagonale e viene chiamato guadagno equivalente associato con la
saturazione dell’azione proporzionale, e soddisfa la condizione:
Kg sat(Kg−1 Kq qe ) = γE (qe )Kq qe ;
D(qe , q̇e ) è una matrice diagonale i cui elementi, (di,i ,
i = 1, ..., n), vengono scelti
nel modo seguente:
di,i :=
dove (qei , q̇ei ,
1, se qei q̇ei ≥ 0
0, altrimenti
(1.18)
i = 1, ..., n) sono, rispettivamente, le componenti dei vettori qe , q̇e .
Quindi, con riferimento all’equazione (1.17), si ha che quando qei , q̇ei hanno lo stesso segno, Kqd (qe , q̇e ) = K0 , indipendentemente dall’ampiezza dei segnali qei e q̇ei .
Capitolo 1 Leggi anti-windup per manipolatori robotici
15
§ 1.4 Prestazione anti-windup
Mentre, se qei e q̇ei , hanno segno opposto, Kqd (qe , q̇e ) = γE (qe )K0 e il termine derivativo dell’espressione (1.16) esercita una forza/coppia frenante che viene modulata
dall’ampiezza del segnale saturato all’interno del termine proporzionale. Questa
modulazione permette quindi di recuperare l’azione di controllo nei casi in cui qe
diventa ragionevolmente grande e l’azione di saturazione fa si che il termine proporzionale diventa troppo piccolo per ’limitare’ l’azione dovuta al termine derivativo.
In conclusione, dalle equazioni (1.10), (1.17) segue che, se qe è abbastanza piccolo
da rendere trascurabile il secondo termine di saturazione dell’equazione (1.10), si
ha γE (qe ) = I e che l’equazione della dinamica (1.16) si trasforma nell’equazione
dinamica semplificata:
B(q)q̈e ≈ −Kq qe − K0 q̇e
(1.19)
Da ciò deriva direttamente che la matrice Kqd (·, ·) è Lipschitziana. Inoltre l’equazione (1.19) suggerisce di scegliere gli elementi diagonali delle matrici Kq e K0 in
modo quasi disaccoppiato (’quasi’ per la presenza del termine B(q)), per garantire
le prestazioni di ciascun giunto. Sostanzialmente si segue la stessa procedura usata
per la determinazione dei guadagni di un controllore PD.
Ricapitolando, la strategia consigliata per la progettazione del compensatore antiwindup è la seguente:
1. Si sceglie v1 secondo le equazioni (1.10), (1.17);
2. si determina una matrice Kg i cui elementi soddisfino la condizione (4.1)
(nota che Kg < I per definizione);
3. si scelgono gli elementi delle matrici Kq e K0 cercando di garantire il miglior
andamento transitorio su ciascun giunto, e seguendo una strategia di selezione
simile a quella adottata per i guadagni di un controllore PD.
Capitolo 1 Leggi anti-windup per manipolatori robotici
16
Capitolo 2
Calcolo delle equazioni dinamiche
del moto di un manipolatore
2.1
Introduzione
La dinamica di un robot tratta le formulazioni matematiche delle equazioni del
moto del manipolatore. Le equazioni dinamiche del moto di un manipolatore costituiscono un sistema di equazioni matematiche che ne descrivono il comportamento dinamico. Tali equazioni sono utili per la simulazione al calcolatore del
movimento del braccio e per formulare le equazioni per il controllo dello stesso. Lo
scopo del controllo di un manipolatore guidato da un calcolatore è far sı̀ che la sua
risposta dinamica soddisfi le prestazioni ipotizzate in fase di progetto. In generale,
le prestazioni di un manipolatore dipendono direttamente dall’efficienza dell’algoritmo di controllo e del modello dinamico del manipolatore. Il modello dinamico
di un robot si ottiene da note leggi fisiche. Questo conduce allo sviluppo delle
equazioni dinamiche del movimento per i vari giunti articolati del manipolatore in
relazione ai parametri geometrici e inerziali specifici dei link. In questo capitolo
verrà quindi descritta una procedura automatica, sviluppata al calcolatore e basata
sulla ’Formulazione di Eulero-Lagrange’, che fornisce quelli che vengono chiamati
’coefficienti dinamici del manipolatore’, che caratterizzano appunto le equazioni
dinamiche del moto.
17
§ 2.2 Formulazione di Eulero-Lagrange
2.2
Formulazione di Eulero-Lagrange
Grazie all’applicazione diretta della formulazione di Eulero-Lagrange per sistemi
non conservativi si possono esprimere convenientemente le equazioni di moto generali di un manipolatore. L’applicazione diretta della formulazione dinamica
di Lagrange, insieme alla rappresentazione delle coordinate dei link di DenavitHartenberg, dà come risultato una formulazione algoritmica conveniente e compatta delle equazioni di moto del manipolatore. L’algoritmo viene espresso tramite
operazioni matriciali e ciò semplifica sia l’analisi che l’implementazione su calcolatore.
La derivazione delle equazioni dinamiche di un manipolatore a n gradi di libertà è
basata sulla comprensione delle seguenti nozioni:
• La matrice di trasformazione 4x4 in coordinate omogenee,i−1 Qi ,che descrive
la relazione spaziale tra i sistemi di coordinate dei link i -esimo e (i−1)-esimo.
Essa mette in relazione un punto stabilito sul link i, espresso in coordinate
omogenee rispetto al sistema di coordinate i -esimo, col sistema di coordinate
(i − 1)-esimo.
• L’equazione di Eulero-Lagrange:
d ∂L
∂L
−
= τi
dt ∂ q˙i
∂qi
i = 1, 2, ..., n
dove:
L = funzione lagrangiana = energia cinetica T - energia potenziale U
T = energia cinetica totale del manipolatore
U = energia potenziale totale del manipolatore
qi = coordinate generalizzate del braccio
q˙i = derivata prima rispetto al tempo delle coordinate generalizzate, qi
τi = forza generallizata (o momento) applicata al sistema al giunto i per muovere
il link i
Capitolo 2 Calcolo delle equazioni dinamiche del moto di un manipolatore
18
§ 2.3 Cinematica e dinamica di un manipolatore
Dalla precedente equazione di Eulero-Lagrange, per prima cosa viene richiesto di
scegliere un insieme di coordinate generalizzate atte a descrivere il sistema, cioè
un insieme di coordinate che rappresenti lo stato di un sistema (posizione e orientamento) rispetto a un sistema di coordinate di riferimento. Per descrivere un
manipolatore si possono trovare diversi insiemi di coordinate generalizzate. Comunque, dal momento che le posizioni angolari dei giunti sono rapidamente ottenibili perchè possono essere misurate con potenziometri o encoder o altri sensori,
esse hanno una ovvia corrispondenza con le coordinate generalizzate. Questo, in
effetti, corrisponde alle coordinate generalizzate con la variabile del giunto definita
in ognuna delle matrici di trasformazione 4x4 delle coordinate dei link. Cosı̀, nel
caso di giunto rotazionale, qi ≡ θi , rotazione del giunto; per un giunto prismatico,
qi ≡ di , distanza percorsa dal giunto.
Infine, la formulazione di Eulero-Lagrange richiede la conoscenza dell’energia potenziale e dell’energia cinetica del sistema fisico, che a sua volta richiede la cognizione della velocità di ogni giunto. Nel seguito dopo aver proposto un richiamo teorico
per introdurre una formulazione algoritmica delle equazioni dinamiche, verrà spiegato come ricavare le suddette grandezze e illustrata per ciscuna, la procedura che
ne permette il calcolo automatico al computer.
2.3
2.3.1
Cinematica e dinamica di un manipolatore
Coordinate omogenee
Con riferimento alla figura 2.1, dato un punto P nello spazio e dati due sistemi
di riferimento (Oxyz )0 e (Oxyz )1 , se le coordinate del punto o1 rispetto al sistema
di riferimento (Oxyz )0 sono rappresentate dal vettore d, e se q0 e q1 denotano le
coordinate del punto P nei due sistemi di riferimento, allora la seguente relazione
può essere verificata con semplici considerazioni geometriche di composizione di
vettori:
q0 = 0 R1 q1 + d
Capitolo 2 Calcolo delle equazioni dinamiche del moto di un manipolatore
(2.1)
19
§ 2.3 Cinematica e dinamica di un manipolatore
Figura 2.1: Rappresentazione di un punto in diversi sistemi di coordinate.
dove 0 R1 è la matrice di rotazione (nella spiegazione si suppone nota la forma delle
matrici 3x3 di rotazione e di traslazione) del sistema di riferimento (Oxyz )1 rispetto
al sistema di riferimento (Oxyz )0 . Per rappresentare in maniera più compatta il
generico spostamento rigido (2.1), invece delle coordinate cartesiane [ x y z ]T
si utilizzano le coordinate omogenee: [ ωx ωy ωz ω ]T , costituite non più da
tre ma da quattro componenti, di cui la quarta costituisce il fattore di scala per
cui le componenti x, y, z sono moltiplicate e può essere utilizzata per risalire
alle coordinate cartesiane. Sulla base delle coordinate omogenee si definiscono le
trasformazioni omogenee, rappresentate da matrici appartenenti allo spazio R 4x4
che consentono la rappresentazione compatta non solo di rotazioni (come avveniva
per le matrici nella spazio R4x4 ) ma anche di traslazioni. La forma più generale
per la trasformazione omogenea è data dalla seguente formula:
ω 1 q1
R d
ω 0 q0
=
ω1
ω0
f s
(2.2)
dove R ∈ R3x3 è la matrice di rotazione, d ∈ R3 è il vettore di posizione, f ∈ R1x3 è il
vettore di trasformazione prospettica e S ∈ R è il fattore di scala. Nelle applicazioni
robotiche (nel nostro caso), si considera sempre f = [ 0 0 0 ] e s = 1, quindi le
coordinate omogenee associate ad un punto di coordinate cartesiane [ x y z ]T
saranno sempre definite come [ x y z 1 ]T (il fattore di scala e il vettore di
trasformazione prospettica sono usati, ad esempio, in applicazioni di computer
grafica).
Capitolo 2 Calcolo delle equazioni dinamiche del moto di un manipolatore
20
§ 2.3 Cinematica e dinamica di un manipolatore
Sulla base della struttura delle trasformazioni omogenee descritta dall’equazione
(2.2), si possono scrivere le seguenti matrici di rotazioni

cos(θ) − sin(θ) 0 0
 sin(θ) cos(θ) 0 0
Qz,θ := 
 0
0
1 0
0
0
0 1

1
0
0
0
 0 cos(θ) − sin(θ) 0
Qx,θ := 
 0 sin(θ) cos(θ) 0
0
0
0
1

cos(θ) 0 sin(θ) 0

0
1
0
0
Qy,θ := 
 − sin(θ) 0 cos(θ) 0
0
0
0
1
e la matrice di traslazione omogenea fondamentale


1 0 0 dx
 0 1 0 dy 

Qtr,d := 
 0 0 1 dz 
0 0 0 1
omogenee fondamentali :





(2.3)



(2.4)



(2.5)

(2.6)
dove d = [ dx dy dz ]T costituisce un generico spostamento traslatorio.
Il vantaggio delle coordinate e trasformazioni omogenee sta nel fatto che l’equazione
(2.1), caratterizzante un generico spostamento rigido, può essere descritta in forma
matriciale come segue, tramite la trasformazione omogenea 0 Q1 :
0
0
R
d
R
q
+
d
q
q0
1
1
1
1
0
=
= Q1 P 1 = P0 =
0 0 0
1
1
1
1
(2.7)
e, analogamente a quanto accadeva per le rotazioni (descritte da matrici in R3x3 ), gli
spostamenti rigidi si possono comporre, consentendola rappresentazione compatta
di catene di sistemi di riferimento in relazione l’uno con l’altro.
In particolare, se P0 = 0 Q1 P1 e P1 = 1 Q2 P2 , allora si può sostituire e ottenere:
P 0 = 0 Q1 P 1 = 0 Q1 1 Q2 P 2 = 0 Q2 P 2
2.3.2
Link, giunti e loro parametri
Un manipolatore meccanico consiste in una sequenza di segmenti rigidi chiamati
link, connessi da giunti prismatici o di rotazione. Ogni coppia giunto-link, rappresenta un grado di libertà. Dunque, per un manipolatore con N gradi di libertà ci
Capitolo 2 Calcolo delle equazioni dinamiche del moto di un manipolatore
21
§ 2.3 Cinematica e dinamica di un manipolatore
sono N coppie giunto-link con il link 0 (non considerato parte del robot) ancorato
a una base di appoggio dove di norma è stabilito un sistema di coordinate inerziale;
all’ultimo link è collegato un utensile. I giunti e i link sono numerati in successione
partendo dalla base; cosı̀ il giunto 1 costituisce il punto di connessione fra il link
1 e la base di supporto. Ogni link è connesso al più a due altri link perchè non
si formino catene chiuse. In generale, due link sono collegati fra loro con un giunto di prima specie, e di questi, solo i giunti rotazionali e prismatici sono comuni
nei manipolatori. I primi sono giunti associati ad uno spostamento angolare dei
link adiacenti, mentre i secondi sono associati ad uno spostamento lineare dei link
adiacenti. L’asse di un giunto si definisce come l’asse di rotazione o di traslazione
individuato dal movimento del giunto. Questo asse avrà due normali, una per ogni
link. Si faccia riferimento alla figura 2.2. Il significato dei link, considerati in un’ot-
Figura 2.2: Sistemi di coordinate dei link e loro parametri.
tica cinematica, è che essi mantengono una configurazione fissa tra i loro giunti che
può essere caratterizzata da due parametri ai , αi . Il parametro ai rappresenta la
distanza più breve misurata lungo la normale comune tra gli assi dei giunti (cioè
gli assi zi−1 e zi rispettivamente per i giunti i e i+1 ), e αi è l’angolo compreso
tra gli assi dei giunti su un piano perpendicolare ad ai . Cosı̀ ai e αi possono essre
chiamati, rispettivamente, lunghezza e angolo di twist del link i. Essi sono associati
alla struttura del link.
Capitolo 2 Calcolo delle equazioni dinamiche del moto di un manipolatore
22
§ 2.3 Cinematica e dinamica di un manipolatore
Dato l’asse zi , la posizione relativa dei due link adiacenti (i−1) e i è data da di , che
è la distanza misurata lungo l’asse del giunto fra la normale proveniente dall’asse
del giunto i e la normale che va al giunto i + 1 (ovvero, il segmento ai ). L’angolo θi
del giunto è l’angolo formato dalle 2 normali, misurato su un piano ortogonale all’asse del giunto. Dunque, di e θi possono essere chiamati rispettivamente distanza
e angolo di giunto, fra due link adiacenti. Essi sono associati alla posizione relativa
tra i due link.
Riassumento a ciascuna coppia giunto/link di un manipolatore sono associati quattro parametri: ai , αi , di , θi . Una volta stabilita una convenzione di segno per ognuno
di essi, questi costituiscono un insieme sufficiente a determinare completamente la
configurazione cinematica di ciascun anello della catena articolata del braccio. In
base alla caratterizzazione precedente, dato un manipolatore, sulla base dei quattro parametri ai , αi , di , θi , si può compilare una tabella: dove ogni link (giunto)
Link/giunto
1
2
.
.
.
N
αi
α1
α2
.
.
.
αN
ai
a1
a2
.
.
.
aN
θi
θ1
θ2
.
.
.
θN
di
d1
d2
.
.
.
dN
Tabella 2.1: Tabella dei parametri per un robot generico.
corrisponde ad una riga su cui sono presenti i parametri caratteristici del link e del
giunto i. Questa tabella sarà utile per il calcolo della cinematica diretta del robot,
ovvero della funzione che trasforma le coordinate del sistema di effettore (o della
mano) nel sistema di coordinate di base.
2.3.3
Rappresentazione di Denavit-Hartenberg
Per descrivere le relazioni di rotazione e traslazione tra link adiacenti, Denavit e
Hartenberg (1955) hanno proposto un metodo matriciale per stabilire sistematicamente un sistema di coordinate per ogni link di una catena articolata. La rappre-
Capitolo 2 Calcolo delle equazioni dinamiche del moto di un manipolatore
23
§ 2.3 Cinematica e dinamica di un manipolatore
sentazione di Denavit-Hartenberg (D-H) consiste in una matrice di trasformazione
omogenea
i−1
Qi ∈ R4x4 che rappresenta il sistema di coordinate del link i rispetto
al riferimento del link precedente (i − 1). Pertanto, attraverso trasformazioni sequenziali, l’estremità dell’effettore, espresso nell’ultimo sistema di coordinate, può
essere trasformata nelle ’coordinate di base’ che costituiscono il sistema di riferimento inerziale di questo sistema dinamico.
Come già accennato, il robot è individuato da un sistema di riferimento inerziale
(Oxyz )0 , detto di base, da un sistema di riferimento di effettore (Oxyz )e (anche detto
“della mano”) e dai sistemi di riferimento intermedi, ognuno assocciato ad un link
(e ad un giunto). Per ognuno di questi N sistemi di riferimento (dove N sono i
gradi di libertà della catena cinematica), la procedura di D-H, fornisce delle regole
di tracciamento (che in questo contesto non verranno illustrate) in termini di:
1. posizione dell’asse zi
2. posizione del centro oi
3. posizione dell’asse xi
e stabilisce una convenzione per la determinazione dei quattro parametri ai , αi , di , θi ,
di ciascun giunto, necessari a compilare la tabella (vedi tabella 1.1) di DenavitHartemberg.
2.3.4
Trasformazione omogenea associata ad un link
Stabilito il sistema di coordinate di D-H per ogni link, si può sviluppare facilmente
una matrice di trasformazione omogenea che metta in relazione il sistema di coordinate i -esimo col sistema di coordinate (i − 1)-esimo. Per ottenere la suddetta
matrice, bisogna prima sovrapporre il sistema di riferimento (Oxyz )i al sistema di
riferimento (Oxyz )i−1 , poi effettuare le seguenti operazioni elementari per riportare
(Oxyz )i nella sua posizione originaria:
1. traslare lungo l’asse zi−1 di una lunghezza pari a di per portare l’origine oi
sul punto di intersezione tra il segmento ai e l’asse zi−1 ;
Capitolo 2 Calcolo delle equazioni dinamiche del moto di un manipolatore
24
§ 2.3 Cinematica e dinamica di un manipolatore
2. ruotare intorno all’asse zi−1 di un angolo θi per allineare l’asse xi con il
segmento ai ;
3. traslare lungo l’asse xi di una lunghezza pari ad ai per portare oi nella
posizione originaria;
4. ruotare intorno all’asse xi di un angolo pari ad αi per portare l’asse zi nella
posizione originaria.
Le prime due trasformazioni sono riferite al sistema di riferimento (i − 1) e corrispondono dunque alla premoltiplicazione per le matrici Qtr,[0,0,di ] e Qz,θi , rispettivamente. Le ultime due trasformazioni, sono invece riferite al sistema di riferimento i e corrispondono alla postmoltiplicazione per le matrici Qtr,[ai ,0,0] e Qx,αi ,
rispettivamente.In sintesi la trasformazione
i−1
Qi si costruisce secondo la seguente
sequenza:
i−1
Qi = Qz,θi Qtr,[0,0,di ] Qtr,[αi ,0,0] Qx,αi
(2.8)


cos(θi ) −cos(αi )sin(θi ) sin(αi )sin(θi ) ai cos(θi )

sin(θi ) cos(αi )cos(θi ) −sin(αi )cos(θi ) ai sin(θi ) 
i−1

Qi = 


0
sin(αi )
cos(αi )
di
0
0
0
1
(2.9)
Poichè la (2.9) è di valore generale, essa può essere applicata iterativamente sostituendo i dati della tabella 2.1 per ottenere in maniera immediata la matrice 0 Qe
(e ≡ N ) della cinematica diretta.
Dunque, il calcolo della cinematica diretta, si scompone in una procedura costituita
dai seguenti passi:
1. Si analizza la struttura del manipolatore posizionando tutti i sistemi di
riferimento secondo la procedura di Denavit-Hartemberg;
2. In base alla posizione dei sistemi di riferimento, si ricavano i parametri di
D-H, compilando la tabella 2.1;
3. Si sostituiscono i valori della riga i -esima della tabella nella corrispettiva
matrice di trasformazione
i−1
Qi e si semplificano le moltiplicazioni;
Capitolo 2 Calcolo delle equazioni dinamiche del moto di un manipolatore
25
§ 2.3 Cinematica e dinamica di un manipolatore
4. Si moltiplicano tutte le matrici ricavate al punto precedente per ricavare la
matrice finale:
P 0 = 0 Qe P e = 0 Q1 1 Q2 · · ·
2.3.5
N −1
Qe P e
Jacobiano geometrico
Si consideri un manipolatore ad N gradi di mobilità. Sia nota l’equazione cinematica diretta nella forma:
0
in cui q =
q1 . . . q i
Qi (q) =
T
0
Ri (q) d0i (q)
0T
1
(2.10)
è il vettore delle variabili di giunto. Per semplificare le
cose supponiamo i = N . Al variare di q variano sia la posizione che l’orientamento
dell’organo terminale del manipolatore. Obiettivo della cinematica differenziale è
quello di determinare la relazione tra velocità ai giunti e velocità lineare e angolare
dell’organo terminale. In altri termini, si vogliono esprimere come vettori liberi la
velocità lineare d˙ e la velocità ω dell’organo terminale in funzione delle velocità
delle variabili di giunto mediante relazioni del tipo:
d˙ = Jp (q)q̇
(2.11)
ω = Jω (q)q̇
(2.12)
Nella (2.11) Jp è la matrice (3 × N ) relativa al contributo delle velocità dei giunti
alla velocità lineare d˙ dell’organo terminale, mentre nella (2.12) Jω è la matrice
(3 × N ) relativa al contributo delle velocità dei giunti alla velocità angolare ω
dell’organo terminale. In forma compatta, i legami (2.11),(2.12) possono essere
scritti come
v=
d˙
ω
= J(q)q̇
(2.13)
che rappresenta l’equazione cinematica differenziale del manipolatore. La matrice
J (6 × N ) è lo Jacobiano geometrico del manipolatore, che in generale risulta funzione delle variabili di giunto. Per effettuare il calcolo dello Jacobiano geometrico è
opportuno richiamare delle proprietà delle matrici di rotazione e alcuni importanti
risultati di cinematica dei corpi rigidi.
Capitolo 2 Calcolo delle equazioni dinamiche del moto di un manipolatore
26
§ 2.3 Cinematica e dinamica di un manipolatore
2.3.6
Derivata di una matrice di rotazione
L’equazione cinematica diretta di un manipolatore (2.10) descrive, se (i = N ),
la posizione e l’orientamento dell’organo terminale, in funzione delle variabili di
giunto, in termini di un vettore di posizione diN e di una matrice di rotazione
0
RN . Se l’obiettivo è quello di caratterizzare velocità lineare e angolare dell’organo
terminale, è opportuno analizzare in primo luogo la derivata di una matrice di
rotazione rispetto al tempo.
Si supponga che la matrice di rotazione vari nel tempo, ovvero R = R(t). Dalla
proprietà di ortogonalità di R si ha la relazione:
R(t)RT (t) = I
che, derivata rispetto al tempo, fornisce l’identità:
Ṙ(t)RT (t) + R(t)ṘT (t) = 0
Posto:
S(t) = Ṙ(t)RT (t)
(2.14)
la matrice (3 × 3) S risulta essere anti-simmetrica in quanto:
S(t) + S T (t) = 0
(2.15)
Moltiplicando da destra ambo i membri della (2.14) per R(t), si ottiene:
Ṙ(t) = S(t)R(t)
(2.16)
che consente di esprimere la derivata temporale di R(t) in funzione della matrice
stessa.
La relazione (2.16), che lega la matrice di rotazione R alla sua derivata attraverso
l’operatore anti-simmetrico S, ammette una interessante interpretazione fisica. Si
considerino un vettore costante p0 ed il vettore p(t) = R(t)p0 . La derivata temporale
di p(t) risulta:
ṗ(t) = Ṙ(t)p0
Capitolo 2 Calcolo delle equazioni dinamiche del moto di un manipolatore
27
§ 2.3 Cinematica e dinamica di un manipolatore
che per la (2.16) può scriversi:
ṗ(t) = S(t)R(t)p0
Se con il vettore ω(t) si indica la velocità angolare della terna R(t) all’istante di
tempo t rispetto alla terna di riferimento, dalla meccanica è noto che:
ṗ(t) = ω(t) × R(t)p0
Pertanto l’operatore matriciale S(t) descrive il prodotto vettoriale tra il vettore ω
e il vettore R(t)p0 .
La matrice S(t) è tale che i suoi elementi simmetrici rispetto alla diagonale princiT
nella forma:
pale rappresentano le componenti del vettore ω(t) = ωx ωy ωz


0 −ωz ωy
0 −ωx 
S =  ωz
(2.17)
−ωy ωx
0
che giustifica la notazione S(t) = S(ω(t)).
Inoltre, se R rappresenta una matrice di rotazione, si può dimostrare che vale la
seguente relazione:
RS(ω)RT = S(Rω)
(2.18)
che tornerà utile in seguito.
2.3.7
Calcolo dello Jacobiano geometrico
Nota l’equazione cinematica diretta nella forma:

R1,1 (q) R1,2 (q) R1,3 (q) dx (q)
0

R
(q)
d
(q)
R2,1 (q) R2,2 (q) R2,3 (q) dy (q)
i
0i
0
Qi (q) =
=
T

0
1
R3,1 (q) R3,2 (q) R3,3 (q) dz (q)
0
0
0
1
in cui q =
q1 . . . q i
T



(2.19)
è il vettore delle variabili di giunto, il calcolo delle
matrici Jp e Jω può essere cosı̀ eseguito:
 ∂dx (q) ∂dx (q)

Jp = 

∂q1
∂dy (q)
∂q1
∂dx (q)
∂q1
∂q2
∂dy (q)
∂q2
∂dz (q)
∂q2
. . .
. . .
. . .
∂dx (q)
∂qi
∂dy (q)
∂qi
∂dz (q)
∂qi



Capitolo 2 Calcolo delle equazioni dinamiche del moto di un manipolatore
(2.20)
28
§ 2.3 Cinematica e dinamica di un manipolatore
con i = 1 . . . N . Per Jω si avrà:
0
Si = 0 Ṙi 0 RiT
calcolata la matrice 0 Si (ω), dalla 2.17 si ottiene il vettore ω(t) =
ωx ωy ωz
T
della velocità angolare della terna 0 Ri (t) all’istante di tempo t. Quindi possiamo
ora calcolare Jω :


Jω = 
∂ωx (q̇)
∂ q̇1
∂ωy (q̇)
∂ q̇1
∂ωz (q̇)
∂ q̇1
∂ωx (q̇)
∂ q̇2
∂ωy (q̇)
∂ q̇2
∂ωz (q̇)
∂ q̇2
. . .
. . .
. . .
∂ωx (q̇)
∂ q̇i
∂ωy (q̇)
∂ q̇i
∂ωz (q̇)
∂ q̇i



(2.21)
con i = 1 . . . N e dove q̇ è il vettore delle velocità angolari dei giunti.
2.3.8
Energia cinetica di un corpo rigido tridimensionale
che si muove nello spazio
Figura 2.3: Posizione e orientamento di un corpo rigido
Si dimostra come può essere calcolata l’energia cinetica di un corpo rigido tridimensionale (es. link di un robot) che si muove nello spazio. Con riferimento alla
figura 2.3, definiamo:
1. il sistema di riferimento inerziale (Oxyz);
2. il sistema di riferimento (Oxyz)0 solidale al corpo, ed avente l’origine O 0 che
coincide con il baricentro G del corpo medesimo.
Capitolo 2 Calcolo delle equazioni dinamiche del moto di un manipolatore
29
§ 2.3 Cinematica e dinamica di un manipolatore
Identifichiamo con P =
x y z
T
=
p 1
T
le coordinate omogenee del
T
puntoP rispetto al sistema di riferimento inerziale, e con P 0 = x0 y 0 z 0 1
=
0
T
p 1
le coordinate omogenee dello stesso punto rispetto al sistema di riferimeto (Oxyz)0 . Nota l’equazione cinematica diretta nella forma:
R d
Q=
0 1
per la (2.7) possiamo scrivere:
P = QP 0 ⇒ p = Rp0 + d
(2.22)
deriviamo (la 2.22), si ottiene:
ṗ = Ṙp0 + d˙ = SRp0 + d˙ ⇒ ṗT = p0T RT S T + d˙T
ora possiamo scrivere l’energia cinetica associata al corpo rigido come:
Z
Z
1
1
T
p0T RT S T SRp0 + d˙T d + 2d˙T SRp0 dV
T = ρ ṗ ṗdV = ρ
2 V
2 V
(2.23)
(2.24)
la (2.24) si può scomporre nella somma di tre contributi:
Z
Z
1
1
1
0T T T
0
p R S SRp dV, T2 = ρ
d˙T ddV, T3 = ρd˙T d + 2d˙T SRp0 dV
T1 = ρ
2 V
2 V
2
(2.25)
semplifichiamo i tre contributi:
1. si può dimostrare, che se O 0 ≡ G, allora T3 = 0;
2.
1
T2 = ρ
2
Z
V
1 M
d˙T d dV = R
2 V dv
Z
V
1
d˙T d dV = M d˙T d
2
(2.26)
per la cinematica differenziale, d˙ = Jp q̇ e quindi:
1
1
T2 = q̇ T (M JpT Jp )q̇ = q̇ T Bt q̇
2
2
(2.27)
3.
1
T1 = T 1 = ρ
2
Z
p0T RT S T SRp0 dV
(2.28)
V
Capitolo 2 Calcolo delle equazioni dinamiche del moto di un manipolatore
30
§ 2.3 Cinematica e dinamica di un manipolatore
p0T RT S T SRp0 = p0T RT S T ISRp0 = p0T RT S T RRT SRp0
per la (2.18), cioè RT S(ω)R = S(RT ω) = S(ω 0 ), si ha:
p0T RT S T RRT SRp0 = p0T S T (RT ω)S(RT ω)p0
p0T S T (ω 0 )S(ω 0 )p0 = traccia(S(ω 0 )p0 p0T S T (ω))
quindi:
1
T1 = ρtraccia(S(ω 0 )
2
Z
p0 p0T dV S T (ω 0 ))
V
posto:
J=
Z
p0 p0T dV S T (ω 0 ))
V
si può scrivere:
1
1
T1 = ρtraccia(S(ω 0 )JS T (ω 0 )) = ω 0T Iω 0
2
2
I si chiama matrice di inerzia e dipende univocamente da J secondo la
relazione:


Jxx Jxy Jxz
J =  ∗ Jyy Jyz  ,
∗
∗ Jzz


Jyy + Jzz
−Jxy
−Jxz
∗
Jxx + Jzz
−Jyz  (2.29)
I=
∗
∗
Jxx + Jyy
Infine per la 2.12, ω = Jω q̇ possiamo scrivere:
1
1
1
T1 = ω T RIRT ω = q̇ T [JωT RIRT Jω ]q̇ = q̇ T Bω q̇
2
2
2
2.3.9
(2.30)
Energia potenziale di un manipolatore
Sia U l’energia potenziale di un braccio e sia Ui l’energia potenziale di ogni suo
link. Con riferimento all’equazione cinematica

R1,1 (q) R1,2 (q)

R2,1 (q) R2,2 (q)
0
Qi (q) = 
 R3,1 (q) R3,2 (q)
0
0
diretta nella forma:

R1,3 (q) dx (q)
R2,3 (q) dy (q) 

R3,3 (q) dz (q) 
0
1
si ha che:
Ui = −Mi gdz (q)
i = 1, 2, ..., N
Capitolo 2 Calcolo delle equazioni dinamiche del moto di un manipolatore
(2.31)
31
§ 2.4 Procedura e per il calcolo automatico dei ’coefficienti dinamici’
L’energia potenziale totale del braccio può essere ottenuta sommando le energie
potenziali di tutti i link:
U=
N
X
Ui
(2.32)
i=1
dove g è la costante gravitazione diretta lungo l’asse z ed Mi è la massa del link
i-esimo.
2.4
Procedura e per il calcolo automatico dei
’coefficienti dinamici’
La procedura per il calcolo automatico delle equazioni e dei coefficienti dinamici del
manipolatore, è implementata con il software, per l’analisi simbolica e numerica,
Maple 8.0 della Waterloo Maple Inc. corporation.
Sulla base delle informazioni fornite dal paragrafo precedente, viene ora presentato
l’algoritmo di calcolo usato per ricavare i coefficienti dinamici del robot SiemensManutec R3 . Al fine di rendere più comprensibile la lettura del codice del programma, gli step dell’algoritmo, vengono descritti uno alla volta, con il corrispondente
codice:
1. Si analizza la struttura posizionando tutti i sistemi di riferimento secondo la
procedura di Denavit-Hartemberg;
2. In base alla posizione dei sistemi di riferimento, si ricavono i parametri di
D-H e si compila la tabella 2.1:
Giunto
1
2
3
4
5
6
αi
90◦
0
−90◦
90◦
−90◦
0
ai
0
0.5
0
0
0
0
θi
q1
q2
q3
q4
q5
q6
di
0.4
0
0
0.73
0
0.275
gxi
gyi
3. Si calcolano le coordinate del baricentro del linki , Gi =
gzi
gx i g y i g z i
rispetto all’origine del sistema di riferimento, solidale al linki .
Capitolo 2 Calcolo delle equazioni dinamiche del moto di un manipolatore
T
,
32
§ 2.4 Procedura e per il calcolo automatico dei ’coefficienti dinamici’
Le suddette coordinate vanno a completare le informazioni fornite dalla
tabella di D-H:
Giunto
1
2
3
4
5
6
αi
90◦
0
−90◦
90◦
−90◦
0
ai
0
0.5
0
0
0
0
θi
q1
q2
q3
q4
q5
q6
di
0.4
0
0
0.73
0
0.275
gxi
0
−0.295
0
0
0
0
gyi
−0.05
0
0.064
−0.41
0
0
gzi
0
−0.172
−0.034
0
0.023
−0.0625
codice maple:
Procedura che data in ingresso la matrice di Denavit-Hartemberg (estesa)
calcola le equazioni di Eulero-Lagrange e restituisce i coefficienti dinamici
del corrispondente robot:
>
restart:#riavvia una sessione maple senza uscire
Dichiara che q è un vettore che varia nel tempo
>
alias(q=q(t));
q
Vettore (’di appoggio’) delle variabili di giunto
>
papp:=[p[1],p[2],p[3],p[4],p[5],p[6]]:
Vettore (’di appoggio’) delle velocità angolari di giunto
>
vapp:=[v[1],v[2],v[3],v[4],v[5],v[6]]:
Vettore (’di appoggio’) delle accelerazioni di giunto
>
aapp:=[a[1],a[2],a[3],a[4],a[5],a[6]]:
>
with(linalg):
#carica la libreria linalg
Warning, the protected names norm and trace have been redefined and
unprotected
Capitolo 2 Calcolo delle equazioni dinamiche del moto di un manipolatore
33
§ 2.4 Procedura e per il calcolo automatico dei ’coefficienti dinamici’
Tabella di Denavit-Hartemberg del manipolatore Siemens-Manutec r3
>
DH:=array(1..6,1..7,[[90,0,q[1]*180/Pi,0.4,0,-0.05,0]
>
,[0,0.5,q[2]*180/Pi,0,-0.295,0,-0.172],
>
[-90,0,q[3]*180/Pi,0,0,0.064,-0.034],
>
[90,0,q[4]*180/Pi,0.73,0,-0.41,0],
>
[-90,0,q[5]*180/Pi,0,0,0,0.023],
>
[0,0,q[6]*180/Pi,0.275,0,0,-0.0625]]);


180 q1
90 0
0.4
0
−0.05
0


π




180 q2


0
0.5
0
−0.295
0
−0.172


π




 −90 0 180 q3
0
0
0.064 −0.034 


π

DH := 


180
q
4
 90 0

0.73
0
−0.41
0


π




180
q


5
−90
0
0
0
0
0.023




π


180 q6
0 0
0.275
0
0
−0.0625
π
4. Si sostituscono i valori della riga i -esima della tabella di D-H nella corrispettiva matrice di trasformazione
i−1
Qi e si semplificano le moltiplicazioni (vedi
2.8);
5. Per il calcolo dell’energia cinetica di ciascun link è neccessario conoscere la
matrice di trasformazione,
i−1
Q0i , che metta in relazione il sistema di riferi-
mento di base con quello solidale al linki , e avente l’origine nel barcicentro
del corpo, Gi . Quindi per ogni link si esegue la trasformazione:
0
0 0 0 Ri d0i
I Gi
Ri d0i
0 0
0
0
Qi = Qi Qi =
=
0
1
0 1
0
1
(2.33)
dove I è la matrice identità (3x3);
codice maple:
>
>
>
Procedura per il calcolo delle matrici di trasformazione associate
ad un link, e delle matrici Bw e Bt
>
Capitolo 2 Calcolo delle equazioni dinamiche del moto di un manipolatore
34
§ 2.4 Procedura e per il calcolo automatico dei ’coefficienti dinamici’
>
>
>
>
>
>
>
>
>
>
>
>
>
>
>
>
>
>
>
>
>
>
>
>
>
>
>
>
>
>
>
>
>
>
>
>
>
>
>
>
>
>
>
>
>
JpJwBt:=proc(DH,nrighe) local Q, QDH, QG, Qt, Qt1, Qgp, ar, tr,
ar1, tr1, ai, di, a1, d1, a1i, d1i, R, Rt, Rd, S, omega, Jw, Jp,
Bt, Bw, B, II, T, U, L, v, i, i1, i2, i3, i4, i5, i6, i7, j, j1,
j2, k3, k4, k5, gxi, gyi, gzi;
v:=array(1..nrighe,1..1,[]);
#Calcolo della matrice di trasformazione Q[0,j] dalla tab.DH#
for i from 1 to nrighe
do
ar:= convert(DH[i,1], units, degrees, radians);
tr:= convert(DH[i,3], units, degrees, radians);
ai:=DH[i,2];
di:=DH[i,4];
Q[i-1,i]:=array(1..4,1..4,[[cos(tr),-cos(ar)*sin(tr),sin(ar)*
sin(tr),ai*cos(tr)],[sin(tr),cos(ar)*cos(tr),-sin(ar)*cos(tr)
,ai*sin(tr)],[0,sin(ar),cos(ar),di],[0,0,0,1]]);
gxi:=DH[i,5];
gyi:=DH[i,6];
gzi:=DH[i,7];
Qgp[i]:=array(1..4,1..4,[[1,0,0,gxi],[0,1,0,gyi],[0,0,1,gzi],
[0,0,0,1]]);
end do;
# Calcolo delle matrici di trasformazione Q[0,i] #
Qt1[0,1]:=evalm(Q[0,1]);
for j from 2 to nrighe
do
Qt1[0,j]:=evalm(Qt1[0,j-1]&*Q[j-1,j]);
evalm(Qt1[0,j]);
end do;
# Calcolo delle matrici di trasformazione Q’[0,i] (Q’=Qt) #
for k4 from 1 to nrighe
do
Qt[0,k4]:=evalm(Qt1[0,k4]&*Qgp[k4]);
end do;
6. Usando le formula 2.20,2.21 si calcolano ,per ogni link del robot, Jpi e Jωi ;
codice maple per il calcolo di Jpi :
>
>
>
>
>
>
for j1 from 1 to nrighe
do
Jp[j1]:=array(1..3,1..nrighe,[]);
for i1 from 1 to nrighe
Capitolo 2 Calcolo delle equazioni dinamiche del moto di un manipolatore
35
§ 2.4 Procedura e per il calcolo automatico dei ’coefficienti dinamici’
>
>
>
>
>
>
>
do
Jp[j1][1,i1]:=diff(Qt[0,j1][1,4],q[i1]);
Jp[j1][2,i1]:=diff(Qt[0,j1][2,4],q[i1]);
Jp[j1][3,i1]:=diff(Qt[0,j1][3,4],q[i1]);
end do;
end do;
codice maple per il calcolo di Jωi :
>
>
>
>
>
>
>
>
>
>
>
>
>
>
>
>
>
>
>
>
>
>
>
>
>
>
>
>
>
>
>
for j2 from 1 to nrighe do R[0,j2]:=submatrix(Qt[0,j2], 1..3,
1..3); Rt[0,j2]:=transpose(R[0,j2]);
Rd[0,j2]:=array(1..3,1..3,[]);
for i2 from 1 to 3
do
for i3 from 1 to 3
do
Rd[0,j2][i2,i3]:=sum(’diff(R[0,j2][i2,i3],q[k])*v[k,1]’,
’k’=1..nrighe);
end do;
end do;
S[0,j2]:=evalm(Rd[0,j2]&*Rt[0,j2]);
omega[j2]:=Vector(1..3,[]);
omega[j2][1]:=S[0,j2][3,2];
omega[j2][2]:=S[0,j2][1,3];
omega[j2][3]:=S[0,j2][2,1];
Jw[j2]:=array(1..3,1..nrighe,[]);
for i4 from 1 to 3
do
for k5 from 1 to nrighe
do
Jw[j2][i4,k5]:=diff(omega[j2][i4],v[k5,1]);
end do;
end do;
end do;
7. Noti Jpi e Jωi , sempre per ogni link, si calcolano:
T
Bωi = [JωTi R0i Ii R0i
Jωi ],
Bti = Mi JpTi Jpi
(2.34)
codice maple:
>
>
>
>
>
>
>
# Calcolo delle matrici Bt, Bw #
for i5 from 1 to nrighe do
Bt[i5]:=m[i5]*evalm(transpose(map(simplify,Jp[i5],trig))
&*map(simplify,Jp[i5],trig));
II[i5]:=array(1..3,1..3,[[Ixx[i5],0,0],[0,Iyy[i5],0],
Capitolo 2 Calcolo delle equazioni dinamiche del moto di un manipolatore
36
§ 2.4 Procedura e per il calcolo automatico dei ’coefficienti dinamici’
>
>
>
>
>
>
>
>
>
[0,0,Izz[i5]]]);
Bw[i5]:=evalm(evalm(evalm(evalm(transpose(map
(simplify,Jw[i5],trig))
&*map(simplify,R[0,i5],trig))&*II[i5])&*
transpose(map(simplify,R[0,i5],trig)))&*map
(simplify,Jw[i5],trig));
end do;
semplificando e sommando tutti i contributi Bωi , Bti , si ottiene la matrice
generalizzata d’inerzia B:
B=
N
X
(Bti + Bωi )
(2.35)
i=1
codice maple:
>
>
>
>
>
>
>
>
>
>
>
BT[1]:=evalm(Bt[1]+Bw[1]):
BT[2]:=evalm(Bt[2]+Bw[2]):
BT[3]:=evalm(Bt[3]+Bw[3]):
BT[4]:=evalm(Bt[4]+Bw[4]):
BT[5]:=evalm(Bt[5]+Bw[5]):
BT[6]:=evalm(Bt[6]+Bw[6]):
B:=evalm(BT[1]+BT[2]+BT[3]+BT[4]+BT[5]+BT[6]):
Bn:=array(1..6,1..6,[]);
Bn := array(1..6, 1..6, [])
>
>
>
>
>
>
>
>
>
>
>
>
>
>
>
>
>
>
>
>
>
>
>
Semplifico la matrice B e assegno il risultato a Bn
for l1 from 1 to 6
do
Bn[1,l1]:=simplify(B[1,l1],trig);
end do:
for l2 from 1 to 6
do
Bn[2,l1]:=simplify(B[12,l2],trig);
end do:
for l3 from 1 to 6
do
Bn[3,l3]:=simplify(B[3,l3],trig);
end do:
for l4 from 1 to 6
do
Bn[4,l4]:=simplify(B[4,l4],trig);
end do:
Capitolo 2 Calcolo delle equazioni dinamiche del moto di un manipolatore
37
§ 2.4 Procedura e per il calcolo automatico dei ’coefficienti dinamici’
>
>
>
>
>
>
>
>
>
>
>
>
>
for l5 from 1 to 6
do
Bn[5,l5]:=simplify(B[5,l5],trig);
end do:
for l6 from 1 to 6
do
Bn[6,l6]:=simplify(B[6,l6],trig);
end do:
evalm(Bn): # MATRICE B(q) SEMPLIFICATA
8. Si calcola l’energia cinetica totale del manipolatore:
1
T = q̇ T B q̇
2
(2.36)
codice maple:
>
T:=(1/2)*evalm(evalm(evalm(transpose(v))&*B)&*v);
9. Si calcola l’energia potenziale del manipolatore:
U=
N
X
−Mi gd0z0i
(2.37)
i=1
codice maple:
>
U:=sum(’m[k2]*g*evalm(Qt[0,k2][3,4])’,’k2’=1..nrighe);
10. Note T e U si calcola la funzione lagrangiana:
L=T −U
(2.38)
codice maple:
>
L:=evalm(T)-evalm(U);
>
evalm(L)
quindi dall’equazione di Eulero-Lagrange:
d ∂L
∂L
−
= τi
dt ∂ q˙i
∂qi
i = 1, 2, ..., N
(2.39)
si ricavano le equazioni dinamiche del moto del manipolatore, che possiamo
scrivere in forma algebrica compatta come:
B(q)q̈ + C(q, q̇)q̇ + R(q)q̇ + h(q) = τ
Capitolo 2 Calcolo delle equazioni dinamiche del moto di un manipolatore
(2.40)
38
§ 2.4 Procedura e per il calcolo automatico dei ’coefficienti dinamici’
in realtà per il nostro robot, non avendo considerato le forze di attrito la 2.40
diventa:
B(q)q̈ + C(q, q̇)q̇ + h(q) = τ
(2.41)
dove B(q) è la matrice generalizzata d’inerzia, C(q, q̇) rappresenta la matrice
dei termini centrifughi e di Corilios, h(q) è il vettore delle forze gravitazionali,
τ il vettore delle forze/coppie esterne applicate ai giunti del robot.
codice maple:
Calcolo delle equazioni dinamiche del manipolatore:
>
>
>
>
for i9 from 1 to 6
do
Ldq[i9]:=map(diff,L,q[i9]);
end do:
>
for i10 from 1 to 6
do
Ldv[i10]:=map(diff,L,vapp[i10]);
end do:
for i11 from 1 to 6
do
Ldvold[i11]:=Ldv[i11];
Ldqold[i11]:=Ldq[i11];
for i12 from 1 to 6 do
Ldvnew[i11]:=subs(vapp[i12]=diff(q[i12],t)
,evalm(Ldvold[i11]));
Ldvold[i11]:=Ldvnew[i11];
Ldqnew[i11]:=subs(vapp[i12]=diff(q[i12],t)
,evalm(Ldqold[i11]));
Ldqold[i11]:=Ldqnew[i11];
end do;
end do:
for i13 from 1 to 6
do
Ldvt[i13]:=map(diff,Ldvnew[i13],t);
end do:
>
Eqn:=array(1..6,[]);
>
>
>
>
>
>
>
>
>
>
>
>
>
>
>
>
>
>
>
>
Capitolo 2 Calcolo delle equazioni dinamiche del moto di un manipolatore
39
§ 2.4 Procedura e per il calcolo automatico dei ’coefficienti dinamici’
>
>
>
>
Eqn := array(1..6, [])
for i14 from 1 to 6
do
Eqn[i14]:=evalm(Ldvt[i14])-evalm(Ldqnew[i14]);
end do:
Capitolo 2 Calcolo delle equazioni dinamiche del moto di un manipolatore
40
§ 2.4 Procedura e per il calcolo automatico dei ’coefficienti dinamici’
11. Riferendoci alla forma dell’equazione 2.41 e considerando nullo il vettore τ
calcoliamo i coefficienti dinamici, cioè gli elementi delle matrici B(q), C(q, q̇), h(q):
(a) B(i, j):
for i from 1 to N
do
for j from 1 to N
do
B(i, j) =
∂Equazionei
∂qj
end do
end do
(b) C(i, j):
for i from 1 to N
do
for j from 1 to N
do
C(i, j) =
1 ∂Equazionei
2
∂ q̇j
end do
end do
(c) Il vettore h(q) si ottiene valutando la 2.41 in q̈ = 0,q = 0
codice maple:
>
Binv:=array(1..6,1..6,[]);
Binv := array(1..6, 1..6, [])
Capitolo 2 Calcolo delle equazioni dinamiche del moto di un manipolatore
41
§ 2.4 Procedura e per il calcolo automatico dei ’coefficienti dinamici’
>
for i16 from 1 to 6
do
Binv[1,i16]:=(map(diff,Eqnnewb[1],aapp[i16]))[1,1];
end do:
for i16 from 1 to 6
do
Binv[2,i16]:=(map(diff,Eqnnewb[2],aapp[i16]))[1,1];
end do:
for i16 from 1 to 6
do
Binv[3,i16]:=(map(diff,Eqnnewb[3],aapp[i16]))[1,1];
end do:
for i16 from 1 to 6
do
Binv[4,i16]:=(map(diff,Eqnnewb[4],aapp[i16]))[1,1];
end do:
for i16 from 1 to 6
do
Binv[5,i16]:=(map(diff,Eqnnewb[5],aapp[i16]))[1,1];
end do:
for i16 from 1 to 6
do
Binv[6,i16]:=(map(diff,Eqnnewb[6],aapp[i16]))[1,1];
end do:
>
evalm(Binv):
>
############### Calcolo della matrice C ###############
>
Cmatrix:=array(1..6,1..6,[]);
>
>
>
>
>
>
>
>
>
>
>
>
>
>
>
>
>
>
>
>
>
>
>
Cmatrix := array(1..6, 1..6, [])
>
evalm(Cmatrix);
Capitolo 2 Calcolo delle equazioni dinamiche del moto di un manipolatore
42
§ 2.4 Procedura e per il calcolo automatico dei ’coefficienti dinamici’








Cmatrix 1, 1
Cmatrix 2, 1
Cmatrix 3, 1
Cmatrix 4, 1
Cmatrix 5, 1
Cmatrix 6, 1
Cmatrix 1, 2
Cmatrix 2, 2
Cmatrix 3, 2
Cmatrix 4, 2
Cmatrix 5, 2
Cmatrix 6, 2
Cmatrix 1, 3
Cmatrix 2, 3
Cmatrix 3, 3
Cmatrix 4, 3
Cmatrix 5, 3
Cmatrix 6, 3
Cmatrix 1, 4
Cmatrix 2, 4
Cmatrix 3, 4
Cmatrix 4, 4
Cmatrix 5, 4
Cmatrix 6, 4
Cmatrix 1, 5
Cmatrix 2, 5
Cmatrix 3, 5
Cmatrix 4, 5
Cmatrix 5, 5
Cmatrix 6, 5
Cmatrix 1, 6
Cmatrix 2, 6
Cmatrix 3, 6
Cmatrix 4, 6
Cmatrix 5, 6
Cmatrix 6, 6
>
for i from 1 to 6
do
for j from 1 to 6
do
Cmatrix[i,j]:=(1/2)*map(diff,Eqnnewb[i],vapp[j])[1,1];
end do:
end do:
>
########## Calcolo del vettore h(q) ########
>
h:=array(1..6,[]);
>
>
>
>
>
>








h := array(1..6, [])
>
evalm(h);
[h1 , h2 , h3 , h4 , h5 , h6]
>
>
>
>
>
h[1]:=subs(\{diff(q[1],t$2)=0,diff(q[2],t$2)=0,diff(q[3],t$2)=0,
diff(q[4],t$2)=0,diff(q[5],t$2)=0,diff(q[6],t$2)=0,diff(q[1],t)=0,
diff(q[2],t)=0,diff(q[3],t)=0,diff(q[4],t)=0,diff(q[5],t)=0,
diff(q[6],t)=0, evalm(Eqn[1]))[1,1];
h1 := 0.
>
>
>
>
>
>
>
h[2]:=subs( \{diff(q[1],t$2)=0,diff(q[2],t$2)=0,
diff(q[3],t$2)=0,diff(q[4],t$2)=0,diff(q[5],t$2)=0,
diff(q[6],t$2)=0,diff(q[1],t)=0,diff(q[2],t)=0,
diff(q[3],t)=0,diff(q[4],t)=0,diff(q[5],t)=0,
diff(q[6],t)=0,evalm(Eqn[2]))[1,1];
Capitolo 2 Calcolo delle equazioni dinamiche del moto di un manipolatore
43
§ 2.4 Procedura e per il calcolo automatico dei ’coefficienti dinamici’
h2 := 0.205 m2 g cos(q2 )
+ m3 g (0.034 cos(q2 ) sin(q3 ) + 0.034 sin(q2 ) cos(q3 ) + 0.5 cos(q2 ))
+ m4 g (−0.32 cos(q2 ) sin(q3 ) − 0.32 sin(q2 ) cos(q3 ) + 0.5 cos(q2 )) + m5 g(
−0.023 (−sin(q2 ) sin(q3 ) + cos(q2 ) cos(q3 )) cos(q4 ) sin(q5 )
+ 0.023 (−cos(q2 ) sin(q3 ) − sin(q2 ) cos(q3 )) cos(q5 ) − 0.73 cos(q2 ) sin(q3 )
− 0.73 sin(q2 ) cos(q3 ) + 0.5 cos(q2 )) + m6 g(
−0.2125 (−sin(q2 ) sin(q3 ) + cos(q2 ) cos(q3 )) cos(q4 ) sin(q5 )
+ 0.2125 (−cos(q2 ) sin(q3 ) − sin(q2 ) cos(q3 )) cos(q5 ) − 0.73 cos(q2 ) sin(q3 )
− 0.73 sin(q2 ) cos(q3 ) + 0.5 cos(q2 ))
>
>
>
>
>
>
>
h[3]:=subs( \{diff(q[1],t$2)=0,diff(q[2],t$2)=0,
diff(q[3],t$2)=0,diff(q[4],t$2)=0,diff(q[5],t$2)=0,
diff(q[6],t$2)=0,diff(q[1],t)=0,diff(q[2],t)=0,
diff(q[3],t)=0,diff(q[4],t)=0,diff(q[5],t)=0,
diff(q[6],t)=0,evalm(Eqn[3]))[1,1];
h3 := m3 g (0.034 sin(q2 ) cos(q3 ) + 0.034 cos(q2 ) sin(q3 ))
+ m4 g (−0.32 sin(q2 ) cos(q3 ) − 0.32 cos(q2 ) sin(q3 )) + m5 g(
−0.023 (−sin(q2 ) sin(q3 ) + cos(q2 ) cos(q3 )) cos(q4 ) sin(q5 )
+ 0.023 (−cos(q2 ) sin(q3 ) − sin(q2 ) cos(q3 )) cos(q5 ) − 0.73 sin(q2 ) cos(q3 )
− 0.73 cos(q2 ) sin(q3 )) + m6 g(
−0.2125 (−sin(q2 ) sin(q3 ) + cos(q2 ) cos(q3 )) cos(q4 ) sin(q5 )
+ 0.2125 (−cos(q2 ) sin(q3 ) − sin(q2 ) cos(q3 )) cos(q5 ) − 0.73 sin(q2 ) cos(q3 )
− 0.73 cos(q2 ) sin(q3 ))
>
>
>
>
>
>
>
h[4]:=subs(\{diff(q[1],t$2)=0,diff(q[2],t$2)=0,
diff(q[3],t$2)=0,diff(q[4],t$2)=0,diff(q[5],t$2)=0,
diff(q[6],t$2)=0,diff(q[1],t)=0,diff(q[2],t)=0,
diff(q[3],t)=0,diff(q[4],t)=0,diff(q[5],t)=0,
diff(q[6],t)=0,evalm(Eqn[4]))[1,1];
h4 := 0.023 m5 g (sin(q2 ) cos(q3 ) + cos(q2 ) sin(q3 )) sin(q4 ) sin(q5 )
+ 0.2125 m6 g (sin(q2 ) cos(q3 ) + cos(q2 ) sin(q3 )) sin(q4 ) sin(q5 )
>
>
>
>
>
>
>
h[5]:=subs(\{diff(q[1],t$2)=0,diff(q[2],t$2)=0,
diff(q[3],t$2)=0,diff(q[4],t$2)=0,diff(q[5],t$2)=0
,diff(q[6],t$2)=0,diff(q[1],t)=0,diff(q[2],t)=0,
diff(q[3],t)=0,diff(q[4],t)=0,diff(q[5],t)=0,
diff(q[6],t)=0,evalm(Eqn[5]))[1,1];
Capitolo 2 Calcolo delle equazioni dinamiche del moto di un manipolatore
44
§ 2.4 Procedura e per il calcolo automatico dei ’coefficienti dinamici’
h5 := m5 g(−0.023 (sin(q2 ) cos(q3 ) + cos(q2 ) sin(q3 )) cos(q4 ) cos(q5 )
− 0.023 (−sin(q2 ) sin(q3 ) + cos(q2 ) cos(q3 )) sin(q5 )) + m6 g(
−0.2125 (sin(q2 ) cos(q3 ) + cos(q2 ) sin(q3 )) cos(q4 ) cos(q5 )
− 0.2125 (−sin(q2 ) sin(q3 ) + cos(q2 ) cos(q3 )) sin(q5 ))
>
>
>
>
>
>
>
>
h[6]:=subs(\{diff(q[1],t$2)=0,diff(q[2],t$2)=0,
diff(q[3],t$2)=0,diff(q[4],t$2)=0,
diff(q[5],t$2)=0,diff(q[6],t$2)=0,
diff(q[1],t)=0,diff(q[2],t)=0,diff(q[3],t)=0,
diff(q[4],t)=0,diff(q[5],t)=0,diff(q[6],t)=0,
evalm(Eqn[6]))[1,1];
h6 := 0
12. Verifica dei risultati ottenuti, cioè calcolate le matrici B(q), C(q, q̇), h(q), si
verifica che sia soddisfatta l’equazione:
Eqn − [B(q)q̈ + C(q, q̇)q̇ + h(q)] = 0
(2.42)
dove Eqn è il vettore che contiene le N equazioni dinamiche del moto.
codice maple:
Verifica delle equazioni
>
>
>
>
>
>
>
>
>
>
>
>
a1:=evalm(simplify(Eqnnewb[1]))[1,1]:
b1:=expand(simplify(sum(’B[1,i]*aapp[i]’,’i’=1..6))):
c1:=expand(simplify((sum(’Cc[1,1,i]*vapp[i]’,’i’=1..6)*vapp[1])
+(sum(’Cc[1,2,i]*vapp[i]’,’i’=1..6)*vapp[2])+(sum(’Cc[1,3,i]*
vapp[i]’,’i’=1..6)*vapp[3])+(sum(’Cc[1,4,i]*vapp[i]’,’i’=1..6)*
vapp[4])+(sum(’Cc[1,5,i]*vapp[i]’,’i’=1..6)*vapp[5])+
(sum(’Cc[1,6,i]*vapp[i]’,’i’=1..6)*vapp[6])) ):
d1:=h[1];
d1 := 0.
>
r1:=simplify(a1-b1-c1-d1);
r1 := 0
>
>
>
>
>
>
>
a2:=evalm(simplify(Eqnnewb[2]))[1,1]:
b2:=expand(simplify(sum(’B[2,i]*aapp[i]’,’i’=1..6))):
c2:=expand(simplify((sum(’Cc[2,1,i]*vapp[i]’,’i’=1..6)*vapp[1])
+(sum(’Cc[2,2,i]*vapp[i]’,’i’=1..6)*vapp[2])+(sum(’Cc[2,3,i]*
Capitolo 2 Calcolo delle equazioni dinamiche del moto di un manipolatore
45
§ 2.4 Procedura e per il calcolo automatico dei ’coefficienti dinamici’
>
>
>
>
>
vapp[i]’,’i’=1..6)*vapp[3])+(sum(’Cc[2,4,i]*vapp[i]’,’i’=1..6)*
vapp[4])+(sum(’Cc[2,5,i]*vapp[i]’,’i’=1..6)*vapp[5])+
(sum(’Cc[2,6,i]*vapp[i]’,’i’=1..6)*vapp[6])) ):
>
r2:=simplify(a2-b2-c2-d2);
d2:=h[2]:
r2 := 0.
>
>
>
>
>
>
>
>
>
>
>
>
>
>
a3:=evalm(simplify(Eqnnewb[3]))[1,1]:
b3:=expand(simplify(sum(’B[3,i]*aapp[i]’,’i’=1..6))):
c3:=expand(simplify((sum(’Cc[3,1,i]*vapp[i]’,’i’=1..6)*vapp[1])+
(sum(’Cc[3,2,i]*vapp[i]’,’i’=1..6)*vapp[2])+(sum(’Cc[3,3,i]*
vapp[i]’,’i’=1..6)*vapp[3])+(sum(’Cc[3,4,i]*vapp[i]’,’i’=1..6)*
vapp[4])+(sum(’Cc[3,5,i]*vapp[i]’,’i’=1..6)*vapp[5])+
(sum(’Cc[3,6,i]*vapp[i]’,’i’=1..6)*vapp[6])) ):
d3:=h[3]:
r3:=simplify(a3-b3-c3-d3);
r3 := 0.
>
>
>
>
>
>
>
>
>
>
>
>
>
>
a4:=evalm(simplify(Eqnnewb[4]))[1,1]:
b4:=expand(simplify(sum(’B[4,i]*aapp[i]’,’i’=1..6))):
c4:=expand(simplify((sum(’Cc[4,1,i]*vapp[i]’,’i’=1..6)*vapp[1])+
(sum(’Cc[4,2,i]*vapp[i]’,’i’=1..6)*vapp[2])+(sum(’Cc[4,3,i]*
vapp[i]’,’i’=1..6)*vapp[3])+(sum(’Cc[4,4,i]*vapp[i]’,’i’=1..6)*
vapp[4])+(sum(’Cc[4,5,i]*vapp[i]’,’i’=1..6)*vapp[5])+
(sum(’Cc[4,6,i]*vapp[i]’,’i’=1..6)*vapp[6])) ):
d4:=h[4]:
r4:=simplify(a4-b4-c4-d4);
r4 := 0.
>
>
>
>
>
>
>
>
>
>
>
>
a5:=evalm(simplify(Eqnnewb[5]))[1,1]:
b5:=expand(simplify(sum(’B[5,i]*aapp[i]’,’i’=1..6))):
c5:=expand(simplify((sum(’Cc[5,1,i]*vapp[i]’,’i’=1..6)*vapp[1])+
(sum(’Cc[5,2,i]*vapp[i]’,’i’=1..6)*vapp[2])+(sum(’Cc[5,3,i]*
vapp[i]’,’i’=1..6)*vapp[3])+(sum(’Cc[5,4,i]*vapp[i]’,’i’=1..6)*
vapp[4])+(sum(’Cc[5,5,i]*vapp[i]’,’i’=1..6)*vapp[5])+
(sum(’Cc[5,6,i]*vapp[i]’,’i’=1..6)*vapp[6])) ):
d5:=h[5]:
Capitolo 2 Calcolo delle equazioni dinamiche del moto di un manipolatore
46
§ 2.4 Procedura e per il calcolo automatico dei ’coefficienti dinamici’
>
>
r5:=simplify(a5-b5-c5-d5);
r5 := 0.
>
>
>
>
>
>
>
>
>
>
>
>
>
>
a6:=evalm(simplify(Eqnnewb[6]))[1,1]:
b6:=expand(simplify(sum(’B[6,i]*aapp[i]’,’i’=1..6))):
c6:=expand(simplify((sum(’Cc[6,1,i]*vapp[i]’,’i’=1..6)*vapp[1])+
(sum(’Cc[6,2,i]*vapp[i]’,’i’=1..6)*vapp[2])+(sum(’Cc[6,3,i]*
vapp[i]’,’i’=1..6)*vapp[3])+(sum(’Cc[6,4,i]*vapp[i]’,’i’=1..6)*
vapp[4])+(sum(’Cc[6,5,i]*vapp[i]’,’i’=1..6)*vapp[5])+
(sum(’Cc[6,6,i]*vapp[i]’,’i’=1..6)*vapp[6])) ):
d6:=h[6]:
r6:=simplify(a6-b6-c6-d6);
r6 := 0
13. Scrittura del file manutecdyn.m che rappresenta il codice M atlab per il blocco
S-function robotdyn, che riproduce la dinamica del robot, e si trova all’interno
dello schema simulink dove vengono implementate le tecniche anti-windup
proposte nel capitolo 1.
File manutecdyn.m
>
>
>
>
>
>
>
>
>
>
>
>
>
>
>
>
>
>
>
>
fd := fopen("manutecdyn5.m", WRITE):
fprintf(fd, "\%cFunzione per il calcolo delle matrici
B,C,h\\n\\n",37):
fprintf(fd, "function [Bout,Cout,hout] = manutecdyn5(q,dq)
\\n\\n"):
fprintf(fd, "m=[0, 56.5, 26.4, 28.7, 5.2, 2.4063];\\n"):
fprintf(fd, "Ixx=[0,0.64,0.413,1.67,1.53,0.018125*2.4063/12];
\\n"):
fprintf(fd,"Iyy=[1.16,2.73,0.279,0.081,1.25,0.018125*2.4063/12]
;\\n"):
fprintf(fd, "Izz=[0,2.58,0.245,1.67,0.81,0.0050*2.4063/12];
\\n"):
fprintf(fd, "g=9.81;\\n\\n"):
for i from 1 to 6 do
fprintf(fd, "cq\%d=cos(q(\%d));\\nsq\%d=sin(q(\%d));
\\n",i,i,i,i):
od:
Capitolo 2 Calcolo delle equazioni dinamiche del moto di un manipolatore
47
§ 2.4 Procedura e per il calcolo automatico dei ’coefficienti dinamici’
>
>
>
>
>
>
>
>
>
>
>
>
>
>
>
>
>
>
>
>
>
>
>
>
>
fprintf(fd,
for i1 from
for i2 from
fprintf(fd,
od:
fprintf(fd,
od:
fprintf(fd,
"\\n\\n\\n"):
1 to 6 do
1 to 6 do
"B(\%d,\%d) = \%a;\\n",i1,i2,Bnew[i1,i2]):
for i1 from
for i2 from
fprintf(fd,
od:
fprintf(fd,
od:
fprintf(fd,
1 to 6 do
1 to 6 do
"C(\%d,\%d) = \%a;\\n",i1,i2,Cnew[i1,i2]):
"\\n"):
"\\nBout=B;\\n\\n"):
"\\n"):
"\\nCout=C;\\n\\n"):
for i1 from 1 to 6 do
fprintf(fd, "h(\%d) = \%a;\\n",i1,hnew[i1]):
od:
fprintf(fd,
"\\nhout=[h(\%d);h(\%d);h(\%d);h(\%d);h(\%d);h(\%d)];
\\n\\n",1,2,3,4,5,6):
fclose(fd):
File manutech.m
>
>
>
>
>
>
>
>
>
>
>
>
>
>
>
>
>
>
>
fd := fopen("manutech5.m", WRITE): fprintf(fd, "\%cFunzione per
il calcolo del vettore h\\n\\n",37):
fprintf(fd, "function [hout] = manutech5(q,dq)\\n\\n"):
fprintf(fd, "m=[0, 56.5, 26.4, 28.7, 5.2, 2.4063];\\n"):
fprintf(fd,
"Ixx=[0,0.64,0.413,1.67,1.53,0.018125*2.4063/12];\\n"):
fprintf(fd,
"Iyy=[1.16,2.73,0.279,0.081,1.25,0.018125*2.4063/12];\\n"):
fprintf(fd, "Izz=[0,2.58,0.245,1.67,0.81,0.0050*2.4063/12];
\\n"):
fprintf(fd, "g=9.81;\\n\\n"): for i from 1 to 6 do fprintf(fd,
"cq\%d=cos(q(\%d));\\nsq\%d=sin(q(\%d));\\n",i,i,i,i): od:
fprintf(fd, "\\n\\n"): for i1 from 1 to 6 do fprintf(fd,
"h(\%d) =\%a;\\n",i1,hnew[i1]); od:
fprintf(fd, "\\nhout=[h(\%d);h(\%d);h(\%d);h(\%d);h(\%d);
h(\%d)];\\n\\n",1,2,3,4,5,6):
fclose(fd):
Capitolo 2 Calcolo delle equazioni dinamiche del moto di un manipolatore
48
Capitolo 3
SimMechanics
3.1
Introduzione
Questo capitolo costituisce una breve guida introduttiva all’uso del toolbox MATLAB, SimMechanics. Più precisamente, vengono fornite solo le informazioni relative agli elementi necessari ed essenziali per analizzare il comportamento di un
particolare modello meccanico. Il modello in questione è quello del robot industriale Siemens-Manutec r3, a cui si è fatto riferimento per verificare la correttezza del
modello semplificato, descritto dai coefficienti dinamici, calcolati con la procedura
automatica proposta nel capitolo 2. Dopo aver spiegato che cos’è il SimMechanics, e quali sono i passi essenziali per la realizzazione di un sistema, vedremo in
dettaglio come è strutturato il programma, quali sono le sue librerie e che tipo
di blocchi contengono. In particolare, lo studio dei blocchi sarà fondamentale per
capire come è stato realizzato e come funziona il modello SimMechanics del nostro
manipolatore.
3.2
Che cos’è SimMechanics
SimMechanics è un toolbox MATLAB per la progettazione e la simulazione dei
sistemi meccanici. I sistemi meccanici considerati, sono costituiti da corpi rigidi
connessi mediante giunti e rispettano le leggi classiche della dinamica Newtoniana.
Il SimMechanics è in grado di simulare i movimenti rotazionali e traslazionali di un
corpo rigido nello spazio. Per semplificare la modellazione e lo studio dei sistemi, il
49
§ 3.3 Creare un modello SimMechanics
programma fornisce all’utente numerosi menù di dialogo, che permettono, ad esempio, di specificare le caratteristiche geometriche e fisiche del corpo, oppure rilevare
facilmente grandezze come posizione, velocità ed accelerazione. Inoltre SimMechanics si intengra bene con l’ambiente Simulink del MATLAB, dato che come
in Simulink la rappresentazione dei sistemi avviene sempre mediante gli schemi a
blocchi. Infine, questo toolbox MATLAB consente la visualizzazione in tempo reale e in tre dimensioni del moto del meccanismo simulato avvalendosi degli ambienti
grafici:
• M AT LAB Handle Graphicsr windows;
• Virtual world rendered in a virtual reality viewer.
3.3
Creare un modello SimMechanics
Viene di seguito descritta la procedura di base per la creazione di un modello SimMechanics. Questo tipo di procedura risulta valida per meccanismi sia semplici che
complessi. In ogni caso la realizzazione di modelli più complessi potrà richiedere
solo l’uso di alcuni step aggiuntivi.
Procedura per la creazione di un modello SimMechanics:
1. Scelta dei Ground, Body e Joint blocks. Dalle librerie Bodies e Joints selezioniamo e trasciniamo nella finestra Simulink i blocchi necessari per rappresentare il meccanismo. Attenzione, è necessario includere almeno un Ground
Block all’interno del nostro schema. In particolare:
• I ground blocks sono degli speciali Body blocks e rappresentano un punto
di supporto immobile nello spazio inerziale;
• i Body blocks rappresentano i corpi rigidi;
• i Joint blocks permettono il movimento relativo tra i corpi ai quali sono
connessi.
Capitolo 3 SimMechanics
50
§ 3.3 Creare un modello SimMechanics
2. Posizionamento e connessione dei blocchi. Posizioniamo i giunti e i corpi
rigidi nelle loro posizioni relative per poi conneterli nel giusto ordine. Il
risultato è una sequenza del tipo:
Ground − Joints − Body − Joints − Body − ... − Body
nella quale ci sia almeno un Ground block e non ci sia mai un giunto (Joint)
connesso a più di due corpi (Body).
3. Configurazione dei Body blocks. Cliccando su ciascun Body block, apriamo
la relativa finestra di dialogo in cui impostare massa e momento di inerzia
del corpo, posizione e orientamento dei sistemi di riferimento (CSs), solidali
al corpo, rispetto al sistema di riferimento inerziale (W orld CS). Non bisogna dimenticare di specificare, sempre rispetto al sistema di riferimento
(W orld CS), le coordinate e l’orientamento del Ground point.
4. Configurazione dei Joint blocks. Nella finestra di dialogo del Joint block
vanno specificati gli assi di traslazione e di rotazione. In alternativa si può
scegliere direttamente il tipo di giunto: rotoidale, sferico, prismatico...
5. Scelta, connessione e configurazione dei Constraint & Driver blocks necessari
per limitare o aumentare i gradi di libertà relativi tra i corpi.
6. Scelta, connessione e configurazione degli Actuator and Sensor blocks. Dalla
libreria Sensors & Actuators, scegliamo gli attuatori e i sensori necessari,
rispettivamente, per imporre e misurare il moto tra i corpi.
7. Creazione di un sottosistema. Il risultato degli step 1 − 6 è un sistema Simulink realizzato con il toolbox SimMechanics. In realtà questo sistema
può essere contenuto in un blocco subsystem e funzionare quindi come un
sottosistema per un normale modello Simulink più complesso.
Capitolo 3 SimMechanics
51
§ 3.4 Librerie
3.4
Librerie
I blocchi SimMechanics sono organizzati per categorie all’interno di apposite librerie. Per visualizzarle è sufficiente digitare, nella finestra di comando del MATLAB, il comando mechlib. Il risultato è quello che vediamo nella figura (3.1),
dove vengono mostrate tutte le librerie disponibili:
Figura 3.1: Librerie SimMechanics.
Bodies Library: contiene i blocchi per la rappresentazione dei corpi rigidi e dei
’ground points’.
Joints Library: contiene i blocchi per la rappresentazione dei gradi di libertà
(DoFs) di moto tra i corpi.
Constraints & Drivers Library: contiene i blocchi per limitare o aumentare i
gradi di libertà relativi tra i corpi.
Sensors & Actuators Library: contiene i blocchi per l’attuazione e la misurazione del moto.
Utilities Library: Contiene dei blocchi addizionali utili per la modellazione e la
simulazione dei meccanismi.
Demos Library Contiene dei modelli dimostrativi realizzati con il SimMechanics.
Capitolo 3 SimMechanics
52
§ 3.5 Blocchi
3.5
Blocchi
In questa sezione verranno introdotti i blocchi necessari per lo studio e l’analisi del
modello relativo al manipolatore Siemens-Manutec r3.
3.5.1
Body
Figura 3.2: Body block.
Libreria: Bodies
Descrizione: Il Body block in figura 3.2 serve per rappresentare un corpo rigido
mediante i seguenti parametri:
1. Massa e tensore d’inerzia;
2. Coordinate del centro di gravità (CG);
3. Un certo numero di sistemi di riferimento (CSs), necessari per individuare
la posizione e l’orientamento del corpo nello spazio.
Con riferimento alla figura 3.3, vediamo che la finestra di dialogo, del blocco Body,
è divisa in due aree. Nella prima area ci sono i campi per la massa ed il tensore
d’inerzia. Nella seconda invece sono presenti degli appositi spazi in cui vanno
inserite le coordinate dei sistemi di riferimento, (CSs), solidali al corpo rigido.
Inserire i valori è molto semplice. La sintassi è quella classica del MATLAB, e le
unità di misura, per le grandezze da indicare, si trovano già all’interno dei menù
0
U nits0 .
Capitolo 3 SimMechanics
53
§ 3.5 Blocchi
Dialog Box:
Figura 3.3: Body’s dialog box.
Capitolo 3 SimMechanics
54
§ 3.5 Blocchi
3.5.2
Ground
Figura 3.4: Ground block.
Libreria: Bodies
Descrizione: Il Ground block rappresenta un punto immobile rispetto al sistema
di riferimento inerziale SimMechanics, detto W orld CS. Come si può notare in
figura 3.5 gli unici dati richiesti per questo oggetto, dato che è un punto, sono
proprio le sue coordinate.
Dialog Box:
Figura 3.5: Ground’s dialog box.
Capitolo 3 SimMechanics
55
§ 3.5 Blocchi
3.5.3
Revolute
Figura 3.6: Revolute block.
Libreria: Joints
Descrizione: Il Revolute block rappresenta un giunto rotazionale. Quindi esprime
il solo grado di libertà di rotazione intorno ad uno specifico asse compreso tra
due corpi. Il blocco si connette al link precedente e a quello successivo mediante
le due porte B (Base) ed F (F ollower) visibili in figura 3.6. Al Joint block, si
possono aggiungere anche i blocchi sensori ed attuatori, necessari per misurare
ed azionare il movimento (vedi figura 3.7). Il numero di sensori desiderati viene
indicato nell’apposito spazio della finestra di dialogo mostrata in figura 3.8.
Figura 3.7: Joint block a cui sono connessi sensori ed attuatori.
Capitolo 3 SimMechanics
56
§ 3.5 Blocchi
Dialog Box:
Figura 3.8: Revolute’s dialog box.
Capitolo 3 SimMechanics
57
§ 3.5 Blocchi
3.5.4
Joint Actuator
Figura 3.9: Joint Actuator block.
Libreria: Sensor & Actuators
Descrizione: Il Joint Actuator block permette di attuare il moto al giunto a cui
è connesso, mediante uno dei seguenti segnali (vedi figura 3.10):
• Forza per imprimere un movimento traslazionale ai giunti prismatici;
• Coppia per imprimere un movimento rotazionale ai giunto rotoidali;
• Movimento traslazionale per giunto prismatico, espresso in termini di posizione, velocità ed accelerazione lineare;
• Movimento rotazionale per giunto rotoidale, espresso in termini di posizione,
velocità ed accelerazione angolare.
Le forze, le coppie e i movimenti rotazionali/trazionali sono funzioni del tempo
specificate tramite i segnali d’ingresso Simulink. E’ importante notare che anche i
segnali di controreazione forniti dai Joint Sensor block, costituiscono un possibile
segnale in grado di azionare il movimento del giunto.
Capitolo 3 SimMechanics
58
§ 3.5 Blocchi
Dialog Box:
Figura 3.10: Actuator’s dialog box.
Capitolo 3 SimMechanics
59
§ 3.5 Blocchi
3.5.5
Joint Sensor
Figura 3.11: Joint Sensor block.
Libreria: Sensor & Actuators
Descrizione: Il Joint Sensor block consente di misurare il movimento e la coppia/forza applicate al giunto di un meccanismo. A seconda del tipo di giunto a cui
è connesso, il Sensor block permette di rilevare uno dei seguenti tipi di movimento
(vedi fig 3.12):
• Movimento traslazionale di un giunto prismatico, in termini di posizione,
velocità ed accelerazione lineare;
• Movimento rotazionale di un giunto rotoidale, in termini di posizione, velocità
ed accelerazione angolare;
• Movimento sferico di un giunto sferico, in termini di quaternione e di derivata
prima e seconda del quaternione.
L’output del Joint Sensor block è un set di segnali Simulink, che possono essere
multiplati su un’unica uscita, che collegata al blocco Simulink, Scope, mostra a
video l’andamento della posizione, velocità ed accelerazione rilevate sul giunto.
Capitolo 3 SimMechanics
60
§ 3.5 Blocchi
Dialog Box:
Figura 3.12: Sensor’s dialog box.
Capitolo 3 SimMechanics
61
§ 3.5 Blocchi
3.5.6
Joint Initial Condition Actuator
Figura 3.13: Joint Initial Condition Actuator block.
Libreria: Sensor & Actuators
Descrizione: Il Joint Initial Condition Actuator block serve per fissare i valori
della posizione e della velocità iniziale di un giunto, necessari per la correttezza
della simulazione. A seconda del tipo di giunto a cui è connesso, il blocco modifica
automaticamente le unità di misura relative alle grandezze da specificare (vedi ad
esempio la figura 3.14 relativa ad un giunto rotoidale).
Dialog Box:
Figura 3.14: IC’s dialog box.
Capitolo 3 SimMechanics
62
§ 3.5 Blocchi
3.5.7
Continuous Angle
Figura 3.15: Continuous Angle block.
Libreria: Utilities
Descrizione: Il Continuous Angle block converte una misura angolare compresa
nell’intervallo (−180◦ , +180◦ ] gradi o (−π, +π] radianti, in una variabile angolare,
funzione del tempo, continua e non più contenuta in un intervallo. In particolare,
il blocco riceve in input posizione e velocità angolare (vedi figura 3.16), e fornisce
in output il segnale di posizione angolare, continuo e non limitato.
Dialog Box:
Figura 3.16: Continuos Angle’s dialog box.
Capitolo 3 SimMechanics
63
§ 3.6 Modello SimMechanics del robot Siemens-Manutec r3
3.6
Modello SimMechanics del robot SiemensManutec r3
Il file dimostrativo mech robot.mdl del SimMechanics, contiene il modello del robot
Siemens-Manutec r3. Si tratta di un modello dinamico molto dettagliato, in cui
sono inclusi tutti gli effetti fisici necessari per avere un sistema che sia quanto più
possibile fedele alla realtà. Per quanto riguarda i parametri del manipolatore, si fa
riferimento alla guida ’The Manutec r3 Benchmark Models for Dynamic Simulation
of Robots’, che si trova on-line sul sito internet www.citeseer.com.
Con riferimento alla figura 3.17, che mostra un’immagine del robot, cominciamo a
descrivere lo schema a blocchi del manipolatore r3. In SimMechanics, un sistema
Figura 3.17: Robot Siemens-Manutec r3.
meccanico complesso, viene suddiviso in più sottosistemi elementari. In particolare,
quindi, aprendo la maschera in figura 3.17, che viene mostrata a video appena
lanciato il file mech robot.mdl, si può accedere (con un doppio clic del tasto sinistro
del mouse) alla sottomaschera mech robot\robot e a tutti i successivi sottosistemi
che costituiscono il manipolatore. La sottomaschera mech robot\robot in figura
3.18 mostra il link di base (link 0) e gli altri sei link del robot, connessi tra loro
Capitolo 3 SimMechanics
64
§ 3.6 Modello SimMechanics del robot Siemens-Manutec r3
Figura 3.18: Link del robot SMr3.
a formare la struttura del manipolatore. Di nuovo, ad ogni link è associata una
maschera, un blocco che riporta l’immagine del link stesso, e che ha un ingresso ed
un’uscita che lo connettono rispettivamente, al link precedente e a quello successivo.
In dettaglio ciascun blocco link è costituito (vedi figura 3.19) da:
1. Un blocco acceleration che contiene l’accelerazione desiserata per il giunto,
che verrà successimente derivata due volte per ottenere, rispettivamente, la
velocità e la posizione desiderata del giunto;
2. un blocco BASE che permette il collegamento con il link precedente;
3. un blocco FOLLOWER che permette il collegamento al link successivo;
4. un blocco BODY in cui sono specificate le caratteristiche geometrice e l’inerzia del link;
Capitolo 3 SimMechanics
65
§ 3.6 Modello SimMechanics del robot Siemens-Manutec r3
5. un blocco DRIVE che contiene la parte relativa al motore e al controllore del
giunto.
Figura 3.19: Blocco link.
Il blocco DRIVE in realtà è ancora un sottosistema. Inoltre i DRIVE block dei
link 4,5,6 contengono uno schema a blocchi diverso da quello relativo ai link 1,2,3.
Tuttavia, come vedremo, le differenze non sono poi cosı̀ notevoli. Con riferimento
alla figura 3.20, analizziamo il sottosistema DRIVE relativo ai link 1,2,3. Come
possiamo vedere, lo schema a blocchi di figura 3.20 è costituito da:
1. Un segnale di ingresso, acceleration, che viene derivato due volte dai blocchi
integratori, (1/s), per ottenere velocità ed accelerazione angolare di riferimento;
2. Un blocco subsystem, controller, che implementa la funzione di trasferimento
del controllore del motore;
3. Un blocco subsystem, motor circuity, relativo al motore elettrico montato sul
giunto del robot;
4. Un blocco subsystem, Coulomb friction, che modella le nonlinearità del sistema;
Capitolo 3 SimMechanics
66
§ 3.6 Modello SimMechanics del robot Siemens-Manutec r3
Figura 3.20: Blocco DRIVE per i link 1,2,3.
5. Due blocchi Revolute, uno per considerare l’inerzia del link, l’altro per descriverne il moto rotazionale;
6. Due blocchi Continuous angle;
7. Un blocco Joint Actuator, connesso al blocco Revolute, per imprimere la
coppia desiderata sul giunto;
8. Due blocchi Simulink, Gain, in sono memorizzate alcune costanti caratteristiche del giunto.
All’interno del blocco Continuous angle, contrassegnato con un asterisco in figura
3.20, si trovano due blocchi Simulink, Goto, ed un blocco Joint Sensor (vedi figura
3.21). I Goto blocks inviano i due segnali di posizione e velocità angolare, rilevati
sul giunto dal Joint Sensor, ai blocchi di tipo From, Body e rate, contenuti nei
blocchi angular position sm ed angular rate sm mostrati in figura 3.19.
Con
riferimento alla figura 3.22, analizziamo ora lo schema DRIVE dei link 4,5,6, che
risulta costiutito dai seguenti elementi:
1. Un segnale di ingresso, acceleration, che viene derivato due volte dai blocchi
Capitolo 3 SimMechanics
67
§ 3.6 Modello SimMechanics del robot Siemens-Manutec r3
Figura 3.21: Continuous Angle block *.
Figura 3.22: Blocco DRIVE per i link 4,5,6.
integratori, (1/s), per ottenere velocità ed accelerazione angolare di riferimento;
2. Un blocco subsystem, controller, che implementa la funzione di trasferimento
del controllore del motore;
3. Un blocco subsystem, motor circuity, relativo al motore elettrico montato sul
giunto del robot;
4. Un blocco subsystem, Coulomb friction, che modella le nonlinearità del sistema;
5. Un blocco subsystem, ideal gear, contenente i blocchi Revolute, Joint Ac-
Capitolo 3 SimMechanics
68
§ 3.6 Modello SimMechanics del robot Siemens-Manutec r3
tuator, Joint Sensor e Continuous angle necessari per azionare e rilevare il
movimento del link.
Come nel caso dei link 1,2,3, il Continuous angle block, contenuto nel blocco ideal
gear, fornisce i valori istantanei di posizione e velocità angolare rilevati sul giunto
dal Joint Sensor block.
I suddetti segnali di posizione e velocità angolare, forniti dallo schema di ciascun
link, sono proprio quelli che permettono di confrontare e verificare la correttezza
del modello semplificato del robot ottenuto con la procedura suggerita nel Capitolo
2. In conclusione, il SimMechanics consente di riprodurre in modo dettagliato
lo schema di controllo del robot SMr3 mostrato in figura 3.23, e di simulare e
visualizzare in tempo reale il movimento, desiderato, compiuto dal manipolatore
(vedi figura 3.24). Per visualizzare il robot è sufficiente impostare, prima di lanciare
Figura 3.23: Schema di controllo del manipolatore.
la simulazione, l’ambiente grafico desiderato all’interno del Dialog Box, Mechanical
enviroment, a cui si accede dal menù Simulation della finestra di lavoro Simulink.
Capitolo 3 SimMechanics
69
§ 3.6 Modello SimMechanics del robot Siemens-Manutec r3
Click On Object To Display Information
1
0.8
0.6
Y−axis
0.4
0.2
0
−0.2
−0.4
−0.8
−0.6
−0.4
−0.2
0
0.2
0.4
0.6
0.8
1
X−axis
Figura 3.24: MATLAB Graphics.
Capitolo 3 SimMechanics
70
Capitolo 4
Simulazioni
4.1
Introduzione
In questo capitolo si riportano i risultati forniti dalle simulazioni eseguite con il
toolbox Simulink 5.0 del M AT LAB R13. Lo studio dei grafici forniti dagli schemi
di simulazione è molto importante perché costituisce una verifica diretta della validità e dell’efficienza delle leggi di compensazione, soluzioni del problema “windup
per manipolatori robotici”, oggetto di questo lavoro. Verrà presentata per prima la
simulazione relativa alla verifica della validità del modello semplificato, del robot
Siemens-Manutec r3, descritto mediante i coefficienti dinamici forniti dalla procedura di calcolo automatico introdotta nel capitolo 2. Seguiranno le simulazioni
effettuate sul modello semplificato del robot, in cui vengono implementate le leggi
anti-windup descritte nel capitolo 1. Le leggi simulate sono quella relativa alla
soluzione non lineare discussa nel paragrafo §1.2, e quella relativa alla soluzione
non lineare generalizzata in cui viene modificato il segnale di compensazione v1
descritta nel §1.3.
4.2
Congruenza dei modelli del robot
Verifichiamo la corrispondenza tra i due modelli del robot Siemens-Manutec r3 :
quello contenuto nel file dimostrativo mech robot.mdl, e quello ottenuto tramite
la procedura M aple di calcolo automatico dei coefficienti dinamici. Per effettuare
questa verifica, il modello dettagliato SimMechanics è stato leggermente semplifica-
71
§ 4.2 Congruenza dei modelli del robot
to, cioè sono stati rimossi i blocchi che implementavano le dinamiche degli attuatori
e dei sensori. Le figure 4.1, 4.2 mostrano gli schemi a blocchi semplificati, relativi
rispettivamente ai link 1, 2, 3 ed ai link 4, 5, 6.
Come si può facilmente vedere,
Figura 4.1: Blocco DRIVE semplificato per i link 1,2,3.
Figura 4.2: Blocco DRIVE semplificato per i link 4,5,6.
la parte restante all’interno di ciascuno schema è quella relativa solo alla dinamica
e alla rilevazione del moto del robot. In questo modo è possibile agire su ciascun
giunto con un segnale di coppia τi e rilevare mediante i Joint Sensor block, contenuti all’interno dei blocchi continuos angle ed ideal gear, i segnali di posizione e
velocità angolare necessari per il confronto. Nella figura 4.3 è riportato lo schema a
blocchi che consente di eseguire il matching dei due robot. Lo schema è costituito
da:
1. un blocco angular position ref. che fornisce il valore desiderato per la variabile
di posizione angolare del giunto;
2. due blocchi unconstrained che ricevono in input il segnale fornito dal blocco
angular position ref. e forniscono in output i segnali di controllo (cont), di
Capitolo 4 Simulazioni
72
§ 4.2 Congruenza dei modelli del robot
Figura 4.3: Schema a ciclo chiuso per il matching dei due modelli.
posizione (pos) e velocità angolare (rate) relativi al modello SimMechanics
semplificato e al modello fornito dalla procedura M aple;
3. un blocco position che permette di visualizzare l’andamento dei segnali di
posizione (pos) forniti dai blocchi unconstrained ;
4. un blocco angular rate che permette di visualizzare l’andamento dei segnali
di velocità angolare (rate) forniti dai blocchi unconstrained ;
5. un blocco controller signal che consente di visualizzare l’andamento dei segnali di controllo.
I blocchi unconstrained in realtà rappresentano due sottosistemi che realizzano lo
schema di controllo a ciclo chiuso relativo al modello considerato del robot SiemensManutec r3. Nelle figure 4.4, 4.5 sono mostrati gli schemi a blocchi dei due modelli
del robot.
I due schemi differiscono solo per i blocchi robotdyn e robot sm che
implemantano in due modi diversi la dinamica del manipolatore. A parità di segnali
di coppia forniti in ingresso dal controllore, gli schemi generano in uscita dei segnali
per le corrispondenti variabili di posizione e velocità angolare, che risultano essere
quasi identitici tra loro. Ciò può essere facilmente verificato osservando le figure
4.6 - 4.11 che riportano i grafici dei risultati forniti dalle simulazioni riguardo ai
suddetti segnali. In particolare la figura 4.8 evidenzia l’errore commesso al giunto
Capitolo 4 Simulazioni
73
§ 4.2 Congruenza dei modelli del robot
Figura 4.4: Schema di controllo a ciclo chiuso per il modello SimMechanics del
robot.
Figura 4.5: Schema di controllo a ciclo chiuso per il modello del robot derivato
dalla procedura Maple.
3 del manipolatore. Il motivo per cui questi segnali non coincidono perfettamente,
è legato al modo in cui vengono considerate le matrici d’inerzia per la derivazione
delle equazioni dinamiche del moto. All’interno della procedura Maple, infatti,
per semplificare la complessità dell’algoritmo di calcolo, tutte le matrici d’inerzia
dei link del robot sono state considerate diagonali. In SimMechanics, invece, dove
la modellazione risulta più accurata, le matrici d’inerzia dei link 2 e 3 non sono
diagonali. Tuttavia se a parità di ingressi otteniamo delle uscite simili possiamo
concludere che il modello dinamico derivato dalla procedura di calcolo automatico
è fedele a quello originale semplificato, e può essere utilizzato per studiare in modo
significativo l’effetto delle leggi di compensazione anti-windup per i manipolatori
robotici.
Capitolo 4 Simulazioni
74
giunto 1 [gradi]
§ 4.2 Congruenza dei modelli del robot
60
40
20
0
Smodel
Mmodel
0
0.5
1
1.5
2
2.5
3
3.5
4
4.5
5
giunto2 [gradi]
160
140
120
100
80
Smodel
Mmodel
0
0.5
1
1.5
2
2.5
3
3.5
4
4.5
5
giunto 3 [gradi]
−50
−60
−70
−80
Smodel
Mmodel
−90
−100
0
0.5
1
1.5
2
2.5
tempo [sec.]
3
3.5
4
4.5
5
Figura 4.6: Posizione angolare dei giunti 1,2,3: Smodel ≡ modello SimM echanics,
M model ≡ modello M aple.
giunto 4 [gradi]
60
40
20
giunto 5 [gradi]
0
Smodel
Mmodel
0
0.5
1
1.5
2
2.5
3
3.5
4
4.5
5
60
40
20
0
Smodel
Mmodel
0
0.5
1
1.5
2
2.5
3
3.5
4
4.5
5
giunto 6 [gradi]
50
40
30
20
Smodel
Mmodel
10
0
0
0.5
1
1.5
2
2.5
tempo [sec.]
3
3.5
4
4.5
5
Figura 4.7: Posizione angolare dei giunti 4,5,6: Smodel ≡ modello SimM echanics,
M model ≡ modello M aple.
Capitolo 4 Simulazioni
75
giunto 1 [gradi/sec]
§ 4.2 Congruenza dei modelli del robot
80
Smodel
Mmodel
60
40
20
0
0
0.5
1
1.5
2
2.5
3
3.5
4
4.5
5
giunto 2 [gradi/sec]
60
Smodel
Mmodel
40
20
0
−20
0
0.5
1
1.5
2
2.5
3
3.5
4
4.5
5
giunto 3 [gradi/sec]
20
Smodel
Mmodel
10
0
−10
0
0.5
1
1.5
2
2.5
tempo [sec.]
3
3.5
4
4.5
5
giunto 4 [gradi/sec]
Figura 4.8: Velocità angolare dei giunti 1,2,3: Smodel ≡ modello SimM echanics,
M model ≡ modello M aple.
80
Smodel
Mmodel
60
40
20
0
giunto 5 [gradi/sec]
0
1
1.5
2
2.5
3
3.5
4
4.5
5
Smodel
Mmodel
100
50
0
0
giunto 6 [gradi/sec]
0.5
0.5
1
1.5
2
2.5
3
3.5
4
4.5
5
Smodel
Mmodel
100
50
0
0
0.5
1
1.5
2
2.5
tempo [sec.]
3
3.5
4
4.5
5
Figura 4.9: Velocità angolare dei giunti 4,5,6: Smodel ≡ modello SimM echanics,
M model ≡ modello M aple.
Capitolo 4 Simulazioni
76
§ 4.2 Congruenza dei modelli del robot
giunto 1 [Nm]
60
20
0
giunto 2 [Nm]
−20
0
0.5
1
1.5
2
2.5
3
3.5
4
4.5
5
Smodel
Mmodel
0
−200
−400
−600
giunto 3 [Nm]
Smodel
Mmodel
40
0
0.5
1
1.5
2
2.5
3
3.5
4
4.5
5
Smodel
Mmodel
0
−50
−100
−150
0
0.5
1
1.5
2
2.5
tempo [sec.]
3
3.5
Figura 4.10:
Segnale di controllo dei giunti
modello SimM echanics, M model ≡ modello M aple.
4
4.5
1,2,3:
5
Smodel
≡
giunto 4 [Nm]
60
Smodel
Mmodel
40
20
0
−20
0
0.5
1
1.5
2
2.5
3
3.5
4
4.5
5
giunto 5 [Nm]
60
Smodel
Mmodel
40
20
0
−20
giunto 6 [Nm]
0
0.5
1
1.5
2
2.5
3
3.5
4
4.5
5
Smodel
Mmodel
40
20
0
−20
0
0.5
1
1.5
2
2.5
tempo [sec.]
3
3.5
Figura 4.11:
Segnale di controllo dei giunti
modello SimM echanics, M model ≡ modello M aple.
Capitolo 4 Simulazioni
4
4.5
4,5,6:
5
Smodel
≡
77
§ 4.3 Anti-windup applicato al robot SM-r3
4.3
Anti-windup applicato al robot SM-r3
4.3.1
Soluzione non lineare
Figura 4.12: Schema a blocchi per la simulazione sul robot SM-r3.
Lo schema di figura 4.12 consente di simulare il comportamento del robot
Siemens-Manutec r3 in tre diversi casi d’interesse:
1. blocco unconstrained : sistema non soggetto alla saturazione (vedi figura 4.5);
2. blocco sat: sistema soggetto alla saturazione dei segnali di controllo (vedi
figura 4.13);
3. blocco anti-windup: sistema soggetto alla saturazione dei segnali di controllo
e dotato del compensatore anti-windup (vedi figura 4.14).
Con riferimento alla figura 4.14 analizziamo in dettaglio l’azione prodotta da ciascun blocco dello schema di controllo a ciclo chiuso dotato del compensatore
anti-windup:
1. il blocco In1 invia allo schema il segnale di riferimento di posizione angolare;
Capitolo 4 Simulazioni
78
§ 4.3 Anti-windup applicato al robot SM-r3
Figura 4.13: Blocco sat.
Figura 4.14: Blocco anti-windup.
2. il blocco locale è un controllore PID ad inversione della dinamica che consente
l’inseguimento della traiettoria desiderata. Il blocco riceve in input il segnale
d’ingresso In1 e il segnale di controreazione uc , e fornisce in uscita il segnale
di controllo (le coppie da applicare ai giunti del robot) secondo le equazioni:
uc = (q, q̇)
q = retroazione di posizione
q̇ = retroazione di velocita0
r = In1
ẋc = q − r
yc = B(q) (−Kp (q − r) − Kd q̇ − Ki xc )
+ C(q, q̇)q̇ + R(q)(q̇) + h(q)
dove xc ∈ Rn è lo stato del controllore e Kp , Kd , Ki sono delle matrici quadraCapitolo 4 Simulazioni
79
§ 4.3 Anti-windup applicato al robot SM-r3
te (tipicamente diagonali) scelte in maniera oppurtuna secondo le equazioni
(1.1), (1.2), (1.3) del capitolo 1.
Per la simulazione abbiamo scelto:
Kp = diag([100 100 100 100 100 100]/4)
Kd = diag([10 10 10 10 10 10])
Ki = diag(5 · [1 1 1 1 1 1])
3. il blocco saturazione contiene i livelli di saturazione mi , i = 1 . . . N , relativi
agli N attuatori dei giunti del robt;
4. il blocco ROBOT contiene la dinamica del robot stesso. Sostanzialmente
riceve in input i segnali di coppia forniti dal controllore (ed eventualmente
saturati), e fornisce in output le variabili di posizione e velocità angolari
corrispondenti al movimento compiuto dal manipolatore sotto l’azione del
segnale di controllo;
5. il blocco anti-windup riproduce la dinamica del compensatore descritta dalle
seguenti equazioni:
x = (q, q̇) stato del robot
xe = (qe , q̇e ) stato del compensatore
yc = uscita del controllore
up = sat(yc )
q̈e = B −1 (q)(sat(yc + v1 ) − C(q, q̇)q̇ − R(q)q̇ − h(q))
− B −1 (q − qe )(yc − C(q − qe , q̇ − q̇e )(q̇ − q̇e )
− R(q − qe )(q̇ − q̇e ) − h(q − qe ))
v1 = β(x, xe ) := h(q) − h(q − qe ) − Kg sat(Kg−1 qe ) − K0 q̇e
v2 = −xe = −(qe , q̇e )
dove K0 e Kg sono due matrici diagonali e definite positive che rappresentano
il cosiddetto ’tuning’ dei parametri della legge anti-windup. Gli elementi, k gi ,
Capitolo 4 Simulazioni
80
§ 4.3 Anti-windup applicato al robot SM-r3
che sono sulla diagonale della matrice Kg , verificano la condizione:
h Mi + k gi mi < m i ,
i = 1, ..., n
(4.1)
Si ricorda che se vale la condizione (1.5) dell’Assunzione 2 del capitolo 1, è
sempre possibile trovare una matrice diagonale e definita positiva, Kg , che
soddisfi la (4.1).
Per la simulazione abbiamo scelto:
Kg = diag([0.99 0.10 0.15 0.97 0.96 0.99])
K0 = diag([110 850 200 30 80 30])
La verifica dell’efficienza del compensatore anti-windup segue direttamente dai
grafici riportati nelle figure 4.15 - 4.18. Per viasualizzare i grafici è sufficiente
aprire, dopo che la simulazione è terminata, i blocchi Ang.Pos e Cont di figura
giunto 1 [gradi]
4.12.
60
40
20
giunto 2 [gradi]
0
0.5
1
1.5
2
2.5
3
3.5
4
4.5
5
0
0.5
1
1.5
2
2.5
3
3.5
4
4.5
5
0
0.5
1
1.5
2
2.5
tempo [sec.]
3
3.5
4
4.5
5
160
140
120
100
80
giunto 3 [gradi]
0
0
−50
−100
Figura 4.15: Posizione angolare dei giunti 1,2,3: unconstrained (rosso), saturato
senza anti-windup (blu), e anti-windup (verde).
Capitolo 4 Simulazioni
81
§ 4.3 Anti-windup applicato al robot SM-r3
giunto 4 [gradi]
80
60
40
20
giunto 5 [gradi]
0
0
0.5
1
1.5
2
2.5
3
3.5
4
4.5
5
0
0.5
1
1.5
2
2.5
3
3.5
4
4.5
5
0
0.5
1
1.5
2
2.5
tempo [sec.]
3
3.5
4
4.5
5
80
60
40
20
0
giunto 6 [gradi]
60
40
20
0
Figura 4.16: Posizione angolare dei giunti 4,5,6: unconstrained (rosso), saturato
senza anti-windup (blu), e anti-windup (verde).
giunto 1 [Nm]
200
100
0
−100
0
0.5
1
1.5
2
2.5
3
3.5
4
4.5
5
0
0.5
1
1.5
2
2.5
3
3.5
4
4.5
5
0
0.5
1
1.5
2
2.5
tempo [sec.]
3
3.5
4
4.5
5
giunto 2 [Nm]
1500
1000
500
0
−500
giunto 3 [Nm]
600
400
200
0
−200
Figura 4.17: Segnale di controllo dei giunti 1,2,3: unconstrained (rosso), saturato
senza anti-windup (blu), e anti-windup (verde).
Capitolo 4 Simulazioni
82
§ 4.3 Anti-windup applicato al robot SM-r3
giunto 4 [Nm]
40
30
20
10
0
giunto 5 [Nm]
−10
0
0.5
1
1.5
2
2.5
3
3.5
4
4.5
5
0
0.5
1
1.5
2
2.5
3
3.5
4
4.5
5
0
0.5
1
1.5
2
2.5
tempo [sec.]
3
3.5
4
4.5
5
100
50
0
giunto 6 [Nm]
0.1
0.05
0
−0.05
Figura 4.18: Segnale di controllo dei giunti 4,5,6: unconstrained (rosso), saturato
senza anti-windup (blu), e anti-windup (verde).
4.3.2
Soluzione anti-windup generallizzata
La simulazione relativa alla soluzione anti-windup generalizzata, descritta nel §1.4
del capitolo 1, si avvale degli stessi schemi a blocchi usati per implementare la
soluzione non lineare del problema. L’unica differenza è costituita dal codice della
S-function richiamata dal blocco compensatore. In questo caso infatti cambia il
segnale di compensazione v1 che assume la seguente forma:
v1 = sat(yc ) − yc + h(q) − h(q − qe ) − Kg sat(Kg−1 Kq qe ) − Kqd (qe , q̇e )q̇e
con Kg , K0 , Kq matrici diagonali scelte opportunamente secondo le espressioni
(1.11), (1.15), (1.17) riportate nel capitolo 1. Per la simulazione è stato scelto:
Kg = diag([0.99 0.10 0.15 0.97 0.96 0.99])
K0 = diag([110 300 450 30 80 30])
Kq = diag([100 200 200 100 100 100])
Capitolo 4 Simulazioni
83
§ 4.3 Anti-windup applicato al robot SM-r3
Anche in questo caso la verfica dell’efficienza delle leggi di compensazione antiwindup segue direttamente dall’esame visivo dei grafici riportati nelle figure 4.19 4.22, che mostrano l’andamento delle variabili di posizione angolare e controllo nei
tre casi d’interesse:
1. sistema non soggetto a saturazione;
2. sistema soggetto alla saturazione dei segnali di controllo;
3. sistema soggetto alla saturazione dei segnali di controllo ma dotato del com-
giunto 1 [gradi]
pensatore anti-windup.
60
40
20
giunto 2 [gradi]
0
0.5
1
1.5
2
2.5
3
3.5
4
4.5
5
0
0.5
1
1.5
2
2.5
3
3.5
4
4.5
5
0
0.5
1
1.5
2
2.5
tempo [sec.]
3
3.5
4
4.5
5
160
140
120
100
80
giunto 3 [gradi]
0
0
−50
−100
Figura 4.19: Posizione angolare dei giunti 1,2,3: unconstrained (rosso), saturato
senza anti-windup (blu), e anti-windup (verde).
Capitolo 4 Simulazioni
84
§ 4.3 Anti-windup applicato al robot SM-r3
giunto 4 [gradi]
80
60
40
20
giunto 5 [gradi]
0
0
0.5
1
1.5
2
2.5
3
3.5
4
4.5
5
0
0.5
1
1.5
2
2.5
3
3.5
4
4.5
5
0
0.5
1
1.5
2
2.5
tempo [sec.]
3
3.5
4
4.5
5
80
60
40
20
0
giunto 6 [gradi]
60
40
20
0
Figura 4.20: Posizione angolare dei giunti 4,5,6: unconstrained (rosso), saturato
senza anti-windup (blu), e anti-windup (verde).
giunto 1 [Nm]
200
100
0
−100
0
0.5
1
1.5
2
2.5
3
3.5
4
4.5
5
0
0.5
1
1.5
2
2.5
3
3.5
4
4.5
5
0
0.5
1
1.5
2
2.5
tempo [sec.]
3
3.5
4
4.5
5
giunto 2 [Nm]
1500
1000
500
0
−500
giunto 3 [Nm]
600
400
200
0
−200
Figura 4.21: Segnale di controllo dei giunti 1,2,3: unconstrained (rosso), saturato
senza anti-windup (blu), e anti-windup (verde).
Capitolo 4 Simulazioni
85
§ 4.3 Anti-windup applicato al robot SM-r3
giunto 4 [Nm]
40
30
20
10
0
giunto 5 [Nm]
−10
0
0.5
1
1.5
2
2.5
3
3.5
4
4.5
5
0
0.5
1
1.5
2
2.5
3
3.5
4
4.5
5
0
0.5
1
1.5
2
2.5
tempo [sec.]
3
3.5
4
4.5
5
100
50
0
giunto 6 [Nm]
0.1
0.05
0
−0.05
Figura 4.22: Segnale di controllo dei giunti 4,5,6: unconstrained (rosso), saturato
senza anti-windup (blu), e anti-windup (verde).
Capitolo 4 Simulazioni
86
Capitolo 5
Conclusioni
L’oggetto di questo lavoro di tesi è stato lo studio simulativo al calcolatore delle
leggi anti-windup per i manipolatori robotici con saturazione sugli ingressi. Le
leggi di compensazione proposte nel Capitolo 1 permettono di risolvere i problemi
di instabilità e perdita di prestazioni che spesso intervengono a causa del fenomeno
del windup. In particolare, le soluzioni proposte hanno il vantaggio di garantire un
ottima risposta transitoria ed il recupero delle prestazioni del sistema a ciclo chiuso
non soggetto a saturazione. Tutto ciò si ottiene scegliendo opportunamente solo i
guadagni proporzionale e derivativo del compensatore anti-windup. Tuttavia l’uso
di queste tecniche risolutive è basato sulla conoscienza di un modello dinamico del
robot a cui vengono applicate, che deve essere inserito nello schema di controllo.
Per questo motivo è stato necessario scrivere una procedura di calcolo automatico
dei coefficienti dinamici che permettesse di derivare le equazioni dinamiche di moto
e quindi il modello del robot. La scelta del manipolatore per lo studio simulativo
non è stata poi casuale. Il file dimostrativo mech robot.mdl del toolbox Matlab,
SimMechanics, ha suggerito il robot industriale Siemens-Manutec r3. Questa scelta
ha permesso di verificare per confronto diretto con il modello SimMechanics del
robot r3 la validità della procedura di calcolo automatico e quella del modello semplificato derivato dalla procedura stessa. Infine le simulazioni svolte sul modello
semplificato del manipolatore r3 hanno confermato le prestazioni della soluzione
anti-windup non lineare e di quella generalizzata presentate nel Capitolo 1. Il passo
successivo era quello di applicare queste soluzioni anche al sistema descritto dallo
87
schema SimMechanics del robot in cui fossero presenti tutte le nonlinearità del sistema ed anche la parte elettrica relativa al dispositivo di controllo e ai motori dei
giunti. Lo schema di compensazione è stato applicato a questo modello completo,
tuttavia non sono stati ottenuti risultati del tutto soddisfacenti. Il motivo è da
ricercare in più cause concomitanti, come la complessità della simulazione, alcuni difetti di modellazione nello schema SimMechanics del robot r3, e soprattutto
problemi legati al solver utilizzato dallo stesso Matlab. Uno degli obbiettivi futuri
sarà risolvere questo problema simulativo, cercando di migliorare l’implentazione
di queste leggi al calcolatore, avvalendosi anche di nuovi ambienti di simulazione.
Inoltre un ulteriore passo in avanti in questa ricerca sarebbe riuscire ad effettuare
una verifica dei risultati ottenuti tramite prove sperimentali.
Capitolo 5 Conclusioni
88
Bibliografia
[1] F. Morabito, A.R. Teel, and L. Zaccarian. “Anti-windup design for EulerLagrange systems”. In IEEE Conference on Robotics and Automation, pages
3442-3447, Washington (DC), USA, May 2002.
[2] F. Morabito, S. Nicosia, A.R. Teel, and L. Zaccarian. “Measuring and improving performance in anti-windup laws for robot manipulators”. In C. Melchiorri
B. Siciliano, A. De Luca and G. Casalino, editors, Advances in Control of Articulated and Mobile Robots, chapter 3, pages 61–85. Springer Tracts in Advanced
Robotics, 2004.
[3] Jörg Franke, Martin Otter. “The Manutec r3 Benchmark Models for the Dynamic Simulation of Robots”. DLR FF-DR-ER, Technical Report TR R101-93,
March 1993.
c
[4] SimMechanics User’s Guide. COPYRIGHT
2001-2002 by The MathWorks,
Inc.
[5] King-Sun Fu, Rafael C. Gonzalez, C.S. George Lee. “Robotica”. Copyright
c
1989
McGraw-Hill Libri Italia srl.
c
[6] Lorenzo Sciavicco, Bruno Siciliano. “Robotica Industriale”. Copyright 2000
McGraw-Hill Libri Italia srl.
89