Upcoming SlideShare
×

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 Likes
Statistics
Notes
• Full Name
Comment goes here.

Are you sure you want to Yes No
• Be the first to comment

• Be the first to like this

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

No notes for slide

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 Deﬁnition: Motion planning Introduction Given the initial and goal conﬁg.s to Trajectory ginit , ggoal , ﬁnd 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 Deﬁnition: Motion planning Introduction Given the initial and goal conﬁg.s to Trajectory ginit , ggoal , ﬁnd 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 conﬁguration, 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 Deﬁnition: 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 Deﬁnition: 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 conﬁg. space C C = Rn , SO(n), SE(n), . . . obstacle avoidance potential ﬁeld/voronoi dia- gram/visibility graph methods, etc
7. 7. Chapter 5 Trajectory Generation 5.1 Introduction to Trajectory Generation 4 Deﬁnition: 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 conﬁg. space C Interpolation (exact ﬁtting) C = Rn , SO(n), SE(n), . . . Approximation (inexact ﬁtting) obstacle avoidance of via points potential ﬁeld/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 Conﬁg. space C, obstacles, constraints; initial, goal conﬁg. g , g n ∈ C Chapter Path planning Trajectory Generation Computed via points: Introduction g , g , . . . , gn ∈ C to Trajectory Conﬁg. 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 ﬁeld 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 ﬁeld 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-eﬀector Introduction conﬁgurations (such as G-code for machine tools) to Trajectory Generation inverse kinematics required for planning joint velocity proﬁle 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-eﬀector Introduction conﬁgurations (such as G-code for machine tools) to Trajectory Generation inverse kinematics required for planning joint velocity proﬁle 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 diﬀerential/diﬀerence 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 ◻ Classiﬁcation 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 ◻ Classiﬁcation 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 conﬁg.) g0 g1 inverse g2 ...... kinematics → g3 → gn Joint Space Trajectory Planning sequence of end-eﬀector conﬁg.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 Deﬁnition: Interpolation Constructing new data points within the range of a discrete set of known data points (exact ﬁtting).
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 Deﬁnition: Interpolation Constructing new data points within the range of a discrete set of known data points (exact ﬁtting). Deﬁnition: Approximation Inexact ﬁtting 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 inﬁnite 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: (ﬁgure 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 proﬁles 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 Deﬁne 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 Deﬁne 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 deﬁne 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 ﬁrst 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 veriﬁes 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 Deﬁnition: 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 Deﬁnition: 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 deﬁnition: 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 speciﬁes 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 speciﬁed 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: Deﬁnition 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)