Path Control in Robotics
ME 4135, F 2012
Richard R. Lindeke, Ph. D.
Motion Types of Interest

Point – to – Point Motion:
 All Axes start and end simultaneously
 All Geometry is computed for targets and relevant Joint
changes which are then forced to be followed during program
execution

Path or Trajectory Controller Motion
 Here the motion is performed through a time sequence of
intermediate configurations computed ahead of time (like
above but without stop-start operation) or in real time
 Paths are “Space Curves” for the n-Frame to follow
 This motion is a continuous scheme to move the TCP from
one location to the next along a desired (straight or curved)
line under direct operational control
Path Control and Motion Types:

We will explore the following types of Motion:




Lead Through Path Creation
(Cubic) Polynomial Paths w/ Via Points
Minimum Time Trajectory w/ controlled Acceleration
Lower order Path-Poly Control
 LSPB Paths
 Craig’s Method for acceleration smoothing
 Strict Velocity Control
 Joint Interpolated Control
 Full Cartesian Control
Lead Through Path Creation

Basically this was a technique whereby a skilled operator took a robot
arm (for welding or painting) and used it like his/her weld tool or paint
sprayer and performed the required process at reasonable speed

The robot is equipped with a position recording device and memorizes
a large number of points during the teaching session
These learned points then would be “played back” to replicate the
skilled operators motions

Lead Through Path Creation

Advantages:
 Simple way to create complex paths
 All points are sure to be physically attainable
 Playback speed can be controlled by an external
device

Disadvantages:
 Precision placements are required (program must
be replayed at exactly the initial placement)
 Major concern with operator safety: robot is
powered and operator is physically touching it
(OSHA rules it unsafe practice!)
Modern Path Control:
(Lets look at a simple example)
Dr. D’s new ‘Self-powered Automated Coffee
Drinker’ Robot
 It is a simple cantilevered Cartesian device equipped
with a spherical wrist that responds to eye
movement and thoughts to help the overworked
design engineer get coffee while designing and
drafting & typing of Reports
 It follows a straight line path from the cup’s point on
a table to the worker’s mouth in ½ second

Lets look at a simple example:
Lets look at a simple example:

We see that the ‘Bot must travel a space path of
16.45” which can be decomposed into a
movement of 9.5” along each of the prismatic
joints

For ‘accuracy’ lets divide each of the these joint
paths into 100 segments

From Physics:Vjoint  d/ t = (9.5/100)/(.5/100)
= 19in/second (a reasonable speed!)
Lets look at a simple example:

During the 1st Step then:
Joint 1 starts at 0 and moves to 0.095”
Moves there in 0.005 seconds
How will it do it?
Of course by Accelerating from a stop to 19 in/sec in
0.005 seconds
 Compute Accreqr  V/t




Darn – this says that the acceleration is
3800in/sec2 – this is  10G!!!!!
Lets look at a simple example:
So this will certainly be difficult to accomplish!
(more likely it will not work)
 OSHA would be just as upset as when we had the
worker holding on to the powered robot – what
should we do?
 I think our approach is too naïve!
 If we examine the Pos vs. Time, Vel Vs. Time and Acc
vs. Time plots we may see why:

Look at a simple example’s
Trajectory Curves:
10 g
Acc
-10 g
19
in/sec
Vel
9.5"
POS
0.005
Time
0.495
0.5
This is Physically Impossible (or
rather ‘very energy intensive’)
Can we build a reasonable solution that
keeps the acceleration to an achievable
level?
 What this would mean is we wouldn’t
“instantly” – in one time step – go from
stopped to full speed
 This can be achieved with a “time
polynomial” model of motion

Building a ‘Path Polynomial’
Motion Set
q  t   a0  a1t  a2t  a3t 
2
3
dq
2
q t  
 a1  2a2t  3a3t 
