7. Sistema di guida aptica
In questa tesi vengono sviluppati una serie di algoritmi di assistenza per aiutare l’operato-re durante la guida del veicolo (Capitolo 1) mediante un’interfaccia aptica. Gli algoritimi implementati forniscono all’operatore un aiuto a guidare la base mobile del veicolo, non coinvolgono l’uso del braccio manipolatore.
Questi algoritmi di assistenza vengono chiamati navigazione standard, navigazione co-stiera e navigazione ottima. In questo capitolo approfondiremo queste tre modalità di navigazione e spiegeremo quali stimoli aptici abbiamo usato.
Oltre agli algoritmi di assistenza abbiamo affrontato il problema che chiamiamo compren-sione del workspace. Ovvero l’operatore mentre guida il veicolo riceve uno stimolo aptico nel momento in cui un punto target per il manipolatore entra nel workspace del braccio. In questo modo l’operatore riesce ad avere un’idea delle dimensioni del workspace del braccio. Un operatore esperto può quindi riuscire a posizionare la base mobile del veicolo in modo corretto per la successiva lavorazione con il braccio. Dagli esempi fatti si evince che la guida del veicolo e l’azionamento del braccio, generalmente, avviene in fasi separate.
Per garantire una guida sicura in aggiunta a questi algoritmi viene aggiunto un ulteriore stimolo che chiamiamo barriers. In questo modo l’operatore non può guidare il veicolo oltre una soglia di sicurezza evitando così di provocare danni al mezzo ed alle persone situate nelle vicinanze.
Gli stimoli aptici che abbiamo implementato sono pensati come dei suggerimenti aptici. Per questo le forze rese mediante l’interfaccia aptica non sono eccessivamente alte. Questo consente all’operatore, in base alla propria esperienza, di decidere le operazioni da compiere per quella particolare missione.
In sintesi gli algoritmi di assistenza alla guida implementati sono i seguenti: • F1 force field per la navigazione standard
• F2 force field per la navigazione costiera
• F3 force field per la navigazione ottima
• V vibrazione per la comprensione del workspace • B barriere per impedire danni al veicolo e alle persone
In questa prima analisi l’operatore, secondo la propria esperienza, può passare da una modalità di navigazione ad un’altra premendo i bottoni presenti sul grip dell’interfaccia aptica.
7.1. Standard Navigation
La modalità di navigazione standard ha l’obiettivo di creare un campo di forza F1 in modo
da evitare gli ostacoli presenti nell’ambiente. Questa modalità nasce dall’esigenza, per l’o-peratore, di riuscire a guidare il veicolo nella zona di lavoro nel minor tempo e sopratutto evitando situazioni potenzialmente pericolose come la presenza ravvicinata di ostacoli.
Il veicolo in esame è composto da una base mobile e da un braccio manipolatore solidale con la base stessa. Il nostro algoritmo di assistenza che in questo caso aiuta l’operatore ad evitare ostacoli tiene in considerazione delle dimensioni e della forma della base mobile del veicolo. In particolare la base mobile del veicolo viene approssimata con un rettangolo (contorno di colore nero in Figura 7.1) di dimensioni di poco superiori a quelle reali.
Nella Figura 7.1 si rappresenta il robot di forma rettangolare e solidale al robot abbiamo definito il sistema di riferimento base_link posizionato nel centro del veicolo. La linea blu definisce le rette rispetto alle quali si calcolca la distanza con gli ostacoli, e rappresenta un contorno invalicabile per gli ostacoli. La linea rossa definisce une regione compresa fra la linea rossa e linea blu. Tale regione rappresenta la regione di rilevazione degli ostacoli.
Figura 7.1.: Defining regions: il confine rosso separa la regione di free motion con la regione di rilevazione ostacoli; il confine blu rappresenta i bordi per il calcolo delle distanze con gli ostacoli; la linea verde indica la minima regione di rilevazione ostacoli: la linea rossa e la linea verde coincidono quando il robot viene guidato a basse velocità.
Il nostro algoritmo infatti riconosce un ostacolo solo se questo è situato all’interno della regione di rilevazione degli ostacoli. In questo modo si definisce una regione di free motion ed una regione in cui gli ostacoli contribuiscono nel calcolo della forza da restituire all’operatore. Il nostro algoritmo si basa sul definire una regione di rilevazione ostacoli non statica ma dinamica. Infatti la linea rossa si deforma dinamicamente in funzione del vettore di navigazione (Figura 7.2) ed in funzione della velocità del veicolo. Se il veicolo naviga a velocità elevate allora la linea rossa viene traslata in avanti considerando anche la direzione in cui il robot si sta muovendo. Se invece il veicolo si muove a velocità basse la linea rossa coincide con la linea verde (Figura 7.1) e in più si deforma a seconda della direzione di avanzamento del veicolo. Questa linea verde rappresenta la distanza minima entro la quale il robot, in caso di un ostacolo imminente, riesce a reagire in tempo. Considerando quindi una regione di influenza ostacoli che si deforma si ottiene una serie di vantaggi. Infatti l’operatore, quando guida il robot a velocità elevate percepisce prima gli ostacoli e può reagire in tempo. Un tale comportamento è analogo al comportamento di un automobilista. Infatti durante la guida il punto di vista del guidatore è rivolto in avanti per riuscire a vedere più lontano eventuali pericoli e così può reagire in tempo evitando la situazione di pericolo. Inoltre quando l’operatore guida il veicolo a basse velocità, probabilmente perchè vuole guidare il robot in un ambiente stretto, la regione di influenza degli ostacoli viene ristretta al minimo. Così facendo l’operatore sente solo gli ostacoli prossimi al veicolo senza eventuali interferenze dovuti agli altri ostacoli. Inoltre avendo ristretto al minimo la regione di influenza degli ostacoli tenendo conto dell’accelerazione massima del robot, l’operatore
7.1 Standard Navigation
che in qualche modo stà cercando di guidare con attenzione il robot riesce a fermarlo in tempo.
Figura 7.2.: Regione di influenza degli ostacoli che si deforma in funzione del vettore di navigazione
Solidale al veicolo abbiamo definito il sistema di riferimento base_link (Figura 7.3). Ri-spetto a questo sistema di riferimento è disponibile la mappa degli ostacoli registrata grazie al framework ROS. Questa mappa mantiene le informazioni degli ostacoli mediante un insie-me di celle e rimane sempre centrata rispetto al sistema di riferiinsie-mento base_link. Ongi cella ha le dimensioni di 1 cm ed è possibile leggere le coordinate degli ostacoli rispetto al frame base_link. Inoltre abbiamo definito un sistema di riferimento fisso odom come rappresentato in Figura 7.3.
Oltre alla definizione della regione di free motion e della regione di rilevazione ostacoli, il nostro veicolo viene suddiviso in otto zone numerate da 1 a 8 (Figura 7.3).
Figura 7.3.: Suddivisione del robot in zone
In particolare i lati del rettangolo ( linee nere Figura 7.3) che rappresenta la forma del robot vengono prolungati. Ogni lato prolungato assieme al confine rosso, il quale si deforma dinamicamente, delimitano queste 8 zone (Figura 7.3). Una volta definite queste zone che circondano il robot, gli ostacoli (o meglio le celle ) vengono letti ed etichettati a seconda della zona in cui appartengono. Al termine di questa fase, otteniamo una suddivisione delle celle per ognuna delle 8 regioni. Una volta suddiviso gli ostacoli per zona, questi vengono
filtrati eliminando le celle che si trovano a distanza più elevata. Questo filtraggio sarà utile in particolare per la navigazione costiera e successivamente ne daremo la motivazione.
La distanza tra robot ed ostacoli avviene nel seguente modo. In Figura 7.3 il robot viene circondato da un poligono ( linea blu) composto da otto lati. I vertici di questo poligono (convesso) definiscono 8 rette, una per ogni lato. Queste rette vengono indicate con Γi e le
normali a queste rette con ni per i = 1, . . . , 8. Questo confine poligonale (in blu) circonda
il robot racchiudendolo in un confine invalicabile per gli ostacoli. Infatti la distanza con gli ostacoli avviene proprio rispetto a questo confine. In particolare se un ostacolo è presente nella regione 1 allora si misurata la distanza rispetto alla retta Γ1. Questo metodo viene
replicato per le altre otto regioni si ottiene così una serie di distanze punto retta per ognuna delle zone. Si è quindi utilizzato la distanza punto retta grazie al fatto che il confine per il calcolo delle distanze è un poligono convesso ed il controllo se gli ostacoli penetrano nella regione è più facile.
Figura 7.4.: Standard navigation: contributi di forza e coppia nelle diverse zone Una volta calcolate le distanze tra robot e ostacoli per ognuna delle 8 zone calcoliamo la forza da rendere all’operatore. Questa forza verrà applicata sull’interfaccia aptica per dimi-nuire i comandi impartiti dall’operatore. Infatti egli guida il veicolo utilizzando solamente due gradi di libertà dell’interfaccia. Nel sistema di riferimento dell’interfaccia, l’asse x corri-sponde ad impartire una velocità di avanzamento per il robot. Mentre sull’asse y l’operatore invia al robot la velocità di imbardata desiderata.
Ognuna delle 8 zone produce una forza indipendente dalle altre. Il calcolo delle forze è funzione dell’ostacolo prossimale al veicolo. Infatti per ogni zona si considera l’ostacolo che ha prodotto la distanza minima con il robot per calcolare il modulo della forza.
Come detto in precedenza, lo scopo di questa navigazione è quello di evitare ostacoli. Per questa ragione la zona 1 e 3 forniscono un contributo in termini di coppia τ1e τ3 come un
meccanismo di evasione. Queste coppie vengono rese sull’interfaccia nella direzione lungo la quale l’operatore invia il comando di velocità di imbardata. In questo modo il veicolo cambia la sua orientazione e si allontana dall’ostacolo rilevato. Per quanto riguarda la seconda zona, quella frontale, essa fornisce una contributo in forza f2 per la percezione di ostacoli in fase di
avanzamento. Questa forza viene inviata sull’interfaccia lungo la direzione in cui l’operatore invia al robot il comando di velocità di avanzamento. In questo modo l’operatore percepisce la presenza degli ostacoli che si trovono di fronte al proprio cammino e la velocità del robot si riduce. La presenza di ostacoli sui lati del veicolo producono una contributo in termini di coppia. Infatti, se il sistema rileva un ostacolo nella zona laterala destra (la zona numero 4) allora produce una coppia τ4. Questa coppia fa ruotare il veicolo nella direzione opposta
7.1 Standard Navigation
rispetto a dove è situato l’ostacolo. Inoltre per evitare che il lato destro frontale oppure il lato destro posteriore vada a sbattere contro un ostacolo la coppia cambia verso fino al ripristino di una distanza di sicurezza. Quanto appena detto viene replicato per la zona laterale sinistra (la zona 8). In Figura 7.4 si rappresentano le forze e le coppie spiegate precedentemente. Il loro simultaneo effetto suggerisce all’operatore il comando da inviare al veicolo per mantenere esso lontano dagli ostacoli e quindi evitare situazioni potenzialmente pericolose. La forza resa all’utente tramite l’interfaccia aptica è data quindi:
Fx = f2
Fy = τ1+ τ3 + τ4+ τ8
Per quanto riguarda il modulo delle forze e delle coppie (Figura 7.5) abbiamo deciso di implementare un profilo lineare con due tratti lineari di pendenza differente. Il modulo di queste forze e coppie cresce al diminuire della distanza tra robot e ostacoli. Il primo tratto lineare ha una maggiore pendenza per far fronte al fatto che la regione di rilevazione degli ostacoli può considerare ostacoli anche a distanza elevata.
Figura 7.5.: Standard navigation: profile forces and torques
Per questo è necessario avere un primo tratto più ripido per far sentire all’operatore la presenza degli ostacoli.
Figura 7.6.: Validazione della navigazione standard
Questo algoritmo di assistenza è stato testato inizialmente su una serie di ostacoli (Figura 7.6) di forma differente ed in una serie di scenari come la presenza di ostacoli solo laterali, solo frontali ed infine in un ambiente disordinato. In questo modo è stato possibile valutare i singoli effetti delle forze che abbiamo implementato e regolare i guadagni per ottenere una
resa aptica efficace. In Figura 7.7 viene mostata una prima fase di test sul simulatore V-Rep a sinistra; mentre la mappa degli ostacoli registrata da ROS, viene visualizzata mediante il visualizzatore RViz.
Figura 7.7.: Test preliminare con lo youBot virtuale su V-Rep
I nostri algoritmi di navigazione vengono inizialmente testati sul robot youBot della Kuka. Questo robot però viene guidato come se fosse anolonomo per renderlo simile al veicolo da lavoro preso in esame in questo lavoro.
7.2. Coastal Navigation
Questa modalità di navigazione che indico come coastal navigation, nasce dalla necessità di far navigare il veicolo vicino agli ostacoli. Infatti in questo contesto gli ostacoli rappresentano dei luogi di lavoro per il veicolo in esame. Per esempio l’operatore vuole guidare il veicolo vicino ad una parete per poi compiere su di essa un certo task. Inoltre l’operatore, dovendo esseguire un tipo di lavoro sulla parete, vuole avvicinare per poter eseguire l’operazione al meglio. Inoltre dagli esempi fatti, il veicolo deve riuscire a passare anche in ambienti angusti. Per questa ragione vogliamo fornire all’operatore, che guida il veicolo mediante una consolle, uno stimolo aptico per impedire che la macchina urti contro gli ostacoli. Inoltre, spesso, è importante mantenere il veicolo allineato alle pareti. L’operatore infatti, durante la demolizione di una parete, guida il veicolo in modo da mantenerlo allineato con esso. In questo modo le operazioni con il braccio sono più facili e più veloci dato che non sono necessari grossi spostamenti da parte del braccio per raggiungere la parete.
Quindi lo scopo di questo campo di forze è quello di aiutare l’operatore a stare bene accostato agli ostacoli, per questo si parla di una navigazione costiera, e si vuole aiutare l’o-peratore ad allineare la piattaforma rispetto alle pareti. Essendo una navigazione costiera, la posizione dell’interfaccia aptica viene mappata in comandi per il robot ma stavolta con una matrice di guadagni più bassi, poichè il veicolo si trova a navigare prossimo ad ostacoli. Inol-tre rispetto alla navigazione standard, la regione di influenza che prima si deformava, adesso rimane fissa e corrisponde alla soglia di sicurezza minima per la rilevazione degli ostacoli. Le altre regioni e le 8 zone rimangono invariate, ma le celle vengono gestite diversamente. Inoltre anche in questo caso la mappa viene filtrata con le stesse modalità.
7.2 Coastal Navigation
Per essere ben allineati lungo pareti sia frontali che laterali viene utilizzata la registrazione degli ostacoli e la mappa offerta da ROS. Questa mappa si comportano come filtro passa basso ovvero contorni con lievi irregolarita appartengono alla stessa cella. Questa tolleranza implicita dettata dal fatto di modellare il mondo come una griglia di granularità regolabile ha permesso di sviluppare questo algoritmo di assistenza in un modo più semplice.
Figura 7.8.: Coastal navigation: allineamento lungo pareti frontali e suddivisione del robot in 4 parti
Prendiamo in considerazione, almeno per il momento, solamente la zona frontale e sud-dividiamo la larghezza del robot in quattro parti. Si identifica con FL parte sinistra e FR
la parte destra quando dividiamo la larghezza del robot in due, e le 4 parti del robot le identifichiamo a partire da sinistra come Zi con i = 1, . . . 4. Questa suddivisione servirà
per allineare meglio il robot alla parete. La prima questione è capire quando ci troviamo di fronte ad una parete; si definisce parete un insieme di celle contigue che occupano una lunghezza superiore ad un quarto della larghezza del robot. L’altra questione è definire il termine “ben allineati” con la perete, in particolare per quantificare se la posa del nostro veicolo è correttamente allineata con la parete si controlla che, rispetto al numero di cel-le attualmente registrate, ci siano almeno un numero di celcel-le contigue che si trovano alla stessa distanza rispetto al piano Γ2, dato che stiamo considerando la zona frontale. Si deve
però porre attenzione a come entra questa parete nella regione di influenza. Infatti, vedi Figura 7.8, la parete minima identificata può trovarsi nella zona frontale sinistra, oppure nella zona frontale destra oppure in entrambe le zone. Se la parete identificata risiede nella parte sinistra FL allora l’allineamento del robot avviene in base ad un nuovo sistema di
rife-rimento che non è più centrato ma è traslato verso sinistra. Se invece la parete si trova nella zona FRallora l’allineamento del robot avviene rispetto al sistema di riferimento traslato a
destra. Ed infine se la parete risiede sia nella parte FLche nella parte FRallora il sistema di
riferimento usato per l’allineamento torna ad essere centrato al robot.
Figura 7.9.: Coastal navigation: allineamento a pareti laterali e suddivisione del robot in 4 parti
Quindi a seconda della zona in cui si viene a trovare la parete leggiamo gli ostacoli da posizioni diverse sfruttando i sistemi di riferimento traslati e situati nella sottozone destra o sinistra. Il ragionamento fatto fino ad ora rigurdava la sola regione frontale ma sono del tutto simili nel caso in cui la parete entra nella zona laterale del robot. Suddividiamo stavolta la lunghezza del robot in 4 parti ed identifichiamo come parete un insieme di celle contigue di lunghezza superiore ad un quarto della lunghezza del robot ed ispezioniamo gli ostacoli rispetto ai frame traslati. In Figura 7.9 abbiamo che la parete situata sul lato sinistro del robot, possono quindi verificarsi tre condizioni. Nel caso in cui la parete si trovi nella zona frontale oppure nella zona posteriore oppure in entrambe le zone, la lettura e l’allineamento del robot avvengono rispetto ai sistemi di trasferimento stavolta traslati in avanti e indietro.
Figura 7.10.: Coastal navigation: contributo delle forze nel caso di pareti su entrambi i lati
In Figura 7.10 si mette in evidenza che l’algoritmo proposto facilità l’allineamento del veicolo anche rispetto ad un corridoio dove abbiamo pareti su entrambi i lati, infatti le forze che si generano su entrambi i lati nel caso di un mal allineamento risultano essere di segno concorde e riportano il veicolo parallelo alle pareti del corridoio.
Per la resa aptica ogni zona fornisce un contributo in termini di coppia che si traducono sull’interfaccia come comandi che gestiscono la velocità di imbardata. Il segno ed il modulo delle coppie generate in modo indipendente dalle zone 1,3,4 e 8 sono gestite nel seguente modo. Il segno è dettato dal verso del vettore ottenuto dal prodotto vettoriale tra il vettore r della cella più arretrate ed il versore ni, che identifica l’ascissa del sistema di riferimento
traslato oppure no a seconda della zona in cui si trova il muro. Il Modulo è dato dalla differenza tra il numero di celle allineate ed il numero di celle ancora da allineare.
7.3 Optimal Navigation
Fino ad ora abbiamo parlato nel caso in cui si identifica le celle come un muro da inseguire o comunque per essere allineati bene, nel caso in cui le celle non fossero una parete si potrebbero gestire e creare delle forze e coppie stavolta evasive per la zona 1 e 3 vedi Figura 7.11. Dopo una prima analisi ci è sembrato innaturale lasciare attive queste coppie evasive, le abbiamo quindi escluse nel contributo finale, la forza risultante risulta essere proporzionale all’errore di allineamento con i contributi:
Fy = τ2+ τ4 + τ8
Figura 7.12.: Validazione della coastal navigation
Questa guida doveva rispettare almeno i seguenti scenari (Figura 7.12 e Figura 7.13): alli-neato a parete frontale; allialli-neato a prete laterale destra osinistra; follow a wall; allineamento su corridoio; guida in passaggi stretti.
Figura 7.13.: Coastal navigation: guida del veicolo in ambienti stretti come corridoio e inseguimento di parete
7.3. Optimal Navigation
Questa modalità di navigazione che indico come optimal navigation, carca di aiutare l’opera-tore a posizionare bene la base del veicolo all’interno della zona di lavoro per il manipolal’opera-tore montato a bordo del veicolo. Infatti la macchina da lavoro presa in esame presenta una particolare caratteristica, prima di poter lavorare con il braccio manipolatore la base del
veicolo deve essere stazionata a terra. Se l’operatore posiziona il veicolo troppo lontano dalla zona di lavoro sono necessari continui spostamenti e stazionamenti, questa operazione comporta perdita di tempo e maggiore stress da parte dell’operatore. Si vuole quindi offrire all’operatore un suggerimento aptico per il posizionamento ottimo della base in modo da compiere successivamente una corretta manipolazione.
Supponiamo che l’operatore specifichi, con quale strategia per esempio un sensore laser proiettato sul muro, una serie di punti target per il manipolatore. Senza perdità di generalità supponiamo di avere due punti target identificati rispetto ad un sistema di riferimento odo-metrico, che rappresentano punti in cui si vuole compiere un operazione con il manipolatore, o meglio rappresentano il punto iniziale e finale di una traiettoria che per il momento consi-deriamo lineare. Ci interessa capire quale sia la posizione ottima della base che massimizza una certa funzione costo che definiremo successivamente, ma che comunque deve garantire una completa accessibilità nei punti target e magari nell’intorno di questi punti. Un altra osservazione per quanto riguarda il veicolo ed il braccio, in letteratura troviamo molti lavori sulla coordinazione “gambe e braccio” [53, 54, 55] ovvero il fatto di montare a bordo di un veicolo la base di un braccio manipolatore che in generale è fissata a terra comporta una serie di vantaggi come per esempio quello di rendere il workspace del manipolatore illimitato. Un altro vantaggio di montare un braccio a bordo di un veicolo potrebbe essere anche quello di sfruttare i gradi di libertà del veicolo durante la manipolazione per evitare le singolarità del braccio o compiere un task più velocemente. Per esempio Monastero et al. [56] presenta un algoritmo per la pianificazione di un percorso per manipolatori mobili anolonomi. Gli autori calcolano la treiettoria del robot che massimizza le performance del braccio (manipolabilità) e la stabilità della base mobile. Nel caso in esame abbiamo, braccio manipolatore e base mobile che vengono azionati non simultanemante quindi la base del veicolo viene usata come un mezzo per posizionare la base del braccio. In tale senso, T. Asokan et al.[57] propongono un metodo per il posizionamento di un veicolo subacqueo massimizzando la destrezza per aumentare la capacità di intervento per il task. Gli autori si basano sul calcolo del workspa-ce, dei confini e delle superfici di singolarità del braccio manipolatore sfruttando il metodo proposto da K. Abdel-Malek et.al in[58, 59] per poi impostare un calcolo iterativo che porta il robot nella posa migliore in base all’indice di manipolabilità. Nel lavoro di tesi si utilizza il metodo da loro proposto per il calcolo del workspace e delle superfici singolari per poi impostare, diversamente dal loro metodo, un problema di ottimizzazione vincolata invece di un calcolo iterativo.
Figura 7.14.: Denavit–Hartenberg parametri e specifiche del braccio dello youBot Prese le specifiche del braccio dello youBot a 5 DOF, si costruisce la tabella di D-H Figura 7.14, per la modellazione del braccio (Figura 7.15) ci siamo affidati al robotic toolbox
7.3 Optimal Navigation
[60] su Matlab poichè dalla versione 9 è possibile interfacciare Matlab ed il modello del braccio manipolatore virtuale implementato sul simulatore V-Rep.
Figura 7.15.: Superfici singolari e braccio dello youBot su Matlab Costruita la tabella di Denavit Hartenberg si ottiene la cinematica diretta
x= Φ(q) = T0e(1 : 3, 4)
dove x = [ x y z ] e T0e è la matrice omogenea tra la base del braccio e l’end-effector; nei
casi reali le coordiante generalizzate sono limitate da limiti superiori ed nferiori qL
k ≤ qk ≤ qkU,
questi limiti comportano nello spazio di lavoro del manipolatore ad una serie di superfici dove uno due o più giunti raggiungono il loro valore massimo o minimo, e rappresentano un insieme di superfici che limitano la destrezza. Si introduce λ = [ λ1 , λ2 , . . . , λn ]T slack
variable, ottenendo i vincoli nella forma qi = (qL i + qiU) 2 + ( (qU i − qiL) 2 ) sin(λi)
Il calcolo delle superifici di singolarità, dei bordi avviene andando a definire la matrice X = Φqqλ
e calcoliamo le condizioni per cui tale matrice perde di rango indicate come rank def icient delineate singular sets: s(i) f or i= 1, . . . , k
Tali condizioni sono:î
λ1 = ±Π2; λ1,2 = ±Π2; λ1,3 = ±Π2; λ1 = ±Π2; q3 = 0, q4 = ±Π2; λ1,2,3= ±Π2; λ1,4 = ±Π2; λ1,2,4 = ±Π2; λ1,3,4 = ±Π2; λ1,4 = ±Π2, q3 = 0; λ2,3 = ±Π2; λ2,3,4 = ±Π2; λ3,4 = ±Π2; λ2,4 = ±Π2; λ1 = ±Π2, q4 = ±Π2; λ1,2 = ±Π 2, q4 = ± Π 2; λ1,3 = ± Π 2, q4 = ± Π 2; λ1 = ± Π 2, q3 = 0, q4 = ± Π 2; λ2 = ±Π2, q= ±Π2; λ2,3 = ±Π2, q4 = ±Π2; λ2 = ±Π2, q3 = 0, q4 = ±Π2; λ3 = ±Π2, q3 = 0, q4 = ±Π2; q3 = 0, q4 = ±Π2 ï
L’espressione analitica delle superfici di singolarità che indichiamo con ξ(i)(u(i)) = Φ(u(i), s(i)) sono state calcolate calcolate con l’aiuto del software Wolfram Mathematica e il Symbolic toolbox di Matlab.
Figura 7.16.: Superfici singolari e confini per il braccio dello youBot (a)
Una lista più completa delle singolarità e dei confini del manipolatore in esame sono indicate in sezione A.1.
7.3 Optimal Navigation
Figura 7.18.: Superfici singolari e confini per il braccio dello youBot (c)
Le superfici singolari ed i confini (Figura 7.16, Figura 7.17, Figura 7.18) vengono roto-traslate, ed anche la cinematica diretta per tenere traccia di queste superfici rispetto ad un sistema di riferimento fisso in questo caso quello odometrico. Si ottiene quindi:
Ψ(i)(w, u) = R(α, β, γ) ξ(i)(u) + v(xw, yw, zw)
Γ(w, q) = R(α, β, γ) Φ(q) + v(xw, yw, zw)
Indicati i punti target come P(j)j = 1, . . . l espressi nel sistema di riferimento fisso, si
7.3 Optimal Navigation w∗ = argmax (ω 1 ñ J(q1) J(q1)T + ω 2 ñ J(q2) J(q2)T + ëPm− v(x w, yw, zw)ë2) Subject to . . .P (1)− Γ(q1, w).. .2 ≤ β . . .P (2)− Γ(q2, w).. .2 ≤ β . . .P (1)− Ψ(i)(u(i), w).. .2 >ε . . .P (2)− Ψ(i)(u(i), w).. . 2 >ε qL ≤ q ≤ qU wL ≤ w ≤ wU a− |(Rdb([di]b− Rbo(Pd− Pa))) i| ≤ 0 b− |(Rdb([di]b− Rbo(Pd− Pa))) j| ≤ 0
Il vettore w = [ xw yw zw α β γ ] tiene traccia della posa del robot rispetto al
sistema di riferimento fisso (Figura 7.19); a e b rappresentano rispettivamente metà lunghezza e larghezza della base del veicolo; con Pa e Pdsi indica la posizione del robot attuale e
desiderata; le matrici di rotazioni Rdb e Rbopermettono il cambio di coordinate tra il frame
odom, il frame base_link ed il frame desired. Gli ultimi due vincoli tengono conto degli ostacoli presenti al momento dell’avvio del processo di ottimizzazione indicati con di ed
espressi nel frame base_link.
Figura 7.19.: Optimal navigation: descrizione dei sistemi di riferimento
Il problema di ottimizzazione vincolato definito sopra è stato risolto su Matlab tramite optimization toolbox. Sono stati fatti test preliminari su un modello semplice di mani-polatore planare composto da due giunti rotoidali, i risultati prima e dopo il processo di ottimizzazione sono mostrati inFigura 7.20.
Figura 7.20.: Studio preliminare dell’ottimizzazione vincolata su robot 2 DOF planare
Successivamente lo abbiamo applicato al braccio della Kuka, cercando di variare l’algo-ritmo di ottimizzazione, le condizioni iniziali, i parametri per il solver, e testando anche il parallel toolbox offerto da matlab per cercare di rendere più rapido il calcolo della posa ottima. In Figura 7.21
Figura 7.21.: Constrained optimization preliminary test for yoBot’s arm: in red the two target point
vediamo la base del robot prima e dopo l’ottimizzazione ed i punti target specificati in rosso.
Il metodo usato è composto quindi da due parti, fino adesso abbiamo ottenuto la posa ottima. Adesso per suggerire all’operatore, che guida il veicolo attraverso l’interfaccia, quale sia la posa ottimale per la base mobile
7.4 Comprehension workspace and barriers
Figura 7.22.: Il campo di forza porta l’operatore a guidare il veicolo verso la posa corretta si crea un campo attrattivo Figura 7.22, definito proporsionale all’errore tra la posa attuale del robot e quella desiderata.
7.4. Comprehension workspace and barriers
Per garantire una guida sicura per queste tre modalità descritte sopra, si definisce una serie di virtual wall che si attivano in presenza di ostacoli all’interno di una regione di sicurezza definita in fase di implementazione. Inoltre l’operatore viene informato con uno stimolo aptico aggiuntivo quando i punti target entrano nello spazio di lavoro, mediante una vibrazione con frequenza a ampiezza tarata in ambiente simulato.
