Simulazioni del modello in cinematica inversa

La visualizzazione completa del modello cinematico, che è stato implementato nell’ambiente SimMechanics, è mostrata in Fig. 4.32. Per riconoscere e distinguere i numerosi blocchi, che costituiscono il modello, nel Toolboox è possibile personalizzare il colore dei blocchi

67 (proprietà disabilitata di default) in base alla particolare funzione meccanica. Si riconoscono pertanto:

gli 8 link o corpi rigidi raffigurati in verde i 3 giunti esterni dipinti di blu

i 4 giunti interni colorati di giallo

gli attuatori ed i relativi dati di attuazione, imposti in input, raffigurati in rosa i sensori ed i relativi dati, rilevati in output, evidenziati in celeste

Si noti il link L0, che costituisce il ground del modello e che è solidale al World CS: il SDR globale è stato scelto in modo tale da avere la forza di gravità lungo l’asse verticale z. Nell’implementazione sono state rispettate le scelte di modellazione illustrate nella Tab. 4.1, così come è stata mantenuta la classificazione funzionale dei giunti descritta in precedenza.

Fig. 4.32

Modello completo in cinematica inversa implementato nell’ambiente SimMechanics

La modalità di analisi scelta per lo studio del modello è la Forward Dynamics per la sua capacità di risolvere anche problemi di dinamica inversa, non solo in catena chiusa, ma anche in catena aperta o seriale come questa.

Il solutore scelto per le simulazioni è l’ode 45 a passo variabile; in particolare, per problemi di instabilità del sistema, che portavano ad interruzioni nelle simulazioni, è stato necessario restringere la tolleranza rispetto a quella imposta di default. In questo modo si è ottenuto un miglioramento delle prestazioni dell’algoritmo. Si sono utilizzati i blocchi Simulink

To Workspace, che hanno permesso di memorizzare le grandezze sensorizzate e trovare le uscite,

68 Il modello in cinematica inversa, presentato in Fig. 4.32, impone un moto in input e permette di rilevare, in uscita, le coppie ai giunti del robot, definiti Computed Torque nel linguaggio tipico dell’ambiente SimMechanic, come mostrato nello schema a blocchi di Fig. 4.33.

Fig. 4.33

Cinematica diretta e inversa

Il problema cinematico inverso riguarda la determinazione delle variabili di giunto: nell’espressione 4.12 il vettore q è incognito, il vettore x è noto.

q = K-1(x) (4.12)

Il vettore q delle variabili di giunto è determinabile una volta assegnati posizione e orientamento dell’organo terminale nello spazio di lavoro, attraverso la pianificazione della traiettoria del manipolatore e il controllo del moto. L’attuazione in moto, utilizzata per questo scopo, è un’attuazione robotica di tipo spiccatamente biologica, ottenuta mediante l’implementazione del polinomio 5 4 3; l’obiettivo è simulare il movimento naturale del chirurgo durante l’intervento, intento a muovere gli esoscheletri per manovrare il manipolatore slave.

La legge per il controllo del moto è stata imposta ad ogni giunto mediante il blocco di

SimMechanics Joint Actuator. Si è deciso di imporre un’attuazione in moto da una posizione

iniziale generica ad una posizione finale generica (angolare per giunto rotoidale, lineare per giunto prismatico), tale per cui devono risultare contestualmente nulle, all’istante iniziale t = 0 e finale t = T, sia la velocità che l’accelerazione del giunto stesso. Si noti che è stato imposto un tempo di simulazione pari a T. Se il giunto in questione è rotoidale si sono imposte le quattro condizioni presentate dall’equazione 4.13 all’equazione 4.16:

0 = 0 (4.13)

! " = 0 (4.14)

!# 0 = 0 (4.15)

69 Le quattro condizioni da imporre, nel caso in cui il giunto d’interesse sia prismatico, sono espresse dall’equazione 4.17 all’equazione 4.20:

$ 0 = 0 (4.17)

$ " = 0 (4.18)

$# 0 = 0 (4.19)

$# " = 0 (4.20)

Le precedenti condizioni, se imposte, danno luogo ad un tipo di attuazione robotica spiccatamente

human-like perché nei movimenti umani si parte da fermi, lentamente si accelera, di nuovo

lentamente si decelera fino ad arrestarsi del tutto. Volendo simulare, con il modello cinematico del

