Comau SmartSix Robotic Arm simulation, with direct kinematics, inverse kinematics and trajectory planning using Matlab and V-REP simulator.

1. Università degli studi di Salerno
Corso di Laurea Magistrale in Ingegneria Informatica
Anno 2015/2016
Corso di Automazione e Robotica
Presentazione progetto Robotica
Gruppo: Di Gruttola Carmine – Esposito Emiddio

2. Outline
1. Analisi del manipolatore
2. Cinematica diretta del manipolatore
3. Cinematica differenziale, Inversione cinematica e pianificazione
della traiettoria
4. Controllo centralizzato nello spazio giunti

3. Strumenti utilizzati
• Matlab versione r2015a per lo sviluppo delle procedure.
• Ambiente di simulazione V-REP.

4. Analisi
• Il manipolatore analizzato è lo SmartSix della Comau poggiato su una
base mobile e con un polso sferico all’estremità.
• Il sistema risulta quindi composto da 6 giunti rotoidali disposti su
diversi assi.
• La base mobile è stata modellata aggiungendo due giunti prismatici,
idealmente posti nello stesso punto del primo giunto rotoidale.

5. Utilizzando le convenzioni di
Denavit-Hartenberg, sono state
individuate le terne solidali ai
diversi giunti

6. Dalle terne individuate e dal
datasheet sono stati estratti i
parametri DH che permettono
in maniera agevole di ricavare
le matrici di rotazione
Transizione a 𝜶 d 𝜽
𝑇𝑒𝑟𝑛𝑎 0 → 𝑇𝑒𝑟𝑛𝑎 1 0
𝜋
2
𝑞1
𝜋
2
𝑇𝑒𝑟𝑛𝑎 1 → 𝑇𝑒𝑟𝑛𝑎 2 0
𝜋
2
𝑞2
𝜋
2
𝑇𝑒𝑟𝑛𝑎 2 → 𝑇𝑒𝑟𝑛𝑎 3 0.15 −
𝜋
2
0.45 𝑞3
𝑇𝑒𝑟𝑛𝑎 3 → 𝑇𝑒𝑟𝑛𝑎 4 0.59 0 0 𝑞4
𝑇𝑒𝑟𝑛𝑎 4 → 𝑇𝑒𝑟𝑛𝑎 5 0.13 −
𝜋
2
0 𝑞5
𝑇𝑒𝑟𝑛𝑎 5 → 𝑇𝑒𝑟𝑛𝑎 6 0
𝜋
2
0.64707 𝑞6
𝑇𝑒𝑟𝑛𝑎 6 → 𝑇𝑒𝑟𝑛𝑎 7 0 −
𝜋
2
0 𝑞7
𝑇𝑒𝑟𝑛𝑎 7 → 𝑇𝑒𝑟𝑛𝑎 8 0 0 0.095 𝑞8

7. Cinematica Diretta
• Il primo passo è stato lo sviluppo di una procedura per il calcolo della
cinematica diretta.
• La procedura prende in ingresso la configurazione dei giunti del
manipolatore, una stringa per la tipologia degli angoli di Eulero e le
matrici 𝐴0
𝑏
e 𝐴 𝑒
𝑛
di relazione con la terna mondo e quella end effector
• In uscita restuisce la posizione dell’end effector, l’orientamento
secondo gli angoli scelti, la matrice di rotazione 𝑅 𝑒
𝑏 e un cell array con
tutte le matrici intermedie calcolate.
function [p, phi, R, A] = cindir(q, str, Ab0, Ane)

8. Procedura DHMatrix
• Per aiuto nello sviluppo, è stava sviluppata una procedura che dati i
parametri DH calcola la matrice di rototraslazione corrispondente.
• La stessa poi è stata utilizzata più volte per calcolare 𝐴 𝑒
𝑏, da cui sono
stati ricavate tutte le altre informazioni ritornate da cindir.
function [A] = DHMatrix(a, alpha, d, theta)

9. Verifica
• Confronto dei risultati in uscita dalla procedura realizzata con quelli
simulati dall’ambiente V-REP.
• In primo luogo sono stati verificati i risultati in posizione di
calibrazione, ovvero [0,0,0,−
𝜋
2
,0,0,0,0]T e i valori sono congruenti.
• Un’ulteriore prova è stata fatta in una posizione casuale
[5,5,0,0,−
𝜋
2
,0,0,0]T e si è notata una discrepanza di 0.06 mm nella
coordinata z della posizione causata dai limiti fisici dei giunti, nel caso
particolare dal giunto 5.
• Questo problema è stato risolto utilizzando il file
LimitiManipolatore.m

