Robot Technology: Ch.7 Trajectory Planning of Robots

Robot Technology
Ch.7 Trajectory Planning of Robots
Prof. Wang Zhongmin

[email protected]
2014
1
7.1 General Considerations in Robot Trajectory Planning
7.2 Interpolated Calculation of Joint Trajectories
7.3 Planning of Cartesian Path Trajectories
7.4 Real Time Generation of Planning Trajectories
7.5 Summary
Ch.7 Trajectory Planning of Robots 2

7.5 Summary

7.1 General Considerations in Trajectory
Planning
Basic Problem:
Move the manipulator arm from some initial position to some
desired final position (May be going through some via points).
7.1 General considerations 4

7.1 General Considerations in Trajectory Planning
Trajectory : Time history of position, velocity and acceleration

for each DOF
Path points : Initial, final and via points
Constraints: Spatial, time, smoothness

General Considerations - Solution Space
Joint space （ smooth ）

Easy to go through via points
(Solve inverse kinematics at all path points)
No problems with singularities （奇点）
Less calculations
Can not follow straight line
Cartesian space
We can track a shape
(for orientation : equivalent axes, Euler angles,…)
More expensive at run time
(after the path is calculated need joint angles in a lot of points)
Discontinuity problems

General Considerations - Solution Space
Cartesian planning difficulties:
Initial (A) and

Goal (B) Points are
reachable, but
intermediate points (C)
unreachable.


7.5 Summary

7.2 Interpolated Calculation of Joint
Trajectories
Joint-Space Schemes
Each path point is "converted" into a set of desired joint
angles by application of the inverse kinematics.
A smooth function is found for each of the n joints which

pass through the via points and end at the goal point.
Time required for each segment is the same for each joint.
The determination of the desired joint angle function for a

particular joint is independent with other joints.
7.2 Joint–Space Schemes 9
Joint-Space Schemes
Choice of interpolation function is not unique!
Several possible path shapes for a single joint.

Joint-Space Schemes
Some possible interpolation functions:

Cubic polynomials
Cubic polynomials for a path with via points
Higher-order polynomials
Linear function with parabolic blends （抛物线）
Linear function with parabolic blends for a path with
via points

7.2.1 Cubic Polynomials
In making a single smooth motion, at least four constraints

on  t  are evident:
 ( 0)   0 

 (t f )   f  (7.1)
 (0)  0 
  (7.2)
 (t f )  0
Combining the four constraints yields four equations with

four unknowns:
 0  a0 
2 3
 f  a0  a1t f  a t  a t 
2 f 3 f
 (7.5)
0  a1 
0  a1  2a2t f  3a3t 2f 

a0   0 
a1  0 

3 
 a2  2 ( f   0 )  (7.6)
tf 
2 
a3   3 ( f   0 ) 
tf  7.2 Joint–Space Schemes 13
These four constraints uniquely specify a particular cubic:

 (t )  a 0  a1t  a 2 t 2  a3t 3 (7.3)
The joint velocity and acceleration along this path are:
 (t )  a1  2a 2 t  3a3t 2 
 (7.4)
(t )  2a 2  6a3t 

Eg. 7.1 A single-link robot with a rotary joint is motionless at θ

= 15 degrees. It is desired to move the joint in a smooth manner
to θ= 75 degrees in 3 seconds. Find the coefficients of a cubic
which accomplishes this motion and brings the manipulator to
rest at the goal. Plot the position, velocity, and acceleration of
the joint as a function of time.

Solution: Plugging θ0 =15 ， θf =75 ， tf = 3 into (7.6), we find

a0   0  15 
a1  0 

3 
a2  2 ( f   0 )  20 
tf 
2 
a3   3 ( f   0 )  4.44 
tf 
 (t )  a0  a1t  a2t 2  a3t 3  15  20t 2  4.44t 3


  (t )  a1  2a2t  3a3t 2  40t  13.33t 2
 
 (t )  2a2  6a3t  40  26.66t
Solution:  (t )  a0  a1t  a2t 2  a3t 3  15  20t 2  4.44t 3
Starts at 15 degrees and ends at 75 degrees!

