Professional Documents
Culture Documents
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)
#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
!B - A!
A
x(t)
p(t) = y(t)
z(t)
C- B
..
p(t) = (v2 KBC - v1 KAB)/#T
= KAB
= KBC
!C - B!
$
%
.
p(t) = v1 KAB + (v2 KBC - v1 KAB) t/#T
$
%
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
A complete position/orientation
Cartesian trajectory
" initial given configuration q(0) = (0 " /2 0 0 0 0)
" initial end-effector position p(0) = (0.540 0 1.515)
x(0)!
y(0)!
# 0 0 1&
R(0) = % 0 "1 0(
!
%
(
1
0
0
$
'
linear path
for position
!
" final end-effector position
zw!
axis-angle method
for orientation
p(T) = (0 0.540 1.515)
yw!
z(T)!
# 1 0 0&
R(T) = % 0 0 1(
%
(
!
$ 0 "1 0'
" the final configuration is NOT specified a priori
y(T)!
zw!
yw!
Robotics 1
18
video
L = p final " pinit
= 0.763 [m]
" = r # $ " = #
" = r # $ " = #
triangular
speed profile s(t)
with minimum
time T = 5.52 s!
!
!
#0 0
Rinit = %0 "1
%
$1 0
#1 0
R final = %0 0
%
$0 "1
Robotics 1
1&
T
0( = Rinit
(
0'
0&
1(
(
0'
!
#0 "1 0 &
Rinit R final = %0 0 "1( = Rot(r, )if )
%
(
$1 0 0 '
#1&
1 % (
2*
r=
"1 , )if =
[rad] (= 120)
3
3% 1 (
!
$ '
T
s = s(t),
t " [0, T]
19
triangular
profile for
linear speed
planned
motion of
Cartesian
position
and velocity
T = 5.52 s!
2.5
joint 2
T = 5.52
T = 5.52
1.5
1
[rad]
actual
joint
motion
0.5
0.5
1
0
4
time [s]
T = 5.52
Robotics 1
20
z(0)!
zw!
# 0 0 "1&
% 0 "1 0 (
R(T)
=
!
%
(
$ "1 0 0 '
x(T)!
Robotics 1
x(0)!
p(0)!
z(T)! p(T)!
# 0 0 1&
R(0) = % 0 "1 0(
%
(
!
$ 1 0 0'
xw!
T
21
#0 0 1&
Rinit = %0 "1 0(
%
(
$1 0 0'
# 0 &
) * ZYZ , init = % + /2(
%
(
$ + '
# "1 0 0 &
Rinit R final = % 0 1 0 (
%
(
$ 0 0 "1'
#0&
) r = % "1(, * = +
% (
$0'
T
# 0 0 "1&
R final = % 0 "1 0 (
%
(
$ "1 0 0 '
# "+ &
) * ZYZ , final = % + /2(
%
(
$ 0 '
zw!
yw!
xw!
video
video
using ZYZ Euler angles
Robotics 1
1.8
1.6
1.6
1.4
1.4
1.2
1.2
linear motion
only along the
x-direction
0.8
0.6
0.4
planned
Cartesian
components
of position
and velocity
[m]
[m]
x=
y=
z=
0.8
0.6
0.4
0.2
0.2
0.2
4
time [s]
0.2
T ! 7.2 s!
endeffector velocity
endeffector position
1.8
4
time [s]
T ! 6 s!
endeffector velocity
0.01
0.02
0.02
faster motion
time with the
axis-angle method
(imposed by the
previous bounds
on angular motion)
0.04
0.03
[m/s]
[m/s]
0.04
0.06
0.05
0.08
0.06
0.07
0.1
0.08
0.09
Robotics 1
4
time [s]
0.12
4
time [s]
23
[rad]
[rad]
2=
1=
0=
orientation
in terms of ZYZ
pre-planned
Euler angles
offline
0.5
1.5
0.5
1.5
0.5
2.5
1.5
4
time [s]
4
time [s]
T ! 6 s!
0.5
3
2.5
by postprocessing
T ! 7.2 s!
[rad]
[rad]
1.5
Robotics 1
actual
joint
motion
4
time [s]
1 = 0 (singularity of
the representation)
only three
joints move
4
time [s]
24
for a given path p(s) (in joint or Cartesian space) and a given timing law
s(3) (3=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!
25
Numerical example
!
!
-1
p1
q0
p0
s=s(t)
non-zero
(symmetric)
acceleration
T=1
Robotics 1
26
Numerical example
!
violation of both joint velocity and acceleration bounds with T=1 [s]
!
!
-2
kacc
kvel
= joint 1
Robotics 1
= joint 2
27
Numerical example
!
-3
speed [acceleration] on path and joint velocities [accelerations] scale linearly [quadratically]
Robotics 1
= joint 1
= joint 2
28