10. Inversione Cinematica
• Secondo passo è stato il calcolo della cinematica differenziale ai fini
dell’inversione cinematica, implementata attraverso un algoritmo CLIK
del secondo ordine
• La funzione prende in ingresso una configurazione iniziale, una
traiettoria in posizione, velocità e accelerazione, oltre che
informazioni su eventuali ostacoli.
function [q, dq, ddq, e] =
InversioneCinematica(q0, xd, dxd, ddxd, str, Ab0, Ane, obs)

11. Jacobiano Geometrico e Analitico
• Per l’inversione il primo passo è il calcolo dello Jacobiano Geometrico
𝐽 𝑞 e dello Jacobiano Analitico 𝐽 𝐴(𝑞)
• Il primo è stato calcolato a partire dalle formule presenti sul testo e
dalle informazioni ricavate da cindir
• Il secondo è stato calcolato partendo dal primo e utilizzando la
matrice di trasformazione ω = T Φ Φ, per ricavare Φ partendo da
ω.

12. Algoritmo CLIK del 2° ordine

13. Algoritmo CLIK del 2° ordine
• Utilizzato per compensare l’errore di integrazione in Eulero in avanti.
• Due costanti di retroazione, 𝐾 𝑑 e 𝐾𝑝 scelte uguali per dare un tempo
di assestamento di 50 ms senza oscillazioni, ovvero 𝜔 𝑛 = 100 e 𝜁 = 1
• Proiezione nel nullo per allontanarsi da ostacoli (supposti fermi e
cilindrici) e dai limiti di giunti
𝑞0 = 𝑞0 − 𝑞
𝑞0𝑖 = −𝑘1
𝑞 𝑖−𝑞 𝑚
𝑞 𝑖𝑀− 𝑞 𝑖𝑚
𝑞0𝑖 = 2𝑘2 𝑞𝑖 − 𝑜𝑖
• 𝑘1 e 𝑘2 sono stati fissati a 100 e 10 dopo prove sperimentali

14. Traiettoria - 1
• Traiettoria per operazione di pick and place
• Divisa in due sottotraiettorie:
• Dalla posizione di calibrazione alla posizione di pick
• Dalla posizione di pick alla posizione di place
• Percorso calcolato con ascissa curvilinea con archi di circonferenza
𝛾𝑝 = 𝑐 + 𝑅 ∗
𝑟 ∗ cos(
𝑠
𝑟
)
𝑟 ∗ sin(
𝑠
𝑟
)
0
𝛾𝑓 = 𝑓0 + 𝑠 ∗
𝑓 𝑓−𝑓0
𝐿
• Punto di via calcolato per passare lontano dagli ostacoli

15. Traiettoria - 2
Vista laterale
Vista dall’alto

16. Traiettoria - 3
• Per l’ascissa curvilinea è stata scelto un profilo trapezoidale con
due secondi in aggiunta, per simulare le operazioni di pick e place
• 𝑠(𝑡) =
𝑎 𝑚𝑎𝑥 ∗
𝑡2
2
𝑐𝑜𝑛 𝑡 < 𝑡 𝑐
𝑣 𝑚𝑎𝑥 ∗ 𝑡 −
𝑣 𝑚𝑎𝑥
2
2∗𝑎 𝑚𝑎𝑥
𝑐𝑜𝑛 𝑡 𝑐 < 𝑡 < 𝑡𝑓 − 𝑡 𝑐
−𝑎 𝑚𝑎𝑥 ∗
𝑡−𝑡 𝑓
2
2
+ 𝑣 𝑚𝑎𝑥 ∗ 𝑡𝑓 −
𝑣 𝑚𝑎𝑥
2
𝑎 𝑚𝑎𝑥
𝑐𝑜𝑛 𝑡𝑓 − 𝑡 𝑐 < 𝑡 < 𝑡𝑓
𝑣 𝑚𝑎𝑥 𝑡𝑓 −
𝑣 𝑚𝑎𝑥
2
𝑎 𝑚𝑎𝑥
, 𝑡𝑓 < 𝑡 < 𝑡𝑓 + 2
• 𝑡𝑓 e 𝑣 𝑚𝑎𝑥 sono stati calcolati in funzione di L, lunghezza della curva.

17. Verifica - 1
• Partendo dalla posizione di calibrazione l’errore è:

18. Verifica - 2
• Partendo dalla posizione 𝑞1 = [0.5; 0.5; 0; −
𝑝𝑖
2
; 0 ; 0.1; 1.2; 0.9]
l’errore è:

19. Controllo giunti
• L’obiettivo, data una traiettoria nello spazio giunti, è quello di far
inseguire la traiettoria ai giunti.
• Si usa il modello dinamico del manipolatore:
𝐵 𝑞 𝑞 + 𝐶 𝑞, 𝑞 𝑞 + 𝐹 𝑞 + 𝑔 𝑞 = 𝑢
• Sono stati ignorati contributi di attrito statico e forze esterne agenti
sul sistema.

20. Simulazioni
• Ai fini del test, sono state utilizzate due traiettorie.
• Traiettoria costante: si tenta di imporre, per 5 s ad un passo di
campionamento T=0.001 s, 𝑞 =
0.5 0.5 0 −
𝜋
2
0 0.1 1.2 0.9
𝑇
partendo dalla
configurazione di calibrazione 𝑞0 = 0 0 0 −
𝜋
2
0 0 0 0
𝑇
• Traiettoria generata nel secondo homework.

21. Controllo PD + gravità -1
• La strategia di controllo PD + gravità prevede di generare le coppie ai
giunti come composizione di tre contributi:
• Contributo proporzionale alla differenza tra posizione dei giunti desiderata e
posizione reale.
• Contributo proporzionale alla differenza tra velocità dei giunti desiderata e
velocità reale
• Contributo pari al contributo gravitazionale del modello dinamico.
𝑢 = 𝐾𝑝 𝑞 𝑑 − 𝑞 − 𝐾 𝑑 𝑞 + 𝑔(𝑞)
• La strategia converge senza errori per traiettorie costanti.

22. Controllo PD + gravità - 2

23. Controllo PD + gravità - 3
• La simulazione è stata effettuata variando i parametri 𝐾𝑝 e 𝐾 𝑑,
impostando entrambe le matrici come matrici diagonali con un valore
fisso sulla diagonale principale e variando questo valore tra 10, 100 e
200, scelti in maniera euristica.
• 𝐾𝑝 = 𝐾 𝑑 =
𝜆 0
0 𝜆
⋯
0 0
0 0
⋮ ⋱ ⋮
0 0
0 0
⋯
𝜆 0
0 𝜆

24. Controllo PD + gravità - Simulazioni

25. Controllo a dinamica inversa - 1
• La strategia a Dinamica Inversa prevede di compensare la dinamica
del manipolatore in toto.
𝑢 = 𝐵 𝑞 𝑦 + 𝐶 𝑞, 𝑞 𝑞 + 𝐹 𝑞 + 𝑔 𝑞
𝑦 = 𝑞 𝑑 + 𝐾 𝑑 𝑞 𝑑 − 𝑞 + 𝐾𝑝(𝑞 𝑑 − 𝑞)
• Questo permette di ottenere l’equazione dell’errore 𝑒 + 𝐾 𝑑 𝑒 + 𝐾𝑝 𝑒 =
0 avendo posto 𝑒 = 𝑞 𝑑 − 𝑞.
• Questo porta ad un errore che converge a zero, considerando che le
matrici delle costanti di retroazione sono definite positive.

26. Controllo a dinamica inversa - 2

27. Controllo a dinamica inversa - 3
• Costanti di retroazione 𝐾 𝑝 e 𝐾 𝑑 come matrici diagonali basate su i due
parametri che caratterizzano gli autovalori del sistema, 𝜁 e 𝜔 𝑛.
• 𝐾 𝑝 =
𝜔 𝑛
2
0
0 𝜔 𝑛
2 ⋯
0 0
0 0
⋮ ⋱ ⋮
0 0
0 0
⋯
𝜔 𝑛
2
0
0 𝜔 𝑛
2
𝐾 𝑑 =
2𝜁𝜔 𝑛 0
0 2𝜁𝜔 𝑛
⋯
0 0
0 0
⋮ ⋱ ⋮
0 0
0 0
⋯
2𝜁𝜔 𝑛 0
0 2𝜁𝜔 𝑛
• 𝜔 𝑛 è posto pari a 10 e 𝜁 prima pari a 1, per eliminare le oscillazioni, e poi
pari a 0.3, tenendo quindi in conto le oscillazioni nell’errore, per la
traiettoria costante.
• Per la traiettoria non costante si è posto 𝜁 pari a 1.

28. Controllo a dinamica inversa - 4

29. Controllo a dinamica inversa - 5