[Download] rev chapter-5-june26th

Post on 11-May-2015

898 views 0 download

Tags:

description

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

Transcript of [Download] rev chapter-5-june26th

Chapter5 TrajectoryGeneration

Introductionto TrajectoryGeneration

Joint SpaceTrajectoryGeneration(1-D)

Chapter 5 Trajectory Generation

1

Lecture Notes for

A Geometrical Introduction to

Robotics and Manipulation

Richard Murray and Zexiang Li and Shankar S. SastryCRC Press

Zexiang Li1 and Yuanqing Wu1

1ECE, Hong Kong University of Science & Technology

July 29, 2010

Chapter5 TrajectoryGeneration

Introductionto TrajectoryGeneration

Joint SpaceTrajectoryGeneration(1-D)

Chapter 5 Trajectory Generation

2

Chapter 5 Trajectory Generation

1 Introduction to Trajectory Generation

2 Joint Space Trajectory Generation (1-D)

Chapter5 TrajectoryGeneration

Introductionto TrajectoryGeneration

Joint SpaceTrajectoryGeneration(1-D)

5.1 Introduction to Trajectory GenerationChapter 5 Trajectory Generation

3

◻ Motion planning and Trajectory generation:

Play/Pause Stop

Definition: Motion planningGiven the initial and goal config.sginit , ggoal , find a collision-free path,g ∶ [0, 1] ↦ SE(3), such that g(0) = ginitand g(1) = ggoal .

Chapter5 TrajectoryGeneration

Introductionto TrajectoryGeneration

Joint SpaceTrajectoryGeneration(1-D)

5.1 Introduction to Trajectory GenerationChapter 5 Trajectory Generation

3

◻ Motion planning and Trajectory generation:

Play/Pause Stop

Definition: Motion planningGiven the initial and goal config.sginit , ggoal , find a collision-free path,g ∶ [0, 1] ↦ SE(3), such that g(0) = ginitand g(1) = ggoal .

Let gk = g(kT), k = 0, . . . , n, where T = 1n , and θk ∈ Q the corresponding

joint configuration, through the inverse kinematics.● θk , k = 0, . . . , n with θ0 = θ init and θn = θgoal, can also be obtainedthrough a teaching pendant

(Continues next slide)

Chapter5 TrajectoryGeneration

Introductionto TrajectoryGeneration

Joint SpaceTrajectoryGeneration(1-D)

5.1 Introduction to Trajectory GenerationChapter 5 Trajectory Generation

4

Definition: Trajectory generationGiven θ init and θgoal, and a sequence of via points θk , k = 1, . . . , n − 1,compute a joint trajectory θ ∶ [t0 , tn] ↦ Q such that θ(t0) = θ init,θ(tn) = θgoal , and θ(tk) = θk , k = 1, . . . , n − 1.

Chapter5 TrajectoryGeneration

Introductionto TrajectoryGeneration

Joint SpaceTrajectoryGeneration(1-D)

5.1 Introduction to Trajectory GenerationChapter 5 Trajectory Generation

4

Definition: Trajectory generationGiven θ init and θgoal, and a sequence of via points θk , k = 1, . . . , n − 1,compute a joint trajectory θ ∶ [t0 , tn] ↦ Q such that θ(t0) = θ init,θ(tn) = θgoal , and θ(tk) = θk , k = 1, . . . , n − 1.

valid region

forbidden region

forbidden region

forbidden configurations

valid configurations

Motion planningof via points in config. space C● C = R

n , SO(n), SE(n), . . .● obstacle avoidance● potential field/voronoi dia-gram/visibility graph methods,etc

Chapter5 TrajectoryGeneration

Introductionto TrajectoryGeneration

Joint SpaceTrajectoryGeneration(1-D)

5.1 Introduction to Trajectory GenerationChapter 5 Trajectory Generation

4

Definition: Trajectory generationGiven θ init and θgoal, and a sequence of via points θk , k = 1, . . . , n − 1,compute a joint trajectory θ ∶ [t0 , tn] ↦ Q such that θ(t0) = θ init,θ(tn) = θgoal , and θ(tk) = θk , k = 1, . . . , n − 1.

valid region

forbidden region

forbidden region

forbidden configurations

valid configurations

Motion planningof via points in config. space C● C = R

n , SO(n), SE(n), . . .● obstacle avoidance● potential field/voronoi dia-gram/visibility graph methods,etc

Joint Space Trajectory Planning

Workspace Trajectory Planning

Trajectory Generation● Interpolation (exact fitting)● Approximation (inexact fitting)of via points● with appropriate basis functions● satisfying certain dynamic con-straints

Chapter5 TrajectoryGeneration

Introductionto TrajectoryGeneration

Joint SpaceTrajectoryGeneration(1-D)

5.1 Introduction to Trajectory GenerationChapter 5 Trajectory Generation

5

Config. space C, obstacles, constraints;

initial, goal config. g0 , gn ∈ C

Computed via points:

g0 , g1 , . . . , gn ∈ C

Via points in joint space:

θ0 , θ 1 , . . . , θn ∈ Θ

Interp./approx. in N-D space:

C = Rn , SO(n), SE(n)

Online generation of g(t), g(t), . . .

and computation of θ(t), θ(t), . . . .Interp./approx. of each joint ×N

Trajectory tracking control

Path planning

Inverse kinematicsJoint traj. planning

Config. space traj. planning

(Continues next slide)

Chapter5 TrajectoryGeneration

Introductionto TrajectoryGeneration

Joint SpaceTrajectoryGeneration(1-D)

5.1 Introduction to Trajectory GenerationChapter 5 Trajectory Generation

6

◻ Notes on assigning the via points:1. Via points assigned by path planner♢ Type of path planner:

Visibility graph

Voronoi diagrams

Approximate Cell decomposition

Numerical Potential field

ggoal

ginit

Visibility graph

DT(S)

V(S)w

p

s

vr

q

Voronoi diagrams Cell decomposition Potential field⇒ Usually generate a sequence of via-points rather than a smoothtrajectory. (Continues next slide)

Chapter5 TrajectoryGeneration

Introductionto TrajectoryGeneration

Joint SpaceTrajectoryGeneration(1-D)

5.1 Introduction to Trajectory GenerationChapter 5 Trajectory Generation

7

2. Via Points generated by a teaching pendant● via points recorded as joint angles, no inverse kinematics required

3. Via points assigned by a sequence of end-effectorconfigurations (such as G-code for machine tools)● inverse kinematics required for planning joint velocity profile● constraints from both joint and workspace need be considered

Chapter5 TrajectoryGeneration

Introductionto TrajectoryGeneration

Joint SpaceTrajectoryGeneration(1-D)

5.1 Introduction to Trajectory GenerationChapter 5 Trajectory Generation

7

2. Via Points generated by a teaching pendant● via points recorded as joint angles, no inverse kinematics required

3. Via points assigned by a sequence of end-effectorconfigurations (such as G-code for machine tools)● inverse kinematics required for planning joint velocity profile● constraints from both joint and workspace need be considered

◻ Issues on trajectory design:● Dynamic constraints:

1 Continuity in velocity, acceleration, jerk, etc (trapezoidal,s-curve, squared sine,. . . )

2 Velocity limits ∣θ i(t)∣ ≤ θ imax (saturated velocity)

3 Acceleration limits ∣θ i(t)∣ ≤ θ imax (rated torque)

(Continues next slide)

Chapter5 TrajectoryGeneration

Introductionto TrajectoryGeneration

Joint SpaceTrajectoryGeneration(1-D)

5.1 Introduction to Trajectory GenerationChapter 5 Trajectory Generation

8

● Performance requirements:

1 minimal time trajectory (achieved with maximalvelocity/acceleration)

2 Bounded Jerk trajectory (avoiding excitation): ∣...θ i(t)∣ ≤ ...θ imax

3 Least square error

Chapter5 TrajectoryGeneration

Introductionto TrajectoryGeneration

Joint SpaceTrajectoryGeneration(1-D)

5.1 Introduction to Trajectory GenerationChapter 5 Trajectory Generation

8

● Performance requirements:

1 minimal time trajectory (achieved with maximalvelocity/acceleration)

2 Bounded Jerk trajectory (avoiding excitation): ∣...θ i(t)∣ ≤ ...θ imax

3 Least square error

● Choice of parameterization:The trajectory θ(u) can be parameterized by a parameter u otherthan time t (e.g. u(t) length of the traj.)

θ = dθ

dt= dθ

du

du

dt

where v(t) ≜ dudt is the feedrate

Chapter5 TrajectoryGeneration

Introductionto TrajectoryGeneration

Joint SpaceTrajectoryGeneration(1-D)

5.1 Introduction to Trajectory GenerationChapter 5 Trajectory Generation

8

● Performance requirements:

1 minimal time trajectory (achieved with maximalvelocity/acceleration)

2 Bounded Jerk trajectory (avoiding excitation): ∣...θ i(t)∣ ≤ ...θ imax

3 Least square error

● Choice of parameterization:The trajectory θ(u) can be parameterized by a parameter u otherthan time t (e.g. u(t) length of the traj.)

θ = dθ

dt= dθ

du

du

dt

where v(t) ≜ dudt is the feedrate● Online traj. generation (solving differential/difference

equations):

u((k + 1)T) = u(kT) + T ⋅ v(kT),T sampling time

Chapter5 TrajectoryGeneration

Introductionto TrajectoryGeneration

Joint SpaceTrajectoryGeneration(1-D)

5.1 Introduction to Trajectory GenerationChapter 5 Trajectory Generation

9

◻ Classification of Trajectories:Trajectory

One-dimensional Multi-dimensional

Point-to-pointPoint-to-point MultipointMultipoint

InterpolationInterpolation ApproximationApproximation

Chapter5 TrajectoryGeneration

Introductionto TrajectoryGeneration

Joint SpaceTrajectoryGeneration(1-D)

5.1 Introduction to Trajectory GenerationChapter 5 Trajectory Generation

9

◻ Classification of Trajectories:Trajectory

One-dimensional Multi-dimensional

Point-to-pointPoint-to-point MultipointMultipoint

InterpolationInterpolation ApproximationApproximation

∎ 1-D v.s. N-D1-D: scalar function (e.g., single joint variable)N-D: vector function (e.g., position, orientation, rigid config.)

g0

g1

g2

g3......

gn

sequence of end-effector config.s

inversekinematicsÐÐÐÐÐÐ→

sequence of joint variables