dt
d 2q
q  t   2  2a2  6a3t 
dt
These are the ‘trajectory’
equations for a joint (Position,
Velocity and Acceleration)
Solving the ‘Path Polynomial’ means
finding ai’s for SPECIFIC PATHS
We would have “boundary” conditions for
position and velocity at both ends of the
path
 We would have the desired total time of
travel
 Using these conditions we can solve for a0,
a1, a2 and a3 to build a 3rd order path
polynomial for the required motion

Solving the ‘Path Polynomial’ means
finding ai’s for SPECIFIC PATHS
q0  a0  a1t0  a t  a t
2
2 0
3
3 0
q0  a1  2a2t0  3a t
2
3 0
q f  a0  a1t f  a t  a t
2
2 f
3
3 f
q f  a1  2a2t f  3a t
2
3 f
‘Poly’s’ holding at
starting time and
position
‘Poly’s’ holding at
ending time and
position
Solving the ‘Path Polynomial’ means
finding ai’s for SPECIFIC PATHS

Writing these as Matrix Forms:
 1 t0

0
1

1 t f

0 1
2
0
t
2t0
2
tf
2t f
3
0
2
0
3
f
2
f
t
3t
t
3t
  a0   q0 
    
q
a
 1   0 
  a2   q f 
    
  a3   q f 
Solving the ‘Path Polynomial’ means
finding ai’s for SPECIFIC PATHS

If we set t0 = 0 (starting time is when we start
counting motion!) then:
1 0
0 1

1 t f

0 1
0
0
t
2
f
2t f
0   a0   q0 





0
q0
a
1
   
3
t f   a2   q f 
  
2 
3t f   a3   q f 
By examination, a0 = q0 & a1 = q0(dot)
Solving the ‘Path Polynomial’ means
finding ai’s for SPECIFIC PATHS
Completing the solution consists of forming
relationships for: a2 & a3
 Done by substituting a0 & a1 values and solving the
last two equation simultaneously:

Be Careful and
note the order of
the positions and
velocities!
a2
3 q


a3
2q


f
0
 q0   t f  2q0  q f 
 q f   t f  q f  q0 


t 2f
t 3f
Applying it to the Coffee ‘Bot
Start: X = 0; v = 0 @ time = 0
 End: X = 9.5”; v = 0 @ time = .5 sec
 a0 = 0 ; a1 = 0
 a2 = (3 * 9.5)/(0.52) = 114
 a3 = (2 *(- 9.5))/(0.53) = -152

Applying it to the Coffee ‘Bot

Here (specifically):
qi  0  0ti  114t  (152)t
2
i
3
i
qi  0  2(114)ti  3(152)t
2
i
qi  2(114)  6(152)ti
Applying it to the Coffee ‘Bot

Simplifying:
qi  114t  152t
2
i
3
i
qi  228ti  456t
2
i
qi  228  912ti
Applying it to the Coffee
‘Bot: Position
Pos
10
9
Position vs. Time
8
Pos of Joint (in)
7
6
5
4
3
2
1
0
0
0.05
0.1
0.15
0.2
0.25
time (sec)
0.3
0.35
0.4
0.45
0.5
Applying it to the Coffee
‘Bot: Velocity
Vel
30
25
Velocity (in/sec)
20
15
Joint Velocity Vs. Time
10
5
0
0
0.05
0.1
0.15
0.2
0.25
Time (sec)
0.3
0.35
0.4
0.45
0.5
Applying it to the Coffee
‘Bot: Acceleration
Acc
300
Acceleration (in/sec^2)
200
Acceleration Vs. Time
100
0
0
0.05
0.1
0.15
0.2
0.25
-100
-200
-300
time (sec)
0.3
0.35
0.4
0.45
0.5
Applying it to the Coffee ‘Bot





Using the Path Polynomial Approach:
Max Velocity is: 28.5 in/sec (compares to 19 in/sec)
Max Acceleration is: 228 in/sec2 (.6 g) compared to
 10 g