Solution:  (t )  a1  2a2t  3a3t 2  40t  13.33t 2
Starts and ends at rest!

Solution: (t )  2a2  6a3t  40  26.66t
Acceleration profile is linear!

7.2.2 Cubic polynomials with via points
If we come to rest at each point

use formula from previous slide
or continuous motion (no stops)
need velocities at intermediate points:
Initial Conditions: Solutions:
 (0)   0  a0   0 
   
 (t f )   f  a1  0 
 0  a0  3 2  1  
 a2  2 ( f   0 )   0   f 
f  a0  a1t f  a2t 2f  a3t 3f  tf tf tf 
 2 1   
0  a1  a3   3 ( f   0 )  2 ( 0   f ) 
 f  a1  2a2t f  3a3t 2f  tf tf 

7.2.2 Cubic polynomials with via points
How to specify velocity at the via points:

The user specifies the desired velocity at each via point in
terms of a Cartesian linear and angular velocity of the tool
frame at that instant.
The system automatically chooses the velocities at the via
points by applying a suitable heuristic （启发） in either
Cartesian space or joint space (average of 2 sides etc.).
The system automatically chooses the velocities at the via
points in such a way as to cause the acceleration at the via
points to be continuous.

7.2.3 Higher-order polynomials
Higher order polynomials are sometimes used for

path segments. For example, if we wish to be able to
specify the position, velocity, and acceleration at the
beginning and end of a path segment, a quintic
polynomial （五次多项式） is required:
 (t )  a0  a1t  a 2 t 2  a3t 3  a 4 t 4  a5 t 5 (7.10)

Where the constraints are given as:
 0  a0 

f  a0  a1t f  a2t 2f  a3t 3f  a4t 4f  a5t 5f 
  a1 
0 
 (7.11)
 f  a1  2a2t f  3a3t 2f  4a4t 3f  5a5t 4f 
0  2a 2 

f  2a2  6a3t f  12a4t 2f  20a5t 3f 


Solution to these equations:

a0   0 

a  
1 0 
0 
a2  
2 
20 f  20 0  (8 f  120 )t f  (30  f )t 2f 
a3 
2t 3f  (7.12)

30 0  30 f  (14 f  160 )t f  (30  2f )t f 
2
a4  
2t 4f 

12 f  12 0  (6 f  60 )t f  (0  f )t 2f 
a5 
2t 5f 


7.2.4 Linear function with parabolic blends
Linear interpolation (Straight line):

Note: Although the motion of each joint in this scheme is
linear, the end-effector in general does not move in a straight
line in space.
Discontinuous velocity - can not be controlled!

To create a smooth path with continous position and

velocity, we start with the linear function but add a parabolic
blend （抛物线混合） region at each path point.
Constant acceleration is used during the blend portion to
change velocity smoothly.

Assume that the parabolic blends both have the same

duration, and therefore the same constant acceleration
(modulo a sign).
There are many solutions to the problem-but the answer is
always symmetric about the halfway point.

The velocity at the end of the blend region must equal the
velocity of the linear section:
   h  b
 tb  tb  (7.13)
t h  tb

Let t=2th ， combining (7.13) and (7.14)

t b2  tt b  ( f   0 )  0 (7.15)
 2t 2  4(   )

t f 0
tb   (7.16)
2 2
 The acceleration chosen must be sufficiently high, to ensure

the existence of a solution:
4( f   0 )
  2
(7.17)
t

for a path with via points
Below shows a set of joint space via points for some joints.
Linear functions connect the via points, and parabolic blend
regions are added around each via point.
Multi-segment linear path with blends.

Given:
positions
desired time durations
the magnitudes of the accelerations
Compute:
blends times
straight segment times
slopes (velocities)
signed accelerations

Inside segment:
 k  j 
 jk  
t djk 
k  sgn( kl   jk ) k 
 kl   jk 
tk  
k 
1 1 
t jk  t djk  t j  t k
2 2 

First segment:
1  sgn( 2  1 ) 1 


2 2( 2   1 ) 
t1  t d 12  t d 12  