ÐÐÐÐ→

Joint Space Trajectory Planning

1-D traj. planning ×n

(Continues next slide)

Chapter5 TrajectoryGeneration

Introductionto TrajectoryGeneration

Joint SpaceTrajectoryGeneration(1-D)

5.1 Introduction to Trajectory GenerationChapter 5 Trajectory Generation

10

◻ Interpolation v.s. approximation:

interpolation approximation

Definition: InterpolationConstructing new data points within the range of a discrete set ofknown data points (exact fitting).

Chapter5 TrajectoryGeneration

Introductionto TrajectoryGeneration

Joint SpaceTrajectoryGeneration(1-D)

5.1 Introduction to Trajectory GenerationChapter 5 Trajectory Generation

10

◻ Interpolation v.s. approximation:

interpolation approximation

Definition: InterpolationConstructing new data points within the range of a discrete set ofknown data points (exact fitting).

Definition: ApproximationInexact fitting of a discrete set of known data points.

† End of section †

Chapter5 TrajectoryGeneration

Introductionto TrajectoryGeneration

Joint SpaceTrajectoryGeneration(1-D)

5.2 Joint Space Trajectory Generation (1-D)Chapter 5 Trajectory Generation

11

Problem 1: 1-D Point-to-point Trajectory Generationboundary condition: θ(t0), θ(t1), θ(t0), θ(t1), . . .

Solution: a parametric curve θ(t), t ∈ [t0 , t1]

Chapter5 TrajectoryGeneration

Introductionto TrajectoryGeneration

Joint SpaceTrajectoryGeneration(1-D)

5.2 Joint Space Trajectory Generation (1-D)Chapter 5 Trajectory Generation

11

Problem 1: 1-D Point-to-point Trajectory Generationboundary condition: θ(t0), θ(t1), θ(t0), θ(t1), . . .

Solution: a parametric curve θ(t), t ∈ [t0 , t1]Solution 1: Polynomial curves:

θ(t) = a0 + a1t + a2t2 + a3t3 +⋯+ antn , t ∈ [t0 , t1]Basis function: {ti}n + 1 Boundary conditions: θ(k)(tj), k = 0, 1, . . . , j = 0, 1

⇒ k!ak + (k + 1)!ak+1tj +⋯+ n!

(n − k)!antn−kj = θ(k)(tj)Ma = b

where

a = [ a0⋮an] ∈ Rn+1 , b = ⎡⎢⎢⎢⎣

⋮θ(k)(tj)⋮

⎤⎥⎥⎥⎦ ∈ Rn+1 ,M = [m0,0 ⋯ m0,n⋮ ⋱ ⋮mn,0 ⋯ mn,n

] ∈ R(n+1)×(n+1)⇒ a =M−1b

Chapter5 TrajectoryGeneration

Introductionto TrajectoryGeneration

Joint SpaceTrajectoryGeneration(1-D)

5.2 Joint Space Trajectory Generation (1-D)Chapter 5 Trajectory Generation

12

◇ n = 1: Linear trajectory (constant velocity)

θ(t) = a0 + a1(t − t0)

0246810

0 2 4 6 8 t

θ

00.51.01.52.02.5

0 2 4 6 8 t

θ

−1−0.5

00.51.01.5

0 2 4 6 8 t

θ

Example: Let t0 = 0, t1 = 8, θ0 = 0, θ1 = 10

Boundary condition: θ0 , θ1

