TRAJECTORY PLANNING - RIS · INDUSTRIAL ROBOTICS Prof. Bruno SICILIANO TRAJECTORY PLANNING • generation of reference inputs to motion control system Path and trajectory Joint space

INDUSTRIAL ROBOTICS Prof. Bruno SICILIANO

TRAJECTORY PLANNING

• generation of reference inputs to motion control system

Path and trajectory

Joint space trajectories

Operational space trajectories


PATH AND TRAJECTORY

⋆ generation of suitably smooth trajectories

• Path: locus of points in joint space (operational space) whichthe manipulator has to follow in the execution of the assignedmotion (pure geometric description)

• Trajectory: path on which a timing law is specified (e.g.velocities and/or accelerations)

• Trajectory planningalgorithm

⋆ inputs

path description

path constraints

constraints imposed by manipulator dynamics

⋆ outputs

joint (end-effector) trajectories in terms of time sequence ofvalues attained by position, velocity and acceleration


• Reduced number of parameters

⋆ path

extremal points

possible intermediate points

geometric primitives

⋆ timing law

total time

max velocity and/or acceleration

velocity and/or acceleration at given points

• Operational space trajectories

⋆ natural task description

⋆ path constraints

⋆ singularities

⋆ redundancy

• Joint space trajectories

⋆ inverse kinematics

⋆ control action


JOINT SPACE TRAJECTORIES

• Generation of functionq(t) interpolating the given vectorsof joint variables at each point, in respect of the imposedconstraints

⋆ the generated trajectories should be not very demandingfrom a computational viewpoint

⋆ the joint positions and velocities (and accelerations) shouldbe continuous functions of time

⋆ undesirable effects should be minimized (e.g., nonsmoothtrajectories)

• Point-to-point motion

⋆ initial and final points, and traveling time

• Motion through a sequence of points

⋆ initial and final points, intermediate points, and travelingtimes


Point-to-point motion

⋆ generation ofq(t) describing motion fromqi to qf in a timetf

• Cubic polynomial

q(t) = a3t3 + a2t

2 + a1t + a0

q(t) = 3a3t2 + 2a2t + a1

q(t) = 6a3t + 2a2

⋆ computation of coefficients

a0 = qi

a1 = qi

a3t3f + a2t

2f + a1tf + a0 = qf

3a3t2f + 2a2tf + a1 = qf

• Quintic polynomial (initial and final accelerations)

q(t) = a5t5 + a4t

4 + a3t3 + a2t

2 + a1t + a0


• Example

0 0.2 0.4 0.6 0.8 1

0

1

2

3

pos

[s]

[rad

]

0 0.2 0.4 0.6 0.8 1

0

1

2

3

4

5vel

[s]

[rad

/s]

0 0.2 0.4 0.6 0.8 1

−20

−10

0

10

20

acc

[s]

[rad

/s^2

]


• Trapezoidal velocity profile

qctc =qm − qc

tm − tc

qc = qi +1

2qct

2c

qct2c − qctf tc + qf − qi = 0


• qc specified (sgn qc = sgn (qf − qi))

tc =tf2

−1

2

√

t2f qc − 4(qf − qi)

qc

|qc| ≥4|qf − qi|

t2f

⋆ trajectory

q(t) =

qi + 1

2qct

2 0 ≤ t ≤ tc

qi + qctc(t − tc/2) tc < t ≤ tf − tc

qf − 1

2qc(tf − t)2 tf − tc < t ≤ tf


• Example

0 0.2 0.4 0.6 0.8 1

0

1

2

3

pos

[s]

[rad

]

0 0.2 0.4 0.6 0.8 1

0

1

2

3

4

5vel

[s]

[rad

/s]

0 0.2 0.4 0.6 0.8 1

−20

−10

0

10

20

acc

[s]

[rad

/s^2

]


• qc specified

|qf − qi|

tf< |qc| ≤

2|qf − qi|

tf

tc =qi − qf + qctf

qc

qc =q2c