1 

  
12  2 1 
1
t d 12  t1 
2 
1 
t12  t d 12  t1  t 2 
2 

Last segment:
n  sgn( n 1   n ) n 


2 2( n   n 1 ) 
t n  t d ( n 1) n  t d ( n 1) n  

n1 

 n   n 1 
 ( n 1) n  
1
t d ( n 1) n  t n 
2 
1 
t ( n 1) n  t d ( n 1) n  t n  t n 1 
2 

To go through the actual via points:

Introduce “Pseudo Via Points”
Use sufficiently high acceleration


7.5 Summary

7.3 Cartesian-Space Schemes
When path shapes are described in terms of functions of

Cartesian position and orientation, we can also specify the
spatial shape of the path between path points.
The most common path shape is a straight line; but
circular, sinusoidal, or other path shapes could be used.
Cartesian schemes are more computationally expensive to
execute since at run time, inverse kinematics must be
solved at the path update rate.
7.3 Cartesian-Space Schemes 37

Description of a task

Cartesian straight line motion

Move from point Pi to Pi+1 , which described by relative
homogenous transformation:
nix oix aix pix 
 n o a pi  niy oiy aiy p 
iy 
B
Pi   i i i
 
0 0 0 1   niz oiz aiz piz 
 
0 0 0 1
 ni 1x oi 1x ai 1x pi 1x 
n o a p n o a p 
   i 1 y i 1 y 
B
Pi 1   i 1 i 1 i 1 i 1

 n
i 1 y i 1 y
 0 0 0 1  i 1 z oi 1z ai 1z pi 1z 

 
 0 0 0 1 

In order to ensure continuous velocities in trajectory, a

spline of linear functions with parabolic blends is always
used.
During the linear portion of each segment, since all three
components of position change in a linear fashion, the end-
effector will move along a linear path in space.

Cartesian planning difficulties (1/3):
Initial (A) and

Goal (B) Points are
reachable, but
intermediate points
(C) unreachable.

Approaching
singularities some
joint velocities go to
∞ causing deviation
from the path.

Start point (A)

and goal point (B)
are reachable in
different joint space
solutions (The
middle points are
reachable from
below.)


7.5 Summary

7.4 Path Generation at Real-Time
At run time the path generator routine constructs

the trajectory, usually in terms of  ,  , and  , and
feeds this information to the manipulator's control
system.
This path generator computes the trajectory at the
path update rate.
7.4 Path Generation at Run Time 45

7.4.1 Generation of joint space paths
In the case of cubic splines, the path generator

simply computes (7.3) and (7.4) as t is advanced.
When the end of one segment is reached, a new set
of cubic coefficients is recalled, t is set back to
zero, and the generation continues.
 (t )  a 0  a1t  a 2 t 2  a3 t 3 (7.3)
 (t )  a1  2a 2 t  3a3 t 2 
(t )  2a  6a t  (7.4)
2 3 

In the case of linear splines with parabolic blends,

the value of time, t, is checked on each update to
determine whether we are currently in the linear or
the blend portion of the segment.
In the linear portion, the trajectory for each joint is
calculated as
   j   jk t 

   jk  (7.41)
  0 


In the case of linear splines with parabolic blends,

the value of time, t, is checked on each update to
determine whether we are currently in the linear or
the blend portion of the segment.
In the blend region, the trajectory for each joint is
calculated as 1 
   j   jk (t  tinb )   ktinb
2

2

  
   jk  ktinb 
 (7.42)
  k 

1 
where tinb  t   t j  t jk 
2 
7.4.2 Generation of Cartesian space paths
In the case of linear spline with parabolic blends

path. Rewrite (7.45) and (7.46) with the symbol X
representing a component of the Cartesian position
and orientation vector.
In the linear portion of the segment, each degree
of freedom in X is calculated as
x  x j  x jk t 

x  x jk  (7.44)
x  0 


In the case of linear spline with parabolic blends

path. Rewrite (7.45) and (7.46) with the symbol X
representing a component of the Cartesian position
and orientation vector.
In the blend region, the trajectory for each degree
of freedom is calculated as
1  
tinb  t   t j  t jk  
2  
1 
x  x j  x jk t  tinb   xk t 2 inb  (7.45)
2 
x  x jk  x k tinb 

