Pianificazione di traiettorie nello spazio cartesiano

Transcript

1 Corso di Roboica 1 Pianificazione di raieorie nello spazio caresiano Prof. Alessandro De Luca Roboica 1 1

2 Traieorie nello spazio caresiano le ecniche di pianificazione nello spazio dei giuni si possono applicare in generale anche nello spazio caresiano una pianificazione per l orienameno basaa su (re) parameri minimali che idenificano la posa non favorisce la visualizzazione dell andameno complessivo in 3D si cerca comunque di pianificare separaamene per posizione e orienameno normalmene i nodi di posizione da inerpolare nello spazio caresiano sono pochi (es., P-T-P: 2 nodi, se aggiungo un via poin divenano 3): si possono usare funzioni spaziali inerpolani più semplici (ipicamene, ree) Roboica 1 2

3 Esempio di raieoria caresiana (solo posizione) p i L p f cammino paramerico p(s) = p i + s (p f - p i ) ASSEGNATI p i, p f, v max, a max v i, v f (ipicamene 0) p f - p i p f - p i = L = p f - p i versore dei coseni direori della rea s [0,1] se pongo s = σ/l, σ [0,L] è l ascissa curvilinea (lunghezza del cammino percorso). dp.... d p(s) = s = (p f - p i ) s p(s) = 2 p. s 2 + ds ds 2 p f - p i. = σ = p f - p i.. σ L L dp.... s = (p f - p i ) s ds Roboica 1 3

4 Legge oraria con velocià rapezoidale σ(). σ() a max bang- coas- bang v max assegnai*: L, v max, a max deerminare: T s, T V max (T - T s ) = L area rapezio velocià σ() T s T-T s T L T s = v max a max L a max + v 2 max T = a max v max esisenza fase coas : L > v max2 /a max * = sono possibili alre combinazioni di dai in ingresso (vedi libro) Roboica 1 4

5 Legge oraria con velocià rapezoidale σ() a max a max 2 /2 [0,T s ]. σ() v max σ() = v max - v max 2 2 a max [T s,t-t s ] - a max (-T) 2 /2 + v max T - v max 2 a max σ() L [T-T s,t] T s T-T s T si può usare anche nello spazio dei giuni! Roboica 1 5

6 Concaenazione di raieorie reilinee A z A B overfly via poin C C y B - A B - A = K AB C - B C - B = K BC veori dei coseni direori x dae: velocià cosane di modulo v 1 su AB v 2 su BC ransizione: ad accelerazione cosane per un empo ΔT p() = x() y() z() [0, ΔT], = 0: inizio ransizione noa: durane l over-fly, il cammino rimane sempre nel piano specificao dai due rai reilinei che si inersecano in B (in praica, è un problema planare) Roboica 1 6

7 .. x() sulle singole componeni v 1 K AB,x. x() v 2 K BC,x.. y() ΔT. y() v 2 K BC,y.. z() v 1 K AB,y. z() v 1 K AB,z ΔT v2 KBC,z Roboica 1 7

8 Legge oraria della ransizione x A z A B p() = C x() y() z() C y.. p() = 1/ΔT (v 2 K BC - v 1 K AB ) B - A B - A = K AB C - B C - B = K BC veori dei coseni direori [0, ΔT], = 0: inizio ransizione. p() = v 1 K AB + /ΔT (v 2 K BC - v 1 K AB ) p() = A + v 1 K AB + 2 /2ΔT (v 2 K BC - v 1 K AB ) Roboica 1 8

9 Deerminazione ΔT (1) A B d 1 d 2 C C B - A = d 1 K AB 1 C - B = d 2 K BC p() = A + v 1 K AB + 2 /2ΔT (v 2 K BC - v 1 K AB ) A p(δt) = A + ΔT/2 (v 1 K AB + v 2 K BC ) = C - B + A + ΔT/2 (v 1 K AB + v 2 K BC ) = C - B 1 d 1 K AB + d 2 K BC = ΔT/2 (v 1 K AB + v 2 K BC ) d 1 = v 1 ΔT/2 d 2 = v 2 ΔT/2 se ad es. fisso d 1 (ossia A ) ΔT = 2d 1 /v 1 d 2 = d 1 v 2 /v 1 Roboica 1 9

10 Esempio numerico ransizione da A=(3,3) a C=(8,9) via B=(1,9), con velocià da v 1 =1 a v 2 =2 due esempi di opzioni per la soluzione (forniscono cammini differeni!) assegnao il empo di ransizione: ΔT=4 (qui, cenrao con [-ΔT/2, ΔT/2]) assegnaa la disanza da B di disacco: d 1 =3 (assegnare d 2 si raa in modo analogo) B C B C B A A ΔT=4 d 1 =3 Roboica 1 10

11 Esempio numerico (coninua) prima opzione: ΔT=4 (si oiene d 1 =2, d 2 =4) seconda opzione: d 1 =3 (si oiene ΔT=6, d 2 =6) di fao, sono gli sessi profili di vel/acc ma con scale dei empi diverse!! Roboica 1 11

