Trajectory Planning

1

Reference:

- M. W. Spong, S. Hutchinson, M. Vidayasagar, Robot Modeling and

Control, Wiley, 2006 (Chap. 5).

Path Planning

� Path planning:

Assuming known initial and final configurations of the robot, to find a

collision-free path connecting these two configurations.

� A configuration of a manipulator:

� A combination of positions and orientations of all links

� ≡ a combination of joint variables

� ≡ a unique pose (position and orientation) of the end-effecter.

Obstacle avoidance is a main issue in path planning.

2

� Obstacle avoidance is a main issue in path planning.

� Path planning usually leads to a search or an optimization problem that

needs to be solved in real-time.

� Accelerations or velocities obtained from path planning may be

unachievable.

xy

z

A

B

Trajectory Planning

� Trajectory planning:

Designing the motions of the joints through the time to generate a desired

movement.

� The trajectory planner computes a joint function q(t) that specifies the motion

of the robot as it traverses the path.

� Trajectory planning versus path planning:

� Path planning provides a geometric description of the motion; does not

specify any dynamic aspect of the motion.

3

specify any dynamic aspect of the motion.

� In trajectory planning, the velocities and accelerations of the joints are also

designed.

� Trajectory planning is more general than path planning (path + velocity planning)

Trajectory planning through via points

� Definition of a trajectory:

� A function of time q(t) such that q(t0)= qs at the initial time and q(tf)= qf at

the final time.

� qs and qf are joint variables at the starting and final configurations of robot.

� Via points:

� Many path planning algorithms do not give a total path from the starting

point to the final point, but give only a sequence of points along the path

4

point to the final point, but give only a sequence of points along the path

(so-called via-points).

� Many industrial robots work in teach and playback mode:

� The robot is placed manually in several positions along the desired path and

the positions are saved.

� Then, as robot operates, it goes through these via-points.

� The joint angles are recorded at via-points (inverse kinematic is not needed)

� A trajectory with several via-points can be considered as several

trajectories with point-to-point motions.

� We first consider point-to-point trajectory planning.

Segmented Motions

� Possible segments of a manipulator motion:

� Guarded motion: Lower speed at the start and the end of motion to avoid

obstacles,

� Free motion: Higher speeds in free spaces between the two guarded

segments.

5

Trajectories for Point to Point Motion

� Trajectory planning problem:

� To find a trajectory that connects initial and final configurations while

satisfying extra constraints such as velocity and acceleration constraints at

the endpoints.

� Single joint trajectory planning:

� Each joint is planned independently, so it is a sample for more general

cases.

)( ,)( vtqqtq == &

6

� Initial joint constraints at t = t0 :

� Final joint constraints at t = tf :

� In this chapter, we get familiar with the below trajectory planning methods:

� Cubic Polynomial Trajectory: Initial and final positions and velocities are

assigned.

� Quintic (fifth-order) Polynomial Trajectory: Initial and final positions, velocities

and accelerations are assigned.

� Linear Segments with Parabolic Blends: A trapezoidal velocity profile is

generated.

� Minimum Time Trajectory: Maximum acceleration and deceleration to

minimize the time.

0000 )( ,)( vtqqtq == &

ffff vtqqtq == )( ,)( &

Cubic Polynomial Planning

� To present q(t) as a third-order polynomial of time:

to satisfy the position and velocity constraints at the start and end points:

� Joint velocity:

,)( ,)( 0000 vtqqtq == &

3

3

2

210)( tatataatq +++=

ffff vtqqtq == )( ,)( &

2

321 32)( tataatq ++=&

7

� 4 equations obtained from 4 constraints:

321 32)( tataatq ++=&

3

03

2

020100 tatataaq +++=

3

3

2

210 ffff tatataaq +++=

2

030210 32 tataav ++=

2

321 32 fff tataav ++=

=

⇒

f

f

ff

fff

v

q

v

q

a

a

a

a

tt

ttt

tt

ttt

0

0

3

2

1

0

2

32

2

00

3

0

2

00

3210

1

3210

1

T

=

⇒ −

f

f

v

q

v

q

T

a

a

a

a

0

0

1

3

2

1

0

)(tq⇒

An Example for Cubic Polynomial Planning

� Initial and final time:

� Position and velocity constraints (zero end velocities):

� Plots for the values:

sec,deg/ 0)( ,)( 0000 === vtqqtq &

sec1 ,00 == ftt

sec,deg/ 0)( ,)( === ffff vtqqtq &

oo 20 ,100 −== fqq

3

0

2

00 )(2)(3)( tqqtqqqtq ff −−−+=⇒

8

� Positions and velocities are continuous at the start and final points, but

accelerations are discontinuous!

� Discontinuity in acceleration results leads to impulsive jerk, which may

excite vibration modes of the manipulator. It also causes non-smooth

motions.

Quintic Polynomial Planning

� To present q(t) as a fifth-order polynomial of time:

to satisfy the position, velocity and acceleration constraints at the start and

end points:

� 6 equations obtained from 6 constraints:

000000 )( ,)( ,)( α=== tqvtqqtq &&&

5

5

4

4

3

3

2

210)( tatatatataatq +++++=

ffffff tqvtqqtq α=== )( ,)( ,)( &&&

9

T

=

⇒ −

f

f

f

v

q

v

q

T

a

a

a

a

a

a

α

α0

0

0

1

5

4

3

2

1

0

)(tq⇒

=

f

f

f

fff

ffff

fffff

v

q

v

q

a

a

a

a

a

a

ttt

tttt

ttttt

ttt

tttt

ttttt

α

α0

0

0

5

4

3

2

1

0

32

432

5432

3

0

2

00

4

0

3

0

2

00

5

0

4

0

3

0

2

00

20126200

543210

1

20126200

543210

1

An Example for Quintic Polynomial Planning

� Initial and final time:

� Position and velocity constraints (zero end velocities and accelerations):

, 0 ,0 000 === αvq

sec2 ,00 == ftt

, 0 ,40 === fff vq αo

10

� Positions, velocities and accelerations are continuous all the times.

Linear Segments with Parabolic

Blends: Trapezoidal Velocity Profile

� Trapezoidal Velocity Profile

� The trajectory has 3 parts:

1. Ramp up: Increasing velocity to a desired value

with a constant acceleration.

2. Constant velocity: Keeping the desired velocity.

3. Ramp down: Decreasing velocity to zero with a

constant deceleration.

11

� The joint angles will be parabolic (second-order)

in time during Parts 1 and 3 and will be linear

during Part 2.

� The parabolic and linear segments blend in

smoothly as their slopes (velocities) are equal at

the blending time.

Derivation of Linear Segments with Parabolic

Blends (LSPB) Equations

� Part 1 (Acceleration Part):

� Time interval: [0, tb] (tb : blending time)

� Constraints:

2

210)( tataatq ++=

0)0( ;)0( 0 == qqq & 0 , 100 ==⇒ aqa

12

� Trajectory planning equation:

)or ()( ωVtq b =&22

2

α==⇒

bt

Va

2

0

2

022

)( tt

Vqtqtq

b

+=+=α

Derivation of LSPB Equations (Cont.)

� Part 2 (Constant Velocity):

� Time interval: [tb, tf -tb] (tf : final time)

� From symmetry of trajectory in interval [0, tf ]:

)()()( bb ttVtqtq −+=

22

0 ff qqtq

+=

13

� Joint position constraint at t = tb :

� From Part 1:

� Equating q(tb) from the above two equations:

� Substituting from above in q(t) :

22

2)

