Cinematica differenziale e statica 121

3.7.4 Algoritmi del secondo ordine

Gli algoritmi per l’inversione cinematica appena illustrati possono essere definiti
come algoritmi del primo ordine, nel senso che consentono l’inversione di una tra-
iettoria di moto, specificata all’organo terminale in termini di posizione e velocità,
nelle posizioni e velocità dei giunti equivalenti.
Purtuttavia, ai fini del controllo come si vedrà successivamente nel Capito-
lo 8, può sorgere la necessità di invertire una traiettoria di moto specificata in
termini di posizione, velocità e accelerazione. D’altro canto, il manipolatore è
naturalmente un sistema meccanico del secondo ordine, come rivelerà il modello
dinamico che sarà ricavato nel Capitolo 7.
La derivazione dell’equazione cinematica differenziale (3.58) consente di
scrivere
ẍ = J A (q)q̈ + J̇ A (q, q̇)q̇ (3.94)
che fornisce il legame tra accelerazioni nello spazio dei giunti e accelerazioni
nello spazio operativo.
Nell’ipotesi di matrice J A quadrata e non singolare, la cinematica differen-
ziale del secondo ordine (3.94) può essere invertita in termini delle accelerazioni
ai giunti come ³ ´
q̈ = J −1
A (q) ẍ − J̇ A (q, q̇)q̇ . (3.95)

L’integrazione numerica della (3.95) per ricostruire le velocità e le posizioni dei

giunti comporterebbe inevitabilmente un fenomeno di deriva della soluzione; per-
tanto, in analogia con l’algoritmo per l’inversione cinematica con inversa dello
Jacobiano, è opportuno considerare l’errore definito in (3.64) unitamente alla sua
derivata
ë = ẍd − ẍ (3.96)
che, tenendo conto della (3.94) fornisce

ë = ẍd − J A (q)q̈ − J̇ A (q, q̇)q̇. (3.97)

A questo punto, è possibile scegliere il vettore delle accelerazioni ai giunti

come ³ ´
q̈ = J −1
A (q) ẍd + K D ė + K P e − J̇ A (q, q̇)q̇ (3.98)

ove K D e K P sono matrici definite positive (solitamente diagonali). Sostituendo

la (3.98) nella (3.97) conduce al sistema lineare di errore equivalente

ë + K D ė + K P e = 0 (3.99)

che risulta asintoticamente stabile: l’errore tende a zero lungo la traiettoria con
una velocità di convergenza dipendente dalla scelta delle matrici K P e K D . L’al-
goritmo per l’inversione cinematica del secondo ordine è illustrato nello schema
a blocchi di Figura 3.14.
122 Capitolo 3

Figura 3.14 Algoritmo per l’inversione cinematica del secondo ordine con inversa
dello Jacobiano

Nel caso di un manipolatore ridondante, la generalizzazione della (3.98)

consente di ricavare una soluzione algoritmica basata sulla pseudo-inversa dello
Jacobiano del tipo
³ ´
q̈ = J †A ẍd + K D ė + K P e − J̇ A (q, q̇)q̇ + (I − J †A J A )q̈ 0 (3.100)

ove il vettore q̈ 0 è rappresentativo di accelerazioni arbitrarie ai giunti che possono

essere scelte in maniera tale da ottimizzare (localmente) una funzione obiettivo
del tipo di quelle considerate nel Paragrafo 3.5.1.
Come per gli algoritmi per l’inversione cinematica del primo ordine, è pos-
sibile caratterizzare diverse espressioni per l’errore di orientamento che, in alter-
nativa agli angoli di Eulero, facciano riferimento a una descrizione asse/angolo,
ovvero al quaternione unitario.

3.7.5 Confronto tra gli algoritmi per l’inversione cinematica

Allo scopo di eseguire un confronto di prestazioni tra gli algoritmi per l’inversio-
ne cinematica illustrati, si consideri il manipolatore planare a tre bracci di Figu-
ra 2.20, i cui bracci abbiano lunghezze a1 = a2 = a3 = 0.5 m. La cinematica
diretta di tale manipolatore è riportata nella (2.81), laddove il suo Jacobiano può
essere ricavato dalla (3.31) considerando le tre righe non nulle di interesse per lo
spazio operativo.
Il manipolatore sia nella postura iniziale q = [ π −π/2 −π/2 ]T rad, cor-
rispondente alla locazione dell’organo terminale: p = [ 0 0.5 ]T m, φ = 0 rad. Si
assegni all’organo terminale un percorso circolare, di raggio 0.25 m e con centro
168 Capitolo 4

Si noti che il vettore velocità è orientato nella direzione di t mentre il vettore

accelerazione risulta dal contributo di due termini: il primo, orientato come n,
rappresenta l’accelerazione centripeta e il secondo, orientato come t, rappresenta
l’accelerazione tangenziale.
Si consideri infine, un percorso costituito da una sequenza di n + 1 punti,
p0 , p1 , . . . , pn , congiunti mediante n segmenti. Una possibile rappresentazione
parametrica del percorso complessivo è la seguente:
n
X sj
p = p0 + (p − pj−1 ), (4.46)
j=1
kpj − pj−1 k j

