[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





2


1 Introduction to Trajectory Generation

2 Joint Space Trajectory Generation (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 .





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)





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.





4


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





4


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





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






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)





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





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)






8

● Performance requirements:

1 minimal time trajectory (achieved with maximalvelocity/acceleration)

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

3 Least square error





8





● 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





8





● 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





9

◻ Classification of Trajectories:Trajectory

One-dimensional Multi-dimensional

Point-to-pointPoint-to-point MultipointMultipoint

InterpolationInterpolation ApproximationApproximation





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






10

◻ Interpolation v.s. approximation:

interpolation approximation

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





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 †




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]





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





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





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)





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





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






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]






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





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






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:






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





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]






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






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






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 (∗∗)






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.





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





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]






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]





29

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

basis function: {Bi,n(t)}





29


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

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





29


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)





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!)





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)





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





31



Solutions:

1 interpolation by polynomial functions

2 interpolation by splines (piecewise polynomial)

3 approximation by smoothing splines





31



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.





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.





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





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





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






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

⎤⎥⎥⎥⎥⎦ ,






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 (∗)





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]





38



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





38



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






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)





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)





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 − µ

6µ

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

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





41

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

A =⎡⎢⎢⎢⎢⎢⎢⎣


⎤⎥⎥⎥⎥⎥⎥⎦





41

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

A =⎡⎢⎢⎢⎢⎢⎢⎣


⎤⎥⎥⎥⎥⎥⎥⎦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)





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






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 †




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))





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.





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






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)





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.






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





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





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)





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





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)





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





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)





52

B0,0

B0,1

B1,1

B0,2

B1,2

B2,2

B0,3

B1,3

B2,3

B3,3

. . . . . .





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)






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





53

p(u) = 3∑i=0

Bi,3(u)pi

u = 23


p0

p1 p2

p3

p1,0

p1,1

p1,2





53

p(u) = 3∑i=0

Bi,3(u)pi

u = 23


p0

p1 p2

p3

p1,0

p1,1

p1,2

p2,0

p2,1





53

p(u) = 3∑i=0

Bi,3(u)pi

u = 23


p0

p1 p2

p3

p1,0

p1,1

p1,2

p2,0

p2,1p3,0

⇒ sequentially taking linear interpolation (Continues next slide)





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)





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)





55

◻ Imposing continuity condition:

b

b b

b

p10

p11 p12p13 = P2

0

u = 0 u = u1u′ = u − 0

u1 − 0





55


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





55


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)





56

♢ Imposing C0 continuity

p13 = p20 , p23 = p30





56


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)





56


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





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 ∣





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






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






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





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





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





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)





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

♢





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





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∣





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.





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)





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)






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)





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):




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.

[Download] rev chapter-5-june26th

Education

Transcript of [Download] rev chapter-5-june26th