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
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
video
Robotics 1
11
12
q(1) = q1
q(!) = q0 +
4 coefficients
#q [a !3
q(0) = v0 q(1) = v1
+
b !2
4 conditions
#q = q1 q0
+ c ! + d]
! $ [0,1]
qN(0) = 0 % d = 0
qN(1) = 1 % a + b + c = 1
a = -2
b=3
13
.
.
q(0) = vin q(T) = vfin
4 conditions
#q = qfin - qin
qN(0) = 0 % d = 0
qN(1) = 1 % a + b + c = 1
a = -2
b=3
14
Quintic polynomial
q(&) = a &5 + b &4 + c &3 + d &2 + e & + f
6 coefficients
& $ [0, 1]
allows to satisfy 6 conditions, for example (in normalized time & = t/T)
q(0) = q0 q(1) = q1
Robotics 1
#q = q1 - q0
15
Higher-order polynomials
!
Robotics 1
16
Numerical examples
9th
degree
4 derivatives
are zero
14 derivatives
are zero!
2.5
Robotics 1
normalized
first derivative
(velocity
in time)
29th
degree
no
overshoot
nor
wandering
4.5!!
peaking
at midpoint
17
4-3-4 polynomials
three phases (Lift off, Travel, Set down) in a pick-and-place operation in time
qf
q2
q1
q0
.
.
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
18
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 up to the second derivative at 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 first derivative (at the internal knots)
! N-2 of continuity for second derivative (at the internal knots)
!
Robotics 1
19
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)
22
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
23
Properties of splines
!
a spline (in space) is the solution with minimum curvature among all
interpolating functions having continuous second derivative
for cyclic tasks (q1 = qN), it is preferable to simply impose continuity of
first and second derivatives (i.e., velocity and acceleration in time) at
the first/last knot as squaring conditions
!
!
!
)k
hk = tN - t1
Robotics 1
24
A modification
Robotics 1
25
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 ()
o
o
o
o !
= placement
Robotics 1
= placement
26