12
Trajectory planning and feedforward design
for electromechanical motion systems
version 2
Report nr. DCT 2003-18
Paul Lambrechts
Email: [email protected]
April 1, 2003
Abstract
This report considers trajectory planning with constrained dynamics and design of an appropriate feedforward controller for single axis motion control. A motivation is given for using
model-based fourth order feedforward with fourth order trajectories. An algorithm is given
for calculating higher order trajectories with bounds on all considered derivatives for point to
point moves. It is shown that these trajectories are time-optimal in the most relevant cases.
All required equations for third and fourth order trajectory planning are explicitly derived.
Implementation, discretization and quantization effects are considered. Simulation results
show the superior effectiveness of fourth order feedforward in comparison with rigid-body
feedforward.
This report differs from DCT 2003-08 in that it contains an improved algorithm for fourth
order trajectory planning (total absence of iteration loops).
1
m
1
m
2
Contents
1 Introduction
3
2 Rigid-body feedforward
6
3 Higher order feedforward
8
4 Higher order trajectory planning
10
5 Third order trajectory planning
13
6 Fourth order trajectory planning
6.1 Main algorithm
6.2 Solution of equation 49
17
17
23
7 Implementation aspects
7.1 Switching times
7.2 Synchronization of profiles
7.3 Implementation of first order filter
7.4 Calculation of reference trajectory
25
25
26
28
29
8 Simulation results
30
9 Conclusions
34
A List of symbols
36
B The Matlab function MAKE4.m
37
2
m
1
1
m
2
Introduction
Feedforward control is a well known technique for high performance motion control problems as
found in industry. It is, for instance, widely applied in robots, pick-and-place units and positioning systems. These systems are often embedded in a factory automation scheme, which provides
desired motion tasks to the considered system. Such a motion task can be to perform a motion
from a position A to a position B, starting at a time t.
Usually this task is transferred to computer hardware dedicated to the control of the system,
leaving the details of planning and execution of the motion to this dedicated motion control
computer. The tasks of this dedicated motion controller will then consist of:
• trajectory planning: the determination of an allowable trajectory for all degrees of freedom,
separately or as a multidimensional trajectory, and the calculation of an allowable trajectory
for each actuation device;
• feedforward control: the representation of the desired trajectory in appropriate form and
the calculation of a feedforward control signal for each actuation device, with the intention
to obtain the desired trajectory;
• system compensation: to reduce or remove unwanted behavior like known or measured
disturbances and non-linearities;
• feedback control: the processing of available measurements and calculation of a feedback
control signal for each actuation device to compensate for unknown disturbances and unmodelled behavior,
• internal checks, diagnostics, safety issues, communication, etc.
To simplify the tasks of the motion controller, the trajectory planning and feedforward control are
usually done for each actuating device separately, relying on system compensation and feedback
control to deal with interactions and non-linearities. In that case, each actuating device is considered to be acting on a simple object, usually a single mass, moving along a single degree of
freedom. The feedforward control problem is then to generate the force required to perform the
acceleration of the mass in accordance with the desired trajectory.
Conversely, the desired trajectory should be such that the required force is allowable (in the
sense of mechanical load on the system) and can be generated by the actuating device. For obvious reasons this approach is often referred to as ‘mass feedforward’ or ‘rigid-body feedforward’.
It allows a simple and practical implementation of both trajectory planning and feedforward control, as the required calculations are straightforward.
The disadvantage of this approach is its dependence on system compensation and feedback control to deal with unmodelled behavior as mentioned before. The resulting problem formulation
can be split in two.
1. During execution of the trajectory the position errors are large, such that feedback control actions are considerable. Actual velocity and acceleration (hence: actuator force) may
therefore be much larger than planned. This may lead to undesired and even dangerous
deviations from the planned trajectory and damage to actuator and system.
3
m
1
m
2
2. When arriving at the desired endpoint, the positioning error is large and the dynamical
state of the controlled system is not settled. Although the trajectory has finished, it is
often necessary to wait for a considerable time before the position error is settled within
some given accuracy bounds before subsequent actions or motions are allowed. A practical
consequence is the need for of a complex test to determine whether settling has sufficiently
occurred. Furthermore, it is a source of time uncertainty that may be undesirable on the
factory automation level.
To improve on this, many academic and practical approaches are possible. These can roughly be
categorized in three.
1. Trajectory smoothing or shaping: This can be done by simply reducing the acceleration
and velocity bounds used for trajectory planning, but also by smoothing or shaping the
trajectory and/or application of force (higher order trajectories, S-curves, input shaping,
filtering). The result of this can be very good, especially if the dynamical behavior of the
motion system is explicitly taken into account. However, it may also lead to a considerable
increase in execution time of the trajectory, often without a clear mechanism for finding a
time optimal solution. Various examples of this approach can be found in [4, 7, 8, 9, 13, 6].
2. Feedforward control based on plant inversion: This attempts to take the effect of unmodelled behavior into account by either using a more detailed model of the motion system
or by learning its behavior based on measurements. An important practical disadvantage
is that they do not provide an approach for designing an appropriate trajectory. Various
examples of this approach can be found in [3, 5, 10, 11, 12, 15, 16, 17].
3. Feedback control optimization (possibly aided by system compensation improvement): By
improving the feedback controller, the positioning errors can be kept smaller during and at
the end of the trajectory. Furthermore, settling will occur in a shorter time. Also in this case
the design of an appropriate trajectory is not considered. Obviously, any feedback control
design method can be used for this. Some references given above also include a discussion
on the effect of feedback control on trajectory following; e.g. see [11, 12, 17].
This report will provide a method for higher order trajectory planning that can be used with
all of the approaches given above. It is attempted to give a better understanding of the effect
of smoothing, especially when considering a more optimal balancing between time optimality,
physical bounds (actuator device and motion system limits) and accuracy.
‘Fourth order feedforward’ will be presented as a clear and well implementable extension of
rigid-body feedforward, with also a clear strategy for obtaining the aforementioned optimal balance. The implementation of a fourth order trajectory planner will be set up as a natural extension
of second and third order planners to show the potential for practical application. Furthermore,
the calculation of the optimal feedforward control signal will be shown.
Next the effect of discrete time implementation will be considered: this includes the planning
of a fourth order trajectory in discrete time and the optimal compensation of time delays in the
feedforward control signal. Finally, some simulation results are given to further motivate the use
of fourth order feedforward.
The next chapter will review rigid-body feedforward, mostly with the purpose of introducing some
notation. Next, chapter 3 will consider the extension of mass feedforward to fourth order feedforward, based on an extended model of the motion system. A general algorithm for higher order
4
m
1
m
2
trajectory planning will be considered in chapter 4, after which the relevant equations are derived
for third order trajectory planning in chapter 5 and for fourth order trajectory planning in chapter 6. Next, implementation aspects are considered in chapter 7, followed by some simulation
results in chapter 8, and conclusions in chapter 9.
5
m
2
1
m
2
Rigid-body feedforward
The specifics of planning a trajectory and calculating a feedforward signal based on rigid-body
feedforward are fairly simple and can be found in many commercially available electromechanical motion control systems. In this chapter a short review is given as an introduction to a standardized approach to third and fourth order feedforward calculations.
Consider the configuration of figure 1 with m denoting the mass of the motion system, F
the force supplied by the actuating device, x the position and k a viscous damping term. The
x
F
m
k
Figure 1: Simple motion system: a single mass.
equation of motion for this simple configuration is of course
(1)
mẍ + k ẋ = F
Now suppose we have a given bound on acceleration ā (i.e. a bound on F ), and we want to
perform a motion from the current position A to a position B over a distance we will denote as
x̄. Then the shortest time within which the motion can be performed is calculated as:
1
x̄ = 2 × āt2 ⇒ tā =
2
r
x̄
⇒ tx̄ = 2tā
ā
(2)
with tā denoting the constant acceleration phase duration and tx̄ denoting the total trajectory
execution time. Hence, this gives rise to a trajectory consisting of a constant maximal acceleration phase followed directly by a constant maximal deceleration phase. Clearly if a bound on
velocity, denoted as v̄, is taken into account, tx̄ can only become larger. We can test whether the
velocity bound v̄ is violated by calculating the maximal velocity obtained using the minimal time
trajectory:
v̂ := ā · tā
(3)
Now if v̂ <= v̄ we are finished: tx̄ = 2tā and no constant velocity phase is required. If v̂ > v̄ we
calculate:
1
v̄
tā =
⇒ xā := 2 × āt2ā < x̄
(4)
ā
2
From this, the constant velocity phase duration tv̄ is calculated:
tv̄ =
(x̄ − xā )
v̄
(5)
We now have: tx̄ = 2tā + tv̄ and x̄ = āt2ā + v̄tv̄ . Note that tv̄ automatically reverts to zero if the
velocity bound is not obtained.
6
m
m
1
2
Construction of the acceleration profile a from tā and tv̄ is straightforward. From this, the desired
trajectory can be determined by integrating it once to obtain the velocity profile v, and integrating
it twice to obtain the position profile x; see figure 2. As the position profile thus establishes
Second order trajectory profiles
a [m/s2]
5
0
−5
0
0.2
0.4
0.6
0.8
1
0
0.2
0.4
0.6
0.8
1
0
0.2
0.4
0.8
1
v [m/s]
1.5
1
0.5
0
x [m]
1
0.5
0
0.6
time [s]
Figure 2: Second order trajectory determination.
the trajectory as a sequence of polynomials in time with a degree of at most two, rigid-body
feedforward is also referred to as ‘second order feedforward’.
The actual implementation of the trajectory planner and feedforward controller is indicated in
figure 3. Note that the feedforward force F is simply calculated from equation 1 and the profiles
a
m
v
Acceleration
profile generator
k
F
1
s
1
s
PD
1
2
m.s +k.s
Velocity
Position
Controller
2nd order model
x
Figure 3: Rigid-body feedforward implementation.
in figure 2 as:
F = ma + kv
(6)
7
m
3
1
m
2
Higher order feedforward
The previous chapter shows that mass feedforward is based on a simple single mass model of
the motion system. This implies that the performance of rigid-body feedforward is determined
by how much the actual motion system deviates from this single mass model.
On the other hand, the performance of the motion system as a whole is also determined by
the quality of system compensation and/or feedback control. It can be stated that the success of
feedback control is such that in many cases the mass feedforward approach is considered sufficient, given that an appropriate feedback controller is required anyway for disturbance reduction
and stabilization.
However, when considering further improvement of motion control system performance, the
use of higher order feedforward is often a very effective approach in comparison with improved
feedback control. The first effect is that higher order trajectories inherently have a lower energy
content at higher frequencies, which results in a lower high frequency content of the error signal, which in turn enables the feedback controller to be more effective. The second effect is that
higher order trajectories have less chance of demanding a motion which is physically impossible
to perform by the given motion system: e.g. most power amplifiers exhibit a ‘rise time’ effect,
such that it is impossible to produce a step-like change in force.
These effects are commonly referred to as ‘smoothing’ and result in a decrease of positioning
errors during execution of the trajectory and a reduced settling time. The disadvantage of higher
order trajectories, i.e. the increase in trajectory execution time, is usually more than compensated
by the reduced settling time. Because of this, many high performance motion systems are already
equipped with a third order trajectory planner as a direct extension of mass feedforward. In this
chapter it will be determined that a fourth order trajectory planner and feedforward calculation
gives a significant further improvement.
The main argument for this is that an electromechanical motion control system will usually have
some compliance between actuator and load, and that both actuator and load will have a relevant
mass. For this reason it is natural to extend the single mass model of figure 1 to the double mass
model of figure 4. Here m1 denotes the mass of the actuator, m2 the mass of the load, F the
force supplied by the actuating device, x1 the actuator position, x2 the load position, c the stiffness between the two masses, k12 the viscous damping between the two masses, k1 the viscous
damping of the actuator towards ground and k2 the viscous damping of the load towards ground.
The equations of motion for this configuration are:
F
m
x
c
1
k
k
1
x
1
m
2
2
1 2
k
2
Figure 4: Extended motion system: double mass.
8
m
(
1
m
2
m1 ẍ1 = −k1 ẋ1 − c(x1 − x2 ) − k12 (ẋ1 − ẋ2 ) + F
m2 ẍ2 = −k2 ẋ2 + c(x1 − x2 ) + k12 (ẋ1 − ẋ2 )
(7)
Laplace transformation and substitution then results in the following expression:
F =
q1 s4 + q2 s3 + q3 s2 + q4 s
· x2
k12 s + c
(8)
with:


q1 = m1 m2



q2 = (m1 + m2 )k12 + m1 k2 + m2 k1
(9)

q3 = (m1 + m2 )c + k1 k2 + (k1 + k2 )k12



q4 = (k1 + k2 )c
This implies that if we have planned some fourth order trajectory for x2 , from which we can
derive the corresponding profiles for velocity v, acceleration a, jerk  and derivative of jerk d, the
feedforward force F can be calculated as:
F =
1
· {q1 d + q2  + q3 a + q4 v}
k12 s + c
(10)
Analogous to the implementation given in figure 3, this feedforward scheme can readily be implemented as given in figure 5. Note that it is convenient to specify the trajectory by means of the
d
q1
Derivative of jerk
profile generator
q2
Rigid Body Feedforward
q3
q4
1
k12.s+c
F
1
s
1
s
Jerk
1
s
Acceleration Velocity
1
s
PD
Motion
System
Position
Controller
4th order model
x
Figure 5: Fourth order feedforward implementation.
derivative of jerk profile d, such that all other profiles can easily be obtained by integration. An
algorithm for obtaining d will be the subject of the next chapters.
9
m
4
1
m
2
Higher order trajectory planning
Planners for second and third order trajectories are fairly well known in industry and academia
and there are many approaches for obtaining a valid solution. Extension to fourth order trajectory
planning is however not trivial. In this chapter we will review the main objectives of trajectory
planning in general and set up an algorithm for obtaining these objectives more or less irrespective of the order of the resulting trajectory.
As mentioned before, a trajectory is usually planned for a motion from the current position A to
some desired end position B under some boundary constraints. This will also be the basis for the
discussion given here, although the resulting algorithm can be adjusted for relevant alternative
objectives like for instance speed control instead of position control, scanning motions or speed
change operations.
Several further objectives that are of importance for applicability of a trajectory planning algorithm are given below.
• Timing. In principle we should strive for time optimality; at least it must be clear what the
consequences are for the trajectory execution time when considering the effect of boundary
conditions.
• Actuator effort. This is usually the basis for the selection of bounds for velocity, acceleration
and jerk, although they may also be related to bounds on mechanics or safety issues.
• Accuracy. For point to point moves, the planned end position of the trajectory must be
equal to the desired end position (within measurement accuracy).
• Complexity. Usually trajectory planning is thought of as being done off-line. In practice
however, the desired end position often becomes available at the moment the trajectory
should start. Hence, the time required for planning the trajectory is lost and should be
minimized.
• Reliability. The planning algorithm should always come up with a valid solution.
• Implementation. Trajectory planning is done in computer hardware, and is therefore subject to discretization and digitization.
Now we argue that these objectives, except the final one, are all met by the simple algorithm given
in chapter 2:
1. calculate tā =
q
x̄
ā
(equation 2),
2. calculate v̂ = ā · tā (equation 3),
3. test v̂ > v̄:
• if true recalculate tā =
v̄
ā
(equation 4),
• if false: no action,
4. calculate xā = āt2ā (equation 4),
5. calculate tv̄ =
(x̄−xā )
v̄
(equation 5), and
10
m
1
m
2
6. finished: x̄ = āt2ā + v̄tv̄ and tx̄ = 2tā + tv̄ .
First consider timing; obviously timing is minimal if the bound on v is discarded as is done in
step 1. If introducing this bound leads to a reduction of tā (verify that equation 4 always leads
to a reduction) the result will be that v = v̄ is obtained in the shortest possible time, and is
continued for as long as possible. Hence the trajectory execution time is always minimal, given
the bounds ā and v̄. Next, actuator effort is immediately taken into account by means of the given
bounds, accuracy is precise, complexity is low and reliability is high (no iteration loops that can
hang up, algorithm only fails if a non-positive bound is specified). The implementation problem
is considered later.
Given the advantages of this algorithm, it is desirable to generalize it for planning higher
order trajectories. The position displacement x̄ and bounds on all derivatives of the trajectory up
¯ are assumed to be given. Furthermore we will
to the highest order derivative d (indicated as d)
require that all derivatives are equal to zero at the start and end positions.
1. Determine a symmetrical trajectory that has d equal to either d¯ or −d¯ at all times and
obtains displacement x̄.
2. Determine td¯: the shortest time that d remains constant (always the first period).
3. Calculate the maximal value of velocity v̂ obtained during this trajectory.
4. Test v̂ > v̄:
• if true re-calculate td¯ based on v̄ (td¯ decreases),
• if false continue.
5. Calculate the maximal value of acceleration â obtained during this trajectory.
6. Test â > ā:
• if true re-calculate td¯ based on ā (td¯ decreases),
• if false continue.
7. Continue these tests and possible recalculations until d; the last test is performed on ,
defined as the highest derivative before d. The resulting td¯ will not be changed anymore.
8. Extend the trajectory symmetrically with periods of constant  whenever  reaches the value
̄ or −̄: this must be done such that the required displacement x̄ is obtained.
9. Determine t̄ : the shortest time that  remains constant.
10. Starting with velocity and ending with the derivative before , calculate the maximal value
and re-calculate t̄ if the appropriate bound is violated (each re-calculation can only decrease
t̄ ).
11. The resulting t̄ will not be changed anymore.
12. Extend the trajectory symmetrically with periods of constant next lower derivative; again
such that the required displacement x̄ is obtained. Determine the associated time interval
and do the tests and recalculations resulting in a final value for it.
11
m
1
m
2
13. Continue until v: the constant speed phase duration tv̄ is calculated such that the required
displacement x̄ is obtained (i.e. the displacement ‘left to go’ divided by v̄).
14. Finished: td¯, t̄ , etc. until tv̄ completely determine the trajectory.
The properties of the resulting trajectories (in the sense of the objectives given above) appear
to depend partly on the order of the trajectory planner. Obviously, the required calculations become more complex with increasing order. Time-optimality is still automatically obtained with
third order trajectories, but not necessarily for fourth order trajectories (this can be obtained but
costs much complexity at a small gain with respect to a good sub-optimal solution). In principle,
higher than fourth order trajectories can also be planned by means of this algorithm, but this is
considered impractical due to the large increase in complexity. It is noted here that all calculations
for third and fourth order planning can be done analytically with fairly basic mathematical functions, except one calculation for fourth order planning. For this case a simple and numerically
stable search algorithm can be used.
The next chapter will provide the calculations for third order trajectory planning; after that
fourth order trajectory planning will be considered.
12
m
5
1
m
2
Third order trajectory planning
Although chapter 3 shows that fourth order trajectory planning and feedforward is strongly motivated by a model based approach, it may still be useful to apply third order trajectory planning.
This chapter will provide the derivations and calculations necessary to ‘fill in’ the algorithm developed in chapter 4 for third order trajectory planning.
We will assume that a trajectory must be planned over a distance x̄, that bounds are given
on velocity (v̄), acceleration (ā) and jerk (̄), and that the trajectory must be time optimal within
these bounds. The bound on jerk can for instance be related to ‘rise time’ behavior as commonly
found in electromechanical actuation systems with a non-ideal power amplifier for generating
the actuation force. This results in a bound on the maximal current change per second, which
translates to a maximal actuation force change per second and a maximal acceleration change per
second: hence a bound on jerk.
The trajectory planning algorithm will be based on the construction of an appropriate jerk
profile (instead of the acceleration profile as in rigid-body feedforward) that can be integrated
three times to obtain the third order position trajectory. A symmetrical trajectory is completely
determined by three time intervals: the constant jerk interval t̄ , the constant acceleration interval
tā and the constant velocity interval tv̄ . The resulting profiles are given in figure 6. Including the
starting time of the trajectory at t0 , there are eight time instances at which the jerk changes. The
following relations are clear:
t̄ = t1 − t0 = t3 − t2 = t5 − t4 = t7 − t6
tā = t2 − t1 = t6 − t5
tv̄ = t4 − t3
(11)
For step 1 of the algorithm, we will discard the bounds on acceleration and velocity, and only
consider the bound on jerk. This implies tv̄ = 0 and tā = 0 and it is clear that this will provide
us with a lower bound for the trajectory execution time tx̄ = 4 × t̄ .
To obtain t̄ (step 2), we need the relation between t̄ and x̄ with given ̄. For this we make use
of the constant value of jerk of +̄ or −̄ during each interval. If we set the time instance of jerk
change to zero, it is easily verified that for any time t during the constant jerk interval the third
order profiles for acceleration, velocity and position can be expressed as follows:
a(t) =
v(t) =
x(t) =
0 t
1
2
2 0 t
1
3
6 0 t
+
a0
+
a0 t
1
2
2 a0 t
+
+ v0
(12)
+ v0 t + x0
Because we assume that the bounds on acceleration and velocity are not violated, we have tā = 0
and tv̄ = 0, and consequently from figure 6: t2 = t1 = t̄ and t4 = t3 . Hence:
a(t2 ) = a(t1 ) =
v(t2 ) = v(t1 ) =
x(t2 ) = x(t1 ) =
̄t̄
1 2
2 ̄t̄
1 3
6 ̄t̄
(13)
and for the next period in which  = −̄:
a(t4 ) = a(t3 ) = −̄t̄ + a(t2 )
v(t4 ) = v(t3 ) = − 12 ̄t2̄ + a(t2 )t̄ + v(t2 )
(14)
x(t4 ) = x(t3 ) = − 16 ̄t3̄ + a(t2 )t2̄ + v(t2 )t̄ + x(t2 )
13
m
1
m
2
Third order trajectory profiles
j [m/s3]
50
t0
0
−50
t1
t2
t3
t4
t5
t6
t7
0
0.2
0.4
0.6
0.8
1
1.2
0
0.2
0.4
0.6
0.8
1
1.2
0
0.2
0.4
0.6
0.8
1
1.2
0
0.2
0.4
0.6
time [s]
0.8
1
1.2
a [m/s2]
5
0
−5
v [m/s]
1.5
1
0.5
0
x [m]
1
0.5
0
Figure 6: Third order trajectory planning.
Substitution gives:
a(t4 ) = a(t3 ) = 0
v(t4 ) = v(t3 ) = ̄t2̄
x(t4 ) = x(t3 ) =
(15)
̄t3̄
and due to symmetry of the profile: x(t7 ) = 2x(t3 ) = 2̄t3̄ . From this we can calculate the
minimal required time to execute the trajectory for a distance x̄, given the bound on jerk ̄ as:
s
tx̄ = 4t̄ = 4 ·
3
x̄
2̄
(16)
As mentioned before, the result of equation 16 does not take into account whether or not the
bounds on acceleration and/or velocity are violated. However, if this would be the case, it is
obvious that tx̄ must increase, but also that t̄ must decrease.
14
m
1
m
2
Step 3 of the algorithm follows from equation 15; the maximum velocity occurs at t3 , such
that:
v̂ = ̄t2̄
(17)
If v̂ > v̄ the bound is violated and we must recalculate t̄ as follows:
s
t̄ =
v̄
̄
(18)
Note that the resulting t̄ will always be smaller than the result from equation 16, and that consequently also the acceleration (from equation 13) will become smaller. This establishes step 4 of
the algorithm.
Step 5 follows from equation 13; the maximum acceleration occurs at t1 , such that:
(19)
â = ̄t̄
If â > ā the bound is violated and we must recalculate t̄ as follows:
t̄ =
ā
̄
(20)
Again t̄ can only be smaller than previously calculated, such that we now have the guarantee that
t̄ is the maximal value for which none of the bounds is violated. At the same time we have that
a maximal value of t̄ will lead to a minimal value of tx̄ and will therefore be (part of) the timeoptimal solution. This establishes step 6 of the algorithm and because a is the highest derivative
before , also step 7.
The next thing to do is to calculate tā : also this value should be maximal for time-optimal
performance, while at the same time it must be such that no bounds are violated. To do the
necessary calculations, assume that tā > 0, such that we now have t2 > t1 (from figure 6).
Equation 13 can then be extended with the help of equation 12 and the fact that  = 0 during the
period from t1 to t2 :
a(t2 ) = a(t1 ) + 0tā = ̄t̄
v(t2 ) = v(t1 ) + a(t1 )tā = 21 ̄t2̄ + ̄t̄ tā
x(t2 ) = x(t1 ) + v(t1 )tā + 12 a(t1 )t2ā
=
1 3
6 ̄t̄
(21)
+ 21 ̄t2̄ tā + 12 ̄t̄ t2ā
Again using equation 12 we can then calculate:
a(t3 ) = a(t2 ) − ̄t̄ = 0
v(t3 ) = v(t2 ) + a(t2 )t̄ − 12 ̄t2̄
=
1 2
2 ̄t̄
+ ̄t̄ tā + ̄t2̄ − 12 ̄t2̄
= ̄t̄ tā + ̄t2̄
(22)
x(t3 ) = x(t2 ) + v(t2 )t̄ + 12 a(t2 )t2̄ − 16 ̄t3̄
= ̄t3̄ + 23 ̄t2̄ tā + 12 ̄t̄ t2ā
Now we assume that the velocity bound is not violated, such that t4 = t3 . Then due to symmetry
of the profile we have:
x(t7 ) = 2x(t3 ) = 2̄t3̄ + 3̄t2̄ tā + ̄t̄ t2ā
(23)
15
m
1
m
2
By setting x̄ = x(t7 ) we can then solve tā from the following:
(̄t̄ ) · t2ā + (3̄t2̄ ) · tā + (2̄t3̄ − x̄) = 0
t2ā + (3t̄ ) · tā + (2t2̄ −
x̄
̄t̄ )
(24)
= 0
As tā must be positive to make sense, the solution is:
tā = −1 21 t̄ +
1
2
q
−1 12 t̄ +
=
9t2̄ − 8t2̄ +
1
2
q
t2̄ +
4x̄
̄t̄
(25)
4x̄
̄t̄
Note that tā >= 0 follows immediately from x̄ >= 2̄t3̄ . Hence, we now have determined tā
under the assumption that the velocity bound will not be violated: equation 25 establishes steps
8 and 9 of the algorithm.
Step 10 introduces the velocity bound again; the maximal velocity is obtained at t3 (see 22):
v̂ = v(t3 ) = ̄t2̄ + ̄t̄ tā
(26)
If v̂ > v̄ the bound is violated and we can recalculate tā as follows:
tā =
v̄ − ̄t2̄
v̄
v̄
=
− t̄ = − t̄
̄t̄
̄t̄
ā
(27)
Now we have determined t̄ and tā to be the time-optimal solution under the restriction of the
given bounds (step 11).
As the next lower derivative is already v, we can skip step 12 and continue with step 13: the
determination of the constant velocity time interval tv̄ such that the required total displacement
x̄ is obtained. From the previous calculations follows that tv̄ = 0 if tā is according to equation 25.
But if tā is reduced according to equation 27, we will have x(t7 ) < x̄ and we must add a constant
velocity phase to the trajectory such that x(t4 )−x(t3 ) = x̄−2x(t3 ). With equation 22 this implies
that we can calculate tv̄ as:
tv̄ =
x̄ − 2̄t3̄ − 3̄t2̄ tā − ̄t̄ t2ā
v̄
(28)
This completes the calculation of the characteristics of the time-optimal third order profile for a
given distance x̄ and given bounds v̄, ā and ̄. The total displacement x̄ can be expressed as a
function of ̄ and the times t̄ , tā and tv̄ :
x̄ = ̄ 2t3̄ + 3t2̄ tā + t̄ t2ā + v(t3 )tv̄
= ̄ 2t3̄ + 3t2̄ tā + t̄ t2ā + t2̄ tv̄ + t̄ tā tv̄
(29)
16
m
6
1
m
2
Fourth order trajectory planning
This chapter will provide the derivations and calculations necessary to ‘fill in’ the algorithm developed in chapter 4 for fourth order trajectory planning. First the main algorithm will be derived,
analogous to the third order algorithm in the previous chapter. Section 6.2 will then give the
derivation of the solution of the third order polynomial equation that will appear in one of the
steps of this main algorithm.
6.1 Main algorithm
Again we will assume that a symmetrical trajectory must be planned for a point to point move
over a distance x̄. We have bounds on velocity (v̄), acceleration (ā), jerk (̄) and derivative of jerk
¯ The trajectory planning algorithm will now be based on the construction of a derivative of
(d).
jerk profile that can be integrated four times to obtain the fourth order position trajectory. A
symmetrical trajectory is completely determined by four time intervals: the constant derivative of
jerk interval td¯, the constant jerk interval t̄ , the constant acceleration interval tā and the constant
velocity interval tv̄ . The resulting profiles are given in figure 7.
Including the starting time of the trajectory at t0 , there are now sixteen time instances at
which the derivative of jerk changes.
For step 1 of the algorithm, we will discard the bounds on jerk, acceleration and velocity, and
only consider the bound on the derivative of jerk. This implies tv̄ = 0, tā = 0 and t̄ = 0 and it is
clear that this will provide us with a lower bound for the trajectory execution time tx̄ = 8 × td¯.
¯ For this we make
To obtain td¯ (step 2), we need the relation between td¯ and x̄ with given d.
¯
¯
use of the constant value of derivative of jerk of +d or −d during each interval. If we set the
time instance of derivative of jerk change to 0, it is easily verified that for any time t during the
constant derivative of jerk interval the fourth order profiles for acceleration, velocity and position
can be expressed as follows:
(t)
= d0 t + 0
a(t) =
v(t) =
x(t) =
1
2
2 d0 t + 0 t + a0
1
1
3
2
6 d0 t + 2 0 t + a0 t + v0
1
1
4
3
2
24 d0 t + 6 0 t + a0 t + v0 t
(30)
+ x0
Because we assume that the bounds on jerk, acceleration and velocity are not violated, we have
t̄ = 0, tā = 0 and tv̄ = 0, and consequently from figure 7: t2 = t1 = td¯, t4 = t3 = 2td¯,
t6 = t5 = 3td¯ and t8 = t7 = 4td¯. Hence:
(t1 )
=
a(t1 ) =
v(t1 ) =
x(t1 ) =
¯¯
dt
d
1 ¯2
dt ¯
2 d
1 ¯3
6 dtd¯
1 ¯4
24 dtd¯
(31)
17
m
1
m
2
Fourth order trajectory profiles
d [m/s4]
1000
0 t t
0 1
−1000
t t
2 3
t t
4 5
t t
8 9
t t
6 7
t t
t t
10 11
t14t15
12 13
0
0.2
0.4
0.6
0.8
1
1.2
0
0.2
0.4
0.6
0.8
1
1.2
0
0.2
0.4
0.6
0.8
1
1.2
0
0.2
0.4
0.6
0.8
1
1.2
0
0.2
0.4
0.6
0.8
time [s]
1
1.2
j [m/s3]
50
0
−50
a [m/s2]
5
0
−5
v [m/s]
1.5
1
0.5
0
x [m]
1
0.5
0
Figure 7: Fourth order trajectory planning.
18
m
1
m
2
¯
and for the next period in which d = −d:
¯ ¯ + (t2 )
= −dt
d
1 ¯2
a(t3 ) = − 2 dtd¯ + (t2 )td¯ + a(t2 )
¯ 3¯ + 1 (t2 )t2¯ + a(t2 )t ¯ + v(t2 )
v(t3 ) = − 1 dt
(t3 )
x(t3 ) =
d
6 d
2
d
1 ¯4
− 24
dtd¯ + 16 (t2 )t3d¯ + 12 a(t2 )t2d¯ +
(32)
v(t2 )td¯ + x(t2 )
Substitution gives:
(t3 )
= 0
¯ 2¯
a(t3 ) = dt
d
¯ 3¯
v(t3 ) = dt
(33)
d
x(t3 ) =
7 ¯4
12 dtd¯
Again using equation 30, we can calculate the results of the subsequent periods as:
¯¯
= −dt
d
1 ¯2
a(t5 ) = 2 dtd¯
¯ 3¯
v(t5 ) = 1 5 dt
(t5 )
x(t5 ) =
(34)
6 d
1 ¯4
2 24
dtd¯
and:
(t7 )
= 0
a(t7 ) = 0
¯ 3¯
v(t7 ) = 2dt
d
¯ 4¯
x(t7 ) = 4dt
(35)
d
¯ 4¯. From this we can calculate t ¯ as
and due to symmetry of the profile: x(t15 ) = 2x(t7 ) = 8dt
d
d
required by steps 1 and 2 of the trajectory planning algorithm as:
r
td¯ =
4
x̄
8d¯
(36)
To continue with step 3 we have that the maximal velocity is obtained at time instance t7 . Hence,
from equation 35 follows:
¯ 3¯
v̂ = v(t7 ) = 2dt
d
(37)
If v̂ > v̄ we must recalculate td¯ as:
r
td¯ =
3
v̄
2d¯
(38)
which establishes step 4. Next, step 5 follows from the calculation of maximal acceleration obtained at time instance t3 . From equation 33 we have:
¯ 2¯
â = a(t3 ) = dt
d
(39)
and if â > ā we must recalculate td¯ as:
r
td¯ =
ā
d¯
(40)
19
m
1
m
2
which establishes step 6. In this case we have one further test (step 7) to do on the maximal jerk
obtained at t1 :
¯¯
̂ = (t1 ) = dt
d
(41)
and if ̂ > ̄ we must recalculate td¯ as:
̄
td¯ = ¯
d
(42)
Hence, we now have a value for td¯ that complies with all of the bounds ̄, ā and v̄.
The next thing to do is to calculate t̄ under the assumption that bounds ā and v̄ are not
violated (steps 8 and 9). To do the necessary calculations, assume that t̄ > 0, such that we now
have t2 > t1 and t6 > t5 (from figure 7). Equation 30 and the fact that d = 0 during the period
from t1 to t2 now enables us to calculate:
¯¯
dt
d
¯ 2¯ + dt
¯ ¯t̄
a(t2 ) = 12 dt
d
d
¯ 3¯ + 1 dt
¯ 2¯t̄ + 1 dt
¯ 2
v(t2 ) = 16 dt
2 d
2 d¯t̄
d
1 ¯4
¯ 3¯t̄ + 1 dt
¯2 2 1 ¯ 3
x(t2 ) = 24
dtd¯ + 16 dt
4 d¯t̄ + 6 dtd¯t̄
d
(t2 ) =
(43)
Again using equation 30 we can then calculate:
(t3 ) =
0
¯ 2¯ + dt
¯ ¯t̄
a(t3 ) = dt
d
d
1 ¯2
3
¯
¯ ¯t2
v(t3 ) = dtd¯ + 1 2 dtd¯t̄ + 12 dt
d ̄
7 ¯4
1 ¯3
3 ¯2 2
¯ ¯t3
x(t3 ) = 12 dtd¯ + 1 6 dtd¯t̄ + 4 dtd¯t̄ + 16 dt
d ̄
(44)
¯¯
−dt
d
1 ¯2
¯ ¯t̄
a(t5 ) = 2 dtd¯ + dt
d
5 ¯3
¯ 2¯t̄ + 1 dt
¯ 2
v(t5 ) = 1 6 dtd¯ + 2 12 dt
2 d¯t̄
d
1 ¯4
¯ 3¯t̄ + 1 1 dt
¯2 2 1 ¯ 3
x(t5 ) = 2 24
dtd¯ + 3 16 dt
4 d¯t̄ + 6 dtd¯t̄
d
(45)
(t5 ) =
¯¯
−dt
d
1 ¯2
a(t6 ) = 2 dtd¯
¯ 3¯ + 3dt
¯ 2¯t̄ + dt
¯ ¯t2
v(t6 ) = 1 5 dt
(t6 ) =
x(t6 ) =
6 d
1 ¯4
2 24
dtd¯ +
(t7 ) =
0
d
¯ 3¯t̄
5dt
d
+
d ̄
¯ 2¯t2
4dt
d ̄
(46)
¯ ¯t3
+ dt
d ̄
a(t7 ) = 0
¯ 3¯ + 3dt
¯ 2¯t̄ + dt
¯ ¯t2
v(t7 ) = 2dt
d ̄
d
d
¯ 4¯ + 8dt
¯ 3¯t̄ + 5dt
¯ 2¯t2 + dt
¯ ¯t3
x(t7 ) = 4dt
d
d
d ̄
(47)
d ̄
Now with t8 = t7 and due to symmetry of the profile we have:
¯ 4¯ + 16dt
¯ 3¯t̄ + 10dt
¯ 2¯t2 + 2dt
¯ ¯t3
x(t15 ) = 2x(t7 ) = 8dt
d ̄
d
d
d ̄
(48)
20
m
1
m
2
By setting x̄ = x(t15 ) we can then solve t̄ from the following:
¯ ¯)t3 + (10dt
¯ 2¯)t2 + (16dt
¯ 3¯)t̄ + (8dt
¯ 4¯ − x̄) = 0
(2dt
d ̄
d ̄
d
d
⇒ t3̄ + (5td¯)t2̄ + (8t2d¯)t̄ + (4t3d¯ −
x̄
¯ ¯)
2dt
d
=0
(49)
The derivation of the solution of this third order polynomial equation is given in section 6.2.
There it will be proven that there always exists a positive (or zero) real solution:
p
:= − 19 t2d¯
q
1 3
:= − 27
td¯ −
x̄
¯¯
4dt
d
D := p3 + q 2
r
:=
t̄
=
q
3
−q +
r−
p
r
√
(50)
D
− 53 td¯
After this, step 10 introduces the velocity and acceleration bounds again; the maximal velocity is
obtained at t7 (see 47):
¯ 3¯ + 3dt
¯ 2¯t̄ + dt
¯ ¯t2
v̂ = v(t7 ) = 2dt
d ̄
d
d
(51)
If v̂ > v̄ the bound is violated and we can recalculate t̄ from the second order polynomial:
v̄
t2̄ + 3td¯t̄ + 2t2d¯ − ¯ = 0
dtd¯
(52)
The positive real solution for this is:
v
u 2
ut¯
1
v̄
t̄ = −1 td¯ + t d + ¯
2
4
(53)
dtd¯
Next, the maximal acceleration is obtained at t3 (see 44):
¯ 2¯ + dt
¯ ¯t̄
â = a(t3 ) = dt
d
d
(54)
If â > ā we can recalculate t̄ as:
t̄ =
ā
− td¯
̄
(55)
and we arrive at step 11 of the planning algorithm.
In accordance with step 12 there is one further extension of the trajectory required for the
calculation of tā . To do the necessary calculations, assume that tā > 0, such that we now have
t4 > t3 (from figure 7). Equation 30 and the fact that d = 0 and  = 0 during the period from t3
to t4 now enables us to calculate:
(t4 ) =
0
¯ 2¯ + dt
¯ ¯t̄
a(t4 ) = dt
d
d
1¯ 2
¯ 3¯ + 1 1 dt
¯2
¯2
¯
v(t4 ) = dt
2 d¯t̄ + 2 dtd¯t̄ + dtd¯tā + dtd¯t̄ tā
d
¯ 4¯ + 1 1 dt
¯ 3¯t̄ + 3 dt
¯ 2¯t2 + 1 dt
¯ ¯t3 +
x(t4 ) = 7 dt
12 d
6 d
4
1 ¯2 2
1¯
2+
dt
t
+
dt
t
t
2 d¯ ā
2 d¯ ̄ ā
1¯ 2
2 dtd¯t̄ tā
d ̄
¯ 3¯tā
dt
d
(56)
d ̄
¯ 2¯t̄ tā +
1 12 dt
d
6
+
21
m
¯¯
−dt
d
¯ 2¯ + dt
¯ ¯t̄
a(t5 ) = 12 dt
d
d
1¯ 2
¯ 3¯ + 2 1 dt
¯2
¯2
¯
v(t5 ) = 1 56 dt
2 d¯t̄ + 2 dtd¯t̄ + dtd¯tā + dtd¯t̄ tā
d
1 ¯4
1 ¯3
1 ¯2 2
1¯ 3
x(t5 ) = 2 24 dtd¯ + 3 6 dtd¯t̄ + 1 4 dtd¯t̄ + 6 dtd¯t̄ +
1 ¯2 2
¯ ¯t̄ t2 + 2dt
¯ 3¯tā + 2 1 dt
¯ 2¯t̄ tā +
dt ¯t + 1 dt
1
m
2
(t5 ) =
2 d ā
2
1¯ 2
dt
t
t
¯
ā
̄
2 d
d
ā
2
d
(57)
d
¯¯
−dt
d
¯ 2¯
a(t6 ) = 12 dt
d
¯ 3¯ + 3dt
¯ 2¯t̄ + dt
¯ ¯t2 + dt
¯ 2¯tā + dt
¯ ¯t̄ tā
v(t6 ) = 1 56 dt
d ̄
d
d
d
d
¯ 4¯ + 5dt
¯ 3¯t̄ + 4dt
¯ 2¯t2 + dt
¯ ¯t3 +
x(t6 ) = 2 1 dt
(t6 ) =
24
d
d
1¯
1 ¯2 2
2
dt
t
+
dt
2 d¯ ā
2 d¯t̄ tā
¯ ¯t2 tā
1 21 dt
d ̄
+
d ̄
¯ 3¯tā
2dt
d
+
(t7 ) =
0
a(t7 ) =
1 ¯2
2 dtd¯
¯ 3¯ + 3dt
¯ 2¯t̄ + dt
¯ ¯t2 + dt
¯ 2¯tā + dt
¯ ¯t̄ tā
2dt
d ̄
d
d
d
d
¯ 4¯ + 8dt
¯ 3¯t̄ + 5dt
¯ 2¯t2 + dt
¯ ¯t3 +
4dt
d ̄
d
d
d ̄
1¯
1 ¯2 2
2 + 3dt
3 t + 4 1 dt
¯
¯2
dt
t
+
dt
t
t
2 d¯ ā
2 d¯ ̄ ā
2 d¯t̄ tā +
d¯ ā
¯ ¯t2 tā
1 21 dt
d ̄
v(t7 ) =
x(t7 ) =
(58)
d ̄
¯ 2¯t̄ tā +
3 12 dt
d
(59)
Now, given that td¯ and t̄ are already determined and the planned trajectory is symmetrical, we
can solve tā such that x(t15 ) = 2x(t7 ) = x̄ from the following polynomial equation:
¯ 2¯ + dt
¯ ¯t̄ }t2 + {6dt
¯ 3¯ + 9dt
¯ 2¯t̄ + 3dt
¯ ¯t2 }tā +
{dt
ā
d
d ̄
d
d
d
4
3
2
2
3
¯
¯
¯
¯
{8dt ¯ + 16dt ¯t̄ + 10dt ¯t + 2dt ¯t − x̄} = 0
d
d ̄
d
(60)
d ̄
Clearly tā must be the positive root of this second order polynomial (compare with equation 25).
Now we can introduce the velocity bound again; the maximal velocity is obtained at t7 (see
59):
¯ 3¯ + 3dt
¯ 2¯t̄ + dt
¯ ¯t2 + dt
¯ 2¯tā + dt
¯ ¯t̄ tā
v̂ = 2dt
d ̄
d
d
d
d
If v̂ > v̄ the bound is violated and we can recalculate tā as:
¯ 3¯ − 3dt
¯ 2¯t̄ − dt
¯ ¯t2
v̄ − 2dt
d ̄
d
d
tā =
2
¯
¯
dt ¯ + dt ¯t̄
d
(61)
(62)
d
Now we have determined td¯, t̄ and tā under the restriction of the given bounds and, as the next
derivative is v, this establishes step 12 of the algorithm.
Finally, step 13 will determine the constant velocity time interval tv̄ such that the required total
displacement x̄ is obtained. From the previous calculations follows that tv̄ = 0 if tā is according
to equation 60. But if tā is reduced according to equation 62, we will have x(t15 ) < x̄ and we
must add a constant velocity phase to the trajectory such that x(t4 ) − x(t3 ) = x̄ − 2x(t7 ). With
equation 59 this implies that we can calculate tv̄ as:
tv̄ =
x̄ − 2x(t7 )
v̄
(63)
22
m
1
m
2
This completes the calculation of the characteristics of the symmetrical fourth order profile for a
¯ We can further state that if the trajectory contains
given distance x̄ and given bounds v̄, ā, ̄ and d.
a constant velocity phase (i.e. tv̄ > 0), the trajectory is time-optimal. The total displacement x̄ can
be expressed as a function of d¯ and the times td¯ t̄ , tā and tv̄ :
x̄ = d¯ ( t2d¯t2ā + td¯t̄ t2ā + 6t3d¯tā + 9t2d¯t̄ tā + 3td¯t2̄ tā +
8t4d¯ + 16t3d¯t̄ + 10t2d¯t2̄ + 2td¯t3̄ + 2t3d¯tv̄ +
3t2d¯t̄ tv̄
+
td¯t2̄ tv̄
+
t2d¯tā tv̄
(64)
+ td¯t̄ tā tv̄ )
6.2 Solution of equation 49
A solution of the third order polynomial equation 49 is a valid solution t̄ if and only if the solution
is real and larger than or equal to zero. This section will consider the existence of such a solution
and derive its calculation. The results of this section are based on Cardan’s formula, as can for
instance be found in [2].
Given a third order polynomial equation (also known as a cubic equation) in variable x of the
form:
x3 + ax2 + bx + c = 0
(65)
with constant real parameters a, b and c. This can be rewritten as:
y 3 + 3py + 2q = 0
(66)
with new variable y defined as:
y = x + 13 a
(67)
and new parameters p and q defined as:
p = 13 b − 19 a2
q=
1 3
27 a
(68)
− 16 ab + 12 c
Applying this to equation 49 gives:
p = 83 t2d¯ −
q=
25 2
9 td¯
40 3
125 3
27 td¯ − 6 td¯ +
= − 19 t2d¯
2t3d¯ −
x̄
¯¯
4dt
d
1 3
= − 27
td¯ −
x̄
¯¯
4dt
d
(69)
From [2] we have that the characteristics of the solution are determined by the sign of the discriminant D:
1 3
D = p3 + q 2 = (− 19 t2d¯)3 + (− 27
td¯ −
x̄ 2
¯ ¯)
4dt
d
x̄t2d¯
1 6
x̄2
1 6
+
td¯ +
td¯ +
54d¯ 16d¯2 t2d¯
| 729 {z 729 }
|
{z
}
= −
=0
>0
(70)
>0
Now D > 0 implies that there exists one real and two complex conjugated solutions, and only
the real solution is a valid candidate for t̄ . Based on Cardan’s formula for equation 67 the real
solution can be found from:
q
q
√
√
3
3
(71)
y = −q + D + −q − D
23
m
1
m
2
such that:
x = y − 13 a ⇒ t̄ = y − 53 td¯
(72)
Next, to prove that this real solution must also be larger than or equal to zero, consider the signs
of the coefficients of equation 49:
a = 5td¯ > 0
b = 8t2d¯ > 0
c=
x̄
4t3d¯ − 2dt
¯¯
d
(73)
≤0
Now consider the function f (x) = x3 + ax2 + bx + c. Obviously, if c = 0 the solution is x = 0
(i.e. t̄ = 0). If c < 0, we have f (0) = c < 0 and f (x) is monotone increasing for x ≥ 0 and
limx→∞ f (x) = ∞. Hence, the function f (x) must cross the positive real axis, thus establishing
a positive real solution. This solution must be given by equations 71 and 72, as this is the only
real solution.
For easier implementation of this solution, equations 71 and 72 are simplified as follows:
√
−q − D − 53 td¯
√
q
√ √
√
3
3
√
3
−q+ D −q− D
√
− 53 td¯
=
−q + D +
√
3
−q+ D
√
q
3
√
−q 2 −D
3
5
=
−q + D + √
√ − 3 td¯
3
t̄ =
q
3
−q +
√
D+
q
3
(74)
−q+ D
=
q
3
−q +
√
D− √
3
p
√
−q+ D
− 53 td¯
q
3
With this result it is not necessary to calculate the second term: −q −
and also has a positive effect on accuracy of finite precision calculations.
√
D, which saves time
24
m
7
1
m
2
Implementation aspects
7.1 Switching times
From the previous chapters it is clear that the derivations for especially fourth order trajectory
planning are quite elaborate. However, the calculations actually required for implementation of
this planner are relatively straightforward. The algorithm of chapter 4 consists of a combination
of polynomial calculations with simple if-then-else tests for which most state-of-the-art motion
control hardware has standard algorithms.
When considering implementation of the planned trajectory into a feedforward control scheme,
it is important to consider the effect of discretization. This implies that the integrators and the
feedforward filter as given in the diagram of figure 5 will all have to be implemented in discrete
time at some given sampling time interval Ts . This also implies that the switching time instances
of the planned trajectory must be synchronized with the sampling time instances: i.e. the time
intervals tv̄ , tā , etc. must be multiples of Ts .
To obtain this, it must be accepted that time-optimality as resulting from the continuous time
calculations in the previous chapters is lost. The calculated time intervals must be rounded-off
towards a multiple of Ts . This must be done such that the given bounds are not violated, but at
the same time approximated as closely as possible. The approach proposed here is to extend the
algorithm of chapter 4. After each calculation or re-calculation of a time interval, the result is
rounded-off upward to the next multiple of Ts . Next the maximal value of the highest derivative
is calculated accordingly.
As an example consider the first calculation of td¯ for a fourth order trajectory (equation 36).
The rounded-off value for td¯ can be calculated as:
t0d¯ =
t¯
ceil d
Ts
× Ts
(75)
with ceil(.) denoting the rounding off towards the next higher integer. From equation 36 we can
¯
then calculate a new value for d:
x̄
d¯0 = 0 4
(76)
8td¯
¯
Note that with t0d¯ ≥ td¯ we must have d¯0 ≤ d.
It can be verified that this same approach is valid for the calculation or re-calculation of all
time intervals. It is important to note that with each new calculation of d¯0 its value must reduce.
This guarantees that none of the bounds that were checked in earlier steps of the algorithm will
be violated.
State-of-the-art motion controllers are equipped with a high resolution floating point calculation
unit, such that quantization effects are usually negligible. To check this, we can make use of equation 64 to calculate the obtained position in case a quantized d¯0 is used. The difference between
desired position and calculated position can be accounted for by means of a correction signal on
the position trajectory. Typically in a digital system, the position signal is also quantized (usually
in encoder increments). The correction signal can then be implemented by adding some increments to the position reference at each sampling instance during the trajectory. In relevant cases
only one or two correction increments at each sampling instance should be sufficient (to prevent
deterioration of the feedforward control). In case larger corrections are necessary, the accuracy
and/or sampling rate of the motion control hardware should be increased.
25
m
m
1
2
A complete algorithm for fourth order trajectory planning is given in the appendix. This algorithm is given as a Matlab .M function, which calculates the times td¯ t̄ , tā and tv̄ . It also includes
the possibility to specify a sampling time interval. It then uses equations 75 and 76 to synchronize
the switching time instances with the sampling time instances and to calculate d¯0 . Finally, it allows compensation for the effect of quantization by specifying a number of significant decimals
for d¯0 and calculating the required amount of correction increments for the position reference
signal.
7.2 Synchronization of profiles
Consider the continuous time implementation of the feedforward signal calculation as given in
figure 5. Now, the generation of the derivative of jerk profile and the four integrators to obtain jerk,
acceleration, velocity and position must be implemented in digital hardware. A straightforward
approach is to replace the continuous time integrators by forward Euler discrete time integrators
as indicated in figure 8. Clearly, all required profiles are thus calculated. However, due to the zero
d
Derivative of jerk
profile generator
Ts
-------z-1
Jerk
j
Ts
-------z-1
Acceleration
a
Ts
-------z-1
Velocity
v
Ts
-------z-1
x
Position
Figure 8: Discrete time planner using forward Euler integrators.
order hold effect, each of the four integrators introduces a specific delay time. This can be seen
in figure 9, in which the discrete time profiles are compared with the corresponding continuous
time profiles. Note that the chosen sampling time is 0.05 seconds, which is large in relation with
the required trajectory to show the discretization effect more clearly. To fix this effect, the higher
order profiles can be delayed individually such that the symmetry of the complete set of profiles
is restored. Figure 10 shows the effect of this in comparison with the continuous time profiles.
The latter are delayed with 0.1 seconds (or 2 times the sample time interval), to show that the
results are now perfectly synchronized. Note that the derivative of jerk profile must be delayed
with 2 × Ts , the jerk profile with 1.5 × Ts , the acceleration profile with 1 × Ts and finally the
velocity profile with 0.5 × Ts . To obtain a delay of 0.5 × Ts when sampling with Ts the average
value is taken from the current and previous amplitude of the considered profile. This operation
appears to work very well, although the associated smoothing effect is undesirable.
26
m
1
m
2
position, velocity, acceleration, jerk, derivative of jerk
Normalized Discrete time fourth order profiles: not synchronized
compared with continuous time profiles
0
0.2
0.4
0.6
time [s]
0.8
1
1.2
Figure 9: Discrete time profiles using forward Euler integrators, compared with continuous time
profiles.
position, velocity, acceleration, jerk, derivative of jerk
Normalized Discrete time fourth order profiles: synchronized
continuous time profiles delayed by 0.1 s
0
0.2
0.4
0.6
time [s]
0.8
1
1.2
Figure 10: Synchronized discrete time profiles using forward Euler integrators, compared with
continuous time profiles delayed with 0.1 seconds.
27
m
m
1
2
7.3 Implementation of first order filter
All required profiles for calculation of the feedforward signal are now available. The multiplication with factors q1 to q4 followed by summation as indicated in figure 5 is straightforward. The
first order filtering is less trivial, as it must also be transferred to the discrete time domain. A
possible implementation that prevents problems with unwanted time delays and gives good results is to make use of the trapezoidal integration method. Figure 11 shows the equivalence of the
first order filter in figure 5 with several possible implementations using the trapezoidal integration method. Note that the inherent disadvantage of using the trapezoidal integration method in
1
u
y
k12.s+c
Continuous time
1
s
1/k12
u
y
c/k12
Algebraic loop !!
Ts(z+1)
-------------2(z-1)
1/k12
u
ZOH
y
Discrete-Time
Integrator: Trapezoidal
c/k12
Algebraic loop !!
1/k12
u
1
Ts
1/2
z
y
ZOH
c/k12
NO algebraic loop !!
-K-
u
ZOH
1
z
y
Ts/(2*k12+c*Ts)
-K(2*k12-c*Ts)/(2*k12+c*Ts)
Figure 11: Discrete implementation of first order filter using the trapezoidal integration method.
a first order filter—the occurrence of an algebraic loop—can be prevented by re-calculating the
loop. This can be checked by verifying that the discrete implementations all have the following
28
m
1
m
2
transfer function:
y=
Ts
2k12 +cTs (z + 1)
u
12 −cTs
z − 2k
2k12 +cTs
(77)
7.4 Calculation of reference trajectory
A final point on synchronization must be made with respect to the calculation of the reference
trajectory that is used for feedback control. When applying the feedforward signal as calculated
above, based on the synchronized profiles as illustrated in figure 10, the actual plant’s response
will be close to the ideal continuous time response with a delay of 2 × Ts . However, in order
to compare this signal with the reference trajectory it must be sampled with the same sampling
frequency as is used for generation of the reference trajectory. The sample and hold device used
for this will then introduce an average delay of exactly 0.5 × Ts .
Hence, to compensate for this further delay, it is necessary to also delay the reference trajectory with this same value. This additional delay can be implemented in the same manner as
mentioned before: by taking the average between the current and previous reference trajectory
value. The result of this will be that the control error will not be affected by sampling. The controller will only act on the effects of disturbances and on discrepancies between the actual plant
and the modelled fourth order behavior.
Obviously, this is only true if the sampling frequency is sufficiently high: otherwise the momentary control error may deviate significantly from the average value. If this is the case, an
increase in sampling frequency must be considered. Usually however, the sampling frequency
is more significantly determined by the demands on stability and performance of the (digital)
feedback controller.
29
m
8
1
m
2
Simulation results
A theoretical motivation for the application of fourth order feedforward as an extension of rigid
body feedforward was already given in chapter 3. Next, subsequent chapters have shown that
fourth order feedforward is feasible in the sense of trajectory planning and (digital) feedforward
implementation. This chapter will give further motivation for application of fourth order feedforward by considering some simulation results.
The main concern when considering model based feedforward control is the occurrence of
discrepancies between the behavior of the actual motion system and the used model. This often
motivates the use of rigid body feedforward, as the total mass of the motion system is usually
known within tight boundaries. Furthermore, a well designed motion system will have limited
friction and damping: especially dry friction must be minimized as it leads to classical problems
in both feedforward and feedback. Linear viscous damping can more easily be dealt with, but
often shows time dependent behavior: if damping has a reasonably constant value, it can be compensated as shown in chapter 2. Now, fourth order feedforward introduces several additional
physical parameters that may, or may not be constant: a mass ratio (division of mass in m1 and
m2 ), a damping ratio (division of damping in k1 and k2 ), a spring stiffness c and an internal
damping k12 (see figures 1 and 4). The simulation results in this chapter will show that fourth
order feedforward will give a significant improvement of performance (in the sense of servo errors during or after trajectory execution) in comparison with rigid body feedback, even if these
additional parameters have relatively large uncertainties.
All simulations will be performed using the configuration of figure 5. The motion system
parameters and their variations used in the simulations are given in table 1 The trajectory paParameter
m1
m2
k1
k2
c
k12
Value
20
10
10
10
6 · 105
500
Unit
Kg
Kg
Ns/m
Ns/m
N/m
Ns/m
Variation
m1 ∈ {15 · · · 25},
m2 = 30 − m1
k1 ∈ {5 · · · 15},
k2 = 20 − k1
±33%
±100%
Table 1: Simulation parameters
rameters are defined in table 2. When comparing with rigid body feedforward, the fourth order
Parameter
derivative of jerk
jerk
acceleration
velocity
displacement
Symbol
d¯
̄
ā
v̄
x̄
Value
1000
50
5
1
1
Unit
m/s4
m/s3
m/s2
m/s
m
Table 2: Trajectory definition parameters
trajectory will be used, such that the smoothing effect of using a high order trajectory is not
accountable for the difference. In that case, it can easily be verified that optimal rigid body feedforward is obtained by setting m1 = 30, m2 = 0, k1 = 20, k2 = 0 and k12 = 0. From
30
m
m
1
2
equation 9 we then have q1 = q2 = 0, q3 = m1 c and q4 = k1 c. Furthermore, the first order
filter reverts to a constant gain of 1c (see also equation 77) and equation 10 reverts to equation 1.
Note that the value of c becomes unimportant as it drops out of the calculations. To remove the
effect of feedback control from the results, most simulations are performed in open loop.
Figure 12 shows the performance of the continuous time fourth order feedforward control
for a fourth order motion system model with perturbations according to table 1. For comparison,
the response of the nominal motion system model with the optimal rigid body feedforward controller is given. Note that in spite of the significant plant variations, the fourth order feedforward
controller performs at least twice as good.
8
Open loop fourth order feedforward response of servo error with plant variations
−5
Dashed line gives nominal rigid body FF response
x 10
6
Position error [m]
4
2
0
−2
−4
−6
0
0.5
1
1.5
time [s]
2
2.5
3
Figure 12: Open loop simulation results of fourth order feedforward controller with plant variations, in comparison with optimally tuned rigid body feedforward.
The approach of choosing the parameters such that fourth order feedforward reverts to rigid
body feedforward suggests the use of third order feedforward as an intermediate form. This
can be done by setting m1 = 30 and m2 = 0, while leaving the viscous damping parameters
unchanged: now only q1 reverts to zero. This would allow to implement the simpler third order
trajectory planner, with jerk being the highest bounded derivative. However, figure 13 shows
that the addition of a jerk feedforward term (i.e. q2 ) does not have a significant effect: it is the
derivative of jerk term q1 that makes the difference. The fact that in practice it appears that third
order trajectory planning may lead to strong improvement of performance is therefore almost
exclusively due to smoothing.
Figure 14 shows that the servo error responses will not significantly deteriorate if fourth order
feedforward is implemented in discrete time. As an example, the motion system with minimal
31
m
2
Open loop feedforward response of servo error
Optimal second, third and fourth order feedforward
−5
8
m
1
x 10
second order FF
third order FF
fourth order FF
6
Position error [m]
4
2
0
−2
−4
−6
0
0.5
1
1.5
time [s]
2
2.5
3
Figure 13: Open loop simulation results of optimal second, third and fourth order feedforward
controller with nominal plant
spring-stiffness is considered. Both open loop and closed loop results are given: the feedback
controller is tuned for a bandwidth of about 10 Hz, whereas the motion system’s first resonance
mode is at 50 Hz. The digital feedforward controller is combined with a discrete time feedback
controller, both with a sampling rate of 200 Hz. Note that this is a low sampling rate for a high
performance servo system: this is chosen to demonstrate the discretization effects more clearly.
32
m
−5
4
x 10
m
1
2
Digital feedforward response of servo error: Ts=5e−3
Optimal fourth order feedforward with minimal spring−stiffness
−6
x 10
8
3
6
4
2
2
Position error [m]
0
−2
1
−4
0.3
0.35
0.4
0.45
0
−1
−2
−3
0
0.5
1
1.5
time [s]
2
2.5
3
Figure 14: Simulation results of open loop and closed loop discrete time fourth order feedforward
controller with minimal stiffness plant. Thick lines: discrete, thin lines: continuous time.
33
m
9
1
m
2
Conclusions
For high performance motion control, especially for electromechanical motion systems, the usefulness of feedforward control is well known and often implemented. This report shows that
the popular simple feedforward scheme known as ‘mass-feedforward’, ‘rigid-body feedforward’
or ‘second-order feedforward’ can be extended to higher order feedforward while still maintaining important practical properties like time-optimality, actuator effort limitation, reliability, implementability and accuracy. Furthermore, it is argumented that the increase in complexity is
manageable in state-of-the-art motion control hardware.
Third order feedforward, which is increasingly applied in practice, appears to be mainly effective due to ‘smoothing’ of the trajectory, i.e. reduction of the high frequency content of the
trajectory, such that feedback control can be more effective. The use of fourth order feedforward
for high performance electromechanical motion systems is motivated by considering an appropriate model and supported by simulation results. Apart from the mentioned smoothing effect,
the feedforward of the derivative of jerk profile appears to result in a significant performance
improvement. This is even more remarkable when considering that, for relevant cases, the calculated feedforward force is hardly affected.
A high level algorithm is given to calculate higher order trajectories for point-to-point moves;
other motion commands, like speed change operations, can be derived from this. For third and
fourth order trajectory planning, the details of the algorithm are worked out, resulting in practical,
reliable and accurate algorithms.
Further implementation issues, like discrete time calculations, quantization effects and synchronization, are explicitly addressed. The trajectory planning algorithms can be implemented
such that switching times are exactly synchronized with sampling instances. A digital implementation is suggested that takes care of the synchronization of the various profiles with each other
and the position trajectory, and also with the measured position. It is shown that deterioration
of the continuous time results due to sampling are small when applying a sufficient sampling
rate. Experience shows that a sampling rate that is required for stable feedback control is also
sufficient for feedforward control.
Simulation results show that the improvement obtained with fourth order feedforward is not
overly sensitive to variations in the parameters that are additional to the ‘classic’ parameters of
rigid-body feedforward (i.e. mass and viscous damping). Obviously, the feedforward performance
will improve if the dynamical behavior of the actual motion system is closer to that of the fourth
order model and said parameters are known within tighter bounds. An important advantage of
the suggested implementation is the possibility to manually fine-tune the feedforward amplification factor for each profile (the q factors). This can be seen as a simple, direct extension of the
well known practice of fine-tuning rigid-body feedforward.
34
m
1
m
2
References
[1] M. Boerlage, M. Steinbuch, P. Lambrechts, M. van de Wal, ‘Model based feedforward for
motion systems’. Submitted, 2003.
[2] I.N. Bronshtein, K.A. Semendyayev, ‘A ’Guide book to mathematics’. Verlag Harri Deutsch,
Frankfurt, ISBN 3-87144-095-7, 1973.
[3] S. Devasia, ‘Robust inversion-based feedforward controllers for output tracking under plant
uncertainty’. Proc. of the American Control Conference, 2000, pp.497-502.
[4] B.G. Dijkstra, N.J. Rambaratsingh, C. Scherer, O.H. Bosgra, M. Steinbuch, S. Kerssemakers,
‘Input design for optimal discrete-time point-to-point motion of an industrial XY positioning
table’, in Proc. 39th IEEE Conference on Decision and Control, 2000, pp.901-906.
[5] L. Hunt, G. Meyer, ‘Noncausal inverses for linear systems’. IEEE Trans. on Automatic Control,
1996, Vol. 41(4), pp.608-611.
[6] P.H. Meckl, ‘Discussion on: comparison of filtering methods for reducing residual vibration’. European Journal of Control, 1999, Vol. 5, pp.219-221.
[7] P.H. Meckl, P.B. Arestides, M.C. Woods, ‘Optimized S-curve motion profiles for minimum
residual vibration’. Proc. of the American Control Conference, 1998, pp.2627-2631.
[8] B.R. Murphy, I. Watanaabe, ‘Digital shaping filters for reducing machine vibration’. IEEE
Trans. on Robotics and Automation, 1992, Vol. 8(2), pp.285-289.
[9] F. Paganini, A. Giusto, ‘Robust synthesis of dynamic prefilters’, Proc. of the American Control
Conference, 1997, pp.1314-1318.
[10] H. S. Park, P.H. Chang, D.Y. Lee, ‘Concurrent design of continuous zero phase error tracking controller and sinusoidal trajectory for improved tracking control’. J. Dynamic Systems,
Measurement, and Control, 2001, Vol. 5, pp.3554-3558.
[11] D. Roover, ‘Motion control for a wafer stage’, Delft University Press, The Netherlands, 1997.
[12] D. Roover, F. Sperling, ‘Point-to-point control of a high accuracy positioning mechanism’,
Proc. of the American Control Conference, 1997, pp.1350-1354.
[13] N. Singer, W. Singhose, W. Seering, ‘Comparison of filtering methods for reducing residual
vibration’. European Journal of Control, 1999, Vol.5, pp.208-218.
[14] M. Steinbuch, M.L. Norg, ‘Advanced motion control: an industrial perspective’, European
Journal of Control, 1998, pp.278-293.
[15] M. Tomizuka, ‘Zero phase error tracking algorithm for digital control’. J. Dynamic Systems,
Measurement, and Control, 1987, Vol.109, pp.65-68.
[16] D.E. Torfs, J. Swevers, J. De Schutter, ‘Quasi-perfect tracking control of non-minimal phase
systems’. Proc. of the 30th Conference on Decision and Control, 1991, pp.241-244.
[17] D.E. Torfs, R. Vuerinckx, J. Swevers, J. Schoukens, ‘Comparison of two feedforward design
methods aiming at accurate trajectory tracking of the end point of a flexible robot arm’, IEEE
Trans. on Control Systems Technology, 1998, Vol.6(1), pp.1-14.
35
m
A
1
m
2
List of symbols
x̄
x̂
x0
tx̄
ti , i ∈ N
bound on |x|
maximum value obtained by |x| if bound is not considered
initial value
time interval during which |x| obtains its bound
switching time instances
D
F
Ts
a
b
c
d

k
m
p
q
q1···4
r
s
t
u
v
x
y
z
discriminant of cubic equation
actuator force, feedforward force [N]
sampling time [s]
acceleration (profile) [m/s2 ], parameter of cubic equation
parameter of cubic equation
spring stiffness [N/m], parameter of cubic equation
derivative of jerk (profile) [m/s4 ]
jerk (profile) [m/s3 ]
viscous damping coefficient [Ns/m]
mass [Kg]
parameter of cubic equation
parameter of cubic equation
feedforward parameters
auxiliary parameter
Laplace transform variable
time [s]
input signal
velocity (profile) [m/s]
position, displacement (profile) [m]
variable, output signal
shift operator
36
m
B
1
m
2
The Matlab function MAKE4.m
function [t,dd]=make4(varargin)
%
%
%
%
%
%
%
%
%
%
%
%
%
%
%
%
%
%
%
%
%
%
%
%
%
%
%
%
%
%
%
%
%
%
%
%
%
%
%
%
%
[t,dd] = make4(p,v,a,j,d,Ts,r,s)
Calculate timing for symmetrical 4th order profiles.
inputs:
p
v
a
j
d
Ts
r
s
=
=
=
=
=
=
=
=
desired path (specify positive)
[m]
velocity bound (specify positive)
[m/s]
acceleration bound (specify positive)
[m/s2]
jerk bound (specify positive)
[m/s3]
derivative of jerk bound (specify positive) [m/s4]
sampling time
[s]
(optional, if not specified or 0: continuous time)
position resolution
[m]
(optional, if not specified: 10*eps)
number of decimals for digitized
derivative of jerk bound
(optional, if not specified: 15)
outputs:
t(1)
t(2)
t(3)
t(4)
=
=
=
=
constant
constant
constant
constant
djerk phase duration
jerk phase duration
acceleration phase duration
velocity phase duration
t1
t1
t1
t1
.-.
.-.
.-.
.-.
| |
| |
| |
| |
| |t2
t3
t2| |
t4
t2| | t3 | |t2
’-’--.-.----.-.--’ ’---------.-.--’-’----’-’--.-.-| |
| |
| |
| |
| |
| |
| |
| |
’-’
’-’
’-’
’-’
t1
t1
t1
t1
In case of discrete time, derivative of jerk bound d is reduced to dd and
quantized to ddq using position resolution r and number of significant decimals s
Two position correction terms are calculated to ’repair’ the position error
resulting from using ddq instead of dd:
cor1 gives the number of position increments that can equally be divided
over the entire trajectory duration
cor2 gives the remaining number of position increments
The result is given as:
dd = [ ddq cor1 cor2 dd ]
% Paul Lambrechts, TUE fac. WTB, last modified: April 1, 2003.
%
% Checking validity of inputs
if nargin < 5 | nargin > 8
help make4
return
else
p=abs(varargin{1});
v=abs(varargin{2});
a=abs(varargin{3});
j=abs(varargin{4});
d=abs(varargin{5});
if nargin == 5
Ts=0; r=eps; s=15;
elseif nargin == 6
Ts=abs(varargin{6});
r=eps; s=15;
elseif nargin == 7
37
m
1
m
2
Ts=abs(varargin{6});
r=abs(varargin{7});
s=15;
elseif nargin == 8
Ts=abs(varargin{6});
r=abs(varargin{7});
s=abs(varargin{8});
end
end
if length(p)==0 | length(v)==0 | length(a)==0 | length(j)==0 | length(d)==0 | ...
length(Ts)==0 | length(r)==0 | length(s)==0
disp(’ERROR: insufficient input for trajectory calculation’)
return
end
tol = 3*eps;
% tolerance required for continuous time calculations
dd = d;
% required for discrete time calculations
% Calculation constant djerk phase duration: t1
t1 = (p/(8*d))^(1/4) ; % largest t1 with bound on derivative of jerk
if Ts>0
t1 = ceil(t1/Ts)*Ts;
dd = p/(8*(t1^4));
end
% velocity test
if v < 2*dd*t1^3
% v bound violated ?
t1 = (v/(2*d))^(1/3) ; % t1 with bound on velocity not violated
if Ts>0
t1 = ceil(t1/Ts)*Ts;
dd = v/(2*(t1^3));
end
end
% acceleration test
if a < dd*t1^2
% a bound violated ?
t1 = (a/d)^(1/2) ; % t1 with bound on acceleration not violated
if Ts>0
t1 = ceil(t1/Ts)*Ts;
dd = a/(t1^2);
end
end
% jerk test
if j < dd*t1
% j bound violated ?
t1 = j/d ;
% t1 with bound on jerk not violated
if Ts>0
t1 = ceil(t1/Ts)*Ts;
dd = j/t1;
end
end
d = dd; % as t1 is now fixed, dd is the new bound on derivative of jerk
% Calculation constant jerk phase duration: t2
% largest t2 with bound on jerk
P = -1/9*t1^2;
Q = -1/27*t1^3-p/(4*d*t1);
D = sqrt(P^3+Q^2);
% D = sqrt( p/d * (t1^2/54 + p/t1^2/(16*d))); % alternative calculation
R = (-Q+D)^(1/3);
t2 = R - P/R - 5/3*t1;
% is solution of: t2^3+5*t1*t2^2+8*t1^2*t2+4*t1^3-p/(2*d*t1) = 0
if Ts>0
t2 = ceil(t2/Ts)*Ts;
dd = p/( 2*t1*(4*t1^3+t2*(8*t1^2+t2*(5*t1+t2))) );
% = p/( 8*t1^4 + 16*t1^3*t2 + 10*t1^2*t2^2 + 2*t1*t2^3 );
end
if abs(t2)<tol t2=0; end % for continuous time case
38
m
1
m
2
% velocity test
if v < (t1*dd*(2*t1^2 + t2*(3*t1 + t2)));
% = (2*dd*t1^3 + 3*dd*t1^2*t2 + dd*t1*t2^2)
t2 = ( (t1^2)/4 + v/(d*t1) )^(1/2) - 1.5*t1 ;
% t2 with bound on velocity not violated
if Ts>0
t2 = ceil(t2/Ts)*Ts;
dd = v/( t1*(2*t1^2 + t2*(3*t1 + t2)) );
% = v/( 2*t1^3 + 3*t1^2*t2 + t1*t2^2 )
end
end
if abs(t2)<tol t2=0; end % for continuous time case
% acceleration test
if a < ( dd*t1*(t1 + t2) )
% a bound violated ?
t2 = a/(d*t1) - t1 ;
% t2 with bound on acceleration not violated
if Ts>0
t2 = ceil(t2/Ts)*Ts;
dd = a/( t1*(t1 + t2) );
end
end
if abs(t2)<tol t2=0; end % for continuous time case
d = dd; % as t2 is now fixed, dd is the new bound on derivative of jerk
% Calculation constant acceleration phase duration: t3
c1 = t1*(t1+t2) ;
% = t1^2+t1*t2
c2 = 3*t1*( 2*t1^2 + t2*(3*t1 + t2) ) ;
% = 6*t1^3 + 9*t1^2*t2 + 3*t1*t2^2
c3 = 2*t1*( t1*( 4*t1^2 + t2*(8*t1 + 5*t2) ) + t2^3 ) ; % = 8*t1^4 + 16*t1^3*t2 + 10*t1^2*t2^2
%
+ 2*t1*t2^3
t3 = (-c2 + sqrt(c2^2-4*c1*(c3-p/d)))/(2*c1) ;
% largest t3 with bound on acceleration
if Ts>0
t3 = ceil(t3/Ts)*Ts;
dd = p/( t3*( c1*t3 + c2) + c3 );
% = p/( c1*t3^2 + c2*t3 + c3 )
end
if abs(t3)<tol t3=0; end % for continuous time case
% velocity test
if v < dd*t1*( t1*( 2*t1 + 3*t2 + t3 ) + t2*(t2+t3) )
% = 2*dd*t1^3 + 3*dd*t1^2*t2 + dd*t1*t2^2 + dd*t1^2*t3 + dd*t1*t2*t3
% t3, bound on velocity not violated
t3 = ( t1*( t1*( 2*t1 - 3*t2) - t2^2 ) + v/d ) / (t1*(t1+t2));
% = -(2*t1^3 + 3*t1^2*t2 + t1*t2^2 - v/d)/(t1^2 + t1*t2)
if Ts>0
t3 = ceil(t3/Ts)*Ts;
dd = v/( t1*( t1*( 2*t1 + 3*t2 + t3 ) + t2*(t2+t3) ) );
% = v/( 2*t1^3 + 3*t1^2*t2 + t1*t2^2 + t1^2*t3 + t1*t2*t3 )
end
end
if abs(t3)<tol t3=0; end % for continuous time case
d = dd; % as t3 is now fixed, dd is the new bound on derivative of jerk
% Calculation constant velocity phase duration: t4
% t4 with bound on velocity
t4 = ( p - d*( t3*(c1*t3 + c2) + c3) )/v ; % = ( p - d*(c1*t3^2 + c2*t3 + c3) )/v
if Ts>0
t4 = ceil(t4/Ts)*Ts;
dd = p/( t3*( c1*t3 + c2) + c3 + t4*( t1*( t1*( 2*t1 + 3*t2 + t3 ) + t2*(t2+t3) ) ) ) ;
% = p/( c1*t3^2 + c2*t3 + c3 + t4*(2*t1^3 + 3*t1^2*t2 + t1*t2^2 + t1^2*t3 + t1*t2*t3) )
end
if abs(t4)<tol t4=0; end % for continuous time case
% All time intervals are now calculated
t=[t1 t2 t3 t4] ;
% This error should never occur !!
if min(t)<0
disp(’ERROR: negative values found’)
end
% Quantization of dd and calculation of required position correction (decimal scaling)
if Ts>0
39
m
1
m
2
x=ceil(log10(dd));
% determine exponent of dd
ddq=dd/10^x;
% scale to 0-1
ddq=round(ddq*10^s)/10^s;
% round to s decimals
ddq=ddq*10^x;
% actual displacement obtained with quantized dd
pp = ddq * p/dd;
%= ddq*( c1*t3^2 + c2*t3 + c3 + t4*(2*t1^3 + 3*t1^2*t2 + t1*t2^2 + t1^2*t3 + t1*t2*t3) )
dif=p-pp;
% position error due to quantization of dd
cnt=round(dif/r); % divided by resolution gives ’number of increments’
% of required position correction
% smooth correction obtained by dividing over entire trajectory duration
tt = 8*t(1)+4*t(2)+2*t(3)+t(4);
ti = tt/Ts;
% should be integer number of samples
cor1=sign(cnt)*floor(abs(cnt/ti))*ti;
% we need cor1/ti increments correction at each
% ... sample during trajectory
cor2=cnt-cor1;
% remaining correction: 1 increment per sample
% ... during first part of trajectory
dd=[ddq cor1 cor2 dd];
else
dd=[dd 0 0 dd]; % continuous time result in same format
end
% Finished.
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
40