Prova Scritta di Robotica I 9 Febbraio 2009

Transcript

1 Esercizio Prova Scritta di Robotica I 9 Febbraio 9 Si consideri l estensione al secondo ordine in accelerazione dello schema di controllo cinematico di traiettoria cartesiana. A tale scopo, si assuma che il modello del manipolatore con n giunti sia rappresentato dall equazione vettoriale q = a, con ingresso di accelerazione a R n. Lo stato iniziale del manipolatore è dato dalla posizione q e dalla velocità q dei giunti. La cinematica diretta è espressa da p = fq, con posizione/orientamento dell organo terminale p R m m = n. Sono misurate e disponibili per il feedback sia la posizione q che la velocità q dei giunti del manipolatore. Infine, è assegnata una traiettoria desiderata cartesiana p d t di cui si conosce la derivata prima ṗ d t e seconda p d t, per t. Scrivere la legge di controllo per a tale che le componenti dell errore cartesiano di traiettoria e p,i t = p d,i t p i, i =,..., m, soddisfino alle seguenti equazioni lineari e disaccoppiate: ë p,i + k D,i ė p,i + k P,i e p,i =, k P,i, k D,i >, i =,..., m. Si assuma di non trovarsi in situazioni di singolarità. Disegnare lo schema a blocchi del sistema complessivo con il controllore. Esercizio Un robot planare R, con lunghezze l e l dei bracci l l, deve lavorare prevalentemente intorno alla posizione P definita rispetto ad un sistema di riferimento assoluto SR w = x w, y w. In fase di installazione, si deve posizionare la base in un punto B in SR w in modo che il robot: sia in una configurazione a massima manipolabilità quando il suo organo terminale è in P ; realizzi in P la massima velocità v possibile per l organo terminale nella direzione cartesiana v d = v d,x v d,y T v d =, tenendo conto dei limiti sulle velocità dei giunti dati da θ V e θ V. Risolvere tale problema di scelta ottimale della postura in forma analitica nota: le soluzioni equivalenti sono multiple; è sufficiente fornirne una. Con le espressioni trovate, calcolare quindi i valori numerici di B e θ, nonchè la θ che fornisce la massima velocità dell organo terminale e l associata v = Jθ θ a partire dai seguenti dati: l = [m], l =.5 [m], P = [m], v d = [m/s], V = [rad/s], V = [rad/s]. [5 minuti di tempo; libri aperti]

2 Soluzioni 9 Febbraio 9 Esercizio A partire dalle relazioni p = fq, ṗ = fq q q = Jq q, p = Jq q + Jq q, e utilizzando il modello q = a, per la dinamica in accelerazione dell errore cartesiano e p si ha Imponendo la dinamica desiderata ë p = p d p = p d Jqa Jq q. ë p = K D ė p K P e p, K D = diag{k D,i } > O, K P = diag{k P,i } > O e risolvendo per a in termini delle misure disponibili ai giunti si ottiene, fuori dalle singolarità, a = J q p d + K D ṗ d Jq q + K P p d fq Jq q. Lo schema a blocchi complessivo è riportato in Figura.. p d.. e! p " K D + #. Jq Jq modello robot. q q + + p.. p a q. q p d!" d J -! q " fq # + ṗ p d! " p d # + e p K p generatore riferimenti Figura : Controllo cinematico cartesiano in accelerazione

