View
235
Download
1
Category
Preview:
Citation preview
INDUSTRIAL ROBOTICS Prof. Bruno SICILIANO
TRAJECTORY PLANNING
• generation of reference inputs to motion control system
Path and trajectory
Joint space trajectories
Operational space trajectories
INDUSTRIAL ROBOTICS Prof. Bruno SICILIANO
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
INDUSTRIAL ROBOTICS Prof. Bruno SICILIANO
• 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
INDUSTRIAL ROBOTICS Prof. Bruno SICILIANO
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
INDUSTRIAL ROBOTICS Prof. Bruno SICILIANO
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
INDUSTRIAL ROBOTICS Prof. Bruno SICILIANO
• 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
]
INDUSTRIAL ROBOTICS Prof. Bruno SICILIANO
• Trapezoidal velocity profile
qctc =qm − qc
tm − tc
qc = qi +1
2qct
2c
qct2c − qctf tc + qf − qi = 0
INDUSTRIAL ROBOTICS Prof. Bruno SICILIANO
• 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
INDUSTRIAL ROBOTICS Prof. Bruno SICILIANO
• 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
]
INDUSTRIAL ROBOTICS Prof. Bruno SICILIANO
• qc specified
|qf − qi|
tf< |qc| ≤
2|qf − qi|
tf
tc =qi − qf + qctf
qc
qc =q2c
qi − qf + qctf
INDUSTRIAL ROBOTICS Prof. Bruno SICILIANO
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
INDUSTRIAL ROBOTICS Prof. Bruno SICILIANO
• 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
INDUSTRIAL ROBOTICS Prof. Bruno SICILIANO
• 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)
INDUSTRIAL ROBOTICS Prof. Bruno SICILIANO
• 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
]
INDUSTRIAL ROBOTICS Prof. Bruno SICILIANO
• 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)
INDUSTRIAL ROBOTICS Prof. Bruno SICILIANO
• 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
]
INDUSTRIAL ROBOTICS Prof. Bruno SICILIANO
• Interpolating polynomials with continuous accelerationsat pathpoints (splines)
Πk−1(tk) = qk
Πk−1(tk) = Πk(tk)
Πk−1(tk) = Πk(tk)
Π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
INDUSTRIAL ROBOTICS Prof. Bruno SICILIANO
⋆ 4(N − 2) equations for theN − 2 intermediate path points:
Πk−1(tk) = qk
Πk−1(tk) = Πk(tk)
Πk−1(tk) = Πk(tk)
Πk−1(tk) = Πk(tk)
⋆ 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
Πk−1(tk) = Πk(tk)
Πk−1(tk) = Πk(tk)
Πk−1(tk) = Πk(tk)
⇓
⋆ System of4(N +1) equations for computation of4(N +1)coefficients ofN + 1 cubic polynomials
INDUSTRIAL ROBOTICS Prof. Bruno SICILIANO
⋆ 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)
INDUSTRIAL ROBOTICS Prof. Bruno SICILIANO
⋆ 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
INDUSTRIAL ROBOTICS Prof. Bruno SICILIANO
• 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
]
INDUSTRIAL ROBOTICS Prof. Bruno SICILIANO
• Interpolating linear polynomials with parabolic blends
qk−1,k =qk − qk−1
∆tk−1
qk =qk,k+1 − qk−1,k
∆t′k
INDUSTRIAL ROBOTICS Prof. Bruno SICILIANO
• 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
]
INDUSTRIAL ROBOTICS Prof. Bruno SICILIANO
• 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
]
INDUSTRIAL ROBOTICS Prof. Bruno SICILIANO
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
INDUSTRIAL ROBOTICS Prof. Bruno SICILIANO
Path primitives
• Parametric description of path in space
p = f(s)
t =dp
ds
n =1
∥
∥
∥
∥
d2p
ds2
∥
∥
∥
∥
d2p
ds2
b = t × n
INDUSTRIAL ROBOTICS Prof. Bruno SICILIANO
• Rectilinear path
p(s) = pi +s
‖pf − pi‖(pf − pi)
dp
ds=
1
‖pf − pi‖(pf − pi)
d2p
ds2= 0
INDUSTRIAL ROBOTICS Prof. Bruno SICILIANO
• 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
INDUSTRIAL ROBOTICS Prof. Bruno SICILIANO
Position and orientation trajectories
• Positionp = f(s)
⋆ s(t) interpolating polynomial
p = sdp
ds= st
⋆ segment
p(s) = pi +s
‖pf − pi‖(pf − pi)
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
INDUSTRIAL ROBOTICS Prof. Bruno SICILIANO
• Orientation
⋆ interpolation of components ofn(t), s(t), a(t) ?
⋆ interpolation ofφ(t)
φ(s) = φi +s
‖φf − φi‖(φf − φi)
φ =s
‖φf − φi‖(φf − φi)
φ =s
‖φf − φi‖(φf − φi)
⋆ 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
Recommended