{θ(t0) = θ0 = a0θ(t1) = θ1 = a0 + a1(t1 − t0) ⇒[ 1 01 t1 − t0 ] [ a0a1 ] = [ θ0

θ1]⇒

a0 = θ0 , a1 = θ1 − θ0t1 − t0

Disadvantage:infinite acceleration at t0 , t1

Chapter5 TrajectoryGeneration

Introductionto TrajectoryGeneration

Joint SpaceTrajectoryGeneration(1-D)

5.2 Joint Space Trajectory Generation (1-D)Chapter 5 Trajectory Generation

13

◇ n = 3: Cubic trajectory

t

θ

t0 t1

θ0

θ 1

t

θ

t0 t1

θ0 θ 1

t

θ

t0 t1

Boundary condition: θ0 , θ1 , θ0 , θ1

θ(t) = a0 + a1(t − t0) + a2(t − t0)2 + a3(t − t0)3⎧⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎨⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎩

θ(0) = a0 = θ0θ(0) = a1 = θ0θ(1) = a0 + a1 + a2 + a3 = θ1θ(1) = a1 + 2a2 + 3a3 = θ1

⎡⎢⎢⎢⎢⎢⎣1 0 0 00 1 0 01 1 1 10 1 2 3

⎤⎥⎥⎥⎥⎥⎦⎡⎢⎢⎢⎢⎣

a0a1a2a3

⎤⎥⎥⎥⎥⎦ =⎡⎢⎢⎢⎢⎢⎢⎣

θ0θ0θ1θ1

⎤⎥⎥⎥⎥⎥⎥⎦⇒

⎧⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎨⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎩

a0 = θ0a1 = θ0a2 = −2θ0 − θ1 + 3(θ1 − θ0)a3 = θ1 + θ0 − 2(θ1 − θ0)

Chapter5 TrajectoryGeneration

Introductionto TrajectoryGeneration

Joint SpaceTrajectoryGeneration(1-D)

5.2 Joint Space Trajectory Generation (1-D)Chapter 5 Trajectory Generation

14

Extension to multi-points: (figure here)

ai0 + ai1(t − ti) + ai2(t − ti)2 + ai3(t − ti)3 , t ∈ [ti , ti+1]θ0: assigned

θk = ⎧⎪⎪⎨⎪⎪⎩0 sign(dk) ≠ (dk+1)12(dk + dk+1) otherwise

θn: assignedFig 2.8Feature:● Continuous position and velocity profiles● Discontinuous acceleration

Chapter5 TrajectoryGeneration

Introductionto TrajectoryGeneration

Joint SpaceTrajectoryGeneration(1-D)

5.2 Joint Space Trajectory Generation (1-D)Chapter 5 Trajectory Generation

15

◇ n = 5: Quintic polynomial

0 1 2 3 4 5 6 7 8

−1

−0.5

0

0.5

1

Acceleration

0

0.5

1

1.5

2

2.5

Velocity

0

2

4

6

8

10

Position

θ(t) = 5∑i=0

ai(t − t0)i , t ∈ [t0 , t1]Advantage:can achieve continuity of accelerationwhen composing two quintic curvesBoundarycondition: θ0 , θ1 , θ0 , θ1 , θ0 , θ1

(Continues next slide)

Chapter5 TrajectoryGeneration

Introductionto TrajectoryGeneration

Joint SpaceTrajectoryGeneration(1-D)

5.2 Joint Space Trajectory Generation (1-D)Chapter 5 Trajectory Generation

16

Define T ≜ t1 − t0 , h ≜ θ1 − θ0, then:a0 = θ0a1 = θ0a2 = 1

2θ0

a3 = 1

2T3[20h − (8θ1 + 12θ0)T − (3θ0 − θ1)T2]

a4 = 1

2T4[−30h − (14θ1 + 16θ0)T − (3θ0 − 2θ1)T2]

a5 = 1

2T5[12h − 6(θ1 + θ0)T − (θ1 − θ0)T2]

(Continues next slide)

Chapter5 TrajectoryGeneration

Introductionto TrajectoryGeneration

Joint SpaceTrajectoryGeneration(1-D)

5.2 Joint Space Trajectory Generation (1-D)Chapter 5 Trajectory Generation

17

◻ Extension to multi-point trajectories:

● Comparison of cubic and quintic segments

0 2 4 6 8 10

−20

0

20

Acceleration

−15

−10

−5

0

5

10

Velocity

0

10

20

30

40

Position

0 2 4 6 8 10

−20

0

20

Acceleration

−20

−10

0

10

Velocity

0

10

20

30

40

Position

composition of cubic polynomials: without accel-eration continuity

composition of quintic polynomials: with acceler-ation continuity

Chapter5 TrajectoryGeneration

Introductionto TrajectoryGeneration

Joint SpaceTrajectoryGeneration(1-D)

5.2 Joint Space Trajectory Generation (1-D)Chapter 5 Trajectory Generation

18

◇ n = 2: Linear Trajectory with Parabolic Blend (Trapezoidal)

t

θ

t0 t1 t2 tf

θ0

θ 1

θ2

θ f

t

θ

t0 t1 t2 tf

θmax

θ0 θ f

t

θ

t0 t1 t2 tf

θmax

t

...θ

t0 t1 t2 tf

Description:Composition of an acc. (quadratic)

phase, a linear phase, and a dec.

(quadratic) phase, with C1 continuity.

Boundary condition: θ0 , θ f , θ0 , θ fDefine acceleration phase Ta, constant

velocity θv

θ(t) =⎧⎪⎪⎪⎪⎪⎨⎪⎪⎪⎪⎪⎩

θ0 + θv2Ta(t − t0)2 t0 ≤ t < t0 + Ta

θ0 + θv (t − t0 − Ta2) t0 + Ta ≤ t < tf − Ta

θ f − θv2Ta(t1 − t)2 tf − Ta ≤ t ≤ tf

(Continues next slide)

Chapter5 TrajectoryGeneration

Introductionto TrajectoryGeneration

Joint SpaceTrajectoryGeneration(1-D)

5.2 Joint Space Trajectory Generation (1-D)Chapter 5 Trajectory Generation

19

◻ Trapezoidal trajectory with preassigned acceleration andvelocity: ⎧⎪⎪⎪⎪⎪⎨⎪⎪⎪⎪⎪⎩

Ta = θmax

θmaxacceleration phase

θmax(T − Ta) = θ f − θ ≜ h displacement

T = hθmax+θ2max

θmax θmaxtotal duration

If h ≥ θ 2max

θmax, linear segment exists:

θ(t) =⎧⎪⎪⎪⎪⎨⎪⎪⎪⎪⎩θ0 + 1

2θmax(t − t0)2 t0 ≤ t ≤ t0 + Ta

θ0 + θmaxTa (t − t0 − Ta

2) t0 + Ta < t ≤ t1 − Ta

θ f − 12θmax(t1 − t)2 t1 − Ta < t ≤ t1

Otherwise:

(Continues next slide)

Chapter5 TrajectoryGeneration

Introductionto TrajectoryGeneration

Joint SpaceTrajectoryGeneration(1-D)

5.2 Joint Space Trajectory Generation (1-D)Chapter 5 Trajectory Generation

20

⎧⎪⎪⎪⎪⎪⎨⎪⎪⎪⎪⎪⎩Ta = √ h

θmaxacceleration phase

T = 2Ta total time

θmax = θmaxTa = √θmaxh = hTa

maximum velocity

and

θ(t) = ⎧⎪⎪⎨⎪⎪⎩θ0 + 1

2θmax(t − t0)2 t0 ≤ t ≤ t0 + Ta

θ f − 12θmax(tf − t)2 tf − Ta < t ≤ tf

Chapter5 TrajectoryGeneration

Introductionto TrajectoryGeneration

Joint SpaceTrajectoryGeneration(1-D)

5.2 Joint Space Trajectory Generation (1-D)Chapter 5 Trajectory Generation

21

◇ n = 3: Double “S”trajectory (7-segment)

t

θ

t0 t1 t2 t3 t4 t5 t6 t7

θ0

θ7

t

θ

t0 t1 t2 t3 t4 t5 t6 t7

θmax

t

θ

t0 t1 t2 t3 t4 t5 t6 t7

θmax

t

...θ

t0 t1 t2 t3 t4 t5 t6 t7

...θ max

♢ Advantage:Bounded jerk avoids excitationfrom jerk impulse.♢ Description of the “S”curve:Composition of acc. (cubic),constant vel. (quadratic) and dec.(cubic), with C2 continuity at via points.

1 Acc.: t ∈ [t0 , t1]2 max vel.: t ∈ [t1 , t2]3 Dec.: t ∈ [t2 , t3]

(Continues next slide)

Chapter5 TrajectoryGeneration

Introductionto TrajectoryGeneration

Joint SpaceTrajectoryGeneration(1-D)

5.2 Joint Space Trajectory Generation (1-D)Chapter 5 Trajectory Generation

22

♢ Assumption:

θmax = −θmin , θmax = −θmin ,...θ max = −...θ min , t0 = 0

then T0 = T2, T4 = T6. Also assume θ0 = θ f = 0 to avoid end pointjerk impulse.♢ Boundary condition: θ0 , θ0 , θ f , θ fGiven θmax , θmax,

...θ max, and define Tk ≜ tk+1 − tk. Consider the limit

case of a single acc/dec. phase (t ∈ [t0 , t3])1 θ f is reached before θ could reach θmax (T1 = 0,T0 = T2)

θ f − θ0 = 1

2

...θ maxT

20´¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¸¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¶

acceleration

+ 1

2

...θ maxT

22´¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¸¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¶

deceleration

= ...θ maxT20

2 θmax is reached before deceleration

T0 = θmax...θ max

(Continues next slide)

Chapter5 TrajectoryGeneration

Introductionto TrajectoryGeneration

Joint SpaceTrajectoryGeneration(1-D)

5.2 Joint Space Trajectory Generation (1-D)Chapter 5 Trajectory Generation

23

Thus

T0 = min

⎧⎪⎪⎪⎨⎪⎪⎪⎩¿ÁÁÀ∣θ f − θ0∣...

θ max

,θmax...θ max

⎫⎪⎪⎪⎬⎪⎪⎪⎭The trajectory is feasible in the first case if (recall that T0 = T2):

θ f − θ0 > ∫ T0

0( ∫ τ

0(...θ maxt)dt + θ0)dτ+

∫T0

0( ∫ τ

0(...θ max(T0 − t))dt + θ0 + 1

2T20)dτ

= T0(θ f + θ0)For the second case,

T1 ⋅ θmax + 2 ⋅ 12θmaxT0 = θ f − θ0 ⇒

T1 = θ f − θ0θmax

− T0

(Continues next slide)

Chapter5 TrajectoryGeneration

Introductionto TrajectoryGeneration

Joint SpaceTrajectoryGeneration(1-D)

5.2 Joint Space Trajectory Generation (1-D)Chapter 5 Trajectory Generation

24

then the second case is feasible if:

θ f − θ0 > ∫ T0

0( ∫ τ

0( θmaxt

T0

)dt + θ0)dτ+∫

T1

0( ∫ τ

0θmaxdt + θ0 + 1

2θmaxT0)dτ+

∫T0

0( ∫ τ

0( θmax(T0 − t)

T0

)dt + θ0 + 1

2θmaxT0 + θmaxT1)dτ

= 1

2(θ f + θ0)⎛⎝T0 + θ f − θ0

θmax

⎞⎠♢ maximal velocity limit: Also has two cases:Case 1: If θmax is reached, T3 ≠ 0

if (θmax − θ0)...θ max < θ2max ⇒ θmax is not reached (∗)if (θmax − θ f )...θ max < θ2max ⇒ θmax is not reached (∗∗)

(Continues next slide)

Chapter5 TrajectoryGeneration

Introductionto TrajectoryGeneration

Joint SpaceTrajectoryGeneration(1-D)

5.2 Joint Space Trajectory Generation (1-D)Chapter 5 Trajectory Generation

25

If (∗) holds, then:T0 = T2 =

¿ÁÁÀ θmax − θ0...θ max

,T1 = 0otherwise

T0 = T2 = θmax...θ max

,T1 = θ f − θ0θmax

− T0

If (∗∗) holds, then:T4 = T6 =

¿ÁÁÀ θmax − θ f...θ max

,T5 = 0otherwise

T0 = T2 = θmax...θ max

,T5 = θ f − θ0θmax

− T4

Finally

T3 = θ f − θ0θmax

− T0 + T1 + T2

2(1 + θ0

θmax

) − T4 + T5 + T6

2

⎛⎝1 +θ f

θmax

⎞⎠If T3 ≥ 0, this verifies that θmax is actually reached. Otherwise, θmax is not

reached must be checked.

Chapter5 TrajectoryGeneration

Introductionto TrajectoryGeneration

Joint SpaceTrajectoryGeneration(1-D)

5.2 Joint Space Trajectory Generation (1-D)Chapter 5 Trajectory Generation

26

◇ Example: Examples of Case 1 and Case 2

0 0.5 1 1.5 2 2.5

−20

0

20

Jerk

−10

−5

0

5

10

Acceleration

−5

0

5

Velocity

0

2

4

6

8

10

Position

0 0.5 1 1.5 2

−20

0

20

Jerk

−10

−5

0

5

10

Acceleration

−10

−5

0

5

10

Velocity

0

2

4

6

8

10

Position

θmax θmax

θmax θmax

...θ max

...θ max

Case 1:θmax is reached Case 2:θmax is not reached

Chapter5 TrajectoryGeneration

Introductionto TrajectoryGeneration

Joint SpaceTrajectoryGeneration(1-D)

5.2 Joint Space Trajectory Generation (1-D)Chapter 5 Trajectory Generation

27

◻ Computation of the double S trajectory (θ f > θ0):

Notations

θ lim maximal velocityθ lima maximal acceleration in the acceleration phaseθ limd

maximal acceleration in the deceleration phaseTa Ta ≜ T0 + T1 + T2 acceleration phaseTd Ta ≜ T4 + T5 + T6 deceleration phaseT T ≜ Ta + T3 + Td total duration

Acceleration phase:

θ(t) =⎧⎪⎪⎪⎪⎪⎨⎪⎪⎪⎪⎪⎩θ0 + θ0t + ...θ max

t3

6t ∈ [0, t1]

θ0 + θ0t + θ lima

6(3t2 − 3T0t + T2

0) t ∈ [t1 , t2]θ0 + (θ lim + θ0) Ta

2− θ lim(Ta − t) + ...θ max

(Ta−t)3

6t ∈ [t2 , t3]

Constant velocity phase:

θ(t) = θ0 + (θ lim + θ0)Ta

2+ θ lim(t − Ta), t ∈ [t3 , t4]

(Continues next slide)

Chapter5 TrajectoryGeneration

Introductionto TrajectoryGeneration

Joint SpaceTrajectoryGeneration(1-D)

5.2 Joint Space Trajectory Generation (1-D)Chapter 5 Trajectory Generation

28

Deceleration phase:

θ(t) =⎧⎪⎪⎪⎪⎪⎪⎪⎪⎨⎪⎪⎪⎪⎪⎪⎪⎪⎩

θ f − (θ lim + θ f ) Td

2+ θ lim(t − T + Td) − ...θ max

(t−T+Td)3

6t ∈ [t4 , t5]

θ f − (θ lim + θ f ) Td

2+ θ lim(t − T + Td)+

θ limd

6(3(t − T + Td)2 − 3T4(t − T + Td) + T2

4) t ∈ [t5 , t6]θ f − θ f (T − t) − ...θ max

(T−t)3

6t ∈ [t6 , t7]

Chapter5 TrajectoryGeneration

Introductionto TrajectoryGeneration

Joint SpaceTrajectoryGeneration(1-D)

5.2 Joint Space Trajectory Generation (1-D)Chapter 5 Trajectory Generation

29

Solution 2: Bezier curves: θ(t) = ∑ni=1 Bi,n(t)pi,

basis function: {Bi,n(t)}

Chapter5 TrajectoryGeneration

Introductionto TrajectoryGeneration

Joint SpaceTrajectoryGeneration(1-D)

5.2 Joint Space Trajectory Generation (1-D)Chapter 5 Trajectory Generation

29

Solution 2: Bezier curves: θ(t) = ∑ni=1 Bi,n(t)pi,

basis function: {Bi,n(t)}Definition: Bernstein Polynomials

Bi,n(t) ≜ (ni)ti(1 − t)n−i , t ∈ [0, 1], i = 0, . . . ,n

Chapter5 TrajectoryGeneration

Introductionto TrajectoryGeneration

Joint SpaceTrajectoryGeneration(1-D)

5.2 Joint Space Trajectory Generation (1-D)Chapter 5 Trajectory Generation

29

Solution 2: Bezier curves: θ(t) = ∑ni=1 Bi,n(t)pi,

basis function: {Bi,n(t)}Definition: Bernstein Polynomials

Bi,n(t) ≜ (ni)ti(1 − t)n−i , t ∈ [0, 1], i = 0, . . . ,n

Property 1:● nonnegativity : Bi,n(t) ≥ 0,∀t ∈ [0, 1], i● symmetry : Bi,n(t) = Bn−i,n(1 − t), t ∈ [0, 1]● recursive definition: Bi,n(t) = tBi−1,n−1(t) + (1 − t)Bi,n−1(t), t ∈ [0, 1]● derivative : Bi,n(t) = n(Bi−1,n−1(t) − Bi,n−1(t)),with B−1,n−1(t) = Bn ,n−1(t) ≡ 0, t ∈ [0, 1]⇒ θ(t) = n∑n−1

i=0 Bi,n−1(t)(θ i+1 − θ i)(Continues next slide)

Chapter5 TrajectoryGeneration

Introductionto TrajectoryGeneration

Joint SpaceTrajectoryGeneration(1-D)

5.2 Joint Space Trajectory Generation (1-D)Chapter 5 Trajectory Generation

30

◇ Example: Cubic Bezier curves

4 boundary conditions θ0 , θ1 , θ0 , θ1 ⇒ 4 unknowns p0 , p1 , p2 , p3(n = 3, cubic)

t

θ

p0

p1

p2

p3

t = 1t = 23

t = 13

t = 0

b

b

b

b

⎧⎪⎪⎪⎪⎪⎪⎪⎪⎨⎪⎪⎪⎪⎪⎪⎪⎪⎩

θ0 = p0θ1 = p3θ0 = 3(p1 − p0)θ1 = 3(p3 − p2)

⇒⎧⎪⎪⎪⎪⎪⎪⎪⎪⎨⎪⎪⎪⎪⎪⎪⎪⎪⎩

p0 = θ0p1 = θ0/3 + θ0p2 = −θ1/3 + θ1p3 = θ1

(more geometrical than algebraic!)

Chapter5 TrajectoryGeneration

Introductionto TrajectoryGeneration

Joint SpaceTrajectoryGeneration(1-D)

5.2 Joint Space Trajectory Generation (1-D)Chapter 5 Trajectory Generation

30

◇ Example: Cubic Bezier curves

4 boundary conditions θ0 , θ1 , θ0 , θ1 ⇒ 4 unknowns p0 , p1 , p2 , p3(n = 3, cubic)

t

θ

p0

p1

p2

p3

t = 1t = 23

t = 13

t = 0

b

b

b

b

⎧⎪⎪⎪⎪⎪⎪⎪⎪⎨⎪⎪⎪⎪⎪⎪⎪⎪⎩

θ0 = p0θ1 = p3θ0 = 3(p1 − p0)θ1 = 3(p3 − p2)

⇒⎧⎪⎪⎪⎪⎪⎪⎪⎪⎨⎪⎪⎪⎪⎪⎪⎪⎪⎩

p0 = θ0p1 = θ0/3 + θ0p2 = −θ1/3 + θ1p3 = θ1

(more geometrical than algebraic!)

Generalizations of Bezier curve: B-spline, NURBS (Non-Uniform

Rational B-Spline)

Chapter5 TrajectoryGeneration

Introductionto TrajectoryGeneration

Joint SpaceTrajectoryGeneration(1-D)

5.2 Joint Space Trajectory Generation (1-D)Chapter 5 Trajectory Generation

31

Problem 2: 1-D Multi-point Trajectory GenerationInit: θ0 , θ1 , . . . , θn , θ0 , θ1 , . . . , θn

Solution: a parametric curve θ(t), t ∈ [t0 , tn] satisfying boundaryand smoothness condition

Chapter5 TrajectoryGeneration

Introductionto TrajectoryGeneration

Joint SpaceTrajectoryGeneration(1-D)

5.2 Joint Space Trajectory Generation (1-D)Chapter 5 Trajectory Generation

31

Problem 2: 1-D Multi-point Trajectory GenerationInit: θ0 , θ1 , . . . , θn , θ0 , θ1 , . . . , θn

Solution: a parametric curve θ(t), t ∈ [t0 , tn] satisfying boundaryand smoothness condition

Solutions:

1 interpolation by polynomial functions

2 interpolation by splines (piecewise polynomial)

3 approximation by smoothing splines

Chapter5 TrajectoryGeneration

Introductionto TrajectoryGeneration

Joint SpaceTrajectoryGeneration(1-D)

5.2 Joint Space Trajectory Generation (1-D)Chapter 5 Trajectory Generation

31

Problem 2: 1-D Multi-point Trajectory GenerationInit: θ0 , θ1 , . . . , θn , θ0 , θ1 , . . . , θn

Solution: a parametric curve θ(t), t ∈ [t0 , tn] satisfying boundaryand smoothness condition

Solutions:

1 interpolation by polynomial functions

2 interpolation by splines (piecewise polynomial)

3 approximation by smoothing splines

How to specify velocities at via points

1 User specifies desired velocities θ i at via points θ i.

2 System automatically chooses the velocities at the via points byapplying heuristics at either Cartesian or joint speeds.

3 The system automatically chooses velocities at the via points sothat acceleration at the via points are continuous.

Chapter5 TrajectoryGeneration

Introductionto TrajectoryGeneration

Joint SpaceTrajectoryGeneration(1-D)

5.2 Joint Space Trajectory Generation (1-D)Chapter 5 Trajectory Generation

32

Solution 1: Interpolation by polynomial

θ(t) = a0 + a1t + a2t2 +⋯+ antn , t ∈ [t0 , tn]Boundary conditions:

θ(ti) = θ i , i = 0, . . . , n⇒ Ta = bT =⎡⎢⎢⎢⎢⎢⎣1 t0 t20 ⋯ tn01 t1 t21 ⋯ tn1⋮ ⋮ ⋮ ⋮ ⋮1 tn t2n ⋯ tnn

⎤⎥⎥⎥⎥⎥⎦, Vandermonde matrix,

a = [ a0a1⋮an] , b =

⎡⎢⎢⎢⎢⎢⎣θ0θ1⋮θn

⎤⎥⎥⎥⎥⎥⎦det(T) = ∏

0≤i<j≤n

(tj − ti) ≠ 0⇒ T always invertible!

Disadvantage: bad numerical conditioning; oscillating behavior.

Chapter5 TrajectoryGeneration

Introductionto TrajectoryGeneration

Joint SpaceTrajectoryGeneration(1-D)

5.2 Joint Space Trajectory Generation (1-D)Chapter 5 Trajectory Generation

33

Solution 2: Interpolation by splines (piecewise polynomial)Solution 2A: Linear trajectory with parabolic blend

1 composition of point-to-point linear trajectory with specified viapoints velocity θ i (local interpolation)

2 using via points as control points (approximation)

t

θ

t0 t1

θ0 , θ0

θ 1 , θ 1

θ2 , θ2

θ3 , θ3θ4 , θ4

t

θ

t0 t1

t

θ

t0 t1

composition with preassigned via point velocities

t

θ

t0 t1

θ0

θ 1

θ2

θ3θ4bb

bb

bbb b

bb b b

t

θ

t0 t1

t

θ

t0 t1

composition with via points as control points

Chapter5 TrajectoryGeneration

Introductionto TrajectoryGeneration

Joint SpaceTrajectoryGeneration(1-D)

5.2 Joint Space Trajectory Generation (1-D)Chapter 5 Trajectory Generation

34

Solution 2B: Piecewise cubic polynomials

1 Composition of point-to-point cubic curves by specifying viapoint velocities.

2 Cubic splines by specifying end point velocity only and requiringcontinuity of acceleration at via points

t

θ

t0 t1

θ0 , θ0

θ 1 , θ 1

θ2 , θ2

θ3 , θ3θ4 , θ4

t

θ

t0 t1

t

θ

t0 t1

composition with preassigned via point velocities

t

θ

t0 t1

θ0 , θ0

θ 1

θ2

θ3θ4 , θ4

t

θ

t0 t1

t

θ

t0 t1

composition with continuity of acceleration

Chapter5 TrajectoryGeneration

Introductionto TrajectoryGeneration

Joint SpaceTrajectoryGeneration(1-D)

5.2 Joint Space Trajectory Generation (1-D)Chapter 5 Trajectory Generation

35

◻ Computation of interpolating cubic splines based oncontinuity of acceleration:

Init.: Given θ0 , . . . , θn, t0 , . . . , tn and boundary condition θ0 , θn. Tk ≜tk+1 − tk, α(t) ≜ θ(t).Step 1: Definition of the cubic spline

θ(t) = (tk+1 − t)36Tk

αk + (t − tk)36Tk

αk+1 + ( θk+1Tk− Tkαk+1

6) (t − tk)

+ ( θkTk− Tkαk

6) (tk+1 − t), t ∈ [tk , tk+1], k = 0, 1, . . . , n − 1

(∗)

⇒⎧⎪⎪⎪⎪⎪⎪⎨⎪⎪⎪⎪⎪⎪⎩θ(t) = (tk+1 − t)2

2Tkαk + (t − tk)2

2Tkαk+1 + θk+1 − θk

Tk− Tk(αk+1 − αk)

6

α(t) = αk+1(t − tk) + αk(tk+1 − t)Tk

(Continues next slide)

Chapter5 TrajectoryGeneration

Introductionto TrajectoryGeneration

Joint SpaceTrajectoryGeneration(1-D)

5.2 Joint Space Trajectory Generation (1-D)Chapter 5 Trajectory Generation

36

Step 2: Imposing C2 continuity

Tk−1

Tkαk−1 + 2(Tk + Tk−1)

Tkαk + αk+1 = 6

Tk( θk+1 − θk

Tk− θk − θk−1

Tk−1)

for k = 1, . . . , n − 1.Step 3: Imposing boundary condition:

θ(t0) = θ0 , θ(tn) = θn ⇒⎧⎪⎪⎪⎪⎪⎪⎨⎪⎪⎪⎪⎪⎪⎩

T20

3α0 + T2

0

6α1 = θ0 − θ1 − T0 θ0

T2n−1

3αn + T2

n−1

6αn−1 = θn−1 − θn + Tn−1 θn

⇒ Aα = B

A =⎡⎢⎢⎢⎢⎢⎢⎢⎣

2T0 T0 0 ⋯ 0T0 2(T0 + T1) T1 ⋮0 ⋱ 0⋮ Tn−2 2(Tn−2 + Tn−1) Tn−10 ⋯ 0 Tn−1 2Tn−1

⎤⎥⎥⎥⎥⎥⎥⎥⎦, α = ⎡⎢⎢⎢⎢⎣

θ0⋮

θn

⎤⎥⎥⎥⎥⎦ ,

(Continues next slide)

Chapter5 TrajectoryGeneration

Introductionto TrajectoryGeneration

Joint SpaceTrajectoryGeneration(1-D)

5.2 Joint Space Trajectory Generation (1-D)Chapter 5 Trajectory Generation

37

and

B =⎡⎢⎢⎢⎢⎢⎢⎢⎢⎢⎣

6( θ 1−θ0T0− θ0)

6( θ2−θ 1T1− θ 1−θ0

T0)⋮

6( θn−θn−1Tn−1− θn−1−θn−2

Tn−2) 6(θn − θn−θn−1

Tn−1)

⎤⎥⎥⎥⎥⎥⎥⎥⎥⎥⎦Step 4: Solving for α

Aα = B⇒ α = A−1BStep 5: Computation of the spline by substituting α back into (∗)

Chapter5 TrajectoryGeneration

Introductionto TrajectoryGeneration

Joint SpaceTrajectoryGeneration(1-D)

5.2 Joint Space Trajectory Generation (1-D)Chapter 5 Trajectory Generation

38

Solution 3: Approximation by smoothing splinesGiven θ0 , . . . , θn and t0 , . . . , tn, design θ(t) s.t.:

θ(t) = argminθ(t)[µ n∑

k=0

ωk(θ(tk) − θk)2 + (1 − µ) ∫tn

t0θ(t)2dt]

Chapter5 TrajectoryGeneration

Introductionto TrajectoryGeneration

Joint SpaceTrajectoryGeneration(1-D)

5.2 Joint Space Trajectory Generation (1-D)Chapter 5 Trajectory Generation

38

Solution 3: Approximation by smoothing splinesGiven θ0 , . . . , θn and t0 , . . . , tn, design θ(t) s.t.:

θ(t) = argminθ(t)[µ n∑

k=0

ωk(θ(tk) − θk)2 + (1 − µ) ∫tn

t0θ(t)2dt]

♢ Special case 1: µ = 1argmin

θ(t)[ n∑k=0

ωk(θ(tk) − θk)2]Exact interpolation

Chapter5 TrajectoryGeneration

Introductionto TrajectoryGeneration

Joint SpaceTrajectoryGeneration(1-D)

5.2 Joint Space Trajectory Generation (1-D)Chapter 5 Trajectory Generation

38

Solution 3: Approximation by smoothing splinesGiven θ0 , . . . , θn and t0 , . . . , tn, design θ(t) s.t.:

θ(t) = argminθ(t)[µ n∑

k=0

ωk(θ(tk) − θk)2 + (1 − µ) ∫tn

t0θ(t)2dt]

♢ Special case 1: µ = 1argmin

θ(t)[ n∑k=0

ωk(θ(tk) − θk)2]Exact interpolation♢ Special case 2: µ = 0

argminθ(t)[ ∫

tn

t0θ(t)2dt]

natural Cubic spline curve: θ(t) piecewise cubic polynomial, ∈ C2 and¨θ0 = ¨θn = 0

(Continues next slide)

Chapter5 TrajectoryGeneration

Introductionto TrajectoryGeneration

Joint SpaceTrajectoryGeneration(1-D)

5.2 Joint Space Trajectory Generation (1-D)Chapter 5 Trajectory Generation

39

0 2 4 6 8 10 12 14 16 18

−4

−2

0

2

4

6

8

Acceleration

−4

−2

0

2

4

6

Velocity

−5

0

5

10

Position

● µ = 1 (solid line): exact interpolation● µ = 0.6 (dashdot line): approximation with smaller error● µ = 0.3 (dashed line): more smooth(Continues next slide)

Chapter5 TrajectoryGeneration

Introductionto TrajectoryGeneration

Joint SpaceTrajectoryGeneration(1-D)

5.2 Joint Space Trajectory Generation (1-D)Chapter 5 Trajectory Generation

40

♢ Derivation of the Smoothing spline

∫tn

t0θ(t)dt = n−1∑

k=0∫

tk+1

tkθk(t)2dt = n−1∑

k=0∫

tk+1

tk(θk + t − tk

tk+1 − tk (θk+1 − θk))2dt= n−1∑

k=0∫

tk+1−tk

0(θk + t

tk+1 − tk (θk+1 − θk))2dt= 1

3(tk+1 − tk)(θ2k + θk θk+1 + θ2k+1)

Chapter5 TrajectoryGeneration

Introductionto TrajectoryGeneration

Joint SpaceTrajectoryGeneration(1-D)

5.2 Joint Space Trajectory Generation (1-D)Chapter 5 Trajectory Generation

40

♢ Derivation of the Smoothing spline

∫tn

t0θ(t)dt = n−1∑

k=0∫

tk+1

tkθk(t)2dt = n−1∑

k=0∫

tk+1

tk(θk + t − tk

tk+1 − tk (θk+1 − θk))2dt= n−1∑

k=0∫

tk+1−tk

0(θk + t

tk+1 − tk (θk+1 − θk))2dt= 1

3(tk+1 − tk)(θ2k + θk θk+1 + θ2k+1)

⇒ L = n∑k=0

ωk(θk − θ(tk))2 + λ n−1∑k=0

2Tk(θ2k + θk θk+1 + θ2k+1)Tk ≜ tk+1 − tk , λ ≜ 1 − µ

⇒ L = (θ − θ)TW(θ − θ) + λαTAα

where θ ≜ [θ0 , θ1 , . . . , θn]T , θ ≜ [θ(t0), . . . , θ(tn)]T , α ≜ [ ¨θ0 , . . . , ¨θn]T(Continues next slide)

Chapter5 TrajectoryGeneration

Introductionto TrajectoryGeneration

Joint SpaceTrajectoryGeneration(1-D)

5.2 Joint Space Trajectory Generation (1-D)Chapter 5 Trajectory Generation

41

and W = diag(ω0, . . . , ωn), weighting matrix

A =⎡⎢⎢⎢⎢⎢⎢⎣

2T0 T0 0 ⋯ 0T0 2(T0 + T1) T1 ⋮0 ⋱ 0⋮ Tn−2 2(Tn−2 + Tn−1) Tn−10 ⋯ 0 Tn−1 2Tn−1

⎤⎥⎥⎥⎥⎥⎥⎦

Chapter5 TrajectoryGeneration

Introductionto TrajectoryGeneration

Joint SpaceTrajectoryGeneration(1-D)

5.2 Joint Space Trajectory Generation (1-D)Chapter 5 Trajectory Generation

41

and W = diag(ω0, . . . , ωn), weighting matrix

A =⎡⎢⎢⎢⎢⎢⎢⎣

2T0 T0 0 ⋯ 0T0 2(T0 + T1) T1 ⋮0 ⋱ 0⋮ Tn−2 2(Tn−2 + Tn−1) Tn−10 ⋯ 0 Tn−1 2Tn−1

⎤⎥⎥⎥⎥⎥⎥⎦For clamped spline (θ0 = θn = 0)

Aα = Cθ ,C =⎡⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎣

− 6T0

6T0

0 ⋯ 06T0−( 6

T0+ 6

T1) T1 ⋮

0 6T1

−( 6T1+ 6

T2) 6

T2⋮ ⋱ 06

Tn−2−( 6

Tn−2+ 6

Tn−1) 6

Tn−1

0 ⋯ 0 6Tn−1

− 6Tn−1

⎤⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎦(Continues next slide)

Chapter5 TrajectoryGeneration

Introductionto TrajectoryGeneration

Joint SpaceTrajectoryGeneration(1-D)

5.2 Joint Space Trajectory Generation (1-D)Chapter 5 Trajectory Generation

42

⎧⎪⎪⎨⎪⎪⎩L = (θ − θ)TW(θ − θ) + λαTAα

Aα = Cθ ⇒ α = A−1Cθ ⇒

L(θ) = (θ − θ)TW(θ − θ) + λθTCTA−1Cθ ⇒ minL⇒ ∂L

∂θ= 0⇒

−(θ − θ)TW + λθTCTA−1C = 0⇒θ = (W + λCTA−1C)−1Wθ

= θ − λW−1CT(A + λCW−1CT)−1Cθ ⇒(A + λCW−1CT)α = Cθ ⇒ α = [ ¨θ(t0), . . . , ¨θ(tn)]T solved

θ = θ − λW−1CT α⇒ θ = [θ(t0), . . . , θ(tn)]T solved

(Continues next slide)

Chapter5 TrajectoryGeneration

Introductionto TrajectoryGeneration

Joint SpaceTrajectoryGeneration(1-D)

5.2 Joint Space Trajectory Generation (1-D)Chapter 5 Trajectory Generation

43

♢ Solution to the Smoothing cubic spline:Denote the smoothing cubic spline by:

θ(t) = ak,0 + ak,1(t − tk) + ak,2(t − tk)2 + ak,3(t − tk)3 , t ∈ [tk , tk+1]⎧⎪⎪⎪⎪⎪⎪⎪⎪⎪⎨⎪⎪⎪⎪⎪⎪⎪⎪⎪⎩

ak,0 = θ(tk)ak,0 + ak,1Tk + ak,2T2

k + ak,3T3k = θ(tk+1)

2ak,2 = ¨θ(tk)2ak,2 + 6ak,3Tk = ¨θ(tk+1)

⇒ ak,0 , ak,1 , ak,2 , ak,3

Note:ω0 = ωn = 0 for the weighting matrix W ⇒ θ(t0) = θ0 , θ(tn) = θn

† End of Section †

Chapter5 TrajectoryGeneration

Introductionto TrajectoryGeneration

Joint SpaceTrajectoryGeneration(1-D)

5.3 Work Space Trajectory Generation (N-D)Chapter 5 Trajectory Generation

44

◻ What is workspace trajectory?:A trajectory in end-effector coordinates. Typical workspace:

1 Euclidean space Rk: Cartesian robot, position of 6-DoF robot

2 Special Euclidean group SE(3) (and subgroup): SCARA (X),orientation of 6-DoF robot (SO(3)), 6-DoF robot (SE(3))

3 Homogeneous space of SE(3): 5-axis machine tool(SE(3)/SO(2))

Chapter5 TrajectoryGeneration

Introductionto TrajectoryGeneration

Joint SpaceTrajectoryGeneration(1-D)

5.3 Work Space Trajectory Generation (N-D)Chapter 5 Trajectory Generation

45

◻ Why workspace traj. generation?:1 In NC machining, the end-effector (tool) is required to movealong trajectories specified in workspace coordinates.

2 In task coordinate tracking control, tolerance/contour error isusually specified in workspace coordinates.

3 Smooth joint space trajectory may still lead to large acceleration/ jerk in workspace coordinates, causing large inertia force ofthe payload.

Chapter5 TrajectoryGeneration

Introductionto TrajectoryGeneration

Joint SpaceTrajectoryGeneration(1-D)

5.3 Work Space Trajectory Generation (N-D)Chapter 5 Trajectory Generation

45

◻ Why workspace traj. generation?:1 In NC machining, the end-effector (tool) is required to movealong trajectories specified in workspace coordinates.

2 In task coordinate tracking control, tolerance/contour error isusually specified in workspace coordinates.

3 Smooth joint space trajectory may still lead to large acceleration/ jerk in workspace coordinates, causing large inertia force ofthe payload.

◻ Workspace traj. generation issues:Choice of workspace coordinates

Parametrization of SO(3): Euler angles, unit quaternion,exponential coordinate, etcParametrization of SE(3): dual quaternion, exponentialcoordinate, etc

(Continues next slide)

Chapter5 TrajectoryGeneration

Introductionto TrajectoryGeneration

Joint SpaceTrajectoryGeneration(1-D)

5.3 Work Space Trajectory Generation (N-D)Chapter 5 Trajectory Generation

46

Geometric properties of the trajectory

Continuity of the geometric path Gk: geometric“slopes”equal at via points, i.e. the change of direction ofvel./acc./jerk,. . . is smooth, but that of the magnitude maybe discontinuouscontinuity of the trajectory Ck: smooth change ofvel./acc./jerk,. . . . Can be realized by reparameterizationof a geometrically smooth trajectory.

umin umax

uminumax

p(u)

p(u) = (p ◦ f)(u)

f

Reparameterization of a curve p(u)(Continues next slide)

Chapter5 TrajectoryGeneration

Introductionto TrajectoryGeneration

Joint SpaceTrajectoryGeneration(1-D)

5.3 Work Space Trajectory Generation (N-D)Chapter 5 Trajectory Generation

47

Motion law of the trajectory

p = p(u), u = u(t)⇒ p = p ○ u(t)Design u(t) so that given constraints on vel.,acc.,. . . , aresatisfied

umin umaxtmin tmax

p = p(u)

p(t) = p(u(t))

u = u(t)

Transformation of workspace trajectory back to joint space byinverse kinematics

The complexity of inverse kinematics may depend on the actualmachine tool where traj. generation is applied to.

(Continues next slide)

Chapter5 TrajectoryGeneration

Introductionto TrajectoryGeneration

Joint SpaceTrajectoryGeneration(1-D)

5.3 Work Space Trajectory Generation (N-D)Chapter 5 Trajectory Generation

48

Dependence of orientation on position

separate generationsimultaneous generation

−5

0

5

−4−2

02

46

8

4

5

6

7

8

9

10

11

12

−4

−2

0

2

4

−4

−2

0

2

4

0

1

2

3

4

5

independent interpolation using posi-tion vector and Euler angles

dependent interpolation using Frenetframe

Chapter5 TrajectoryGeneration

Introductionto TrajectoryGeneration

Joint SpaceTrajectoryGeneration(1-D)

5.3 Work Space Trajectory Generation (N-D)Chapter 5 Trajectory Generation

49

◻ Trajectory generation in Rk:♢ Polynomial trajectory in R

3:

p(t) = ⎡⎢⎢⎢⎢⎣x(t)y(t)z(t)

⎤⎥⎥⎥⎥⎦= [ x0 + x1t +⋯ + xntn

y0 + y1t +⋯+ yntnz0 + z1t +⋯+ zntn ] , pi ≜ [

xiyizi]⇒

⇒ p(t) = p0 + tp1 +⋯+ tnpnBasis functions ∶ {ti}ni=0 ∈ C∞(R)Components ∶ p0 , p1 , . . . , pn ∈ R3

Chapter5 TrajectoryGeneration

Introductionto TrajectoryGeneration

Joint SpaceTrajectoryGeneration(1-D)

5.3 Work Space Trajectory Generation (N-D)Chapter 5 Trajectory Generation

49

◻ Trajectory generation in Rk:♢ Polynomial trajectory in R

3:

p(t) = ⎡⎢⎢⎢⎢⎣x(t)y(t)z(t)

⎤⎥⎥⎥⎥⎦= [ x0 + x1t +⋯ + xntn

y0 + y1t +⋯+ yntnz0 + z1t +⋯+ zntn ] , pi ≜ [

xiyizi]⇒

⇒ p(t) = p0 + tp1 +⋯+ tnpnBasis functions ∶ {ti}ni=0 ∈ C∞(R)Components ∶ p0 , p1 , . . . , pn ∈ R3

Same techniques for R1 can be applied!♢ Choice of basis functions

1 Bernstein polynomials (Bezier curves, for local fitting)

2 B-spline basis functions (B-spline and NURBS, for global fitting)

Chapter5 TrajectoryGeneration

Introductionto TrajectoryGeneration

Joint SpaceTrajectoryGeneration(1-D)

5.3 Work Space Trajectory Generation (N-D)Chapter 5 Trajectory Generation

50

◻ Bezier curve Basics:♢ Bernstein polynomials:

Bi,n(u) ≜ n!

i!(n − i)!ui(1 − u)n−i , u ∈ [0, 1]

u

Bi,3

u = 1u = 0

1

0

B0,3

B1,3 B2,3

B3,3

n = 3u

Bi,4

u = 1u = 0

1

0

B0,4

B1,4 B2,4B3,4

B4,4

n = 4

Chapter5 TrajectoryGeneration

Introductionto TrajectoryGeneration

Joint SpaceTrajectoryGeneration(1-D)

5.3 Work Space Trajectory Generation (N-D)Chapter 5 Trajectory Generation

50

◻ Bezier curve Basics:♢ Bernstein polynomials:

Bi,n(u) ≜ n!

i!(n − i)!ui(1 − u)n−i , u ∈ [0, 1]

u

Bi,3

u = 1u = 0

1

0

B0,3

B1,3 B2,3

B3,3

n = 3u

Bi,4

u = 1u = 0

1

0

B0,4

B1,4 B2,4B3,4

B4,4

n = 4

♢ Derivative of Bernstein polynomials:

Bi,n(u) = n!

i!(n − i)! iui−1(1 − u)n−i −n!

i!(n − i)!(n − i)ui(1 − u)n−i−1= n(Bi−1,n−1 − Bi,n−1)B−1,n−1(u) ≡ Bn,n−1(u) ≡ 0 (Continues next slide)

Chapter5 TrajectoryGeneration

Introductionto TrajectoryGeneration

Joint SpaceTrajectoryGeneration(1-D)

5.3 Work Space Trajectory Generation (N-D)Chapter 5 Trajectory Generation

51

u

Bi,3

u = 1u = 0

3

−3

0

B0,3

B1,3

B2,3

B3,3

n = 3

u

Bi,4

u = 1u = 0

4

−4

0

B0,4

B1,4

B2,4

B3,4

B4,4

n = 4

Chapter5 TrajectoryGeneration

Introductionto TrajectoryGeneration

Joint SpaceTrajectoryGeneration(1-D)

5.3 Work Space Trajectory Generation (N-D)Chapter 5 Trajectory Generation

51

u

Bi,3

u = 1u = 0

3

−3

0

B0,3

B1,3

B2,3

B3,3

n = 3

u

Bi,4

u = 1u = 0

4

−4

0

B0,4

B1,4

B2,4

B3,4

B4,4

n = 4♢ Recursive definition of Bernstein polynomials

Bi,n(u) = (ni)ui(1 − u)n−i = ((n − 1

i − 1) + (n − 1i))ui(1 − u)n−i

= uBi−1,n−1(u) + (1 − u)Bi,n−1(u)(See the diagram in next slide)

Chapter5 TrajectoryGeneration

Introductionto TrajectoryGeneration

Joint SpaceTrajectoryGeneration(1-D)

5.3 Work Space Trajectory Generation (N-D)Chapter 5 Trajectory Generation

52

B0,0

B0,1

B1,1

B0,2

B1,2

B2,2

B0,3

B1,3

B2,3

B3,3

. . . . . .

Chapter5 TrajectoryGeneration

Introductionto TrajectoryGeneration

Joint SpaceTrajectoryGeneration(1-D)

5.3 Work Space Trajectory Generation (N-D)Chapter 5 Trajectory Generation

52

B0,0

B0,1

B1,1

B0,2

B1,2

B2,2

B0,3

B1,3

B2,3

B3,3

. . . . . .

♢ De Casteljau Algorithm (computing points on Bezier curve)

p(u) = n∑i=0

Bi,n(u)pi = n∑i=0

(uBi−1,n−1(u) + (1 − u)Bi,n−1)pi= n−1∑

i=0

Bi,n−1(u)((1 − u)pi + upi+1) ≜ n−1∑i=0

Bi,n−1(u)p1,ipk,i ≜ (1 − u)pk−1,i + upk−1,i+1 , p0,i ≜ pi (control points)

(Continues next slide)

Chapter5 TrajectoryGeneration

Introductionto TrajectoryGeneration

Joint SpaceTrajectoryGeneration(1-D)

5.3 Work Space Trajectory Generation (N-D)Chapter 5 Trajectory Generation

53

p(u) = 3∑i=0

Bi,3(u)pi

u = 23

illustration of the De Casteljau algorithm for a cubic Bezier curve:

p0

p1 p2

p3

Chapter5 TrajectoryGeneration

Introductionto TrajectoryGeneration

Joint SpaceTrajectoryGeneration(1-D)

5.3 Work Space Trajectory Generation (N-D)Chapter 5 Trajectory Generation

53

p(u) = 3∑i=0

Bi,3(u)pi

u = 23

illustration of the De Casteljau algorithm for a cubic Bezier curve:

p0

p1 p2

p3

p1,0

p1,1

p1,2

Chapter5 TrajectoryGeneration

Introductionto TrajectoryGeneration

Joint SpaceTrajectoryGeneration(1-D)

5.3 Work Space Trajectory Generation (N-D)Chapter 5 Trajectory Generation

53

p(u) = 3∑i=0

Bi,3(u)pi

u = 23

illustration of the De Casteljau algorithm for a cubic Bezier curve:

p0

p1 p2

p3

p1,0

p1,1

p1,2

p2,0

p2,1

Chapter5 TrajectoryGeneration

Introductionto TrajectoryGeneration

Joint SpaceTrajectoryGeneration(1-D)

5.3 Work Space Trajectory Generation (N-D)Chapter 5 Trajectory Generation

53

p(u) = 3∑i=0

Bi,3(u)pi

u = 23

illustration of the De Casteljau algorithm for a cubic Bezier curve:

p0

p1 p2

p3

p1,0

p1,1

p1,2

p2,0

p2,1p3,0

⇒ sequentially taking linear interpolation (Continues next slide)

Chapter5 TrajectoryGeneration

Introductionto TrajectoryGeneration

Joint SpaceTrajectoryGeneration(1-D)

5.3 Work Space Trajectory Generation (N-D)Chapter 5 Trajectory Generation

54

♢ Derivative of Bezier curves

p(u) = n∑i=0

Bi,n(u)pi = n n∑i=0

(Bi−1,n−1(u) − Bi−1,n−1(u))pi= n( n∑

i=0

Bi−1,n−1(u)pi − n∑i=0

Bi−1,n−1(u)pi)= n n−1∑

i=0

Bi,n−1(u)(pi+1 − pi)

Chapter5 TrajectoryGeneration

Introductionto TrajectoryGeneration

Joint SpaceTrajectoryGeneration(1-D)

5.3 Work Space Trajectory Generation (N-D)Chapter 5 Trajectory Generation

54

♢ Derivative of Bezier curves

p(u) = n∑i=0

Bi,n(u)pi = n n∑i=0

(Bi−1,n−1(u) − Bi−1,n−1(u))pi= n( n∑

i=0

Bi−1,n−1(u)pi − n∑i=0

Bi−1,n−1(u)pi)= n n−1∑

i=0

Bi,n−1(u)(pi+1 − pi)p(u) = n(n − 1) n−2∑

i=0

Bi,n−2(u)((pi+2 − pi+1) − (pi+1 − pi))= n(n − 1) n−2∑

i=0

Bi,n−2(u)(pi+2 − 2pi+1 + pi)⋮

e.g. n = 3, p(0) = 3(p1 − p0), p(1) = 3(p3 − p2)

Chapter5 TrajectoryGeneration

Introductionto TrajectoryGeneration

Joint SpaceTrajectoryGeneration(1-D)

5.3 Work Space Trajectory Generation (N-D)Chapter 5 Trajectory Generation

55

◻ Imposing continuity condition:

b

b b

b

p10

p11 p12p13 = P2

0

u = 0 u = u1u′ = u − 0

u1 − 0

Chapter5 TrajectoryGeneration

Introductionto TrajectoryGeneration

Joint SpaceTrajectoryGeneration(1-D)

5.3 Work Space Trajectory Generation (N-D)Chapter 5 Trajectory Generation

55

◻ Imposing continuity condition:

b

b b

b

p10

p11 p12p13 = P2

0

u = 0 u = u1u′ = u − 0

u1 − 0

b

b

b

b

p21

p22 p23 = P30

u = u2u′′ = u − u1

u2 − u1

Chapter5 TrajectoryGeneration

Introductionto TrajectoryGeneration

Joint SpaceTrajectoryGeneration(1-D)

5.3 Work Space Trajectory Generation (N-D)Chapter 5 Trajectory Generation

55

◻ Imposing continuity condition:

b

b b

b

p10

p11 p12p13 = P2

0

u = 0 u = u1u′ = u − 0

u1 − 0

b

b

b

b

p21

p22 p23 = P30

u = u2u′′ = u − u1

u2 − u1

b

b b

b

p31 p32

p33

u = 1u′′′ = u − u2

1 − u2(Continues next slide)

Chapter5 TrajectoryGeneration

Introductionto TrajectoryGeneration

Joint SpaceTrajectoryGeneration(1-D)

5.3 Work Space Trajectory Generation (N-D)Chapter 5 Trajectory Generation

56

♢ Imposing C0 continuity

p13 = p20 , p23 = p30

Chapter5 TrajectoryGeneration

Introductionto TrajectoryGeneration

Joint SpaceTrajectoryGeneration(1-D)

5.3 Work Space Trajectory Generation (N-D)Chapter 5 Trajectory Generation

56

♢ Imposing C0 continuity

p13 = p20 , p23 = p30♢ Imposing C1 continuity

p(u1) = dp1(u′)du′

du′

du= 3

u1 − 0(p13 − p12) = 3

u2 − u1 (p21 − p20)

p(u2) = 3

u2 − u1 (p23 − p22) = 3

1 − u2 (p33 − p32)

Chapter5 TrajectoryGeneration

Introductionto TrajectoryGeneration

Joint SpaceTrajectoryGeneration(1-D)

5.3 Work Space Trajectory Generation (N-D)Chapter 5 Trajectory Generation

56

♢ Imposing C0 continuity

p13 = p20 , p23 = p30♢ Imposing C1 continuity

p(u1) = dp1(u′)du′

du′

du= 3

u1 − 0(p13 − p12) = 3

u2 − u1 (p21 − p20)

p(u2) = 3

u2 − u1 (p23 − p22) = 3

1 − u2 (p33 − p32)

u1 − 0 = u2 − u1 = 1 − u2: uniform Bezier spline

otherwise: non-uniform Bezier spline

Chapter5 TrajectoryGeneration

Introductionto TrajectoryGeneration

Joint SpaceTrajectoryGeneration(1-D)

5.3 Work Space Trajectory Generation (N-D)Chapter 5 Trajectory Generation

57

◻ Local interpolation on R2 using Bezier spline:

♢ Applicability:

1 Complete set of via points not known in advance

2 tangent and curvature vectors available at via points

Init: via points p0 , . . . , pn , 0 = u0 < u1 < ⋅ ⋅ ⋅ < un−1 < un = 1vk ≜ pk − pk−1 , d ≜ n∑

k=1

∣vk ∣ (chord length)

uk = uk−1 + ∣vk ∣d

, ∆uk ≜ uk − uk−1 = ∣vk ∣d

, dk ≜ vk∆uk= d vk∣vk ∣

Chapter5 TrajectoryGeneration

Introductionto TrajectoryGeneration

Joint SpaceTrajectoryGeneration(1-D)

5.3 Work Space Trajectory Generation (N-D)Chapter 5 Trajectory Generation

57

◻ Local interpolation on R2 using Bezier spline:

♢ Applicability:

1 Complete set of via points not known in advance

2 tangent and curvature vectors available at via points

Init: via points p0 , . . . , pn , 0 = u0 < u1 < ⋅ ⋅ ⋅ < un−1 < un = 1vk ≜ pk − pk−1 , d ≜ n∑

k=1

∣vk ∣ (chord length)

uk = uk−1 + ∣vk ∣d

, ∆uk ≜ uk − uk−1 = ∣vk ∣d

, dk ≜ vk∆uk= d vk∣vk ∣

Step 1: Computation of derivative Dk (or tangent vectors Tk )

Dk = (1 − αk)dk + αkdk+1 or Tk = Vk∣Vk ∣ Vk = (1 − αk)vk + αkvk+1

(Continues next slide)

Chapter5 TrajectoryGeneration

Introductionto TrajectoryGeneration

Joint SpaceTrajectoryGeneration(1-D)

5.3 Work Space Trajectory Generation (N-D)Chapter 5 Trajectory Generation

58

vk−1

vk vk+1

vk+2

pk−2

pk−1

pk

pk+1

pk+2

Vk

Dk

where αk is give by:● Bessel (three point) method

αk = ∆uk∆uk + ∆uk+1

Dk = (1 − αk)dk + αkdk+1

● Five point method

αk = ∣vk−1 × vk ∣∣vk−1 × vk ∣ + ∣vk+1 × vk+2 ∣Tk = Vk∣Vk ∣ Vk = (1 − αk)vk + αkvk+1

♢ Special treatment at ends

D0 = 2d1 −D1 ,Dn = 2dn −Dn−1

v0 = 2v1 − v2 , v−1 = 2v0 − v1vn+1 = 2vn − vn−1 , vn+2 = 2vn+1 − vn

(Continues next slide)

Chapter5 TrajectoryGeneration

Introductionto TrajectoryGeneration

Joint SpaceTrajectoryGeneration(1-D)

5.3 Work Space Trajectory Generation (N-D)Chapter 5 Trajectory Generation

59

Step 2: Composition of Bezier curvese.g. parabolic Bezier curves on R

2 (using p0 ,R0 , p1 , . . . as controlpoints)

T0

T1T2

T3

T4

T5

p0

p1

p2 p3

p4

p5

Chapter5 TrajectoryGeneration

Introductionto TrajectoryGeneration

Joint SpaceTrajectoryGeneration(1-D)

5.3 Work Space Trajectory Generation (N-D)Chapter 5 Trajectory Generation

59

Step 2: Composition of Bezier curvese.g. parabolic Bezier curves on R

2 (using p0 ,R0 , p1 , . . . as controlpoints)

T0

T1T2

T3

T4

T5

p0

p1

p2 p3

p4

p5

R1

R2

R3

R4

R5

Rk ≜ pk−1 + γk−1Tk−1 = pk + γkTk

Chapter5 TrajectoryGeneration

Introductionto TrajectoryGeneration

Joint SpaceTrajectoryGeneration(1-D)

5.3 Work Space Trajectory Generation (N-D)Chapter 5 Trajectory Generation

59

Step 2: Composition of Bezier curvese.g. parabolic Bezier curves on R

2 (using p0 ,R0 , p1 , . . . as controlpoints)

T0

T1T2

T3

T4

T5

p0

p1

p2 p3

p4

p5

R1

R2

R3

R4

R5

Rk ≜ pk−1 + γk−1Tk−1 = pk + γkTk

Chapter5 TrajectoryGeneration

Introductionto TrajectoryGeneration

Joint SpaceTrajectoryGeneration(1-D)

5.3 Work Space Trajectory Generation (N-D)Chapter 5 Trajectory Generation

59

Step 2: Composition of Bezier curvese.g. parabolic Bezier curves on R

2 (using p0 ,R0 , p1 , . . . as controlpoints)

T0

T1T2

T3

T4

T5

p0

p1

p2 p3

p4

p5

R1

R2

R3

R4

R5

Rk ≜ pk−1 + γk−1Tk−1 = pk + γkTk

Chapter5 TrajectoryGeneration

Introductionto TrajectoryGeneration

Joint SpaceTrajectoryGeneration(1-D)

5.3 Work Space Trajectory Generation (N-D)Chapter 5 Trajectory Generation

59

Step 2: Composition of Bezier curvese.g. parabolic Bezier curves on R

2 (using p0 ,R0 , p1 , . . . as controlpoints)

T0

T1T2

T3

T4

T5

p0

p1

p2 p3

p4

p5

R1

R2

R3

R4

R5

Rk ≜ pk−1 + γk−1Tk−1 = pk + γkTk

Chapter5 TrajectoryGeneration

Introductionto TrajectoryGeneration

Joint SpaceTrajectoryGeneration(1-D)

5.3 Work Space Trajectory Generation (N-D)Chapter 5 Trajectory Generation

59

Step 2: Composition of Bezier curvese.g. parabolic Bezier curves on R

2 (using p0 ,R0 , p1 , . . . as controlpoints)

T0

T1T2

T3

T4

T5

p0

p1

p2 p3

p4

p5

R1

R2

R3

R4

R5

Rk ≜ pk−1 + γk−1Tk−1 = pk + γkTk

Chapter5 TrajectoryGeneration

Introductionto TrajectoryGeneration

Joint SpaceTrajectoryGeneration(1-D)

5.3 Work Space Trajectory Generation (N-D)Chapter 5 Trajectory Generation

59

Step 2: Composition of Bezier curvese.g. parabolic Bezier curves on R

2 (using p0 ,R0 , p1 , . . . as controlpoints)

T0

T1T2

T3

T4

T5

p0

p1

p2 p3

p4

p5

R1

R2

R3

R4

R5

Rk ≜ pk−1 + γk−1Tk−1 = pk + γkTk

Chapter5 TrajectoryGeneration

Introductionto TrajectoryGeneration

Joint SpaceTrajectoryGeneration(1-D)

5.3 Work Space Trajectory Generation (N-D)Chapter 5 Trajectory Generation

60

● Note: C1 v.s. G1

Definition: G1

A spline is said to be G1 if tangent vectors of pieces are collinear atvia points.

The generated spline (on previous slide) is only G1 but not C1

Step 3: Imposing C1 continuity by reparametrization: uk ⇒ uk

u0 = 0, u1 = 1uk = uk−1 + (uk−1 − uk−2) ∣Rk − pk−1 ∣∣pk−1 − Rk−1 ∣uk ≜ uk

un

Chapter5 TrajectoryGeneration

Introductionto TrajectoryGeneration

Joint SpaceTrajectoryGeneration(1-D)

5.3 Work Space Trajectory Generation (N-D)Chapter 5 Trajectory Generation

61

◻ Local interpolation on R3 using Cubic Bezier curves:

♢ Review: General facts of cubic Bezier curves

Construct a Bezier curve p(t), t ∈ [0, 1] satisfying:1 endpoints p0 , p3

2 endpoint unit tangent vector T0 , T33 v = ∣p(0)∣ = ∣p( 1

2)∣ = ∣p(1)∣⇒ p1 = p0 + 1

3vT0 , p2 = p3 − 1

3vT3

By applying DeCasteljau algorithm and ∣p( 12)∣ = v, we have:

8

6v = ∣p3 + p2 − p1 − p0 ∣= ∣p3 + (p3 − 1

3vT3) − (p0 + 1

3vT0) − p0∣(Continues next slide)

Chapter5 TrajectoryGeneration

Introductionto TrajectoryGeneration

Joint SpaceTrajectoryGeneration(1-D)

5.3 Work Space Trajectory Generation (N-D)Chapter 5 Trajectory Generation

62

⇒ av2 + bv + c = 0where

a = 16 − ∣T0 + T3 ∣2 , b = 12(p3 − p0)T(T0 + T3), c = −36∣p3 − p0 ∣2v has two solutions, one positive, one negative.

p0

p3

p2p1

T0 T3

Chapter5 TrajectoryGeneration

Introductionto TrajectoryGeneration

Joint SpaceTrajectoryGeneration(1-D)

5.3 Work Space Trajectory Generation (N-D)Chapter 5 Trajectory Generation

63

◻ Computation of the interpolating cubic Bezier curve:● Init: Given p0 , . . . , pn ∈ R3

● Step 1: If via point velocity not given, apply five point methodto compute Ti, i = 0, . . . , n

vk = pk − pk−1αk = ∣vk−1 × vk∣∣vk−1 × vk∣ + ∣vk+1 × vk+2∣Tk = Vk∣Vk∣ Vk = (1 − αk)vk + αkvk+1v0 = 2v1 − v2, v−1 = 2v0 − v1

vn+1 = 2vn − vn−1 , vn+2 = 2vn+1 − vn

Chapter5 TrajectoryGeneration

Introductionto TrajectoryGeneration

Joint SpaceTrajectoryGeneration(1-D)

5.3 Work Space Trajectory Generation (N-D)Chapter 5 Trajectory Generation

64

● Step 2: Set u0 = 0, for each pair of via points (pk , pk+1) andtangent vectors (Tk , Tk+1), construct a Bezier curve pk(u), u ∈ [0, 1]satisfying

1 pk(0) = pk , pk(1) = pk+12 pk(0) ∥ Tk , pk(1) ∥ Tk+13 ∣pk(0)∣ = ∣pk( 1

2)∣ = ∣pk(1)∣ = v

using the method shown above. Denote the four control points of kth

segment by qk0 = pk , qk1 , qk2 , qk3 = pk+1 , and set:

uk+1 = uk + 3∣qk1 − qk0∣

Chapter5 TrajectoryGeneration

Introductionto TrajectoryGeneration

Joint SpaceTrajectoryGeneration(1-D)

5.3 Work Space Trajectory Generation (N-D)Chapter 5 Trajectory Generation

64

● Step 2: Set u0 = 0, for each pair of via points (pk , pk+1) andtangent vectors (Tk , Tk+1), construct a Bezier curve pk(u), u ∈ [0, 1]satisfying

1 pk(0) = pk , pk(1) = pk+12 pk(0) ∥ Tk , pk(1) ∥ Tk+13 ∣pk(0)∣ = ∣pk( 1

2)∣ = ∣pk(1)∣ = v

using the method shown above. Denote the four control points of kth

segment by qk0 = pk , qk1 , qk2 , qk3 = pk+1 , and set:

uk+1 = uk + 3∣qk1 − qk0∣Step 3: Scale pk from [0, 1] to [uk , uk+1]

pk(u) ≜ pk( u − ukuk+1 − uk )

Now we have n Bezier segments, each having speed equal to 1 at end

and mid points w.r.t u.

Chapter5 TrajectoryGeneration

Introductionto TrajectoryGeneration

Joint SpaceTrajectoryGeneration(1-D)

5.3 Work Space Trajectory Generation (N-D)Chapter 5 Trajectory Generation

65

◇ Example: Local cubic interpolation in R3

0

2

4

6

00.5

11.5

22.5

30

0.5

1

1.5

2

2.5

3

xy

z

0 1 2 3 4 5−2

−1

0

1

2

3−4

−2

0

2

40.5

1

1.5

2

2.5

p(1

)x

p(1

)y

p(1

)z

u

(a)

0 2 4 6 8 10 12−1

−0.5

0

0.5

1−1

−0.5

0

0.5

10.2

0.4

0.6

0.8

1

p(1

)x

p(1

)y

p(1

)z

u

(b)

[p0 , p1 , . . . , p5] = [ 0 1 2 4 5 60 2 3 3 2 00 1 0 0 0 2

](a): velocity before scaling (discontinuous)(b): velocity after scaling (continuous)

Chapter5 TrajectoryGeneration

Introductionto TrajectoryGeneration

Joint SpaceTrajectoryGeneration(1-D)

5.3 Work Space Trajectory Generation (N-D)Chapter 5 Trajectory Generation

66

◻ Interpolation on SO(3):

♢ Review: Basic properties of SO(3)

No linear structure: previous interpolation methods cannot bedirectly applied

“straight ”line: given R0 ,R1 ∈ SO(3), the geodesic between R0

and R1 is defined by:

exp(R0 ,R1 , t) = R0elog(RT

0 R1)t , t ∈ [0, 1]Parameterization:

1 Matrix rep.: computationally heavy (Gram-Schmidtorthogonalization)

(Continues next slide)

Chapter5 TrajectoryGeneration

Introductionto TrajectoryGeneration

Joint SpaceTrajectoryGeneration(1-D)

5.3 Work Space Trajectory Generation (N-D)Chapter 5 Trajectory Generation

67

2 Euler angles: exists parameterization singularity, dependson charts and thus untidy

3 Unit quaternion: no parameterization singularity,computationally efficient (taking normalization)

4 exponential coordinate: suitable for defining “straight”lineon SO(3)

◻ Bezier curve on SO(3) (Park, etc):♢ “linear”interpolation between R0 ,R1 ∈ SO(3):

R(t) = R0elog(RT

0 R1)t , t ∈ [0, 1](Continues next slide)

Chapter5 TrajectoryGeneration

Introductionto TrajectoryGeneration

Joint SpaceTrajectoryGeneration(1-D)

5.3 Work Space Trajectory Generation (N-D)Chapter 5 Trajectory Generation

68

♢ DeCasteljau algorithm on SO(3):Given n + 1 control points R1

0 ,R11 ,R

12 , . . . ,R

1n ∈ SO(3):

Rki (t) ≜ Rk−1

i elog(Rk−1i

TRk−1i+1 )t , t ∈ [0, 1], k = 2, . . . , n + 1, i = 0, . . . , n − k + 1

The interpolated curve is given by:

R(t) ≜ Rn+10 (t)

◻ SVD approach (Belta, Kumar, etc):

Chapter5 TrajectoryGeneration

Introductionto TrajectoryGeneration

Joint SpaceTrajectoryGeneration(1-D)

5.4 ReferencesChapter 5 Trajectory Generation

69

◻ Reference:[1] Latombe, J.C., Robot motion planning, Springer, 1990.[2] Biagiotti, L. and Melchiorri, C.,Trajectory planning for automatic ma-chines and robots. Springer, 2008.[3] Piegl, L and Tiller, W., The NURBS Book. 2nd ed., Springer, 1996.[4] Juttler, B. and Wagner, M.G., Computer-aided design with spatial Ra-tional B-spline Motions. ASME Journal of mechanical design, Vol.118(2),pp.193-201, 1996.[5] Park, F.C. and Ravani, B., Bezier curves on Riemannian manifolds andLie groups with kinematics applications. ASME Journal of MechanicalDesign, Vol.117, pp.36-40, 1995.[6] Belta, C. and Kumar, V.J.,An SVD-Based Projection Method forInterpolation on SE(3). IEEE Transactions on robotics and automation,Vol.18, pp.334-345, 2002.