3 Esercizio Si tratta di risolvere un particolare problema di cinematica inversa, in cui θ è ottenuto dalla specifica sulla manipolabilità e θ da quella che richiede di massimizzare la velocità dell organo terminale nella direzione v d. Trovata una soluzione θ, da questa si ottiene anche la posizione B della base. Di fatto esistono quattro soluzioni distinte e equivalenti. Inoltre il secondo sottoproblema determinazione di θ e delle velocità di giunto che forniscono la massima velocità cartesiana nella direzione desiderata si può affrontare in due modi diversi: con un metodo di natura essenzialmente geometrica oppure formulando e risolvendo un semplice problema di ottimizzazione vincolata. Le relazioni cinematiche di interesse per il robot planare R sono: l cos θ + l cos θ + θ l sin θ l sin θ + θ l sin θ + θ p =, Jθ =, l sin θ + l sin θ + θ l cos θ + l cos θ + θ l cos θ + θ con det Jθ = l l sin θ. Le grandezze sono espresse rispetto ad una terna SR, orientata come SR w e con origine in B. Poichè H man θ = det JθJ T θ = det Jθ = l l sin θ, la prima specifica impone il valore θ = ± π/. La base B sarà su una circonferenza centrata in P e di raggio r = l + l. Scegliendo θ = π/ si vedrà in seguito che la soluzione con il segno negativo è ricavabile in modo del tutto simile, si ha l cos θ l sin θ p θ=π/ =, l sin θ + l cos θ e quindi B = P p θ=π/, che rimane una funzione della sola incognita θ. Questa viene definita dalla seconda specifica, che si può affrontare come detto in due modi. Verrà esposto in dettaglio il primo metodo e poi più brevemente il secondo. Primo metodo Per avere una velocità v dell organo terminale che sia la massima possibile in norma con direzione per ora non fissata, è chiaro che i due giunti devono cooperare al meglio, ossia si dovranno scegliere i massimi valori ammissibili delle velocità di giunto con segno concorde θ = V e θ = V, oppure θ = V e θ = V. La velocità dell organo terminale in una data configurazione è la somma dei contributi v e v dovuti alle velocità dei singoli due giunti è il principio di sovrapposizione degli effetti che porta alla definizione della matrice Jacobiana. Per valutare il valore massimo e la direzione di v, è conveniente riferirsi dapprima ad una terna ausiliaria SR a solidale al primo braccio del robot con origine in B vedi Figura. 3

4 v=v +v v! v y a r V y! V x x a Figura : Calcolo di a v con θ = π/, θ = V e θ = V Scegliendo i valori positivi di velocità massima dei due giunti, θ = V e θ = V, si ha: V + l V a v = v + v = sin α l + l cos α = l l + l l +l l l +l V + l l V + V V = l V. 3 Rispetto alla terna SR orientata come SR w, l espressione di tale velocità è cos v = Rθ a θ sin θ v, Rθ =, 4 sin θ cos θ dove si è utilizzata la matrice di rotazione planare. Poichè Rθ è ortonormale, risulta: v = a v = l V + l V + V. 5 La 5 fornisce quindi la norma della massima velocità realizzabile nella configurazione a massima manipolabilità indipendente da θ. Per allineare la velocità v con la direzione v d, si deve muovere la posizione della base B lungo la circonferenza di raggio r, determinando di conseguenza il valore θ. Occorre in sostanza risolvere l equazione vettoriale a v Rθ v = v d, che risulta non lineare nell incognita θ. Si noti che si è introdotto il fattore di normalizzazione v per tener conto del fatto che la v d assegnata è a norma unitaria. Tale equazione vettoriale si risolve in modo algebrico in maniera analoga a quanto visto per la cinematica inversa del robot R, riscrivendola nelle incognite sin θ e cos θ e ottenendo quindi il sistema lineare l V l V + V sin θ vd,x =. v l V + V l V cos θ v d,y L inversa della matrice dei coefficienti è l V l V + V v l V + V l V 4

5 si noti che il suo determinante è pari a v. La soluzione cercata è dunque sin θ = l V v d,x l V + V v d,y cos θ v l V v d,y l V + V v d,x 6 da cui θ = ATAN{sin θ, cos θ } = ATAN{ l V v d,x l V +V v d,y, l V v d,y l V +V v d,x }. 7 Utilizzando tale valore in, ovvero la soluzione trigonometrica 6 in, si ottiene l espressione finale per B. Si può infine verificare che, nella configurazione θ = θ, π/ con θ fornita dalla 7, la velocità dell organo terminale è pari a V v = Jθ = l V V + l V + V v d. 8 Con i dati numerici del testo, si ha: B = [m], θ =.944 [rad] = 68.69, θ = π.94 [rad] = 9, v = v v d = = [m/s]..748 La soluzione trovata si può denominare B = B +,+ e θ = θ +,+, rispettivamente la posizione della base del robot e il primo angolo di giunto, in quanto è stata ottenuta scegliendo il valore positivo θ = π/ e i valori massimi positivi per le velocità di giunto. Se si fossero scelti i valori massimi negativi delle velocità di giunto si sarebbe ottenuta una soluzione speculare, con base B +, posizionata in modo diametralmente opposto sulla circonferenza di raggio r e con θ +, che differisce di conseguenza di π da θ +,+. Tale seconda soluzione si ottiene direttamente dalla 7 cambiando i segni nei termini dove compaiono V o V : θ +, = ATAN{l V v d,x + l V + V v d,y, l V v d,y + l V + V v d,x }. 9 Da questa si determina poi B +, tramite la valutata con θ = θ +,. Infine, per il calcolo della velocità cartesiana, al posto della 8 si utilizza in questo caso V v = Jθ = V dove lo Jacobiano è ora valutato nella configurazione θ = θ +,, π/. l V + l V + V v d, Con i dati numerici del testo, si ha:.94 B +, = [m], θ +, =.944 [rad] =.3, θ = π.758 [rad] = 9. Ovviamente la velocità cartesiana v che si ottiene è la stessa del caso precedente. 5