con j = 1, . . . , n, dove sj è l’ascissa curvilinea relativa al segmento j-mo del

percorso, congiungente il punto pj−1 al punto pj , cosı̀ definita:

0
 0 ≤ t ≤ tj−1
sj (t) = s0j (t) tj−1 < t ≤ tj (4.47)

1 tj < t ≤ tf ,

in cui tf rappresenta la durata complessiva del percorso, tj l’istante di passaggio

per il punto pj e s0j (t) può essere una funzione analitica di tipo polinomiale cubi-
co, di tipo misto parabolico-lineare, . . . , che varia con continuità dal valore s0j = 0
per t = tj−1 al valore s0j = kpj − pj−1 k per t = tj .
La velocità e l’accelerazione di p possono facilmente essere ricavate derivan-
do la 4.46:
n
X Xn
ṡj
ṗ = (pj − pj−1 ) = ṡj tj (4.48)
j=1
kpj − pj−1 k j=1
n
X Xn
s̈j
p̈ = (pj − pj−1 ) = s̈j tj , (4.49)
j=1
kpj − pj−1 k j=1

dove tj è il versore tangente del segmento j-mo.

A causa della discontinuità della derivata prima in corrispondenza dei punti
di percorso compresi tra due segmenti non allineati, il manipolatore sarà costretto
a fermarsi, per poter poi ripartire secondo la direzione del segmento successivo.
Potendo rilassare il vincolo di passaggio per i punti di percorso, è possibile evi-
tare l’arresto del manipolatore raccordando i segmenti in prossimità di tali punti,
che saranno quindi denominati punti di via nello spazio operativo, in modo da
garantire almeno la continuità della derivata prima.
Cosi come già illustrato per la pianificazione di sequenze di polinomi parabolico-
lineari con passaggio in prossimità di punti di via nello spazio dei giunti, l’utiliz-
zazione di leggi di velocità di tipo trapezoidale per le ascisse curvilinee consente
di sviluppare un algoritmo di generazione particolarmente semplice.
 Pianificazione di traiettorie 169

In particolare, si anticiperà opportunamente la generazione dei singoli tratti di

segmento, prima che il segmento immediatamente precedente sia stato terminato.
Ciò si traduce nel modificare la formulazione della (4.47), come segue:

0
 0 ≤ t ≤ tj−1 − ∆tj
sj (t) = s0j (t + ∆tj ) tj−1 − ∆tj < t ≤ tj − ∆tj (4.50)


1 tj − ∆tj < t ≤ tf − ∆tn ,
dove ∆tj rappresenta l’anticipo con cui iniziare a generare il segmento j-mo, che
può essere valutato in modo ricorsivo come segue
∆tj = ∆tj−1 + δtj ,
con j = 1, . . . , n e ∆t0 = 0. Si noti che tale anticipo è dato dalla somma di due
contributi: il primo, ∆tj−1 , è relativo alla somma degli anticipi con cui sono stati
generati i segmenti precedenti, mentre il secondo, δtj , è l’anticipo con cui iniziare
a generare il segmento corrente.

4.3.3 Orientamento
Si consideri ora l’orientamento della terna utensile. Di solito questo è specificato
tramite la matrice di rotazione della terna utensile (mobile) rispetto alla terna fissa.
Come è noto, le tre colonne della matrice di rotazione rappresentano i tre versori
della terna utensile (n, s, a) espressi nella terna di riferimento. Nella fase di ge-
nerazione della traiettoria, tuttavia, non è conveniente fare riferimento alla matrice
di rotazione per esprimere l’orientamento. Infatti interpolando, ad esempio linear-
mente, i versori n, s, a che caratterizzano rispettivamente orientamento iniziale
e orientamento finale, non si può garantire che la condizione di ortonormalità dei
suddetti versori sia verificata in ogni istante.

Angoli di Eulero Per ovviare all’inconveniente appena evidenziato, ai fini del-

la generazione di traiettoria, spesso si descrive l’orientamento tramite una terna di
angoli di Eulero φ = (ϕ, ϑ, ψ) per i quali viene specificata la dipendenza tem-
porale. Usualmente φ descrive il segmento congiungente il suo valore iniziale
φi con il suo valore finale φf . Anche in questo caso, conviene scegliere per il
moto di φ una legge oraria di tipo polinomiale cubico o misto parabolico-lineare.
Infatti, in questo modo, la velocità angolare ω della terna mobile, che è legata da
una relazione lineare a φ̇ come nella (3.60), presenta una variazione continua del
modulo.
Pertanto, una volta assegnati i valori estremi di φ e data la legge oraria con
cui esso si muove in termini di posizione, velocità e accelerazione, si ottiene:
s
φ = φi + (φ − φi )
kφf − φi k f
ṡ
φ̇ = (φ − φi ) (4.51)
kφf − φi k f