But, in this method, I require a 100% duty cycle
motor since throughout the entire path, the
motor is accelerating (either with positive or
negative orientation)
Can we make a path solution where we accelerate
for only part of the path? Turns out we can and
we will call it LSPB!
Studying the LSPB model
In this model, we will carry forward with a
‘parabolic model’– that is 2nd order
 In this model, we will determine a time where we
will blend from startup until we reach a constant
velocity – (and its greater than 1/100 of the total
time!)
 Here we will see an acceleration followed by a
period of “coasting” and then deceleration (often
called a trapezoidal velocity model)

Model Building:

We must define an acceleration constraint (minimum
value) such that the acceleration is guaranteed to be
completed within half of the allocated time period of the
travel:
q  q  4q  q 
qmin 
B
 t 2
A
2

B
A
t2
based on solving:
Pos  1 qt 2
2
at: t  t (half time)
2
qB  q A 

we want: Pos =
2
This assures
that there is
no overlap for
the ‘BLEND
Regions’
Looking at the motion over
the various regions:

During Region 1 (while the joint is Accelerating)
(time interval 0 to tblend [tb]) the Joint moves:
q = (V/2)*tb
 tb is the acceleration time
During the region of ‘Constant Velocity’ the Joint
moves: q = V*(t – 2tb)
 During Region 3 -- while the joint is decelerating
the joint moves: q = (V/2)*tb
 Total travel distance is qB - qA

Writing a motion equation:
qB  q A  q0tb  qtb t tb   qt tb t
qB  q A  V tb  V  t  2tb   V tb
2
2
qB  q A  Vt  2Vtb  Vtb  Vt  Vtb
BUT :
V  qtb
Substituting and Isolating the
Unknown (which is blend time):
qB  q A  qt  tb  qt
2
b
reforming:
qt  qt  tb   qB  q A   0
2
b
This is a quadratic equation in tb
Solving for tb:
   qt  

t 
 qt 
b
tb  t 
2
 qt 
2
 4q  q A  q B 
2q
2

 4q  q B  q A 
2q
Note: Acceleration is
subject to constraint
determined above
Applying it to the Coffee ‘Bot

Acceleration
4   9.5  0 
2
q

152
in
/
sec
 0.4 g
constraint:
2
.5
lets ' pick ' q  175in / sec 2

Blend time:
tb  .25 
.25 175 
2
 4  175  9.5
2  175
1006.25
 .25 
 .25  0.091
350
 tb  .25  0.091  .159sec
Applying it to the Coffee ‘Bot

tlinear=.5 – 2*0.159 = 0.181 s

Linear Velocity: Vlin  q  tb  175  .159  27.89 in/sec

Positions:
 By tb, the Joint has moved: (27.89/2)*.159 = 2.222”
 During linear velocity joint moves: 0.181*27.89 =
5.055 in (thus the pos = 7.277”)
 During deceleration joint travels 2.222”
 Adding them gives full travel distance: 2.222 + 5.055
+ 2.222 in  9.5in
Plotting the Path trajectory:
175
in/s^2
0 in/s^2
Acc
-175
in/s^2
27.89
in/sec
Vel
9.5"
POS
.159
Time
.361
0.5
Notice: the accelerator is ‘off’ during the linear
travel segment
The 2 Previous Path Control Methods
focused on Start/Stopping Approaches

What can we do if we desire to travel by continuing along
a path w/o stopping at each point?

Here we will focus on a method called ‘dog-tracking’ after
the lead and follow techniques employed in dog racing

Essentially we would have a situation where the path is laid
out (as a series of Via Points) and the joints smoothly
maneuver through and between them
‘Craig’s’ Dog Tracking Method
Craig’s Dog Tracking Method
Upon Examination of the motion, we find that
there are three ‘regimes’ in the motion
 These are:

 Start up regime
 Intermediate regimes
 Stopping regime