6 Le due soluzioni con θ = π/ sono riportate in Figura 3, dove è mostrato anche l ellisse di manipolabilità, identico nelle due configurazioni. A tale proposito, si può notare che l asse principale dell ellisse non è allineato alla direzione di massima velocità ottenibile. Questo indica che i limiti massimi delle velocità di giunto V e V forniti nel problema non sono quelli ottimali da questo punto di vista! D altronde il concetto di manipolabilità non include la considerazione di eventuali limiti massimi sulle velocità di giunto, ma è relativo solo alla trasformazione di velocità di giunto con θ = o pari a un arbitrario k in velocità cartesiane. In modo analogo si può procedere per trovare le altre due soluzioni corrispondenti alla scelta iniziale negativa per θ = π/ il primo apice nella notazione della soluzione diventa negativo. Le formule relative sono: θ,+ = ATAN{ l V v d,x + l V + V v d,y, l V v d,y + l V + V v d,x } per la scelta di massime velocità di giunto positive, e θ, = ATAN{l V v d,x l V + V v d,y, l V v d,y l V + V v d,x } per il caso di massime velocità di giunto negative. Le due posizioni B,+ e B, della base sono ottenute questa volta dalla formula analoga alla l cos θ B = P p θ= π/ = P + l sin θ, l sin θ l cos θ utilizzando rispettivamente la o la per θ. Con i dati numerici del testo, per la scelta positiva delle massime velocità di giunto si ottiene.94 B,+ = [m], θ,+ =.3734 [rad] = 78.69, θ = π.786 [rad] = 9, mentre per quella negativa si ha.758 B, = [m],.786 Le due soluzioni con θ = π/ sono mostrate in Figura 4. Secondo metodo θ, =.768 [rad] =.3, θ = π [rad] = 9. Agli stessi risultati si perviene attraverso la soluzione di un problema di ottimizzazione vincolata. Si assuma θ = π/. Ripetendo l analisi fatta per pervenire alle 3-4, si può scrivere in generale per arbitrarie velocità di giunto θ, θ l θ + θ v = Rθ. 3 l θ La seconda specifica si traduce nella seguente formulazione: max θ, θ s.t v T d v π θ π V θ V V θ V, 4 6

7 dove la v è data dalla 3. La funzione obiettivo in 4 tende ad allineare v con v d, massimizzando al contempo la norma di v. La formulazione 4 si decompone naturalmente in una sequenza di due problemi di massimo vincolato: { } max V θ V V θ V max π θ π Per il problema interno alle parentesi graffe, si ha dalla 3 vt d v v T d v = l θ l θ + θ v d,x v d,y l θ + θ l θ. 5 sin θ cos θ che viene evidentemente massimizzato rispetto a θ dal vettore sin θ cos θ = l θ l θ + θ l θ + θ l θ Sostituendo questa espressione nella 3 si ottiene v = l θ + l θ + θ v d. Pertanto, il problema esterno in 5 diventa T vd,x v d,y. 6 max V θ l θ + l θ + θ, V V θ V essendo v T d v d =. Dalla forma della funzione obiettivo segue immediatamente che il massimo si ha per V V θ = oppure θ =, 7 V V confermando l intuizione che i giunti cooperano al meglio con velocità massime in modulo e di segno concorde. Sostituendo i singoli valori in 7 nella 6 e calcolando quindi θ = ATAN{sin θ, cos θ } e poi B tramite la, si ottengono rispettivamente le soluzioni θ +,+, B +,+ e θ +,, B +,. In modo analogo si procede per θ = π/. 7

8 .5 first posture: th = pi / ; max positive joint velocities.5 second posture: th = pi / ; max negative joint velocities.5.5 y [m] y [m] x [m] x [m] Figura 3: Le due soluzioni con θ = π/ sono mostrate anche l ellisse di manipolabilità e la velocità massima cartesiana ottenuta, entrambe non in scala.5 third posture: th = -pi / ; max positive joint velocities.5 fourth posture: th = -pi / ; max negative joint velocities.5.5 y [m] y [m] x [m] x [m] Figura 4: Le due soluzioni con θ = π/ 8