qi − qf + qctf


Motion through a sequence of points

⋆ opportunity to assign intermediate points (sequence ofpoints)

• For givenN path points, find an interpolating function throughthese points

⋆ N − 1 order polynomial

it is not possible to assign initial and final velocities

as the order of polynomial increases, its oscillatorybehaviour increases (not natural trajectories)

numerical accuracy for computation of polynomialcoefficients decreases as order increases

the resulting system of constraint equations is heavy to solve

polynomial coefficients depend on all the assigned points=⇒ if it is desired to change a point, all of them have tobe recomputed


• Sequence of low-order (cubic)interpolating polynomialscontinuous at path points

⋆ arbitrary values ofq(t) are imposed at path points

⋆ the values ofq(t) at path points are assigned according to acertain criterion

⋆ accelerationq(t) has to be continuous at path points

• Interpolating polynomials of order less than three whichdetermine trajectories passing nearby path points at giveninstants of time


• Interpolating polynomials with imposed velocities at pathpoints

Πk(tk) = qk

Πk(tk+1) = qk+1

Πk(tk) = qk

Πk(tk+1) = qk+1

⋆ continuity of velocity at path points

Πk(tk+1) = Πk+1(tk+1)


• Example

0 2 4 6

0

2

4

6

pos

[s]

[rad

]

0 2 4 6

−5

0

5

vel

[s]

[rad

/s]

0 2 4 6

−40

−20

0

20

acc

[s]

[rad

/s^2

]


• Interpolating polynomials with computed velocities at pathpoints

q1 = 0

qk =

