Trajectory planning
in Cartesian space
Prof. Alessandro De Luca
Robotics 1
Robotics 1
pi
GIVEN
pi, pf, vmax, amax
vi, vf (typically = 0)
L
pf
L = pf - pi
path parameterization
p(s) = pi + s (pf - pi)
s [0,1]
.
.
dp .
p(s) =
s = (pf - pi) s
ds
pf - pi .
=
L
Robotics 1
pf - pi
..
..
d2p . 2
dp ..
p(s) =
s +
s = (pf - pi) s
ds2
ds
pf - pi ..
=
L
3
amax
t
.
(t)
L
(t)
T-Ts T
vmax
Ts
-1
= area of the
speed profile
vmax
Ts =
amax
L amax + vmax2
T=
amaxvmax
a coast phase exists iff: L > vmax2/amax
amax
amax t2/2
t
(t) =
.
(t)
vmax
t
Robotics 1
T-Ts T
t [0,Ts]
t [Ts,T-Ts]
vmax2
Tamax
t [T-Ts,T]
(t)
Ts
vmax
vmax2
t2 amax
-2
no need to pass
(and stop!) there
B - A
over-fly
B- A
C- B
C - B
= KAB
= KBC
unit vectors of
direction cosines
note: during over-fly, the path remains always in the plane specified
by the two lines intersecting at B (in essence, it is a planar problem)
Robotics 1
.
x(t)
v2 KBC,x
v1 KAB,x
..
y(t)
..
z(t)
.
y(t)
v2 KBC,y
v1 KAB,y
.
z(t)
v1 KAB,z
v2 KBC,z
t
T
Robotics 1
t
7
B- A
C
C- B
B - A
x(t)
p(t) = y(t)
z(t)
..
p(t) = 1/T (v2 KBC - v1 KAB)
= KAB
= KBC
C - B
.
p(t) = v1 KAB + t/T (v2 KBC - v1 KAB)
unit vectors of
direction cosines
thus, we obtain a
parabolic blending
(see textbook
for this same approach
in the joint space)
Solution
(various options)
d1
B - A= d1 KAB
d2
C - B = d2 KBC
by choosing, e.g., d1
(namely A)
Robotics 1
d2 = v2 T/2
T = 2d1/v1
d2 = d1 v2/v1
9
A numerical example
transition from A=(3,3) to C=(8,9) via B=(1,9), with speed from v1=1 to v2=2
exploiting two options for solution (resulting in different paths!)
assign transition time: T=4 (we re-center it here for t [-T/2, T/2])
assign distance from B for departing: d1=3 (assign d2 for landing is handled similarly)
T=4
Robotics 1
d1=3
10
A numerical example
(contd)
actually, the same vel/acc profiles only with a different time scale!!
Robotics 1
11
Alternative solution
(imposing acceleration)
B
..
p(t) = 1/T (v2 KBC - v1 KAB)
C
A
C
v1 = v2 = vmax (for simplicity)
..
p(t) = amax
12
Application example
plan a Cartesian trajectory from A to C (rest-to-rest)
that avoids the obstacle O, with a amax and v vmax
add a via point B
sufficiently far from O
B
C
A
C
C
A
on AA amax
Robotics 1
on AB and BC vmax
on CC - amax
+ over-fly between A e C
13
Robotics 1
b(s)
n(s)
t(s)
t(s) = p(s)/p(s)
unit tangent vector
n(s) = p(s)/p(s)
unit normal vector
( osculating plane)
b(s) = t(s) n(s)
unit binormal vector
Robotics 1
15
Optimal trajectories
Robotics 1
16
xA
yA
zB
xB
B
yB
zA
determine the (neutral) axis r and the angle AB: R(r,AB) = RAT RB (rotation
matrix changing the orientation from A to B inverse axis-angle problem)
plan a timing law (t) for the (scalar) angle interpolating 0 with AB (with
possible constraints/boundary conditions on its time derivatives)
t, RAR(r,(t)) specifies then the actual end-effector orientation at time t
Robotics 1
17
for a given path p(s) (in joint or Cartesian space) and a given timing law
s() (=t/T, T=motion time), we need to check if existing bounds vmax
on (joint) velocity and/or amax on (joint) acceleration are violated or not
unless such constraints have already been taken into account during the
trajectory planning, e.g., by using a bang-coast-bang acceleration timing law
Robotics 1
in both cases, after scaling, there will be (at least) one instant of saturation
(for at least one variable)
no need to re-compute motion profiles from scratch after the scaling!
18
Numerical example
-1
p1
q0
p0
s=s(t)
non-zero
(symmetric)
acceleration
T=1
Robotics 1
19
Numerical example
violation of both joint velocity and acceleration bounds with T=1 [s]
-2
= joint 1
Robotics 1
= joint 2
20
Numerical example
-3
speed [acceleration] on path and joint velocities [accelerations] scale linearly [quadratically]
Robotics 1
= joint 1
= joint 2
21