Download - [Download] rev chapter-5-june26th
![Page 1: [Download] rev chapter-5-june26th](https://reader033.vdocuments.us/reader033/viewer/2022052505/5550971fb4c9058b208b46a9/html5/thumbnails/1.jpg)
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
![Page 2: [Download] rev chapter-5-june26th](https://reader033.vdocuments.us/reader033/viewer/2022052505/5550971fb4c9058b208b46a9/html5/thumbnails/2.jpg)
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)
![Page 3: [Download] rev chapter-5-june26th](https://reader033.vdocuments.us/reader033/viewer/2022052505/5550971fb4c9058b208b46a9/html5/thumbnails/3.jpg)
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 .
![Page 4: [Download] rev chapter-5-june26th](https://reader033.vdocuments.us/reader033/viewer/2022052505/5550971fb4c9058b208b46a9/html5/thumbnails/4.jpg)
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)
![Page 5: [Download] rev chapter-5-june26th](https://reader033.vdocuments.us/reader033/viewer/2022052505/5550971fb4c9058b208b46a9/html5/thumbnails/5.jpg)
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.
![Page 6: [Download] rev chapter-5-june26th](https://reader033.vdocuments.us/reader033/viewer/2022052505/5550971fb4c9058b208b46a9/html5/thumbnails/6.jpg)
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
![Page 7: [Download] rev chapter-5-june26th](https://reader033.vdocuments.us/reader033/viewer/2022052505/5550971fb4c9058b208b46a9/html5/thumbnails/7.jpg)
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
![Page 8: [Download] rev chapter-5-june26th](https://reader033.vdocuments.us/reader033/viewer/2022052505/5550971fb4c9058b208b46a9/html5/thumbnails/8.jpg)
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)
![Page 9: [Download] rev chapter-5-june26th](https://reader033.vdocuments.us/reader033/viewer/2022052505/5550971fb4c9058b208b46a9/html5/thumbnails/9.jpg)
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)
![Page 10: [Download] rev chapter-5-june26th](https://reader033.vdocuments.us/reader033/viewer/2022052505/5550971fb4c9058b208b46a9/html5/thumbnails/10.jpg)
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
![Page 11: [Download] rev chapter-5-june26th](https://reader033.vdocuments.us/reader033/viewer/2022052505/5550971fb4c9058b208b46a9/html5/thumbnails/11.jpg)
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)
![Page 12: [Download] rev chapter-5-june26th](https://reader033.vdocuments.us/reader033/viewer/2022052505/5550971fb4c9058b208b46a9/html5/thumbnails/12.jpg)
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
![Page 13: [Download] rev chapter-5-june26th](https://reader033.vdocuments.us/reader033/viewer/2022052505/5550971fb4c9058b208b46a9/html5/thumbnails/13.jpg)
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
![Page 14: [Download] rev chapter-5-june26th](https://reader033.vdocuments.us/reader033/viewer/2022052505/5550971fb4c9058b208b46a9/html5/thumbnails/14.jpg)
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
![Page 15: [Download] rev chapter-5-june26th](https://reader033.vdocuments.us/reader033/viewer/2022052505/5550971fb4c9058b208b46a9/html5/thumbnails/15.jpg)
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
![Page 16: [Download] rev chapter-5-june26th](https://reader033.vdocuments.us/reader033/viewer/2022052505/5550971fb4c9058b208b46a9/html5/thumbnails/16.jpg)
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)
![Page 17: [Download] rev chapter-5-june26th](https://reader033.vdocuments.us/reader033/viewer/2022052505/5550971fb4c9058b208b46a9/html5/thumbnails/17.jpg)
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).
![Page 18: [Download] rev chapter-5-june26th](https://reader033.vdocuments.us/reader033/viewer/2022052505/5550971fb4c9058b208b46a9/html5/thumbnails/18.jpg)
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 †
![Page 19: [Download] rev chapter-5-june26th](https://reader033.vdocuments.us/reader033/viewer/2022052505/5550971fb4c9058b208b46a9/html5/thumbnails/19.jpg)
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]
![Page 20: [Download] rev chapter-5-june26th](https://reader033.vdocuments.us/reader033/viewer/2022052505/5550971fb4c9058b208b46a9/html5/thumbnails/20.jpg)
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
![Page 21: [Download] rev chapter-5-june26th](https://reader033.vdocuments.us/reader033/viewer/2022052505/5550971fb4c9058b208b46a9/html5/thumbnails/21.jpg)
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
![Page 22: [Download] rev chapter-5-june26th](https://reader033.vdocuments.us/reader033/viewer/2022052505/5550971fb4c9058b208b46a9/html5/thumbnails/22.jpg)
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)
![Page 23: [Download] rev chapter-5-june26th](https://reader033.vdocuments.us/reader033/viewer/2022052505/5550971fb4c9058b208b46a9/html5/thumbnails/23.jpg)
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
![Page 24: [Download] rev chapter-5-june26th](https://reader033.vdocuments.us/reader033/viewer/2022052505/5550971fb4c9058b208b46a9/html5/thumbnails/24.jpg)
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)
![Page 25: [Download] rev chapter-5-june26th](https://reader033.vdocuments.us/reader033/viewer/2022052505/5550971fb4c9058b208b46a9/html5/thumbnails/25.jpg)
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)
![Page 26: [Download] rev chapter-5-june26th](https://reader033.vdocuments.us/reader033/viewer/2022052505/5550971fb4c9058b208b46a9/html5/thumbnails/26.jpg)
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
![Page 27: [Download] rev chapter-5-june26th](https://reader033.vdocuments.us/reader033/viewer/2022052505/5550971fb4c9058b208b46a9/html5/thumbnails/27.jpg)
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)
![Page 28: [Download] rev chapter-5-june26th](https://reader033.vdocuments.us/reader033/viewer/2022052505/5550971fb4c9058b208b46a9/html5/thumbnails/28.jpg)
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)
![Page 29: [Download] rev chapter-5-june26th](https://reader033.vdocuments.us/reader033/viewer/2022052505/5550971fb4c9058b208b46a9/html5/thumbnails/29.jpg)
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
![Page 30: [Download] rev chapter-5-june26th](https://reader033.vdocuments.us/reader033/viewer/2022052505/5550971fb4c9058b208b46a9/html5/thumbnails/30.jpg)
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)
![Page 31: [Download] rev chapter-5-june26th](https://reader033.vdocuments.us/reader033/viewer/2022052505/5550971fb4c9058b208b46a9/html5/thumbnails/31.jpg)
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)
![Page 32: [Download] rev chapter-5-june26th](https://reader033.vdocuments.us/reader033/viewer/2022052505/5550971fb4c9058b208b46a9/html5/thumbnails/32.jpg)
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)
![Page 33: [Download] rev chapter-5-june26th](https://reader033.vdocuments.us/reader033/viewer/2022052505/5550971fb4c9058b208b46a9/html5/thumbnails/33.jpg)
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)
![Page 34: [Download] rev chapter-5-june26th](https://reader033.vdocuments.us/reader033/viewer/2022052505/5550971fb4c9058b208b46a9/html5/thumbnails/34.jpg)
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.
![Page 35: [Download] rev chapter-5-june26th](https://reader033.vdocuments.us/reader033/viewer/2022052505/5550971fb4c9058b208b46a9/html5/thumbnails/35.jpg)
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
![Page 36: [Download] rev chapter-5-june26th](https://reader033.vdocuments.us/reader033/viewer/2022052505/5550971fb4c9058b208b46a9/html5/thumbnails/36.jpg)
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)
![Page 37: [Download] rev chapter-5-june26th](https://reader033.vdocuments.us/reader033/viewer/2022052505/5550971fb4c9058b208b46a9/html5/thumbnails/37.jpg)
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]
![Page 38: [Download] rev chapter-5-june26th](https://reader033.vdocuments.us/reader033/viewer/2022052505/5550971fb4c9058b208b46a9/html5/thumbnails/38.jpg)
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)}
![Page 39: [Download] rev chapter-5-june26th](https://reader033.vdocuments.us/reader033/viewer/2022052505/5550971fb4c9058b208b46a9/html5/thumbnails/39.jpg)
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
![Page 40: [Download] rev chapter-5-june26th](https://reader033.vdocuments.us/reader033/viewer/2022052505/5550971fb4c9058b208b46a9/html5/thumbnails/40.jpg)
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)
![Page 41: [Download] rev chapter-5-june26th](https://reader033.vdocuments.us/reader033/viewer/2022052505/5550971fb4c9058b208b46a9/html5/thumbnails/41.jpg)
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!)
![Page 42: [Download] rev chapter-5-june26th](https://reader033.vdocuments.us/reader033/viewer/2022052505/5550971fb4c9058b208b46a9/html5/thumbnails/42.jpg)
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)
![Page 43: [Download] rev chapter-5-june26th](https://reader033.vdocuments.us/reader033/viewer/2022052505/5550971fb4c9058b208b46a9/html5/thumbnails/43.jpg)
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
![Page 44: [Download] rev chapter-5-june26th](https://reader033.vdocuments.us/reader033/viewer/2022052505/5550971fb4c9058b208b46a9/html5/thumbnails/44.jpg)
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
![Page 45: [Download] rev chapter-5-june26th](https://reader033.vdocuments.us/reader033/viewer/2022052505/5550971fb4c9058b208b46a9/html5/thumbnails/45.jpg)
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.
![Page 46: [Download] rev chapter-5-june26th](https://reader033.vdocuments.us/reader033/viewer/2022052505/5550971fb4c9058b208b46a9/html5/thumbnails/46.jpg)
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.
![Page 47: [Download] rev chapter-5-june26th](https://reader033.vdocuments.us/reader033/viewer/2022052505/5550971fb4c9058b208b46a9/html5/thumbnails/47.jpg)
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
![Page 48: [Download] rev chapter-5-june26th](https://reader033.vdocuments.us/reader033/viewer/2022052505/5550971fb4c9058b208b46a9/html5/thumbnails/48.jpg)
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
![Page 49: [Download] rev chapter-5-june26th](https://reader033.vdocuments.us/reader033/viewer/2022052505/5550971fb4c9058b208b46a9/html5/thumbnails/49.jpg)
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)
![Page 50: [Download] rev chapter-5-june26th](https://reader033.vdocuments.us/reader033/viewer/2022052505/5550971fb4c9058b208b46a9/html5/thumbnails/50.jpg)
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)
![Page 51: [Download] rev chapter-5-june26th](https://reader033.vdocuments.us/reader033/viewer/2022052505/5550971fb4c9058b208b46a9/html5/thumbnails/51.jpg)
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 (∗)
![Page 52: [Download] rev chapter-5-june26th](https://reader033.vdocuments.us/reader033/viewer/2022052505/5550971fb4c9058b208b46a9/html5/thumbnails/52.jpg)
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]
![Page 53: [Download] rev chapter-5-june26th](https://reader033.vdocuments.us/reader033/viewer/2022052505/5550971fb4c9058b208b46a9/html5/thumbnails/53.jpg)
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
![Page 54: [Download] rev chapter-5-june26th](https://reader033.vdocuments.us/reader033/viewer/2022052505/5550971fb4c9058b208b46a9/html5/thumbnails/54.jpg)
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)
![Page 55: [Download] rev chapter-5-june26th](https://reader033.vdocuments.us/reader033/viewer/2022052505/5550971fb4c9058b208b46a9/html5/thumbnails/55.jpg)
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)
![Page 56: [Download] rev chapter-5-june26th](https://reader033.vdocuments.us/reader033/viewer/2022052505/5550971fb4c9058b208b46a9/html5/thumbnails/56.jpg)
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)
![Page 57: [Download] rev chapter-5-june26th](https://reader033.vdocuments.us/reader033/viewer/2022052505/5550971fb4c9058b208b46a9/html5/thumbnails/57.jpg)
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)
![Page 58: [Download] rev chapter-5-june26th](https://reader033.vdocuments.us/reader033/viewer/2022052505/5550971fb4c9058b208b46a9/html5/thumbnails/58.jpg)
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
⎤⎥⎥⎥⎥⎥⎥⎦
![Page 59: [Download] rev chapter-5-june26th](https://reader033.vdocuments.us/reader033/viewer/2022052505/5550971fb4c9058b208b46a9/html5/thumbnails/59.jpg)
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)
![Page 60: [Download] rev chapter-5-june26th](https://reader033.vdocuments.us/reader033/viewer/2022052505/5550971fb4c9058b208b46a9/html5/thumbnails/60.jpg)
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)
![Page 61: [Download] rev chapter-5-june26th](https://reader033.vdocuments.us/reader033/viewer/2022052505/5550971fb4c9058b208b46a9/html5/thumbnails/61.jpg)
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 †
![Page 62: [Download] rev chapter-5-june26th](https://reader033.vdocuments.us/reader033/viewer/2022052505/5550971fb4c9058b208b46a9/html5/thumbnails/62.jpg)
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))
![Page 63: [Download] rev chapter-5-june26th](https://reader033.vdocuments.us/reader033/viewer/2022052505/5550971fb4c9058b208b46a9/html5/thumbnails/63.jpg)
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.
![Page 64: [Download] rev chapter-5-june26th](https://reader033.vdocuments.us/reader033/viewer/2022052505/5550971fb4c9058b208b46a9/html5/thumbnails/64.jpg)
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)
![Page 65: [Download] rev chapter-5-june26th](https://reader033.vdocuments.us/reader033/viewer/2022052505/5550971fb4c9058b208b46a9/html5/thumbnails/65.jpg)
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)
![Page 66: [Download] rev chapter-5-june26th](https://reader033.vdocuments.us/reader033/viewer/2022052505/5550971fb4c9058b208b46a9/html5/thumbnails/66.jpg)
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)
![Page 67: [Download] rev chapter-5-june26th](https://reader033.vdocuments.us/reader033/viewer/2022052505/5550971fb4c9058b208b46a9/html5/thumbnails/67.jpg)
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
![Page 68: [Download] rev chapter-5-june26th](https://reader033.vdocuments.us/reader033/viewer/2022052505/5550971fb4c9058b208b46a9/html5/thumbnails/68.jpg)
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
![Page 69: [Download] rev chapter-5-june26th](https://reader033.vdocuments.us/reader033/viewer/2022052505/5550971fb4c9058b208b46a9/html5/thumbnails/69.jpg)
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)
![Page 70: [Download] rev chapter-5-june26th](https://reader033.vdocuments.us/reader033/viewer/2022052505/5550971fb4c9058b208b46a9/html5/thumbnails/70.jpg)
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
![Page 71: [Download] rev chapter-5-june26th](https://reader033.vdocuments.us/reader033/viewer/2022052505/5550971fb4c9058b208b46a9/html5/thumbnails/71.jpg)
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)
![Page 72: [Download] rev chapter-5-june26th](https://reader033.vdocuments.us/reader033/viewer/2022052505/5550971fb4c9058b208b46a9/html5/thumbnails/72.jpg)
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
![Page 73: [Download] rev chapter-5-june26th](https://reader033.vdocuments.us/reader033/viewer/2022052505/5550971fb4c9058b208b46a9/html5/thumbnails/73.jpg)
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)
![Page 74: [Download] rev chapter-5-june26th](https://reader033.vdocuments.us/reader033/viewer/2022052505/5550971fb4c9058b208b46a9/html5/thumbnails/74.jpg)
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
. . . . . .
![Page 75: [Download] rev chapter-5-june26th](https://reader033.vdocuments.us/reader033/viewer/2022052505/5550971fb4c9058b208b46a9/html5/thumbnails/75.jpg)
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)
![Page 76: [Download] rev chapter-5-june26th](https://reader033.vdocuments.us/reader033/viewer/2022052505/5550971fb4c9058b208b46a9/html5/thumbnails/76.jpg)
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
![Page 77: [Download] rev chapter-5-june26th](https://reader033.vdocuments.us/reader033/viewer/2022052505/5550971fb4c9058b208b46a9/html5/thumbnails/77.jpg)
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
![Page 78: [Download] rev chapter-5-june26th](https://reader033.vdocuments.us/reader033/viewer/2022052505/5550971fb4c9058b208b46a9/html5/thumbnails/78.jpg)
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
![Page 79: [Download] rev chapter-5-june26th](https://reader033.vdocuments.us/reader033/viewer/2022052505/5550971fb4c9058b208b46a9/html5/thumbnails/79.jpg)
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)
![Page 80: [Download] rev chapter-5-june26th](https://reader033.vdocuments.us/reader033/viewer/2022052505/5550971fb4c9058b208b46a9/html5/thumbnails/80.jpg)
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)
![Page 81: [Download] rev chapter-5-june26th](https://reader033.vdocuments.us/reader033/viewer/2022052505/5550971fb4c9058b208b46a9/html5/thumbnails/81.jpg)
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)
![Page 82: [Download] rev chapter-5-june26th](https://reader033.vdocuments.us/reader033/viewer/2022052505/5550971fb4c9058b208b46a9/html5/thumbnails/82.jpg)
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
![Page 83: [Download] rev chapter-5-june26th](https://reader033.vdocuments.us/reader033/viewer/2022052505/5550971fb4c9058b208b46a9/html5/thumbnails/83.jpg)
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
![Page 84: [Download] rev chapter-5-june26th](https://reader033.vdocuments.us/reader033/viewer/2022052505/5550971fb4c9058b208b46a9/html5/thumbnails/84.jpg)
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)
![Page 85: [Download] rev chapter-5-june26th](https://reader033.vdocuments.us/reader033/viewer/2022052505/5550971fb4c9058b208b46a9/html5/thumbnails/85.jpg)
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
![Page 86: [Download] rev chapter-5-june26th](https://reader033.vdocuments.us/reader033/viewer/2022052505/5550971fb4c9058b208b46a9/html5/thumbnails/86.jpg)
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)
![Page 87: [Download] rev chapter-5-june26th](https://reader033.vdocuments.us/reader033/viewer/2022052505/5550971fb4c9058b208b46a9/html5/thumbnails/87.jpg)
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
![Page 88: [Download] rev chapter-5-june26th](https://reader033.vdocuments.us/reader033/viewer/2022052505/5550971fb4c9058b208b46a9/html5/thumbnails/88.jpg)
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 ∣
![Page 89: [Download] rev chapter-5-june26th](https://reader033.vdocuments.us/reader033/viewer/2022052505/5550971fb4c9058b208b46a9/html5/thumbnails/89.jpg)
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)
![Page 90: [Download] rev chapter-5-june26th](https://reader033.vdocuments.us/reader033/viewer/2022052505/5550971fb4c9058b208b46a9/html5/thumbnails/90.jpg)
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)
![Page 91: [Download] rev chapter-5-june26th](https://reader033.vdocuments.us/reader033/viewer/2022052505/5550971fb4c9058b208b46a9/html5/thumbnails/91.jpg)
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
![Page 92: [Download] rev chapter-5-june26th](https://reader033.vdocuments.us/reader033/viewer/2022052505/5550971fb4c9058b208b46a9/html5/thumbnails/92.jpg)
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
![Page 93: [Download] rev chapter-5-june26th](https://reader033.vdocuments.us/reader033/viewer/2022052505/5550971fb4c9058b208b46a9/html5/thumbnails/93.jpg)
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
![Page 94: [Download] rev chapter-5-june26th](https://reader033.vdocuments.us/reader033/viewer/2022052505/5550971fb4c9058b208b46a9/html5/thumbnails/94.jpg)
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
![Page 95: [Download] rev chapter-5-june26th](https://reader033.vdocuments.us/reader033/viewer/2022052505/5550971fb4c9058b208b46a9/html5/thumbnails/95.jpg)
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
![Page 96: [Download] rev chapter-5-june26th](https://reader033.vdocuments.us/reader033/viewer/2022052505/5550971fb4c9058b208b46a9/html5/thumbnails/96.jpg)
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
![Page 97: [Download] rev chapter-5-june26th](https://reader033.vdocuments.us/reader033/viewer/2022052505/5550971fb4c9058b208b46a9/html5/thumbnails/97.jpg)
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
![Page 98: [Download] rev chapter-5-june26th](https://reader033.vdocuments.us/reader033/viewer/2022052505/5550971fb4c9058b208b46a9/html5/thumbnails/98.jpg)
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
![Page 99: [Download] rev chapter-5-june26th](https://reader033.vdocuments.us/reader033/viewer/2022052505/5550971fb4c9058b208b46a9/html5/thumbnails/99.jpg)
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)
![Page 100: [Download] rev chapter-5-june26th](https://reader033.vdocuments.us/reader033/viewer/2022052505/5550971fb4c9058b208b46a9/html5/thumbnails/100.jpg)
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
♢
![Page 101: [Download] rev chapter-5-june26th](https://reader033.vdocuments.us/reader033/viewer/2022052505/5550971fb4c9058b208b46a9/html5/thumbnails/101.jpg)
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
![Page 102: [Download] rev chapter-5-june26th](https://reader033.vdocuments.us/reader033/viewer/2022052505/5550971fb4c9058b208b46a9/html5/thumbnails/102.jpg)
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∣
![Page 103: [Download] rev chapter-5-june26th](https://reader033.vdocuments.us/reader033/viewer/2022052505/5550971fb4c9058b208b46a9/html5/thumbnails/103.jpg)
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.
![Page 104: [Download] rev chapter-5-june26th](https://reader033.vdocuments.us/reader033/viewer/2022052505/5550971fb4c9058b208b46a9/html5/thumbnails/104.jpg)
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)
![Page 105: [Download] rev chapter-5-june26th](https://reader033.vdocuments.us/reader033/viewer/2022052505/5550971fb4c9058b208b46a9/html5/thumbnails/105.jpg)
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)
![Page 106: [Download] rev chapter-5-june26th](https://reader033.vdocuments.us/reader033/viewer/2022052505/5550971fb4c9058b208b46a9/html5/thumbnails/106.jpg)
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)
![Page 107: [Download] rev chapter-5-june26th](https://reader033.vdocuments.us/reader033/viewer/2022052505/5550971fb4c9058b208b46a9/html5/thumbnails/107.jpg)
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):
![Page 108: [Download] rev chapter-5-june26th](https://reader033.vdocuments.us/reader033/viewer/2022052505/5550971fb4c9058b208b46a9/html5/thumbnails/108.jpg)
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.