[Download] rev chapter-5-june26th

1,138 views
1,025 views

Published on

2nd International Summer School on Geometric Methods in Robotics, Mechanism Design and Manufacturing Research-Lecture 05 Trajectory Generation

Published in: Education
0 Comments
0 Likes
Statistics
Notes
  • Be the first to comment

  • Be the first to like this

No Downloads
Views
Total views
1,138
On SlideShare
0
From Embeds
0
Number of Embeds
6
Actions
Shares
0
Downloads
53
Comments
0
Likes
0
Embeds 0
No embeds

No notes for slide

[Download] rev chapter-5-june26th

  1. 1. Chapter 5 Trajectory Generation 1 Chapter Lecture Notes for Trajectory Generation A Geometrical Introduction to Introduction to Trajectory Generation Robotics and Manipulation Joint Space Trajectory Richard Murray and Zexiang Li and Shankar S. Sastry Generation ( -D) CRC Press Zexiang Li and Yuanqing Wu ECE, Hong Kong University of Science & Technology July ,
  2. 2. Chapter 5 Trajectory Generation 2 Chapter Trajectory Generation Chapter 5 Trajectory Generation Introduction to Trajectory Generation Joint Space Introduction to Trajectory Generation Trajectory Generation ( -D) Joint Space Trajectory Generation ( -D)
  3. 3. Chapter 5 Trajectory Generation 5.1 Introduction to Trajectory Generation 3 ◻ Motion planning and Trajectory generation: Chapter Trajectory Generation Definition: Motion planning Introduction Given the initial and goal config.s to Trajectory ginit , ggoal , find a collision-free path, g ∶ [ , ] ↦ SE( ), such that g( ) = ginit Generation and g( ) = ggoal . Joint Space Trajectory Generation ( -D) Play/Pause Stop
  4. 4. Chapter 5 Trajectory Generation 5.1 Introduction to Trajectory Generation 3 ◻ Motion planning and Trajectory generation: Chapter Trajectory Generation Definition: Motion planning Introduction Given the initial and goal config.s to Trajectory ginit , ggoal , find a collision-free path, g ∶ [ , ] ↦ SE( ), such that g( ) = ginit Generation and g( ) = ggoal . Joint Space Trajectory Generation ( -D) Play/Pause Stop Let gk = g(kT), k = , . . . , n, where T = n , and θ k ∈ Q the corresponding joint configuration, through the inverse kinematics. θ k , k = , . . . , n with θ = θ init and θ n = θ goal , can also be obtained through a teaching pendant (Continues next slide)
  5. 5. Chapter 5 Trajectory Generation 5.1 Introduction to Trajectory Generation 4 Definition: Trajectory generation Given θ init and θ goal , and a sequence of via points θ k , k = , . . . , n − , compute a joint trajectory θ ∶ [t , tn ] ↦ Q such that θ(t ) = θ init , Chapter Trajectory θ(tn ) = θ goal , and θ(tk ) = θ k , k = , . . . , n − . Generation Introduction to Trajectory Generation Joint Space Trajectory Generation ( -D)
  6. 6. Chapter 5 Trajectory Generation 5.1 Introduction to Trajectory Generation 4 Definition: Trajectory generation Given θ init and θ goal , and a sequence of via points θ k , k = , . . . , n − , compute a joint trajectory θ ∶ [t , tn ] ↦ Q such that θ(t ) = θ init , Chapter Trajectory θ(tn ) = θ goal , and θ(tk ) = θ k , k = , . . . , n − . Generation Introduction to Trajectory Generation valid region Joint Space Trajectory forbidden region valid configurations Generation ( -D) forbidden region forbidden configurations Motion planning of via points in config. space C C = Rn , SO(n), SE(n), . . . obstacle avoidance potential field/voronoi dia- gram/visibility graph methods, etc
  7. 7. Chapter 5 Trajectory Generation 5.1 Introduction to Trajectory Generation 4 Definition: Trajectory generation Given θ init and θ goal , and a sequence of via points θ k , k = , . . . , n − , compute a joint trajectory θ ∶ [t , tn ] ↦ Q such that θ(t ) = θ init , Chapter Trajectory θ(tn ) = θ goal , and θ(tk ) = θ k , k = , . . . , n − . Generation Introduction to Trajectory Generation valid region Joint Space Trajectory forbidden region valid configurations Generation ( -D) forbidden region Joint Space Trajectory Planning forbidden configurations Workspace Trajectory Planning Motion planning Trajectory Generation of via points in config. space C Interpolation (exact fitting) C = Rn , SO(n), SE(n), . . . Approximation (inexact fitting) obstacle avoidance of via points potential field/voronoi dia- with appropriate basis functions gram/visibility graph methods, satisfying certain dynamic con- etc straints
  8. 8. Chapter 5 Trajectory Generation 5.1 Introduction to Trajectory Generation 5 Config. space C, obstacles, constraints; initial, goal config. g , g n ∈ C Chapter Path planning Trajectory Generation Computed via points: Introduction g , g , . . . , gn ∈ C to Trajectory Config. space traj. planning Generation Joint traj. planning Inverse kinematics Joint Space Via points in joint space: Interp./approx. in N-D space: Trajectory Generation θ , θ , . . . , θn ∈ Θ C = Rn , SO(n), SE(n) ( -D) Online generation of g(t), g (t), . . . ˙ Interp./approx. of each joint ×N ˙ and computation of θ(t), θ(t), . . . . Trajectory tracking control (Continues next slide)
  9. 9. Chapter 5 Trajectory Generation 5.1 Introduction to Trajectory Generation 6 ◻ Notes on assigning the via points: 1. Via points assigned by path planner Chapter Type of path planner: Trajectory Generation Visibility graph Introduction to Trajectory Generation Voronoi diagrams Joint Space Approximate Cell decomposition Trajectory Generation ( -D) Numerical Potential field ggoal V(S) w DT(S) s p r v ginit q ⇒ Usually generate a sequence of via-points rather than a smooth Visibility graph Voronoi diagrams Cell decomposition Potential field trajectory. (Continues next slide)
  10. 10. Chapter 5 Trajectory Generation 5.1 Introduction to Trajectory Generation 7 2. Via Points generated by a teaching pendant via points recorded as joint angles, no inverse kinematics required Chapter Trajectory Generation 3. Via points assigned by a sequence of end-effector Introduction configurations (such as G-code for machine tools) to Trajectory Generation inverse kinematics required for planning joint velocity profile Joint Space constraints from both joint and workspace need be considered Trajectory Generation ( -D)
  11. 11. Chapter 5 Trajectory Generation 5.1 Introduction to Trajectory Generation 7 2. Via Points generated by a teaching pendant via points recorded as joint angles, no inverse kinematics required Chapter Trajectory Generation 3. Via points assigned by a sequence of end-effector Introduction configurations (such as G-code for machine tools) to Trajectory Generation inverse kinematics required for planning joint velocity profile Joint Space constraints from both joint and workspace need be considered Trajectory Generation ( -D) ◻ Issues on trajectory design: Dynamic constraints: Continuity in velocity, acceleration, jerk, etc (trapezoidal, s-curve, squared sine,. . . ) Velocity limits θ i (t) ≤ θ max (saturated velocity) ˙ ˙i Acceleration limits θ i (t) ≤ θ max (rated torque) ¨ ¨i (Continues next slide)
  12. 12. Chapter 5 Trajectory Generation 5.1 Introduction to Trajectory Generation 8 Performance requirements: minimal time trajectory (achieved with maximal Chapter Trajectory velocity/acceleration) ...i ...i Bounded Jerk trajectory (avoiding excitation): θ (t) ≤ θ max Generation Introduction to Trajectory Generation Least square error Joint Space Trajectory Generation ( -D)
  13. 13. Chapter 5 Trajectory Generation 5.1 Introduction to Trajectory Generation 8 Performance requirements: minimal time trajectory (achieved with maximal Chapter Trajectory velocity/acceleration) ...i ...i Bounded Jerk trajectory (avoiding excitation): θ (t) ≤ θ max Generation Introduction to Trajectory Generation Least square error Joint Space Trajectory Choice of parameterization: Generation The trajectory θ(u) can be parameterized by a parameter u other ( -D) than time t (e.g. u(t) length of the traj.) ˙ dθ = dθ du θ= dt du dt where v(t) ≜ du dt is the feedrate
  14. 14. Chapter 5 Trajectory Generation 5.1 Introduction to Trajectory Generation 8 Performance requirements: minimal time trajectory (achieved with maximal Chapter Trajectory velocity/acceleration) ...i ...i Bounded Jerk trajectory (avoiding excitation): θ (t) ≤ θ max Generation Introduction to Trajectory Generation Least square error Joint Space Trajectory Choice of parameterization: Generation The trajectory θ(u) can be parameterized by a parameter u other ( -D) than time t (e.g. u(t) length of the traj.) ˙ dθ = dθ du θ= dt du dt where v(t) ≜ du is the feedrate dt Online traj. generation (solving differential/difference equations): u((k + )T) = u(kT) + T ⋅ v(kT), T sampling time
  15. 15. Chapter 5 Trajectory Generation 5.1 Introduction to Trajectory Generation 9 ◻ Classification of Trajectories: Trajectory Chapter Trajectory Generation One-dimensional Multi-dimensional Introduction to Trajectory Multipoint Point-to-point Multipoint Point-to-point Generation Joint Space Trajectory Interpolation Approximation Interpolation Approximation Generation ( -D)
  16. 16. Chapter 5 Trajectory Generation 5.1 Introduction to Trajectory Generation 9 ◻ Classification of Trajectories: Trajectory Chapter Trajectory Generation One-dimensional Multi-dimensional Introduction to Trajectory Multipoint Point-to-point Multipoint Point-to-point Generation Joint Space Trajectory Interpolation Approximation Interpolation Approximation ∎ -D v.s. N-D Generation ( -D) -D: scalar function (e.g., single joint variable) N-D: vector function (e.g., position, orientation, rigid config.) g0 g1 inverse g2 ...... kinematics → g3 → gn Joint Space Trajectory Planning sequence of end-effector config.s sequence of joint variables -D traj. planning ×n (Continues next slide)
  17. 17. Chapter 5 Trajectory Generation 5.1 Introduction to Trajectory Generation 10 ◻ Interpolation v.s. approximation: Chapter Trajectory Generation Introduction to Trajectory Generation Joint Space Trajectory Generation ( -D) interpolation approximation Definition: Interpolation Constructing new data points within the range of a discrete set of known data points (exact fitting).
  18. 18. Chapter 5 Trajectory Generation 5.1 Introduction to Trajectory Generation 10 ◻ Interpolation v.s. approximation: Chapter Trajectory Generation Introduction to Trajectory Generation Joint Space Trajectory Generation ( -D) interpolation approximation Definition: Interpolation Constructing new data points within the range of a discrete set of known data points (exact fitting). Definition: Approximation Inexact fitting of a discrete set of known data points. † End of section †
  19. 19. Chapter 5 Trajectory Generation 5.2 Joint Space Trajectory Generation ( -D) 11 Problem 1: -D Point-to-point Trajectory Generation boundary condition: θ(t ), θ(t ), θ(t ), θ(t ), . . . ˙ ˙ Chapter Solution: a parametric curve θ(t), t ∈ [t , t ] Trajectory Generation Introduction to Trajectory Generation Joint Space Trajectory Generation ( -D)
  20. 20. Chapter 5 Trajectory Generation 5.2 Joint Space Trajectory Generation ( -D) 11 Problem 1: -D Point-to-point Trajectory Generation boundary condition: θ(t ), θ(t ), θ(t ), θ(t ), . . . ˙ ˙ Chapter Solution: a parametric curve θ(t), t ∈ [t , t ] Trajectory Generation Introduction Solution 1: Polynomial curves: to Trajectory Generation θ(t) = a + a t + a t + a t + ⋯ + an t n , t ∈ [t , t ] Joint Space Basis function: {t i } n + Boundary conditions: θ (k) (tj ), k = , , . . . , j = , Trajectory Generation ( -D) ⇒ k!ak + (k + )!ak+ tj + ⋯ + an t n−k = θ (k) (tj ) n! (n − k)! j Ma = b where ⎡ ⎤ m , ⋯ m ,n a ⎢ ⎥ a= ∈ Rn+ , b = ⎢ θ (k) (tj ) ⎥ ∈ Rn+ , M = ⋱ ∈ R(n+ )×(n+ ) ⎢ ⎥ mn, ⋯ mn,n an ⎣ ⎦ ⇒ a = M− b
  21. 21. Chapter 5 Trajectory Generation 5.2 Joint Space Trajectory Generation ( -D) 12 n = : Linear trajectory (constant velocity) θ(t) = a + a (t − t ) Chapter Trajectory θ Generation Boundary condition: θ , θ Introduction θ(t ) = θ = a to Trajectory ⇒ Generation t Joint Space Trajectory . ˙ θ θ(t ) = θ = a + a (t − t ) = θ ⇒ Generation . a t −t . ( -D) . . a θ θ −θ a = θ ,a = t . ¨ θ t −t . . − . Disadvantage: − t infinite acceleration at t , t Example: Let t = , t = , θ = , θ =
  22. 22. Chapter 5 Trajectory Generation 5.2 Joint Space Trajectory Generation ( -D) 13 n = : Cubic trajectory θ ˙ ˙ Boundary condition: θ , θ , θ , θ θ Chapter Trajectory Generation θ(t) = a + a (t − t ) + a (t − t ) + a (t − t ) θ ⎧ θ( ⎪ )=a =θ Introduction t t ⎪ ⎪ ⎪ ⎪ t to Trajectory ⎪ ⎪˙ ⎪ θ( Generation ˙ θ ⎪ ⎪ ⎪ )=a =θ ˙ ⎨ ⇒ Joint Space ⎪ θ( ⎪ ⎪ )=a +a +a +a = θ Trajectory ⎪ ⎪ ⎪ ˙ ˙ ⎪ ⎪ ⎪ ˙ ⎪ θ( Generation θ θ ( -D) t t t ⎪ ⎩ )=a + a + a =θ ˙ ⎡ ⎤⎡ ⎡ ⎤ ⎢ ⎥⎢ ⎤ ⎢ θ ⎥ ⎥ ⎢ ⎥ a ⎢ ⎥⎢ ¨ θ ⎢ ⎥⎢ ⎥=⎢ ⎥⇒ a ˙ θ ⎢ ⎥⎢ ⎥ ⎢ ⎥ ⎢ ⎥⎣ a ⎥ ⎢ θ ⎥ ⎣ ⎦ ⎦ ⎢ ⎥ ⎣ ⎦ a ˙ θ ⎧a ⎪ =θ t t t ⎪ ⎪ ⎪ ⎪ ⎪ ⎪ ⎪a ⎪ ⎪ ⎪ =θ ˙ ⎨ ⎪a ⎪ ⎪ = − θ − θ + (θ − θ ) ⎪ ⎪ ˙ ˙ ⎪ ⎪ ⎪ ⎪ ⎪ ⎪a ⎩ = θ + θ − (θ − θ ) ˙ ˙
  23. 23. Chapter 5 Trajectory Generation 5.2 Joint Space Trajectory Generation ( -D) 14 Chapter Extension to multi-points: (figure here) ai + ai (t − ti ) + ai (t − ti ) + ai (t − ti ) , t ∈ [ti , ti+ ] Trajectory Generation Introduction to Trajectory Generation ˙ θ : assigned ⎧ ⎪ ˙ ⎪ sign(dk ) ≠ (dk+ ) Joint Space θk = ⎨ Trajectory ⎪ (dk + dk+ ) otherwise Generation ( -D) ⎪ ⎩ ˙ θ n : assigned Fig 2.8 Feature: Continuous position and velocity profiles Discontinuous acceleration
  24. 24. Chapter 5 Trajectory Generation 5.2 Joint Space Trajectory Generation ( -D) 15 n = : Quintic polynomial θ(t) = ai (t − t )i , t ∈ [t , t ] Chapter 10 Trajectory 8 Generation i= Position 6 4 Introduction to Trajectory 2 0 Advantage: Generation can achieve continuity of acceleration 2.5 2 when composing two quintic curves Velocity 1.5 Joint Space 1 Trajectory 0.5 Generation 0 Boundary ( -D) 1 ˙ ˙ ¨ ¨ condition: θ , θ , θ , θ , θ , θ 0.5 Acceleration 0 −0.5 −1 0 1 2 3 4 5 6 7 8 (Continues next slide)
  25. 25. Chapter 5 Trajectory Generation 5.2 Joint Space Trajectory Generation ( -D) 16 Define T ≜ t − t , h ≜ θ − θ , then: Chapter a =θ Trajectory Generation a =θ ˙ Introduction to Trajectory Generation Joint Space a = θ ¨ Trajectory a = [ h−( θ + θ )T − ( θ − θ )T ] Generation ( -D) ˙ ˙ ¨ ¨ T a = [− h − ( θ + ˙ θ )T − ( θ − θ )T ] ˙ ¨ ¨ T a = [ h − (θ + θ )T − (θ − θ )T ] ˙ ˙ ¨ ¨ T (Continues next slide)
  26. 26. Chapter 5 Trajectory Generation 5.2 Joint Space Trajectory Generation ( -D) 17 ◻ Extension to multi-point trajectories: Comparison of cubic and quintic segments Chapter Trajectory Generation 40 40 30 30 Introduction Position Position 20 20 to Trajectory Generation 10 10 0 0 Joint Space Trajectory 10 10 Generation 5 Velocity Velocity ( -D) 0 0 −5 −10 −10 −15 −20 20 20 Acceleration Acceleration 0 0 −20 −20 0 2 4 6 8 10 0 2 4 6 8 10 composition of cubic polynomials: without accel- composition of quintic polynomials: with acceler- eration continuity ation continuity
  27. 27. Chapter 5 Trajectory Generation 5.2 Joint Space Trajectory Generation ( -D) 18 n = : Linear Trajectory with Parabolic Blend (Trapezoidal) Chapter θ Description: Trajectory θf Generation θ Composition of an acc. (quadratic) Introduction θ phase, a linear phase, and a dec. θ to Trajectory (quadratic) phase, with C continuity. Generation t t t tf t ˙ θ ˙ ˙ Joint Space Boundary condition: θ , θ f , θ , θ f Trajectory ˙ θ max Define acceleration phase Ta , constant Generation ˙ θ ˙ θf ˙ velocity θ v ( -D) tf t ⎧θ + θ v (t − t ) t t t ⎪ t ≤ t < t + Ta ¨ ⎪ θ maxθ ¨ ⎪ ˙ ⎪ ⎪ Ta θ(t) = ⎨θ + θ v t − t − ˙ t + Ta ≤ t < tf − Ta ⎪ Ta t ⎪ ⎪ ⎪θ f − θ v (t − t) ⎪ ⎩ tf − Ta ≤ t ≤ tf t t t tf ˙ ... Ta θ (Continues next slide) t t t tf t
  28. 28. Chapter 5 Trajectory Generation 5.2 Joint Space Trajectory Generation ( -D) 19 ◻ Trapezoidal trajectory with preassigned acceleration and velocity: ⎧T = θ max Chapter ⎪ a θ ⎪ ˙ ⎪ Trajectory ⎪ acceleration phase ⎪˙ Generation ¨ max ⎨θ max (T − Ta ) = θ f − θ ≜ h displacement ⎪ Introduction ⎪ ⎪T = h θ max +θ max ⎪ to Trajectory ⎪ ¨ ˙ ⎩ Generation ¨ ˙ θ max θ max total duration Joint Space If h ≥ Trajectory ˙ θ max Generation ( -D) ¨ θ max , linear segment exists: ⎧θ + θ (t − t ) ⎪ t ≤ t ≤ t + Ta ⎪ ¨max ⎪ ⎪ θ(t) = ⎨θ + θ max Ta t − t − ¨ Ta t + Ta < t ≤ t − Ta ⎪ ⎪ ⎪θ − θ max (t − t) ⎪ f ⎩ ¨ t − Ta < t ≤ t Otherwise: (Continues next slide)
  29. 29. Chapter 5 Trajectory Generation 5.2 Joint Space Trajectory Generation ( -D) 20 ⎧ ⎪Ta = ¨ h Chapter ⎪ ⎪ ⎪ Trajectory acceleration phase ⎪ θ max Generation ⎨T = Ta ⎪ ⎪˙ total time ⎪ Introduction ⎪θ max = θ max Ta = ⎪ θ max h = to Trajectory ⎩ Generation ¨ ¨ h maximum velocity Ta Joint Space ⎧ and ⎪θ + θ max (t − t ) Trajectory ⎪ ¨ t ≤ t ≤ t + Ta Generation θ(t) = ⎨ ( -D) ⎪θ f − θ max (tf − t) ⎪ tf − T a < t ≤ tf ⎩ ¨
  30. 30. Chapter 5 Trajectory Generation 5.2 Joint Space Trajectory Generation ( -D) 21 n = : Double “S”trajectory (7-segment) Chapter θ Trajectory θ Advantage: Generation Bounded jerk avoids excitation Introduction to Trajectory θ from jerk impulse. Generation ˙ t t t t t t t t t Description of the “S”curve: θ Joint Space ˙ θ max Composition of acc. (cubic), Trajectory Generation constant vel. (quadratic) and dec. ( -D) (cubic), with C continuity at via points. t t t t t t t t t Acc.: t ∈ [t , t ] ¨ θ ¨ θ max max vel.: t ∈ [t , t ] Dec.: t ∈ [t , t ] ... t t t t t t t t t θ ... θ max (Continues next slide) t t t t t t t t t
  31. 31. Chapter 5 Trajectory Generation 5.2 Joint Space Trajectory Generation ( -D) 22 Assumption: ... ... θ max = −θ min , θ max = −θ min , θ max = − θ min , t = ˙ ˙ ¨ ¨ then T = T , T = T . Also assume θ = θ f = to avoid end point Chapter Trajectory ¨ ¨ Generation jerk impulse. Introduction to Trajectory ˙ Boundary condition: θ , θ , θ f , θ f ˙ ... Generation Given θ max , θ max , θ max , and define Tk ≜ tk+ − tk . Consider the limit ˙ ¨ Joint Space Trajectory case of a single acc/dec. phase (t ∈ [t , t ]) 1 θ f is reached before θ could reach θ max (T = , T = T ) Generation ( -D) ˙ ¨ ¨ ... ... ... θf − θ = ˙ ˙ θ max T + θ max T = θ max T acceleration deceleration ¨ 2 θ max is reached before deceleration ¨ T = ... θ max θ max (Continues next slide)
  32. 32. Chapter 5 Trajectory Generation 5.2 Joint Space Trajectory Generation ( -D) 23 ⎧ ⎫ Thus ⎪ ⎪ ⎪ θf − θ ˙ ˙ θ ¨max ⎪ ⎪ ⎪ T = min ⎨ , ... ⎬ ⎪ θ max θ max ⎪ ... Chapter ⎪ ⎪ ⎪ ⎪ Trajectory ⎩ ⎭ The trajectory is feasible in the first case if (recall that T = T ): Generation Introduction to Trajectory τ ... θf − θ > ( θ max t) dt + θ dτ+ T Generation Joint Space ∫ ∫ ˙ Trajectory ... ( θ max (T − t)) dt + θ + T T τ Generation ( -D) ∫ ∫ ˙ dτ = T (θ f + θ ) ˙ ˙ For the second case, T ⋅ θ max + ⋅ θ max T = θ f − θ ⇒ ¨ ¨ ˙ ˙ θf − θ ˙ ˙ T = −T ¨ θ max (Continues next slide)
  33. 33. Chapter 5 Trajectory Generation 5.2 Joint Space Trajectory Generation ( -D) 24 then the second case is feasible if: ¨ θf − θ > dt + θ T τ θ max t Chapter Trajectory ∫ ∫ T ˙ dτ+ Generation θ max dt + θ + θ max T T τ Introduction to Trajectory ∫ ∫ ¨ ˙ ¨ dτ+ θ max (T − t) Generation ¨ dt + θ + θ max T + θ max T T τ Joint Space Trajectory Generation ∫ ∫ T ˙ ¨ ¨ dτ ˙ ⎛ θf − θ ⎞ ( -D) ˙ ˙ = (θ f + θ ) T + ˙ ⎝ θ max ⎠ ¨ maximal velocity limit: Also has two cases: Case 1: If θ max is reached, T ≠ ˙ ... if (θ max − θ )... max < θ max ⇒ θ max is not reached ˙ ˙ θ ¨ ¨ (∗) if (θ˙max − θ f ) θ max < θ ˙ ¨ max ⇒ θ max is not reached (∗∗) ¨ (Continues next slide)
  34. 34. Chapter 5 Trajectory Generation 5.2 Joint Space Trajectory Generation ( -D) 25 If (∗) holds, then: θ max − θ ˙ ˙ T =T = ... ,T = Chapter θ max Trajectory Generation otherwise ¨ θf − θ ˙ ˙ T = T = ... ,T = −T Introduction θ max to Trajectory θ max θ¨max Generation If (∗∗) holds, then: Joint Space Trajectory θ max − θ f ˙ ˙ Generation T =T = ... ,T = ( -D) θ max otherwise ¨ θf − θ ˙ ˙ T = T = ... ,T = −T θ max θ max θ¨max Finally θf − θ T +T +T ˙ T +T +T ⎛ θf ⎞ ˙ T = − + − + θ ˙ θ max ˙ θ max ⎝ θ max ⎠ ˙ If T ≥ , this verifies that θ max is actually reached. Otherwise, θ max is not ˙ ˙ reached must be checked.
  35. 35. Chapter 5 Trajectory Generation 5.2 Joint Space Trajectory Generation ( -D) 26 Example: Examples of Case 1 and Case 2 Chapter 10 10 Trajectory Position 8 8 Position Generation 6 6 4 4 Introduction 2 2 0 0 to Trajectory Generation ˙ 5 θ max ˙ 10 θ max 5 Joint Space Velocity Velocity 0 0 Trajectory −5 Generation −5 −10 ( -D) ¨ 10 θ max ¨ 10 θ max Acceleration Acceleration 5 5 0 0 −5 −5 −10 −10 ... ... θ max θ max 20 20 Jerk Jerk 0 0 −20 −20 0 0.5 1 1.5 2 2.5 0 0.5 1 1.5 2 ˙ Case 1:θ max is reached ˙ Case 2:θ max is not reached
  36. 36. Chapter 5 Trajectory Generation 5.2 Joint Space Trajectory Generation ( -D) 27 ◻ Computation of the double S trajectory (θ f > θ ): Chapter ˙ θ lim maximal velocity Trajectory ¨ θ lima maximal acceleration in the acceleration phase Generation ¨ Notations θ limd maximal acceleration in the deceleration phase Ta ≜ T + T + T acceleration phase Introduction Ta Ta ≜ T + T + T deceleration phase to Trajectory Td T ≜ Ta + T + Td total duration Generation Joint Space T Trajectory Acceleration phase: ⎧θ + θ t + ... t Generation ⎪ ⎪ t ∈ [ ,t ] ( -D) ⎪ ˙ ⎪ θ max ⎪ θ(t) = ⎨θ + θ ˙ t + θ lima ( t − T t + T ) t ∈ [t , t ] ¨ ⎪ ⎪ ⎪ ⎪θ + (θ lim + θ ) Ta − θ lim (Ta − t) + θ max (Ta −t) t ∈ [t , t ] ⎪ ... ⎩ ˙ ˙ ˙ Constant velocity phase: θ(t) = θ + (θ lim + θ ) + θ lim (t − Ta ), t ∈ [t , t ] ˙ ˙ Ta ˙ (Continues next slide)
  37. 37. Chapter 5 Trajectory Generation 5.2 Joint Space Trajectory Generation ( -D) 28 Deceleration phase: Chapter ⎧ ⎪θ f − (θ lim + θ f ) Td + θ lim (t − T + Td ) − θ max (t−T+Td ) Trajectory ... Generation ⎪ ⎪ ˙ ˙ ˙ t ∈ [t , t ] ⎪ ⎪ ⎪θ f − (θ lim + θ f ) Td + θ lim (t − T + Td )+ ⎪ ⎪ Introduction ˙ ˙ ˙ θ(t) = ⎨ θ lim to Trajectory ⎪ d (t − T + Td ) − T (t − T + Td ) + T ⎪ t ∈ [t , t ] Generation ¨ ⎪ ⎪ ⎪ ⎪ ⎪θ f − θ f (T − t) − ...max (T−t) Joint Space Trajectory Generation ⎪ ⎩ ˙ θ t ∈ [t , t ] ( -D)
  38. 38. Chapter 5 Trajectory Generation 5.2 Joint Space Trajectory Generation ( -D) 29 Solution 2: B´zier curves: θ(t) = ∑n Bi,n (t)pi , e basis function: {Bi,n (t)} i= Chapter Trajectory Generation Introduction to Trajectory Generation Joint Space Trajectory Generation ( -D)
  39. 39. Chapter 5 Trajectory Generation 5.2 Joint Space Trajectory Generation ( -D) 29 Solution 2: B´zier curves: θ(t) = ∑n Bi,n (t)pi , e basis function: {Bi,n (t)} i= Chapter Trajectory Generation Definition: Bernstein Polynomials Introduction Bi,n (t) ≜ t ( − t)n−i , t ∈ [ , ], i = , . . . , n to Trajectory n i Generation i Joint Space Trajectory Generation ( -D)
  40. 40. Chapter 5 Trajectory Generation 5.2 Joint Space Trajectory Generation ( -D) 29 Solution 2: B´zier curves: θ(t) = ∑n Bi,n (t)pi , e basis function: {Bi,n (t)} i= Chapter Trajectory Generation Definition: Bernstein Polynomials Introduction Bi,n (t) ≜ t ( − t)n−i , t ∈ [ , ], i = , . . . , n to Trajectory n i Generation i Joint Space Trajectory Generation ( -D) Property 1: nonnegativity : Bi,n (t) ≥ , ∀t ∈ [ , ], i symmetry : Bi,n (t) = Bn−i,n ( − t), t ∈ [ , ] recursive definition: Bi,n (t) = tBi− ,n− (t) + ( − t)Bi,n− (t), t ∈ [ , ] derivative : Bi,n (t) = n(Bi− ,n− (t) − Bi,n− (t)), ˙ with B− ,n− (t) = Bn,n− (t) ≡ , t ∈ [ , ] ⇒ θ(t) = n ∑n− Bi,n− (t)(θ i+ − θ i ) ˙ i= (Continues next slide)
  41. 41. Chapter 5 Trajectory Generation 5.2 Joint Space Trajectory Generation ( -D) 30 Example: Cubic B´zier curves e Chapter boundary conditions θ , θ , θ , θ ⇒ ˙ ˙ unknowns p , p , p , p (n = , cubic) Trajectory Generation Introduction θ to Trajectory ⎧θ ⎧p Generation ⎪ ⎪ =p ⎪ ⎪ =θ ⎪ ⎪ p ⎪ ⎪ ⎪ ⎪ Joint Space ⎪θ ⎪ ⎪ =p ⎪p ⎪ ⎪ =θ +θ Trajectory p ˙ ⎨˙ ⇒⎨ Generation ⎪θ = (p − p ) ⎪p ( -D) ⎪ ⎪ ⎪ ⎪ ⎪ ⎪ = −θ ˙ +θ ⎪ ⎪˙ ⎪θ ⎪ ⎪ ⎪p ⎪ = (p − p ) ⎪ =θ p p ⎩ ⎩ t t= t= t= t= (more geometrical than algebraic!)
  42. 42. Chapter 5 Trajectory Generation 5.2 Joint Space Trajectory Generation ( -D) 30 Example: Cubic B´zier curves e Chapter boundary conditions θ , θ , θ , θ ⇒ ˙ ˙ unknowns p , p , p , p (n = , cubic) Trajectory Generation Introduction θ to Trajectory ⎧θ ⎧p Generation ⎪ ⎪ =p ⎪ ⎪ =θ ⎪ ⎪ p ⎪ ⎪ ⎪ ⎪ Joint Space ⎪θ ⎪ ⎪ =p ⎪p ⎪ ⎪ =θ +θ Trajectory p ˙ ⎨˙ ⇒⎨ Generation ⎪θ = (p − p ) ⎪p ( -D) ⎪ ⎪ ⎪ ⎪ ⎪ ⎪ = −θ ˙ +θ ⎪ ⎪˙ ⎪θ ⎪ ⎪ ⎪p ⎪ = (p − p ) ⎪ =θ p p ⎩ ⎩ t t= t= t= t= (more geometrical than algebraic!) Generalizations of B´zier curve: B-spline, NURBS (Non-Uniform e Rational B-Spline)
  43. 43. Chapter 5 Trajectory Generation 5.2 Joint Space Trajectory Generation ( -D) 31 Problem 2: -D Multi-point Trajectory Generation ˙ ˙ ˙ Init: θ , θ , . . . , θ n , θ , θ , . . . , θ n Chapter Solution: a parametric curve θ(t), t ∈ [t , tn ] satisfying boundary Trajectory Generation and smoothness condition Introduction to Trajectory Generation Joint Space Trajectory Generation ( -D)
  44. 44. Chapter 5 Trajectory Generation 5.2 Joint Space Trajectory Generation ( -D) 31 Problem 2: -D Multi-point Trajectory Generation ˙ ˙ ˙ Init: θ , θ , . . . , θ n , θ , θ , . . . , θ n Chapter Solution: a parametric curve θ(t), t ∈ [t , tn ] satisfying boundary Trajectory Generation and smoothness condition Introduction to Trajectory Solutions: Generation interpolation by polynomial functions Joint Space Trajectory Generation interpolation by splines (piecewise polynomial) ( -D) approximation by smoothing splines
  45. 45. Chapter 5 Trajectory Generation 5.2 Joint Space Trajectory Generation ( -D) 31 Problem 2: -D Multi-point Trajectory Generation ˙ ˙ ˙ Init: θ , θ , . . . , θ n , θ , θ , . . . , θ n Chapter Solution: a parametric curve θ(t), t ∈ [t , tn ] satisfying boundary Trajectory Generation and smoothness condition Introduction to Trajectory Solutions: Generation interpolation by polynomial functions Joint Space Trajectory Generation interpolation by splines (piecewise polynomial) ( -D) approximation by smoothing splines How to specify velocities at via points ˙ User specifies desired velocities θ i at via points θ i . System automatically chooses the velocities at the via points by applying heuristics at either Cartesian or joint speeds. The system automatically chooses velocities at the via points so that acceleration at the via points are continuous.
  46. 46. Chapter 5 Trajectory Generation 5.2 Joint Space Trajectory Generation ( -D) 32 Solution 1: Interpolation by polynomial Chapter θ(t) = a + a t + a t + ⋯ + an t n , t ∈ [t , tn ] Trajectory Generation Boundary conditions: Introduction θ(ti ) = θ i , i = , . . . , n ⇒ Ta = b to Trajectory Generation ⎡ ⋯ tn ⎤ Joint Space ⎢ t t ⎥ Trajectory ⎢ ⋯ t n ⎥ , Vandermonde matrix, T=⎢ ⎥ Generation t t ⎢ ⎥ ( -D) ⎢ n ⎥ tn ⋯ tn ⎦ ⎣ tn ⎡ θ ⎤ ⎢ ⎥ ⎢ ⎥ a a= ,b = ⎢ θ ⎥ a ⎢ ⎥ ⎢ θn ⎥ ⎣ ⎦ an det(T) = (tj − ti ) ≠ ⇒ T always invertible! ≤i<j≤n Disadvantage: bad numerical conditioning; oscillating behavior.
  47. 47. Chapter 5 Trajectory Generation 5.2 Joint Space Trajectory Generation ( -D) 33 Solution 2: Interpolation by splines (piecewise polynomial) Solution 2A: Linear trajectory with parabolic blend Chapter Trajectory composition of point-to-point linear trajectory with specified via Generation ˙ points velocity θ i (local interpolation) Introduction to Trajectory using via points as control points (approximation) Generation θ ˙ θ θ ,θ θ Joint Space ˙ θ ,θ θ ˙ ˙ θ ,θ θ Trajectory θ ,θ θ ˙ θ ,θ θ Generation ( -D) t t t t t t ˙ θ ˙ θ t t t t t t ¨ θ ¨ θ t t t t t t composition with preassigned via point velocities composition with via points as control points
  48. 48. Chapter 5 Trajectory Generation 5.2 Joint Space Trajectory Generation ( -D) 34 Solution 2B: Piecewise cubic polynomials Composition of point-to-point cubic curves by specifying via Chapter Trajectory point velocities. Generation Introduction Cubic splines by specifying end point velocity only and requiring to Trajectory continuity of acceleration at via points Generation θ ˙ θ θ ,θ ˙ θ Joint Space θ ,θ ˙ θ ˙ Trajectory ˙ θ ,θ θ ,θ θ θ ,θ ˙ θ ,θ ˙ θ ,θ Generation ( -D) t t t t t t ˙ θ ˙ θ t t t t t t ¨ θ ¨ θ t t t t t t composition with preassigned via point velocities composition with continuity of acceleration
  49. 49. Chapter 5 Trajectory Generation 5.2 Joint Space Trajectory Generation ( -D) 35 ◻ Computation of interpolating cubic splines based on continuity of acceleration: Init.: Given θ , . . . , θ n , t , . . . , tn and boundary condition θ , θ n . Tk ≜ ˙ ˙ Chapter tk+ − tk , α(t) ≜ θ(t). Trajectory Generation ¨ Introduction to Trajectory Step 1: Definition of the cubic spline (tk+ − t) (t − tk ) Generation θ(t) = αk + α k+ + − (t − tk ) Joint Space θ k+ Tk α k+ (∗) Trajectory Generation Tk Tk Tk + − (tk+ − t), t ∈ [tk , tk+ ], k = , , . . . , n − ( -D) θ k Tk α k Tk ⎧ ⎪ θ(t) = (tk+ − t) α k + (t − tk ) α k+ + θ k+ − θ k − Tk (α k+ − α k ) ⎪˙ ⎪ ⎪ ⎪ ⎪ ⇒⎨ Tk Tk Tk ⎪ ⎪ ⎪ α(t) = α k+ (t − tk ) + α k (tk+ − t) ⎪ ⎪ ⎪ ⎩ Tk (Continues next slide)
  50. 50. Chapter 5 Trajectory Generation 5.2 Joint Space Trajectory Generation ( -D) 36 Step 2: Imposing C continuity (Tk + Tk− ) θ k+ − θ k θ k − θ k− α k− + α k + α k+ = − Chapter Tk− Trajectory Tk Tk Tk Tk Tk− Generation Introduction for k = , . . . , n − . to Trajectory Generation Step 3: Imposing boundary condition: Joint Space θ(t ) = θ , θ(tn ) = θ n ⇒ Trajectory Generation ˙ ˙ ˙ ˙ ( -D) ⎧ ⎪ ⎪ ⎪ T α + T α = θ −θ −T θ ⎪ ⎪ ⎪ ˙ ⎨ ⇒ Aα = B ⎪T ⎪ n− ⎪ ⎪ Tn− ⎪ ⎪ αn + α n− = θ n− − θ n + Tn− θ ˙n ⎩ ⎡ T ⋯ ⎤ ⎢ T ⎥ ⎡θ ⎤ ⎢T (T + T ) T ⎥ ⎢¨ ⎥ ⎢ ⎥ A=⎢ ⎥,α = ⎢ ⎥, ⎢ ⎥ ⎢ ⎥ ⎢ θn ⎥ ⋱ ⎢ (Tn− + Tn− ) Tn− ⎥ ⎣¨ ⎦ ⎢ ⎥ Tn− ⎣ ⋯ Tn− Tn− ⎦ (Continues next slide)
  51. 51. Chapter 5 Trajectory Generation 5.2 Joint Space Trajectory Generation ( -D) 37 Chapter ⎡ ⎤ Trajectory and ⎢ −θ ⎥ Generation ⎢ ⎥ θ −θ ˙ ⎢ ⎥ T ⎢ ⎥ Introduction to Trajectory B=⎢ θ −θ − θT −θ ⎥ ⎢ ⎥ Generation T ⎢ ⎥ ⎢ θ n −θ n− − θn − ⎥ ⎢ ⎥ Joint Space θ n− −θ n− ˙ θ n −θ n− ⎣ ⎦ Trajectory Generation T n− T n− T n− ( -D) Step 4: Solving for α Aα = B ⇒ α = A− B Step 5: Computation of the spline by substituting α back into (∗)
  52. 52. Chapter 5 Trajectory Generation 5.2 Joint Space Trajectory Generation ( -D) 38 Solution 3: Approximation by smoothing splines ˆ Given θ , . . . , θ n and t , . . . , tn , design θ(t) s.t.: Chapter n θ(t) = arg min µ ω k (θ(tk ) − θ k ) + ( − µ) tn Trajectory Generation ˆ θ(t) k= ∫t ¨ θ(t) dt Introduction to Trajectory Generation Joint Space Trajectory Generation ( -D)
  53. 53. Chapter 5 Trajectory Generation 5.2 Joint Space Trajectory Generation ( -D) 38 Solution 3: Approximation by smoothing splines ˆ Given θ , . . . , θ n and t , . . . , tn , design θ(t) s.t.: Chapter n θ(t) = arg min µ ω k (θ(tk ) − θ k ) + ( − µ) tn Trajectory Generation ˆ θ(t) k= ∫t ¨ θ(t) dt Special case : µ = Introduction to Trajectory Generation n ω k (θ(tk ) − θ k ) Joint Space Trajectory Generation arg min θ(t) k= ( -D) Exact interpolation
  54. 54. Chapter 5 Trajectory Generation 5.2 Joint Space Trajectory Generation ( -D) 38 Solution 3: Approximation by smoothing splines ˆ Given θ , . . . , θ n and t , . . . , tn , design θ(t) s.t.: Chapter n θ(t) = arg min µ ω k (θ(tk ) − θ k ) + ( − µ) tn Trajectory Generation ˆ θ(t) k= ∫t ¨ θ(t) dt Special case : µ = Introduction to Trajectory Generation n ω k (θ(tk ) − θ k ) Joint Space Trajectory Generation arg min θ(t) k= ( -D) Exact interpolation Special case : µ = tn arg min θ(t) ∫t ¨ θ(t) dt natural Cubic spline curve: θ(t) piecewise cubic polynomial, ∈ C and ˆ θ = θn = ¨ ˆ ¨ ˆ (Continues next slide)
  55. 55. Chapter 5 Trajectory Generation 5.2 Joint Space Trajectory Generation ( -D) 39 10 Position 5 Chapter 0 Trajectory Generation −5 6 Introduction to Trajectory 4 Generation Velocity 2 Joint Space 0 Trajectory −2 Generation −4 ( -D) 8 6 Acceleration 4 2 0 −2 −4 0 2 4 6 8 10 12 14 16 18 µ = (solid line): exact interpolation µ = . (dashdot line): approximation with smaller error µ = . (dashed line): more smooth (Continues next slide)
  56. 56. Chapter 5 Trajectory Generation 5.2 Joint Space Trajectory Generation ( -D) 40 Derivation of the Smoothing spline n− n− t − tk ¨ θ(t)dt = θ k (t) dt = (θ k + (θ k+ − θ k )) dt tn t k+ t k+ Chapter Trajectory Generation ∫ t ¨ k= ∫ tk ¨ k= ∫ tk ¨ tk+ − tk ¨ n− = (θ k + (θ k+ − θ k )) dt t k+ −t k t ∫ Introduction ¨ ¨ ¨ tk+ − tk to Trajectory Generation k= = (tk+ − tk )(θ k + θ k θ k+ + θ k+ ) Joint Space Trajectory ¨ ¨ ¨ ¨ Generation ( -D)

×