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 − µ
6µ
⇒ 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.