2()(

0 f

b

f

b

qqt

tVtq

+=−+⇒

b

ff

b VtVtqq

tq +−+

=⇒2

)(0

bb

b

b tV

qtt

Vqtq

22)( 0

2

0 +=+=

V

Vtqqt

ff

b

+−= 0

( ) VtVtqqtq ff +−+= 2/)()( 0

Derivation of LSPB Equations (Cont.)

� Some velocity and time constraints:

� Solving for velocity:

2/0 fb tt <<

V

Vtqqt

ff

b

+−= 0 V

qqt

V

qq f

f

f )(2 00 −≤<

−⇒

2/0 fb tt <<qqqq )(2 −−

14

� Part 3 (Deceleration Part):

� Time interval: [tf -tb, tf]

� Similar symmetry and joint position constraint

at t = tf -tb , leads to:

2/0 fb tt <<

V

Vtqqt

ff

b

+−= 0

f

f

f

f

t

qqV

t

qq )(2 00 −≤<

−⇒

2

210)( tataatq ++=

( ) 22 )2/()()2/()( ttttqtq fff ααα −+−=

Minimum Time Trajectory

(Bang-Bang Trajectory)

� Triangular Velocity Profile

� The trajectory has 2 symmetric parts:

1. Ramp up: A constant acceleration part (+α) up to the switching time ts =tf /2.

2. Ramp down: A constant deceleration part (-α) from the switching time to the final time.

� The acceleration switches between to positive

15

and negative values (bang-bang trajectory)

� The velocities and the joint angles are

continuous. ts

Minimum Time Trajectory Equations

� Part 1 (Acceleration Part):

� Time interval: [0, ts] (ts = tf /2)

� Velocity at the switching time:

2

002

1)( ttvqtq α++=

ss tvv α+= 0

s

f

s

saves

t

qq

t

qqvv

00 )(22

−=

−==

16

� Part 2 (Deceleration Part):

� Time interval: [ts, tf]

� For zero initial and final velocity cases:

2)(2

1)()( ssss ttttvqtq −−−+= α

2

0 )(2

1ss tqq α+=

αα00 )(2 qqqq

tfs

s

−=

−=

ss tt

qs-q0

qf -q0

Trajectories for Paths Specified by Via Points

� Trajectory planning problem:

� Planning a trajectory that passes through a sequence of configurations (via

points).

� Two general approaches:

1. Imposing constraints on the position of each via point, and extra velocity

and acceleration constraints at the initial and final points.

� In this approach, a system of equations need to be solved.

17

� The number of equations increases as the number of via points increases and

the method becomes intractable.

� A polynomial of higher degree is obtained for q(t) as the number of via points increases.

2. Using interpolating polynomials with lower degree that connect the

trajectory segments between two adjacent via points.

� In this approach, the velocity and/or acceleration constraints must be satisfied

at the via points.

� The interpolating (or blending) polynomials are switched as the robot goes

through the via points.

Cubic Spline Trajectories

� Cubic trajectory planning for each segment between two adjacent

via points:

� Initial joint constraints at t = t0 :

� Final joint constraints at t = tf :

� Cubic trajectory:

� Using the above constraints, the coefficients are computed:

0000 )( ,)( vtqqtq == &

11 )( ,)( vtqqtq ff == &

3

03

2

02010 )()()()( ttattattaatq −+−+−+=

18

� Using the above constraints, the coefficients are computed:

� When switching to the next segment, the final joint constraint of the

previous step is used as the initial joint constraints for the next segment.

0100 , vaqa ==

2

0

01001

2)(

))(2()(3

tt

ttvvqqa

f

f

−

−+−−=

3

0

01010

3)(

))(()(2

tt

ttvvqqa

f

f

−

−+−−=

Example:

A Three Segment Cubic Spline Trajectory

� A spline trajectory with zero velocity constraints at the switching time:

19

� Accelerations are discontinuous at the switching time!

� A spline trajectory with zero velocity and acceleration constraints at

the switching time: