Professional Documents
Culture Documents
Trajectory planning
Prof. Alessandro De Luca
Robotics 1
external sensors
task planner*
trajectory planner*
* = programming points
control*
robot
internal sensors
TRAJECTORY
PLANNER
reference profile/values
(continuous or discrete)
for the robot controller
Trajectory definition
. .B
D.
.C
a) over-fly A
b) sensor-driven STOP
c) circular path
through 3 points
main drawbacks
Robotics 1
TRAJECTORY
= +
of interaction Fd(t)
GEOMETRIC PATH
parameterized by s: p=p(s)
(e.g., s is the arc length)
p(s(t))
TIMING LAW
.
A
PATH
B
0
px(s)
p(s) = py(s)
pz(s)
.
t
TIME
PARAMETER smax
Trajectory planning
operative sequence
analytic
inversion
TASK planning
1
sequence of pose points (knots) in Cartesian space
interpolation in Cartesian space
Cartesian geometric path (position + orientation): p = p(s)
path sampling and kinematic inversion
2
sequence of knots in joint space
interpolation in joint space
geometric path in joint space: q = q()
Example
.. ...... ..
...... .. ..
q1()
.
.
.
.
.
.
.
.
.
.
..
..
..
..
.
....... . . ..
.
.
.
.
.
.
.
.
..
q1
q2
p(s)
q2()
q3
q3()
A
Cartesian space
Robotics 1
joint space
6
Robotics 1
. ..
q e q (or higher-order derivatives) may also be needed
Cartesian task specifications involve the geometric path,
but also bounds on the associated timing law
for redundant robots, choice among n-m inverse solutions,
based on optimality criteria or additional auxiliary tasks
off-line planning in advance is not always feasible
e.g., when interaction with the environment occurs or
sensor-based motion is needed
7
p = p(s)
s = s(t)
(Cartesian space)
q = q() = (t)
(joint space)
if s(t) = t, path parameterization is the natural one given by time
p(t) =
dp .
s
ds
..
p(t) =
dp ..
d2p .2
s+
s
ds
ds2
8
Trajectory classification
space of definition
task type
timing law
path geometry
Cartesian, joint
coordinated or independent
or
Robotics 1
Relevant characteristics
Robotics 1
10
11
Cubic polynomial
q(0) = qin q(T) = qfin
.
.
q(0) = vin q(T) = vfin
4 conditions
q = qfin - qin
q() = qin + q [a 3 + b 2 + c + d]
4 coefficients
= t/T, [0, 1]
doubly normalized polynomial qN()
qN(0) = 0 d = 0
qN(0) = dqN/d|=0 = c = vinT/q
special case: vin = vfin
qN(1) = 1 a + b + c = 1
qN(1) = dqN/d|=1 = 3a + 2b + c
= vfinT/q
= 0 (rest-to-rest)
qN(0) = 0 c = 0
qN(1) = 1 a + b = 1
qN(1) = 0 3a + 2b = 0
Robotics 1
a=-2
b=3
12
Quintic polynomial
q() = a 5 + b 4 + c 3 + d 2 + e + f
6 coefficients
= t/T, [0, 1]
allows to satisfy 6 conditions, for example (in normalized time )
q(0) = q0 q(1) = q1
Robotics 1
q = q1 - q0
13
4-3-4 polynomials
.
.
t0
initial
t1
t2
depart
approach
tf
final
boundary conditions
q(t0) = q0 q(t1-) = q(t1+) = q1
.
.
q(t0) = q(tf) = 0
. . +
q(ti ) = q(ti )
Robotics 1
6 passages
q(t2-) = q(t2+) = q2 q(tf) = qf
..
..
4 initial/final
q(t0) = q(tf) = 0
velocity/acceleration
.. .. +
4 continuity
q(ti ) = q(ti ) i = 1,2
14
Higher-order polynomials
Robotics 1
15
Numerical examples
9th
degree
4 derivatives
are zero
14 derivatives
are zero!
2.5
Robotics 1
normalized
velocity
29th
degree
no
overshoot
nor
wandering
4.5!!
velocity
peaking
at midpoint
16
problem
interpolate N knots, with continuity up to the second derivative
solution
spline: N-1 cubic polynomials, concatenated so as to pass through N
knots and being continuous in velocity and acceleration in the N-2
internal knots
4(N-1) coefficients
4(N-1)-2 conditions, or
2(N-1) of passage (for each cubic, in the two knots at its ends)
N-2 of continuity for velocity (at the internal knots)
N-2 of continuity for acceleration (at the internal knots)
Robotics 1
17
Building a spline
q = (t) = {K(t), t [tk, tk + hk]}
q(t)
q1
v1
t1
qk
t2
tk
q2
qk+1
tk+1
qN-1
vN
qN
tN-1
tN
time intervals hk
An efficient algorithm
1. if all velocities vk at internal knots were known, then each cubic in the spline
would be uniquely determined by
K(0) = qK = aK0
K(0) = vK = aK1
hK2
hK3
aK2
2hK
3hK2
aK3
qK+1 - qK - vK hK
vK+1 - vK
..
..
K(hk) = 2 aK2 + 6 aK3 hK = K+1(0) = 2 aK+1,2
3. expressing the coefficients ak2, ak3, ak+1,2 in terms of the still unknown knot
velocities (see step 1.) yields a linear system of equations that is always (easily)
solvable
A(h)
tri-diagonal matrix
always invertible
Robotics 1
v2
v3
:
vN-1
unknown
b(h,q,v1,vN)
known vector
Structure of A(h)
2(h1+h2)
h3
h1
2(h2+h3)
h2
...
hN-2
2(hN-3+hN-2)
hN-1
hN-3
2(hN-2+hN-1)
20
Structure of b(h,q,v1,vN)
3
[h12(q3 - q2) + h22(q2 - q1)] - h2v1
h1h2
hN-3hN-2
3
[h22(q4 - q3) + h32(q3 - q2)]
h2h3
3
[hN-22(qN - qN-1) + hN-12(qN-1 - qN-2)] - hN-2vN
hN-2hN-1
Robotics 1
21
Properties of splines
hk = tN - t1
when initial and final accelerations are also assigned, the spline
construction can be suitably modified
Robotics 1
22
A modification
Robotics 1
23
A numerical example
boundary accelerations 1 = 4 = 0
two placements: at t1 = 0.5 and t4 = 4.5 (), or t1 = 1.5 and t4 = 3.5 ()
= placement
Robotics 1
= placement
24