x  xk 
 and X
Finally, this Cartesian trajectory ( X, X,  )
must be converted into equivalent joint space
quantities.
A complete analytical solution to this problem
would use:
inverse kinematics to calculate joint positions,
inverse Jacobian for velocities,
inverse Jacobian plus its derivative for accelerations.

Summary
General Considerations in Robot Trajectory Planning
Joint-Space Schemes
Cubic polynomials
Cubic polynomials for a path with via points
Higher-order polynomials
Linear function with parabolic blends
Linear function with parabolic blends for a path with via points
Cartesian-Space Schemes
Track of any desired shape
More expensive at run time
Discontinuity problems
Real Time Generation of Planning Trajectories
7.5 Summary 52
Thank you
For Attention!
Fundamentals of Robotics 53

Robot Technology: Ch.7 Trajectory Planning of Robots

Uploaded by

Copyright:

Available Formats

Robot Technology: Ch.7 Trajectory Planning of Robots

Uploaded by

Document Information

Original Title

Copyright

Available Formats

Share this document

Share or Embed Document

Sharing Options

Did you find this document useful?

Is this content inappropriate?

Copyright:

Available Formats

Robot Technology: Ch.7 Trajectory Planning of Robots

Uploaded by

Copyright:

Available Formats

Robot Technology

Ch.7 Trajectory Planning of Robots

Prof. Wang Zhongmin

7.1 General Considerations in Robot Trajectory Planning

7.2 Interpolated Calculation of Joint Trajectories

7.3 Planning of Cartesian Path Trajectories

7.4 Real Time Generation of Planning Trajectories

Ch.7 Trajectory Planning of Robots 2

7.1 General Considerations in Robot Trajectory Planning

7.2 Interpolated Calculation of Joint Trajectories

7.3 Planning of Cartesian Path Trajectories

7.4 Real Time Generation of Planning Trajectories

Ch.7 Trajectory Planning of Robots 3

7.1 General considerations 4

Trajectory : Time history of position, velocity and acceleration

7.1 General considerations 5

Joint space （ smooth ）

7.1 General considerations 6

Cartesian planning difficulties:

Initial (A) and

7.1 General considerations 7

7.1 General Considerations in Robot Trajectory Planning

Ch.7 Trajectory Planning of Robots 8

A smooth function is found for each of the n joints which

The determination of the desired joint angle function for a

Choice of interpolation function is not unique!

Several possible path shapes for a single joint.

7.2 Joint–Space Schemes 10

Some possible interpolation functions:

7.2 Joint–Space Schemes 11

In making a single smooth motion, at least four constraints

Combining the four constraints yields four equations with

These four constraints uniquely specify a particular cubic:

7.2 Joint–Space Schemes 14

Eg. 7.1 A single-link robot with a rotary joint is motionless at θ

7.2 Joint–Space Schemes 15

Solution: Plugging θ0 =15 ， θf =75 ， tf = 3 into (7.6), we find

 (t )  a0  a1t  a2t 2  a3t 3  15  20t 2  4.44t 3

Solution:  (t )  a0  a1t  a2t 2  a3t 3  15  20t 2  4.44t 3

Starts at 15 degrees and ends at 75 degrees!

Solution:  (t )  a1  2a2t  3a3t 2  40t  13.33t 2

Starts and ends at rest!

Solution: (t )  2a2  6a3t  40  26.66t

Acceleration profile is linear!

If we come to rest at each point

How to specify velocity at the via points:

7.2 Joint–Space Schemes 21

Higher order polynomials are sometimes used for

7.2 Joint–Space Schemes 22

Where the constraints are given as:

7.2 Joint–Space Schemes 23

Solution to these equations:

7.2 Joint–Space Schemes 24

Linear interpolation (Straight line):

Discontinuous velocity - can not be controlled!

To create a smooth path with continous position and

7.2 Joint–Space Schemes 26

Assume that the parabolic blends both have the same

7.2 Joint–Space Schemes 27