12 Deerminazione ΔT (2) A B C C.. p() = 1/ΔT (v 2 K BC - v 1 K AB ) A v 1 = v 2 = v max (per semplicià).. p() = a max ΔT = (v max /a max ) K BC - K AB = (v max /a max ) 2 (1 - K BC,x K AB,x - K BC,y K AB,y - K BC,z K AB,z ) Roboica 1 12

13 Esempio pianificare una raieoria caresiana da A a C (da fermo a fermo) eviando l osacolo O, con a a max e v v max A B sufficienemene lonano da O C C A O C A su AA a max ; su A B e BC v max su C C - a max + overfly ra A e C ; Roboica 1 13

14 Alre raieorie caresiane circolari per 3 puni in 3D (buil-in) lineari dell E-E con orienameno cosane per robo con polso sferico: raieoria decomposa in posizione del polso e orienameno dell E-E si cerca sempre di paramerizzare il cammino caresiano p(s) rispeo all ascissa curvilinea (ad es., per cammini circolari s = Rθ), cosicché velocià: dp/d = dp/ds ds/d dp/ds = versore ( =1) angene alla raieoria: direzione angene (s) ds/d = modulo della velocià angenziale (scalare) accelerazione: d 2 p/d 2 = d 2 p/ds 2 (ds/d) 2 + dp/ds d 2 s/d 2 d 2 p/ds 2 = curvaura κ(s) (= 1/raggio di curvaura) d 2 p/ds 2 (ds/d) 2 = accelerazione cenripea: direzione normale n(s) al cammino, ma sullo sesso piano osculaore; binormale b(s) = (s) n(s) d 2 s/d 2 = valore scalare (pos/neg) dell accelerazione angenziale Roboica 1 14

15 Terna di Frene dao un cammino p(s) in R 3, paramerizzao da s (non necessariamene l ascissa curvilinea), si definisce una erna come in figura s n(s) b(s) (s) espressione generale della curvaura κ(s) = p (s) p (s) / p (s) 3 p = dp/ds p = d 2 p/ds 2 derivae rispeo al paramero (s) = p (s)/ p (s) versore angene n(s) = p (s)/ p (s) versore normale ( al piano osculaore) b(s) = (s) n(s) versore binormale Roboica 1 15

16 per robo caresiani Traieorie oime 1. il cammino reilineo ra due puni dello spazio operaivo è quello percorribile a empo minimo (ma non l unico!) 2. una legge oraria oima è bang-coas-bang in accelerazione (ovvero in coppia) per robo aricolai 1. e 2. non sono più vere in generale nello spazio caresiano, ma coninuano a essere vere nello spazio dei giuni se si assume di avere vincoli di max velocià e accelerazione però raieorie reilinee nello spazio dei giuni non sono ovviamene reilinee nello spazio operaivo, e viceversa il vincolo di massima accelerazione ai giuni è conservaivo rispeo a quello di massima coppia (in configurazioni diverse servono coppie differeni per oenere la sessa accelerazione) Roboica 1 16

17 Traieorie per l orienameno A x A z A x B zb B y A y B definia una rappresenazione minimale orienameno (ad es., angoli φ θ ψ di Eulero ZXZ), si porebbe raare separaamene la pianificazione di ogni componene ad es., raieoria reilinea nello spazio φ θ ψ ma scarsa visualizzazione meodo alernaivo: roazione inorno ad un asse si deermina un asse r e un angolo θ B : R(r,θ B ) = R A T R B (roazione che fa passare dall orienameno A a quello B problema inverso asse-angolo) si pianifica l andameno emporale per l angolo θ inerpolando da 0 a θ B (con evenuali vincoli sulle derivae), R A R(r,θ()) idenifica l orienameno isananeo dell E-E Roboica 1 17

18 Scalaura emporale uniforme fissao (nello spazio caresiano o in quello dei giuni) un cammino p(s) e una legge oraria s(τ) (τ=/t, T= empo di rasferimeno ) occorre verificare che non siano violai i vincoli (di solio fissai nello spazio dei giuni) di massima velocià v max e accelerazione a max a meno di non averne enuo già cono in fase di pianificazione, ad es. usando una legge bang-coas-bang in accelerazione si calcola e si rova il massimo (in valore assoluo) della velocià: dp/d=dp/ds ds/dτ 1/T accelerazione: d 2 p/d 2 =(d 2 p/ds 2 (ds/dτ) 2 + dp/dτ d 2 s/dτ 2 ) 1/T 2 se i vincoli sono violai, si scala (aumena in queso caso) T kt (k>1), sulla base del vincolo che è maggiormene violao (massimo ra i rappori v /v max e a /a max ) viceversa, se si è roppo leni rispeo ai vincoli si diminuisce T (k<1) Roboica 1 18