Trajectory Generation

Mechanical Engineering Department

Faculty of Mechanical and Aerospace
Engineering
ITB
2017
Trajectory Generation
 Provide a method of computing robot motions required to
perform a high level task
 Must allow a user to describe trajectories at a high level
 Pick A
 Place B
 Describe trajectories in terms of the tool not the joints
 If the user specifies the desired position and orientation
of the tool
 The trajectory generator will calculate the required joint
velocities and accelerations to arrive at the final
destination.
Trajectory Generation Issues
 How do we represent trajectories.
 Do we care about orientation
 How do we store
 Do we care about velocity
 How the path is actually calculated
 Real-time
 In advance
 How they are planned or taught.
 How they can be modified
 If one point changes how does it effect the path.
 What if we need to change the path in real-time (obstacle
avoidance)
 How much computation do we need
Trajectory Generation
 We have a tool frame {T} (current position)
 We have a station frame {S}
 Want to generate accelerations and velocities at the joints to drive
the {T}={S}
 {S} may be changing as a function of time.
 May want to travel through via-points and or avoid obstacles
 Via-Points can be represented as frames.
 This de-couples the planning phase from the particular kinematics
of the robot.
 Cartesian path does not need to be regenerated for different tool.
 Need to check that accelerations or velocities are not to great for
the robot to track.
 Also must consider joint limits
Trajectory Generation
 Spatial
 Concerned with Position
 Just want to get to the Final point or Frame.
 Temporal
 Concerned with Time
 May want to have a certain velocity when reaching the goal or
a via-point.
 Force Trajectories
 Impedance control
 Hybrid
 Track Position along some coordinates
 Track Force along others
Joint Based
 Points Are Taught in Cartesian Space
 Trajectory Generator Converts Points from Cartesian Space to
Joint Space
 Inverse Kinematics
 Between Points Joints are Moved in a Linear item.
 All joints arrive at end of motion at same item
 Joints move at different Rates
 Robot may not follow a straight line
 Cannot follow a path
 Computationally low
 Avoids singularities
Cartesian
 Generates Intermediate Points in
Cartesian Space.
 Then Convert each point to Joint Space.
 Accurate Path following
 Computationally intensive
 Singularities can be an issue.
 Joints may saturate for small Moves
Trajectory Generator
 Cubic Spline
 Quintic Spline
 S curve
 Step Accelerations

15 12

10 10

5 8 A
V
0 6 J
P
0 2 4 6 8 10
-5 4

-10 2

-15 0
Time
 Step J

9 35

8
30
7

6 25
5
J
20
4 A
F(t)

3 V
15
P
2

1 10

0
5
0 2 4 6 8 10
-1

-2 0
Time
How do we Define a Path (Cubic)
 Lets Look at a simple example for 1 Joint or DOF
 We Know  0  ,  f 
 We also Know 0  , f 
 Want to find  t, t  , and t 
 Where 
t    t 
t

t   t 
t
 So we have four Constraints
lets try a Cubic polynomial

 t   a0  a1t  a2t 2  a3t 3

Cubic Continued
 To determine the coefficients we need to look at our boundry conditions.
Namely:
 Position at t=0  t   a0  a1t  a2t 2  a3t 3
 Velocity at t=0 t   a1  2a2t  3a3t 2
 Position at t=final
 Velocity at t=final
 Plugging these in we get.
 0  a0 0  a1

 t f    0  0  t f  a2  t f  a3  t f t f   0  2a2  t f  3a3  t f

2 3 2

 Solving for a2 and a3

a2 
3
2
  f   0   0   f
2 1 a3  
2
3
  f   0  
1
2

  f  0 
tf tf tf tf tf
Cubic Continued
 So the an are
a0   0 a1  0 a2  32   f   0   2 0  1  f
tf tf tf
a3  
2
3
  f   0  
1
2

  f  0 
tf tf
 If the initial and final velocity is zero then
  f   0 
2
  f   0 
3
a0   0 a1  0 a2  2
a3   3
tf tf

 So finally the acceleration is given by

t   2a2  6a3t or t   62   f   0    123   f   0  t

tf tf
Cubic Example
 Given
 0  5 0  0
 Positions

 v   15 
v   17.5
sec
  f   40.0  f   0

 And each segment should last 1 second.

Quintic Polynomial
 Cubics allow us de fine the position and velocity at each location in the
trajectory.
 If we also want to specify the acceleration we would need a Quintic or
order 6 polynomial.

 t   a0  a1t  a2t 2  a3t 3  a4t 4  a5t 5

 This time we would use the initial and final positions, velocities and
accelerations as our boundary conditions to solve for the coefficients.
Linear Functions
 Another alternative is just to just linearly interpolate between the starting
and end position.
 This would use a constant velocity along the whole trajectory.
 However this creates a discontinuity in the velocity at the beginning
and the end.
 This would require infinite acceleration.
 To avoid this parabolic blends are applied at the beginning and end of the
trajectories.
 A simple ramp velocity scheme is often used.
 This results in Step (discontinuities) in the acceleration.

 
  tb  h b
t h  tb
 The velocity of the linear section must equal the velocity at the end of
the first blend and the velocity at the beginning of the second blend.
Linear Functions Cont.
 The position at the end of the blend is given by
