Trajectory planning in Cartesian space

Transcription

1 Roboics 1 Trajecory planning in Caresian space Prof. Alessandro De Luca Roboics 1 1

2 Trajecories in Caresian space in general, he rajecory planning mehods proposed in he join space can be applied also in he Caresian space consider independenly each componen of he ask vecor (i.e., a posiion or an angle of a minimal represenaion of orienaion) however, when planning a rajecory for he hree orienaion angles, he resuling global moion canno be inuiively visualized in advance if possible, we sill prefer o plan Caresian rajecories separaely for posiion and orienaion he number of knos o be inerpolaed in he Caresian space is ypically low (e.g., 2 knos for a PTP moion, 3 if a via poin is added) use simple inerpolaing pahs, such as sraigh lines, arc of circles, Roboics 1 2

3 Planning a linear Caresian pah (posiion only) p i L p f pah parameerizaion p(s) = p i + s (p f - p i ) GIVEN p i, p f, v max, a max v i, v f (ypically = 0) p f - p i p f - p i = L = p f - p i uni vecor of direcional cosines of he line s [0,1] seing s = σ/l, σ [0,L] is he arc lengh (gives he curren lengh of he pah). 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 Roboics 1 3

4 Timing law wih rapezoidal speed σ(). σ() a max bang- coas- bang v max given*: L, v max, a max find: T s, T V max (T - T s ) = L = area of he speed profile σ() T s T-T s T L T s = v max a max L a max + v 2 max T = a max v max a coas phase exiss iff: L > v max2 /a max * = oher inpu daa combinaions are possible (see exbook) Roboics 1 4

5 Timing law wih rapezoidal speed σ() 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 can be used also in he join space! Roboics 1 5

6 x A z A Concaenaion of linear pahs B = via poin over-fly C no need o pass (and sop!) here C y B - A B - A = K AB C - B C - B = K BC uni vecors of direcion cosines given: consan speeds v 1 on linear pah AB v 2 on linear pah BC desired ransiion: wih consan acceleraion for a ime ΔT x() p() = y() [0, ΔT] (ransiion sars a = 0) z() noe: during over-fly, he pah remains always in he plane specified by he wo lines inersecing a B (in essence, i is a planar problem) Roboics 1 6

7 .. x() Time profiles on componens 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 Roboics 1 7

8 Timing law during ransiion 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 uni vecors of direcion cosines [0, ΔT] (ransiion sars a = 0). 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 ) hus, we obain a parabolic blending (see exbook for his same approach in he join space) Roboics 1 8

9 Soluion (various opions) B d 1 d 2 A C C B - A = d 1 K AB C - B = d 2 K BC p() = A + v 1 K AB + 2 /2ΔT (v 2 K BC - v 1 K AB ) 1 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 by choosing, e.g., d 1 (namely A ) ΔT = 2d 1 /v 1 d 2 = d 1 v 2 /v 1 Roboics 1 9

10 A numerical example ransiion from A=(3,3) o C=(8,9) via B=(1,9), wih speed from v 1 =1 o v 2 =2 exploiing wo opions for soluion (resuling in differen pahs!) assign ransiion ime: ΔT=4 (we re-cener i here for [-ΔT/2, ΔT/2]) assign disance from B for deparing: d 1 =3 (assign d 2 for landing is handled similarly) B C B C A A A ΔT=4 d 1 =3 Roboics 1 10

11 A numerical example (con d) firs opion: ΔT=4 (resuling in d 1 =2, d 2 =4) second opion: d 1 =3 (resuling in ΔT=6, d 2 =6) acually, he same vel/acc profiles only wih a differen ime scale!! Roboics 1 11

12 Alernaive soluion (imposing acceleraion) A B C C.. p() = 1/ΔT (v 2 K BC - v 1 K AB ) A v 1 = v 2 = v max (for simpliciy).. 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 ) hen, d 1 = d 2 = v max ΔT/2 Roboics 1 12

13 Applicaion example plan a Caresian rajecory from A o C (res-o-res) ha avoids he obsacle O, wih a a max and v v max A B C add a via poin B sufficienly far from O C A O C A on AA a max on A B and BC v max on C C - a max + over-fly beween A e C Roboics 1 13

14 Oher Caresian pahs circular pah hrough 3 poins in 3D (ofen buil-in feaure) linear pah for he end-effecor wih consan orienaion in robos wih spherical wris: planning can be decomposed ino a pah for wris cener and one for E-E orienaion, wih a common iming law hough more complex in general, i is ofen convenien o parameerize he Caresian geomeric pah p(s) in erms of is arc lengh (e.g., wih s = Rθ for circular pahs), so ha velociy: dp/d = dp/ds ds/d dp/ds = uni vecor ( =1) angen o he pah: angen direcion (s) ds/d = absolue value of angenial velociy (= speed) acceleraion: 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 = curvaure κ(s) (= 1/radius of curvaure) d 2 p/ds 2 (ds/d) 2 = cenripeal acceleraion: normal direcion n(s) o he pah, on he osculaing plane; binormal direcion b(s) = (s) n(s) d 2 s/d 2 = scalar value (wih any sign) of angenial acceleraion Roboics 1 14