Starting and Stopping are similar to LSPB in the
way they compute blend time and acceleration
 During an Intermediate regime we compute
acceleration by comparing incoming and
outgoing velocities about each point

Craig’s Dog Tracking Method

Starting regime Equations:
1  SGN  2  1   global
t1  td 12  t
12 
2
d 12
Start Acceleration
 2  2  1  



1

Start Blend
Time
 2  1
td 12  .5  t1
tl12  td 12  t1  .5  t2
Linear Velocity 1→2
Time @ linear Velocity
Craig’s Dog Tracking Method

Stopping Equations:
 n  SGN  n1   n   global
tn  td  n1n  t
 n1n 
2
d  n 1n

Stop ‘acceleration’
2  n   n1 
 n   n1 
td  n1n  .5  tn
tl  n1n  td  n1n  tn  .5  tn1
n
Stop Blend
Time
L. Velocity to stop
Time @ L. Velocity
Craig’s Dog Tracking Method

Intermediate Equations:
 jk



k
 j 
tdjk
 k  SGN  kl   jk   global
tk



kl
Linear Velocity
  jk

k
tljk  tdjk  .5  t j  .5  tk
Acceleration
Blend time
Time @ L. Velocity
Craig’s Dog Tracking Method

Upon examination of the set of equation on
the previous 3 slides several point should be
noted:
 Start and Stop are essentially the same but very
important differences must be noted
 One can’t complete any of the regimes without looking
ahead – actually looking ahead to the 2nd point beyond to
see if a joint is stopping or continuing
 Start/Stop require position/time relationships
 Intermediate regimes require velocity/time
relationships
Craig’s Dog Tracking Method

Step 1: Calculate Global ‘usable’
acceleration (magnitude)
constraint based on LSPB model
applied Pairwise (1→2; 2→3; etc)

Step 2: Focus on Start and Stop
Segments

Step 3: Complete the table of
accelerations, blend times, linear
velocity and time at linear velocity
4  q 

q
i j
t
2
dij
Lets Expand on Dr. D’s
Coffee ‘Bot:
Point
Name
Position
(inch)
0
Delta
Position
(inch)
0
Arrival
Time
(sec)
---
Delta
Time
(sec)
----
A
B
9.5
9.5
0.5
.5
C
11
1.5
3.5
3
D
9.5
-1.5
6.5
3
E
0
-9.5
7.5
1
Lets Expand on Dr. D’s
Coffee ‘Bot:

Step 1: Global Acc. Constraint
4  9.5
2
q AB 

152
in
/
s
.52
4  1.5
2
qBC 

0.67
in
/
s
32
4  1.5
2
qCD 


.67
in
/
s
32
4  9.5
2
qDE 

38
in
/
s
12
This is largest –
should work globally
– but lets make sure
it doesn’t miss so
choose 200ips2
‘cause it’s easier to
calculate and is only
about .6g
Lets Expand on Dr. D’s
Coffee ‘Bot:

Next we focus on the start & stop equations:
◦ Starting
q A  SGN (9.5  0)  200  200ips 2
t A  .5  .52  2  9.5
q AB  9.5
200
.5  .5  .106 
 0.106s
 21.3ips
qE  SGN  9.5  0  200  200ips 2
◦ Stopping
t E  1  12 
qDE 
2   0  9.5 
 0  9.5
200
1  .5  0.049 
 0.049 s
 9.74ips
NOTE: can’t
compute tlij yet
– we lack the
data!
Lets Expand on Dr. D’s
Coffee ‘Bot:

Considering Intermediate B→C:
qC  qB 


1.5
qBC

 0.5ips
tdBC 3


qB  SGN  q BC  q AB  200  200ips 2




q

q
 BC AB 
.5  21.25 



tB 

 .103s
qB
200
Now to finish the 1st segment:
tlAB  tdAB  t A  .5  t B  .5  .106  .5  .103  .342s
Lets Expand on Dr. D’s
Coffee ‘Bot:

qCD
On to “C→D” Segment
qD  qC 


tdCD
 0.5ips
qC  SGN  qCD  qBC  200  200ips 2
tC
qCD  qBC 


qC
 1
200
 .005s
back to finish B--C Segment
tlBC  tdBC  .5t B  .5tC  3  .5(.103)  .5(.005)  2.946s
Lets Expand on Dr. D’s
Coffee ‘Bot:

Now for Segment D→E:
earlier !
qDE 
 E (is stop point!)  9.74ips
qD  SGN  qDE  qCD  200  SGN  9.74  (.5)   200  200ips 2
tD
qDE  qCD 


qD
 0.049 s
Completing Seg. C--D
tlCD  3  .5tC  .5t D  2.974 s
Seg D--E
tlDE  1  .5t D  t E  0.928s
Summarizing
Pt
POS
T.
Time POS
time
Ti
(blend) Acc
A
0
0
-----
----
.106
+200
B
9.5
.5
9.5
.5
.103
-200
C
11
3.5
1.5
3
.005
-200
D
9.5
6.5
-1.5
3
.046
-200
E
0
7.5
-9.5
1
.049
+200
L. Vel
Time
@L.
Vel
21.3
.342
.5
2.946
-.5
2.974
-9.74 .928
A Final Thought on Dog-Tracking:
But if we must travel
over or through a
certain point, we can
define ‘Pseudo-Via’
points that flank the
desired target and
force the arm to pass
the ‘pseudos’ and
drive right over the
original desired target
point
Looking at Velocity Control
Desired
Path
Achievable
Path
With this “Velocity Control”

The acceleration is set and the new velocity for
an upcoming segment is inserted at the
appropriate time (place)

Over the segment, the arm (joints) lags the
desired path

We extend the next path velocity curve to
intersect the actual achievable path (to a point
that is earlier in time than when we would have
expected to change) this will mean that the joint
can ‘catch up’ to the desired plan for the travel as
it ‘blends’ to the new velocity
Linear Path Control



Sometimes thought of as Cartesian Control
It is based on the idea of Transitions between
consecutive required geometries
These transitions are based on the solution of a
Drive Matrix:
D  r   T  r  Ra  r  Ro  r 
The matrices T(r), Ra(r), and Ro(r) are the
translation, rotation wrt Z and rotation wrt Y in
transitioning from Pinitial to Pfinal
Developing the Drive Matrix:



Given: P1 is (n1, o1, a1, d1)
And: P2 is (n2, o2, a2, d2)
Then:
 n1 n2

o1 n2

Dr  
 a1 n2

 0
n1 o2
o1 o2
a1 o2
n1 a2
o1 a2
a1 a2
0
0
n1  d 2  d1  

o1  d 2  d1  
a1  d 2  d1  

1


Cartesian Control
It is used when very exact
interaction is required
 It ‘guaranties’ accurate tool
placement at all times
 It is typically used in time
dependent solutions – like
interaction while a product is
moving

Cartesian Control
NOTE: On the
conveyor, the H-frame
is a time dependent
pose in C (conveyor
space)
Cartesian Control
We desire to attach the “Quality Tag” to
the part as it moves by the robot station
 Requires that the part and robot tool
must be in exact contact throughout the
attachment process
 This becomes a ‘Time-based’ Mapping
problem

Cartesian Control
At Time 1 (P1):

Tn  t1    To 
0
R
1

U

U
TR 
1 U
TC TH  t1  TP TTar
T 
TC TH  t2  TP TTar
T 
C
H
P
n
1
MW
At Time 2 (P2):

Tn  t2    To 
0
R
1
TR 
1 U
C
H
P
n
MW
1
Cartesian Control
Using these two (time-dependent) Poses,
we can build the desired drive matrix
 We can compute the accuracy of the path