1
 b   0    tb
2

2
 Combining these equation we get

  tb    tb  t   f   0  0
2

 If we specify the desired duration t of motion and the acceleration we

can calculate tb
 If the acceleration is not high enough it will take longer than th to
reach full speed.
 Parabolic blends can be used for via points.
 This however results in the via points never being actually
reached.
 If we need to actually hit a via point. Two pseudo via points on
either side of the via point can be added that would result in the
via point being on the new linear section.
 Care must be taken to make sure that adequate accelerations are
selected such that there is sufficient time to get to the linear
sections.
Cartesian Space Schemes
 Even thought Straight line paths are followed in joint space this does not
guarantee that the robot will follow a straight line.
 If we want to describe the path shape followed by the end-effector then
we need to generate our trajectories in Cartesian space.
 Most common is linear others include:
 Circular
 Sinusoidal
 Hyperbolic
 Each point is usually specified in terms of desired position and orientation
of a tool frame.
 Can describe each point as a 6x1 vector.
 Can be planned directly from users definitions of {T} relative to a
given {S}
 Usually specified in 6 DOF, but other sub-spaces could be used.
Cartesian Trajectory Planning
 Trajectories can include via points
 Pass close to but not necessarily pass through
 (knot points in b-splines: graphics)
 E.g. straight line paths connected by via point
 Both position and orientation have to be interpolated

p1

p(t1−τ ) p(t1+τ )

p2
p0
Positional Translation
p1
 Shape of the interpolated
region ? p(t1−τ ) p(t1+τ )
 RH Taylor 1979
 Start at p0, arrive at p1 in time
p2
t1 under constant velocity p0
 End at p2, from at p1 in time t2
under constant velocity
 At time  before arrival at p1
begin the curved transition
thus p(t1- ) and p(t1+ ) are
the two transition points
 The curved segment is a
parabola with constant
acceleration
 The initial conditions p0, p1, p2, t1and t2 are all
specified.
 We now consider the position and velocity
constraints of the transition points
Position Constraints :

p (t1   )  p1  p1 where p1  p1  p 0
t1

p (t1   )  p1  p2 where p 2  p 2  p1
t1
Velocity Constraints :
p1 p
p (t1   )  and p (t1   )  2
t1 t2
constant acceleration p(t)  a p
 Integrating the acceleration equation we get
at 2
using motion under constant acceleration : s  V0t 
2
ap
p (t )  p (t1   )  p (t1   )(t  t1   )  (t  t1   ) 2
2
 Rearrange and express in terms of the position function p(t)

p1 ap
p (t )  p1  t  t1   (t  t1   ) 2
t1 2
 Solve for ap at the second transition point t=t1+

1  p2 p1 
ap    
2  t2 t1 

 Substitute into the equation for p(t)

p1
p (t )  p1  t  t1   2  p2 (t  t1   ) 2
4t1 4t 2
 The solution to the trajectory reduces to:
 t1  t
 p1  p1 0  t  t 
t

 p1 p2
p (t )   p1  t  t1    
2
t  t1   2 t1    t  t1  
 4t1 4t 2
 t1  t
p1  p2 t1    t  t1  t 2
 t
 Note the solution does not pass through point p1
Rotational Transition
 The rotational transition is found by finding
the equivalent axis of rotation k
 R0 is the start orientation, R1 the orientation at
the via point and R2 the orientation at the
goal.
R0 Rk1 1   R1  Rk1 1   R0T R1
R0 Rk2  2   R2  Rk2  2   R1T R2
where angle - axis Rk1 1  transforms R1 to R0
and Rk2  2  transforms R2 to R1
  The rotation about k1 and k2 can be
made a linear function of time
 Rotation along the straight lines

 t1  t 
Rt   R1 Rk1   1 , 0  t  t1  
 t1 
 t  t1 
Rt   R1 Rk2   2 , t1    t  t1  t 2
 t2 
 The rotations can be derived in a similar
way to the positions
 (t1  t   ) 2   (t1  t   ) 2 
Rt   R1 Rk1   1  Rk1    2 , t1    t  t1  
 t1 4   t 2 4 
Cartesian Schemes
 Spline functions similar to those developed for joints can be used between
Cartesian position
 In this case each X, Y, Z, roll, pitch and yaw would each be splined.
 Blend times for each degree of freedom must be the same.
 This will result in different accelerations for each DOF
 Blend times must be chosen to limit accelerations.
 More computationally intensive.
 Must convert each position on the path to joint space.
 This requires inverse kinematics for position
 The inverse Jacobean for velocities
 And the inverse Jacobean plus for accelerations.
Cartesian Path Problems
 Even though a path can be specified in Cartesian space a robot may not
be able to follow it.
 Common problems include
 Unreachable points due to workspace limitations
 Ex. Cant reach center for 2DOF short outer link.

 High Joint rates due to singularities in workspace

 Ex 2DOF with same link lengths.

 Joint Limits
 Ex robot cant pass through dead zero and stay on path.
Extensions to Path Planning
 Dynamics can be used to generate torque
speed curves as a function of position this
can allow higher overall accelerations.
 Real time
 Lookup table
 Obstacle Avoidance
 Gravity Wells
 Distance based modeling
 Dynamically changing workspaces