15 Definiion of Frene frame For a generic (smooh) pah p(s) in R 3, parameerized by s (no necessarily is arc lengh), one can define a reference frame as in figure s n(s) b(s) (s) p = dp/ds p = d 2 p/ds 2 derivaives w.r.. he parameer (s) = p (s)/ p (s) uni angen vecor n(s) = p (s)/ p (s) uni normal vecor ( osculaing plane) b(s) = (s) n(s) uni binormal vecor general expression of pah curvaure (a a pah poin p(s)) κ(s) = p (s) p (s) / p (s) 3 Roboics 1 15

16 Opimal rajecories for Caresian robos (e.g., PPP joins) 1. he sraigh line joining wo posiion poins in he Caresian space is one pah ha can be execued in minimum ime under velociy/acceleraion consrains (bu oher such pahs may exis, if (join) moion can also be no coordinaed) 2. he opimal iming law is of he bang-coas-bang ype in acceleraion (in his special case, also in erms of acuaor orques) for ariculaed robos (wih a leas a R join) 1. e 2. are no longer rue in general in he Caresian space, bu ime-opimaliy sill holds in he join space when assuming bounds on join velociy/acceleraion sraigh line pahs in he join space do no correspond o sraigh line pahs in he Caresian space, and vice-versa bounds on join acceleraion are conservaive (hough kinemaically racable) w.r.. acual ones on acuaor orques, which involve he robo dynamics when changing robo configuraion/sae, differen orque values are needed o impose he same join acceleraions Roboics 1 16

17 Planning orienaion rajecories A x A z A x B zb B y A y B using minimal represenaions of orienaion (e.g., ZXZ Euler angles φ,θ,ψ), we can plan independenly a rajecory for each componen e.g., a linear pah in space φ θ ψ, wih a cubic iming law bu poor predicion/undersanding of he resuling inermediae orienaions alernaive mehod: based on he axis/angle represenaion deermine he (neural) axis r and he angle θ AB : R(r,θ AB ) = R A T R B (roaion marix changing he orienaion from A o B inverse axis-angle problem) plan a iming law θ() for he (scalar) angle θ inerpolaing 0 wih θ AB (wih possible consrains/boundary condiions on is ime derivaives), R A R(r,θ()) specifies hen he acual end-effecor orienaion a ime Roboics 1 17

18 Uniform ime scaling for a given pah p(s) (in join or Caresian space) and a given iming law s(τ) (τ=/t, T= moion ime ), we need o check if exising bounds v max on (join) velociy and/or a max on (join) acceleraion are violaed or no unless such consrains have already been aken ino accoun during he rajecory planning, e.g., by using a bang-coas-bang acceleraion iming law velociy scales linearly wih moion ime dp/d = dp/ds ds/dτ 1/T acceleraion scales quadraically wih moion ime d 2 p/d 2 = (d 2 p/ds 2 (ds/dτ) 2 + dp/ds d 2 s/dτ 2 ) 1/T 2 if moion is unfeasible, scale (increase) ime T kt (k>1), based on he mos violaed consrain (max of he raios v /v max and a /a max ) if moion is oo slow w.r.. he robo capabiliies, decrease T (k<1) in boh cases, afer scaling, here will be (a leas) one insan of sauraion (for a leas one variable) no need o re-compue moion profiles from scrach afer he scaling! Roboics 1 18

19 Numerical example - 1 2R planar robo wih links of uniary lengh (1 [m]) linear Caresian pah p(s) from q 0 =(110, 140 ) p 0 =f(q 0 )=(-.684, 0) [m] o p 1 =(0.816, 1.4), wih res-o-res cubic iming law s(), T=1 [s] bounds in join space: max (absolue) velociy v max,1 = 2, v max,2 = 2.5 [rad/s], max (absolue) acceleraion a max,1 = 5, a max,2 = 7 [rad/s2 ]. s max 3 [m/s] q 0 p 1 pah lengh L= [m] zero iniial and final speed p 0 s=s() non-zero (symmeric) acceleraion T=1 Roboics 1 19

20 Numerical example - 2 violaion of boh join velociy and acceleraion bounds wih T=1 [s] max relaive violaion of join velociies: k vel = max relaive violaion of join acceleraions: k acc = minimum uniform ime scaling of Caresian rajecory o recover feasibiliy k = max {1, k vel, k acc } = T scaled = kt = > T = join 1 = join 2 Roboics 1 20

21 Numerical example - 3 scaled rajecory wih T scaled = [s] speed [acceleraion] on pah and join velociies [acceleraions] scale linearly [quadraically] raced Caresian pah and associaed join pahs remain he same! a leas 1 insan of sauraion! Roboics 1 = join 1 = join 2 21