robot Da Vinci proposto in questo lavoro, il tipico movimento del polso del chirurgo umano

durante un intervento operatorio, è stata adottata tale attuazione giudicata molto “bio”. Si è quindi implementato un particolare polinomio omogeneo di V grado (espressione 4.21), detto polinomio 5 4 3, in grado di soddisfare le quattro condizioni iniziali human-like sopra introdotte:

% = 6%'_{− 15%}) _{+ 10% (4.21)}

con % = +

,, che è una variabile normalizzata alla lunghezza dell’intervallo temporale di

simulazione scelto.

In Fig. 4.34 è riportato lo schema a blocchi necessario per realizzare il generatore di moto del giunto i-esimo, che nel modello implementato è stato chiamato Joint Motion Generator ed al quale si accede specificando in un apposito script i parametri d’ingresso per ogni giunto, quali:

posizione iniziale (THi per giunto rotoidale, Xi per giunto prismatico) posizione finale (THf per giunto rotoidale, Xf per giunto prismatico)

intervallo temporale T scelto per la simulazione (uguale per tutti i giunti del modello)

Fig. 4.34

70 Per ovviare ad eventuali problemi di saturazione, è preferibile non usare i blocchi derivatori di

Simulink, pertanto si è scelto di imporre l’accelerazione in ingresso al blocco attuatore e di

utilizzare i blocchi di integrazione continua di Simulink per ricavare, attraverso le dovute integrazioni, le espressioni della velocità e della posizione. Come si evince dalla Fig. 4.34, l’uscita dal blocco Joint Motion Generator è un vettore a tre componenti, che fornisce nell’ordine posizione p, velocità v e accelerazione a desiderate per il giunto in questione. Inoltre con il blocco azzurro della Fig. 4.34, avvalendosi di un sottosistema per raggruppare le varie espressioni, si è costruita la generica accelerazione θ’’(t) che, data in ingresso al giunto i-esimo, permette di ottenere un’attuazione human-like; da θ’’ (t), mediante un’operazione di integrazione, si ottiene la velocità e, da questa, integrando nuovamente si ottiene infine la posizione.

In accordo con (36), l’accelerazione angolare θ’’(t) (espressione 4.22) o lineare x’’(t) (espressione 4.23) fornita in ingresso all’attuatore, ha permesso di soddisfare le quattro condizioni “bio” di partenza.

-- _{. =}

,/ 0 − !1 -- . (4.22)

$-- _{. =}

,/ $0 − $1 -- . (4.23)

Si noti che s’’(τ) è proprio la derivata del polinomio s(τ), come riportato nella relazione 4.24:

-- _{% = 120% − 180% + 60% + 0 (4.24)}

Mediante doppio click nel blocco azzurro di Fig. 4.35, è possibile accedere all’interno del sottosistema (si veda la Fig. 4.36) per osservare il dettaglio delle varie formule, costruite con l’aiuto dei blocchi presenti nella libreria Simulink, con cui SimMechanics si interfaccia, e parametrizzate per maggiore convenienza.

71 Fig. 4.35

Blocco Joint Motion Actuators evidenziato in azzurro

Nel modello implementato si notano, per ogni giunto del modello da J1 a J7, i blocchi azzurri degli attuatori di giunto.

Fig. 4.36

Subsystem Poly 5 4 3 Generator

Come si evince dalla Fig. 4.36, ricorrendo al blocco Clock, che rappresenta istante per istante la variabile t tempo di simulazione, e al blocco Polynomial si è implementato il polinomio s’’(τ) desiderato. La produttoria implementata riceve due ingressi, dati rispettivamente dal polinomio s’’(τ) e dall’accelerazione media _,/ 0 − !1 ; in uscita dal sottosistema si ottiene l’espressione dell’accelerazione in funzione del tempo θ’’ (t).

72 Pensando al robot come ad una black box, nel problema cinematico inverso si entra dall’esterno imponendo, in ingresso ai giunti del manipolatore, la legge di moto desiderata mediante Joint Actuator e si va a rilevare sugli stessi giunti in uscita il Computed Torque. Quest’ultima grandezza tipica del SimMechanics indica la coppia (forza o momento a seconda del giunto) da applicare al giunto in questione perché venga compiuto il movimento voluto. Dettagliando ulteriormente il modello implementato, per ognuno dei 7 giunti si definiscono:

due valori d’ingresso che specificano posizione iniziale e finale del giunto (maschera mostrata in Fig. 4.37)

due valori che specificano le posizioni massime e minime raggiungibili dal giunto; i valori di posizione dati in ingresso devono rispettare i vincoli di saturazione sulle integrazioni, per cui nella maschera di Fig. 4.38 possono essere inseriti i due valori estremi che definiscono il range di movimento del giunto.

Fig. 4.37

Maschera Joint J1 Data

Facendo doppio click sul blocco J1 Data, parte dell’attuatore del giunto J1, si apre la maschera in cui è possibile specificare posizione iniziale e finale del giunto J1.

73 Fig. 4.38

Maschera Joint J1 Poly 5 4 3

Facendo doppio click sul blocco J1 Poly 5 4 3, parte dell’attuatore del giunto J1, si apre la maschera in cui è possibile definire le posizioni iniziali e finali del giunto J1.

Lo script “Parametri_da_Vinci”, utilizzato per il modello cinematico inverso e riportato in Appendice C, permette di definire i parametri geometrici e inerziali dei link del modello ed i parametri necessari per l’attuazione in moto (condizioni iniziali e finali su velocità e accelerazioni). Il vantaggio di strutturare script Matlab consiste nella possibilità di lanciare la simulazione direttamente dallo script e di salvare ed elaborare i risultati delle simulazioni.

Le simulazioni del modello hanno portato a due tipologie di risultati:

gli output ottenuti da ognuno dei sette giunti della catena cinematica le uscite rilevate ai due E-E della catena cinematica

Lanciando la simulazione con passo variabile ode 45, per ciascuno dei sette giunti del modello si ottiene un subplot a quattro righe, che mostra ordinatamente in ogni riga l’evoluzione temporale delle quattro grandezze sensorizzate ai giunti: posizione angolare/lineare, velocità angolare/lineare, accelerazione lineare/angolare e Computed Torque. Nella Fig. 4.39 si mostra, a titolo di esempio, il subplot relativo al giunto J1.

74 Fig. 4.39

Evoluzione temporale delle quattro grandezze sensorizzate al giunto J1:posizione angolare,

velocità angolare, accelerazione angolare e Computed Torque

Mediante la function Matlab “plot_J_inversa”, appositamente creata negli script dell’Appendice C, ad ogni lancio della simulazione nel Workspace di Matlab viene caricato il valore massimo e minimo delle coppia ai giunti rilevati (Fig. 4.40).

Fig. 4.40

75 Per ognuno dei due E-E della catena cinematica, durante ciascuna simulazione, viene realizzato un subplot a tre righe, che mostra l’evoluzione temporale delle tre coordinate spaziali x, y, z; nella Fig. 4.41 si mostra il subplot relativo ai uno dei due effettori del modello. Mediante la function

Matlab “plot_J_inversa” viene caricato nel Workspace di Matlab il valore all’istante finale delle

tre coordinate spaziali.

Fig. 4.41

Evoluzione temporale delle tre coordinate spaziali x, y, z relativa ai uno dei due end-effector della catena

Ad ogni lancio della simulazione, viene realizzato un secondo subplot a tre righe, che mostra l’evoluzione temporale dell’orientamento dell’effettore lungo gli assi x, y, z; ciò è stato ottenuto mediante l’apposita function evalTwist (Fig. 4.42).

76 Fig. 4.42

Evoluzione temporale dell’orientamento lungo x, y, z relativa ai uno dei due E-E

Durante la simulazione, viene caricato nel Workspace la variabile a sei componenti (tre traslatorie, tre rotatorie) del twist dell’E-E all’istante finale di simulazione (Fig. 4.43).

Fig. 4.43

Twist di uno dei due E-E relativo all’istante finale di simulazione

Nello script evalTwist, presentato nell’appendice C, si verifica che i valori sensorizzati siano effettivamente appartenenti alla matrice di rotazione, come richiesto nel blocco Joint Sensor e per fare questa verifica si utilizzano le proprietà caratteristiche della matrice di rotazione (Fig. 4.44).

77 Fig. 4.44

Valori, istante per istante, relativi alla posizione lungo x, y, z e alla matrice di rotazione per uno dei due E-E