then as a series of changes to the three
control vectors: a, o and d
 These are updated in real time

Cartesian Control

Problems that can result (and must be accounted for):
 Intermediate points that are unreachable – After we compute the initial
and final points (that prove to be reachable as individuals), we request
the tracking of a, o and d vectors but they exceed joint capabilities or
require positions outside the work envelope during the driving action
 In certain situations where only certain solutions are possible for the
robot, like being near singularities, the desired linear velocity may require
very high joint velocities – exceeding capabilities – and the path actually
followed will deviate from the one desired as the joints run at their
velocity limits
Near Cartesian (Joint
Interpolated) Control



This is a semi-precise control method developed
as a compromise between full-Cartesian and
point-to-point motion
Basically it is used when a process needs to be
held within a ‘band’ about an ideal linear path –
for example during painting or bar-code scanning
The path is designed to ‘track’ the work as it
moves and maintains no more than a given “focal
distance” separation between the tool and work
surface
 It is a path that is close to the target path at all poses
but exact only at a few!
Joint-Interpolated Control




Step 1: determine the desired path
Step 2: Compute the tolerable error and the number
of points (‘VIAs’) needed to maintain tool–to–work
distances
Step 3:Compute IKS’s at each of the VIAs
Step 4: Determine “Move time” for each segment:
TSEGK

 qi  qi
1
 MAX  2
 i
 

each joint i
note: i is joint velocity
Joint-Interpolated Control

Step 5: Divide the Tseg into ‘m’ equal time intervals:
Tseg  1
f sampling
where :
f sampling
is the controller positional sampling rate
m  T
seg
f
sampling
Joint-Interpolated Control


Step 6: For each joint,
determine angular distance
during each time segment tseg:
Step 7: at the beginning of the
nth step over a path, joint i
servo control receives a target
point:
qi / time
q


i2
 qi1

m
target = n  qi  qseg start
Joint-Interpolated Control


Implementing this method begins with
determination of the distance
between and ultimately the number of
Via Points needed
This is (really!) a simple trigonometry
problem based on the offset distance
and error tolerance () at ‘closest
approach’
Joint-Interpolated Control -Model
R
R
1
2
2
2
R
1
Note: R = R1 = R2
Robot
Base
Joint-Interpolated Control -Model

Notice line #1, #2 and R1 form a right triangle
2
2
1  2  R12
2
2 R 1
2
1
R (= R1) is
measured at
point of closest
approach
between the
robot and part!
2
But : 1  R  2
#2 is half the
distance
between Via
Points!

2  R   R  2 
2
1
2  4 R  4 2
2
Lets try one:

A Part 6m long moves by a stationary robot
on a conveyor moving at 0.04mps (counter
flowing compared to painting direction) If
we desire that the robot complete its
spraying in 15 seconds
◦ Then, the robot must travel 5.4 m to spray
the side of the part nearest it since the part
moves during the painting operation.


At closest approach, the robot is 1.5 m
from the part and needs to have its sprayer
20 cm ( 5 cm) from the part.
From this data, the R value is: 1.5 – (.20 +
.05) = 1.35 m
Lets try one:

Distance between Via’s is found using:
2  4 R   4 2
 4  1.35  .05  4  .05   .26
2
2  0.510m
distance between Via's is:
2  2  2  .510  1.020m

Therefore: The number of Via’s
◦ = 5.4/1.020 = 5.29 so round up to 6
◦ plus the initial point = 6 + 1 = 7
Follow-up
What if we equally space the Via’s?

Distance between is: 5.4/6 = .9m
(rather than 1.020m)

What is actual Error band?
 Here we see it is: 3.86cm

Typically, we find that Joint
Interpolated solutions provide
better than required (or
expected) process control!
2 .5
9
.   4  1.35  4 
2
.452  5.4  4 2
A Quadratic eqn. in  !
 2  1.35  .0506  0
  .0772 2 m
  .0386m  3.86cm