{

0 sgn (vk) 6= sgn (vk+1)1

2(vk + vk+1) sgn (vk) = sgn (vk+1)

qN = 0

vk = (qk − qk−1)/(tk − tk−1)


• Example

0 2 4 6

0

2

4

6

pos

[s]

[rad

]

0 2 4 6

−5

0

5

vel

[s]

[rad

/s]

0 2 4 6

−30

−20

−10

0

10

20

30

acc

[s]

[rad

/s^2

]


• Interpolating polynomials with continuous accelerationsat pathpoints (splines)

Πk−1(tk) = qk

Πk−1(tk) = Πk(tk)



⋆ 4N−2 equations in4(N−1) unknown coefficients (fourth-order polynomials for first and last segment?)

⋆ 2 virtual points (continuity on position, velocity andacceleration) =⇒ N + 1 cubic polynomials


⋆ 4(N − 2) equations for theN − 2 intermediate path points:

Πk−1(tk) = qk




⋆ 6 equations for the initial and final points

Π1(t1) = qi

Π1(t1) = qi

Π1(t1) = qi

ΠN+1(tN+2) = qf

ΠN+1(tN+2) = qf

ΠN+1(tN+2) = qf

⋆ 6 equations for the virtual points




⇓

⋆ System of4(N +1) equations for computation of4(N +1)coefficients ofN + 1 cubic polynomials


⋆ computationally efficient algorithm

Πk(t) =Πk(tk)

∆tk(tk+1 − t) +

Πk(tk+1)

∆tk(t − tk)

Πk(t) =Πk(tk)

6∆tk(tk+1 − t)3 +

Πk(tk+1)

6∆tk(t − tk)3

+

(

Πk(tk+1)

∆tk−

∆tkΠk(tk+1)

6

)

(t − tk)

+

(

Πk(tk)

∆tk−

∆tkΠk(tk)

6

)

(tk+1 − t)

⋆ 4 unknowns:Πk(tk), Πk(tk+1), Πk(tk) e Πk(tk+1)


⋆ N variablesqk for k 6= 2, N + 1 specified

⋆ continuity onq2 andqN+1

⋆ continuity onqk for k = 3, . . . , N

⋆ qi andqf specified

⋆ continuity onqk for k = 2, . . . , N + 1

⋆ qi andqf specified

⇓

Π1(t2) = Π2(t2)

...

ΠN (tN+1) = ΠN+1(tN+1)

⋆ system of linear equations

A [ Π2(t2) . . . ΠN+1(tN+1) ]T

= b

A =

a11 a12 . . . 0 0a21 a22 . . . 0 0

......

. . ....

...0 0 . . . aN−1,N−1 aN−1,N

0 0 . . . aN,N−1 aNN


• Example

0 1 2 3 4 5

0

2

4

6

pos

[s]

[rad

]

0 1 2 3 4 5

−5

0

5

vel

[s]

[rad

/s]

0 1 2 3 4 5

−30

−20

−10

0

10

20

30

acc

[s]

[rad

/s^2

]


• Interpolating linear polynomials with parabolic blends

qk−1,k =qk − qk−1

∆tk−1

qk =qk,k+1 − qk−1,k

∆t′k


• Example

0 1 2 3 4 5

0

2

4

6

pos

[s]

[rad

]

0 1 2 3 4 5−6

−4

−2

0

2

4

vel

[s]

[rad

/s]

0 1 2 3 4 5

−40

−20

0

20

40acc

[s]

[rad

/s^2

]


• application of trapezoidal velocity profile law to interpolationproblem

0 0.5 1 1.5 2

0

1

2

3

4

5

pos

[s]

[rad

]

0 0.5 1 1.5 2

0

1

2

3

4

vel

[s]

[rad

/s]

0 0.5 1 1.5 2

−20

−10

0

10

20

acc

[s]

[rad

/s^2

]


OPERATIONAL SPACE TRAJECTORIES

• Sequence of path points

⋆ inverse knematics

⋆ joint space trajectories

⋆ microinterpolation

• Path pointsx(tk)

⋆ componentsxi(tk) interpolated with sequence of polynomials

• Path primitives

⋆ analytical description of motion


Path primitives

• Parametric description of path in space

p = f(s)

t =dp

ds

n =1

∥

∥

∥

∥

d2p

ds2

∥

∥

∥

∥

d2p

ds2

b = t × n


• Rectilinear path

p(s) = pi +s

‖pf − pi‖(pf − pi)

dp

ds=

1


d2p

ds2= 0


• Circular path

p′(s) =

ρ cos (s/ρ)ρ sin (s/ρ)

0

p(s) = c + Rp′(s)

dp

ds= R

−sin (s/ρ)cos (s/ρ)

0

d2p

ds2= R

−cos (s/ρ)/ρ−sin (s/ρ)/ρ

0


Position and orientation trajectories

• Positionp = f(s)

⋆ s(t) interpolating polynomial

p = sdp

ds= st

⋆ segment

p(s) = pi +s


p =s

‖pf − pi‖(pf − pi) = st

p =s

‖pf − pi‖(pf − pi) = st

⋆ circle

p(s) = c + R

ρ cos (s/ρ)ρ sin (s/ρ)

0

p = R

−s sin (s/ρ)s cos (s/ρ)

0

p = R

−s2cos (s/ρ)/ρ − s sin (s/ρ)−s2sin (s/ρ)/ρ + s cos (s/ρ)

0


• Orientation

⋆ interpolation of components ofn(t), s(t), a(t) ?

⋆ interpolation ofφ(t)

φ(s) = φi +s

‖φf − φi‖(φf − φi)

φ =s


φ =s


⋆ angle and axis (Rf = RiRif )

Rif = RT

i Rf =

r11 r12 r13

r21 r22 r23

r31 r32 r33

ϑf = cos−1

(

r11 + r22 + r33 − 1

2

)

r =1

2 sinϑf

r32 − r23

r13 − r31

r21 − r12

Ri(t): Ri(0) = I Ri(tf ) = Rif

R(t) = RiRir(ϑ(t))

ϑ(0) = 0 ϑ(tf ) = ϑf

Documents

TRAJECTORY PLANNING - RIS · INDUSTRIAL ROBOTICS Prof. Bruno SICILIANO TRAJECTORY PLANNING • generation of reference inputs to motion control system Path and trajectory Joint space