# Manual 21390013 ```Chapter 3 3
TRAJECTORY-PLANNING THROUGH
INTERPOLATION BY OVERLAPPING CUBIC ARCS
AND CUBIC SPLINES
3.1
Basic interpolation problem in trajectory planning
Given:
(i) a set of nodal points {Pi =(xpYi),i=O,l,...,N} along a general curve Ctobe followed by a working
point (wp) from Po to P:,< as shown in Figure 3.1,
(ii) prescribed tangential speeds of wp, So and SN at Po and PN respectively,
(iii) either prescribed tangential acceleration So at Po or SN at PN ofwp, and
(iv) dy at both Po and PN
dx
•
Then it is required to determine:
(a) an acceptable time interval [0, T] during which the curve is executed,
(b) time parametric curves X(t) and Y(t) , te[O,T] that interpolate the nodal points (xpYi),and
(c) continuous time parametric curves for velocities X(t) and yet) as well as for the accelerations
X(t) and Yet), te[O,T].
y
/\/c
P,
p,
~,(X)
PN(X)
~----------------------------__
•
x
Figure 3.1: Basic interpolation problem.
Chapter 3
90
TRAJECTORY-PLANNING THROUGH INTERPOLATION BY OVERLAPPING CUBIC ARCS AND CUBIC SPLINES
Without loss in generality consider the case in (iii) where
SN
is known at PN
•
Objectives (a), (b) and (c)
may then be achieved by following the procedure outlined in the following subsections.
3.1. 1 Determination of time parametric intervals
In order to achieve objective (a) stipulated above, the first task is to determine the total path length S
from Po to PN' This is done as follows: for each arc (x i-I> YH) to (x i ' Yi)' i
=1,2, ... , N , determine the
corresponding path length s,. Firstly determine a cubic interpolating polynomial approximation Pi (x)
over each arc interval, i.e. y(x)::::: p,(x) , x E [xi-] ,x;] (or x(y)::::: Pi(Y)' Y E [Yi-I ,Y i ] depending on what
is the most convenient).
3.1.1.1 Determination of interpolating and overlapping cubic arcs
It is assumed that for any three consecutive nodal points (nodes) PH' Pi and Pi+1, i
given set of nodal points {Pi
= (x i' Yi)' i
= 1,2, ... , N -1
in the
0,1,... , N} , at least one of the following conditions must hold:
Condition l(a):
Xi+1 >x i >x H
Condition l(b):
Xi+I<Xi<X H
Condition 2(a):
Yi+1 > Yi > Yil
Condition 2(b):
Yi+I<Yi<YH
I
i
I
!
Table 3.1
In the event of non-compliance with all four the above conditions, the set of nodal points is considered
insufficient since it implies that the section ofthe curve, represented by the three nodes that do not satisfy
any of the conditions, cannot be expressed in the form where one of the coordinate variables is a unique
valued function of the other. An extreme example of this non-uniqueness is depicted in Figure 3.2 which
shows a complete curve C represented by the three nodes Po, PI and P2 •
Y
L--------------------------------------.X
Figure 3.2: Insufficient set of nodal points.
Chapter 3
91
TRAJECTORY-PLANNING THROUGH INTERPOLATION BY OVERLAPPING CUBIC ARCS AND CUBIC SPLINES
Clearly x 2 < XI while
XI
> x o ' violating both conditions lea) and l(b), and further y 2 < y, while
YI > Yo , violating both conditions 2(a) and 2(b). This situation can easily be remedied by specifying, for
example, two additional nodes, resulting in a set of five nodal points, as shown in Figure 3.3.
y
C
/--J
~--------------------------------------~X
Figure 3.3: Sufficient set of nodal points.
The five nodal points shown in Figure 3.3,
{Pi = (X i' yJ, i = O,I,... ,4}
may be grouped to form three
overlapping triplets of consecutive nodes, Le.
i
Triplet 1:
I x 2 > XI > Xo (satisfYing condition lea))
• y 2 < Y1 and y 1 > Yo (violating conditions 2(a) and 2(b))
!
. Triplet 2:
I
X3
< x 2 and x 2 > Xl (violating conditions I (a) and l(b))
I y 3 < Y2 < Y1 (satisfYing condition 2(b)
1------.;---------+-lx--<--X--<-x-2-(s~tisfYing condition 1(b»
4
3
(P2 ,P3 ,P4 )
Triplet 3:
i y 4 < Y3 < Y2 (satisfYing condition 2(b»
Table 3.2
Curve C shown in Figure 3.3 may now be approximated by four (in general N) overlapping interpolating
arcs shown separately in Figures 3.4, 3.5 and 3.6. Along each arc one variable may be expressed as a
unique function of the other depending on which condition in Table 3.1 is satisfied. In particular it may
be desired to represent each arc by a cubic polynomial interpolating function.
Chapter 3
92
TRAJECTORY-PLANNING THROUGH INTERPOLA nON BY OVERLAPPING CUBIC ARCS AND CUBIC SPLINES
y
Cubic arc 1
2
Figure 3.4: Cubic arc 1.
It follows by inspection that arc 1 in Figure 3.4, passing through the three nodes of triplet 1, may be
represented by a unique valued function ofx (see Tables 3.1 and 3.2). In particular it may be expressed
as a cubic polynomial in x of the form:
PI (x)
= a l + b,(x- x o) + c l (X-X O )2 + d , (x-x o)} ,
X
E [X O'x 2 ]
(3.1 )
The derivative of (3.1) with respect to x is given by
~p,(x)=b,+2c,(x-xo)+3dl(x-xo)2,
dx
xE[x o,xz1
(3.2)
The four unknown coefficients of the cubic interpolating polynomial (ai' b l ,c 1 and d!) may be uniquely
determined by utilizing the initial known gradient, i.e. dy at Po (see given data (iv), at the start of
dx
Section 3.1), as well as the three nodal points of triplet 1. More specifically substituting each of the three
nodal points of triplet 1 into equation (3.1), results in three independent equations:
(3.3)
(3.4)
(3.5)
The fourth independent equation follows from substituting the initial gradient dy at Po dyl '\ into
[ dx Po
dx
J
equation (3.2):
dyl
d X Po
= dy(xo) ~~
dX
d X PI
(x) = b
0
I
(3.6)
With a l and b l known from expressions (3.3) and (3.6) respectively, one may solve for c 1 and d} using
expressions (3.4) and (3.5):
Chapter 3
93
TRAJECTORY-PLANNING THROUGH INTERPOLATION BY OVERLAPPING CUBIC ARCS AND CUBIC SPLINES
from (3.4): c t
where e l = Yt -at-b1(x l, xo) ,and substituting the expression for c 1
(xl-X O )"
into (3.5): yz =at+bt(x2-xo)+(el-d,(xl-xo))(x2 xo)2 +d l (x 2 x o )3
and therefore d I
Y2 al-bl(x2-xo)-et(x2 Xo)2
(Xl Xo)3 (XI X O)(x 2 Xo)2
It is proposed here that the cubic interpolating polynomial PI(X) constructed in this manner be called a
clamped three-order interpolating cubic arc, or simply a C-3 node cubic arc.
Although cubic arc 1 interpolates through points Po, PI and P2 and is thus valid over the interval
[X O,x 2 ], it will only be used to represent the cubic polynomial function over the first interval [xo,x t ],
Cubic arc 1 is forced through the three nodes of triplet 1 (see Table 3.1) with only the initial gradient
(:\J
being enforced.
The approximation p,(x) is therefore expected to deteriorate in gradient
accuracy in the vicinity of node P2 and this is the reason why it will only be used over [x 0' X t]. To
obtain a more reliable approximation between nodes PI and Pz an overlapping strategy is employed to
avoid the accumulation of excessive gradient approximation errors, which will result in increasingly
inaccurate fitted cubic arcs.
y
Cubic arc 2
~-------------------------------------.x
Figure 3.5: Cubic arc 2.
The cubic arc 2 passing through the three nodes of triplet 2, (see Figure 3.5) is also represented by a
cubic polynomial function this time of y (see also Table 3.1 and Table 3.2). This function is formally
expressed as
Chapter 3
94
TRAJECTORY -PLANNING THROUGH INTERPOLATION BY OVERLAPPING CUBIC ARCS AND CUBIC SPLINES
P2(y)=a 2 +b 2(y
YI)+C 2 (Y
YI)2+d 2(y-yY, ye[Yi>Y 3 ]
(3.7)
The derivative of (3.7) with respect to Y is
d
-P2 (y)
dy
= b 2 + 2c 2(y -Y I) + 3d 2(y YI)
Again the initial gradient of cubic arc 2 at PI
r:x
Ii
(3.8)
is required together with the three nodes of triplet
\ YP, J
2 to calculate the four unknown coefficients (a 2 ' b 2 ,c z and d 2) of polynomial (3.7). Here, at the start of
arc 2 (as for the subsequent arcs) the value of the gradient is not explicitly prescribed and is therefore
unknown.
However, once the four unknown coefficients of the previous cubic arc 1 have been
determined, equation (3.2) provides an approximation to the required gradient at PI' i.e.
(3.9)
Thus in computing the coefficients of approximating arc 2 the condition dxl :;:: [dP t (Xl)]-l is used.
dx
dy p
I
Polynomial P2 (x) will only be used to describe the arc between nodes Pj and P2
•
y
P4
C...
-7 ........
P4(x)7
Cubic arc 3
C~bic arc 4
~----------------------------------~x
Figure 3.6: Cubic arcs 3 and 4.
Cubic arc 3 passing through the three nodes of triplet 3 (see Figure 3.6) is different to the previous arcs
in that both conditions 1(b) and 2(b) are satisfied (see Table 3.1 and Table 3.2). This implies that cubic
arc 3 can be expressed as a cubic polynomial function of either x or y and a choice has to be made. If
this situation occurs, only one of the following two conditions will be satisfied:
Condition 3(a):
Condition 3(b):
I
Table 3.3
Chapter 3
95
TRAJECTORY-PLANNING THROUGH INTERPOLATION BY OVERLAPPING CUBIC ARCS AND CUBIC SPLINES
For the case depicted in Figure 3.6, cubic arc 3 satisfies condition 3(a) in Table 3.3, and it is therefore
clearly preferable to express the arc as a cubic polynomial function of x:
(3.10)
r.
The unknown coefficients of cubic polynomial (3.10) are determined in a similar manner to those of
cubic arc 2 (equation (3.7», with the initial gradient at P, taken as
:~ I.,
[dp ~~' )
Again p, (x)
will only be used over the first interpolating interval, i.e., between nodes P2 and P3 (Figure 3.6).
Having determined cubic arc 3, it remains to find a suitable cubic interpolating polynomial function
P4(X), XE[X 3 ,XJ, which is to represent the final interval P3 to P4 of the total curve under
consideration.
The overlapping strategy may be continued for the final part of the curve by fitting a cubic arc through
nodes P4 , P3 and P2 , and utilizing the given gradient at P4 (see given data (iv) at the beginning of
Section 3.1). The polynomial has the form
P4(X) = a 4 + b 4(x-x 4)+c 4 (x- X4)2 + d 4 (x-x 4)3 , X E [x 4 ,X Z ]
(3.11 )
The derivative of (3 .11) with respect to x is
~p4(x)=b4+2C4(x-x4)+3d4(x-x4)2,
dx
dx
X E[X 4 ,XJ
(3.12)
at P4 and interpolating through the three nodes P4 , P3 and P2 , four
independent equations in the unknown coefficients follow from expressions (3.11) and (3.12) as before.
These equations may be simultaneously solved to give a 4 , b 4, c 4 and d 4 • The resultant interpolating
polynomial p 4 (x) , representing arc 4 and to be used over the final interval between nodes P3 and P4 , is
also indicated in Figure 3.6.
The following truth table shows how the different conditions listed in Table 3.1 and Table 3.3 determine
the dependent variable for a specific arc. The rows show different combinations of satisfied and violated
conditions, where a 1 indicates that a specific condition is satisfied and a 0 that it is violated.
Chapter 3
96
TRAJECTORY -PLANNING THROUGH INTERPOLATION BY OVERLAPPING CUBIC ARCS AND CUBIC SPLINES
I
i
Condition
Condition
Condition
Condition
Condition
Condition
Dependent
1(a)
1(b)
2(a)
2(b)
3(a)
3(b)
variable
1
0
0
0
°
°0
1
0
0
x
0
1
0
y
0
0
1
Y
1
0
1
0
x
i
i
1
0
°
1
1
1
°
1
0
1
0
x
0
1
Y
1
0
x
0
1
y
i
1
0
x
I
0
1
Y
1
0
x
0
I
y
I
Table 3.4
Here, for illustrative purposes, the presentation of the overlapping cubic arc methodology for
constructing an approximation to a curve defined by nodal points Po, PI ,..., PN' has been restricted to the
case N
= 4.
The extension of the method to the more general case where N may be larger than 4 is clear
and obvious.
3.1.1.2 Computation of total path length S
With p,(x) (or Pj(Y» representing the approximation to the curve to be used over [xH,xJ known, the
corresponding curve length Sj may be obtained by integration.
ds 2 == dx 2 + dl it follows that ds
Sj
From the differential relationship
~l + (~)2 dx and therefore the curve length s, is given by
=
r ~1
+ C! Pj(x»2dx ==
H
r
f(x)dx
for each i:= 1,2, ... , N
It is convenient to do the integration numerically by using the composite Simpson's rule for n
subintervals :
h[
~
]
f(x)dx':':;"3 f(a)+2 (~I
f.f(x 2j ) +4f.f(x
2i - , )+ f(b)
r
.
where n IS even, h
Chapter 3
(b a)
=- , and
n
I
1
0
0
i
(3.13)
x k = a + kh for k = O,l,...,n
97
i
I
I
TRAJECTORY-PLANNING THROUGH INTERPOLATION BY OVERLAPPING CUBIC ARCS AND CUBIC SPLINES The total path length S of curve C is then given by
(3.14)
3.1.1.3 Dependence of curve length on parameter t
Assume that the distance s of the working point (wp) along the curve is represented by a cubic
polynomial in time t of the form:
(3.15)
s(t)=a.+bst+cse+dse, tE[O,T]
Expression (3.15) is associated with a known initial curve length s(O)
initial tangential speed s(O) = Vo (usually
Vo
So (usually So = 0), a known
= 0), as well as a known final curve length seT)
S
(usually found via expression (3.14) above).
For the time being, assume that a gradual increase in tangential speed is required over the time interval
t E [0, T], such that a specified tangential speed v· is attained at time T, i.e. seT)
v'. The time
derivative of(3.15), representing the tangential speed over the total path is
set) = b s + 2c s t+ 3d,
e, t
E
[0, T]
(3.16)
The acceleration over the total path may be obtained from the time derivative of (3 .16):
set)
2c. + 6d, t, t E [0, T]
(3.17)
It is further required that the magnitude of the final tangential acceleration has to be zero, i.e. seT) =
Now, with s(O)
so, seT)
°.
S, s(O) = va' seT) =v· and seT) = 0 known, one may solve for as' b s ' c s '
d s and T as follows:
s(O) = as
So
bs
Vo
S(O)
seT)
S = as + b, T + c, T2 + d, T3 seT) = v·
(3.18)
b s + 2c, T + 3d, T2 seT) =0
from (3.20):
and substituting (3.21) into (3.19): v·
(3.19)
2c,+6d, T Cs
(3.20)
= -3d, T v0
-
(3.21)
3d, T2 , from which it follows that
(3.22)
.
Substituting (3.22) into (3.21): c. = v -
T
Chapter 3
Vo
(3.23)
98
TRAJECTORY -PLANNING THROUGH INTERPOLATION BY OVERLAPPING CUBIC ARCS AND CUBIC SPLINES
and substituting (3.22) and (3.23) into (3.18): S =So + T(
~o + 2;·)
from which it finally follows that
T= 3(S- s o)
Vo + 2v·
Note that if the initial time instant to
(3.24)
* 0, the corresponding polynomial set) can readily be obtained by
the simple replacement of T by T - to in expressions (3.18) - (3.24) and the corresponding replacement
oft by t
to in expressions (3.17) - (3.19).
For each node Pi
(xi,yJ, i = O,I,... ,N the total path length up to the specific node may be determined
from (3.14):
S;
=
~:Sj , i =O,I,...,N
(3.25)
j~I
Further, with Si' i = 1,2, ... , N -1 known, the Newton-Raphson iterative method may be used to solve for
the corresponding nodal times t;, i
1,2, ... ,N-1 .
This well-known and powerful numerical
method solves for the root ofa non-linear equation of the fonn f(x) == 0 via the iterative scheme:
,
x(j-l)
f(x U- I »
.
xl)~ = [ df(x (')-1)) ] , J== 1,2, ...
dx
where an initial estimate x (0) is given.
To solve for a specific t; corresponding to distance S" requires the solution of the non-linear equation
f(tJ
where from (3.15): s(t,) == as + b. ti + C s
s(tJ - S,
0
t/ + d, t/ , and thus, more explicitly, the equation to be solved
becomes
An equation corresponding to the above, is to be solved for each i
=1,2, ... , N - 1 .
Since tN == T is known from (3.24), good initial approximations for t;, i = 1,2, ... ,N-1 may be found:
t/O)
={~). i=1,2,... ,N-1
Starting with an initial approximate root given by the above, the Newton-Raphson method generates, for
"
{(j).
apprOXImatIons
t{ , J =012
" ,... } :
each l,· a sequence 0 f
Chapter 3
99
TRAJECTORY-PLANNING THROUGH INTERPOLATION BY OVERLAPPING CUBIC ARCS AND CUBIC SPLINES
The iteration continues until convergence is obtained. In practice, with a tolerance
E
>
°
specified, the Newton-Raphson iteration continues for each i until one of the following convergence criteria is met: It/i) - t/ i-)1 < E 1
It.
w-
t
t.
t (j-1)
Cl t
J
I
(j)
<E,t.:t=
I
°
I
3.1.2 Cubic spline representations for X(t) and yet)
With the time interval [0, T] during which the curve is executed determined (see objective (a) Section
3.1), and the nodal times t;> i:= 1,2, ... , N - 1 known, time parametric curves X( t), and Y (t), t E [0, T]
interpolating the nodal points (x;,y;) (see objective (b) Section 3.1), can be determined.
Since a unique time instant t; E[O,T] is associated with each nodal point (XpYi), i=O,l, ...,N and
since 0= to < t) < ... < tN = T , separate cubic spline interpolations X(t) and yet) may be fitted to the
respective nodal point sets, (xi'tJ and (Yi'tJ .
According to Burden and Faires  cubic spline interpolation, which fits cubic polynomials between
each successive pair of nodes, is the most common piecewise polynomial approximation. Based on their
defmition, the cubic spline interpolant X(t) , is a function that satisfies the following conditions:
X(t) is a cubic polynomial, denoted XJt) ,on subinterval [tl' t'+I] for each i = 0,1,... , N
X(t;) =x; for each i = O,l,...,N
X'+I(t i+1 )
X,(t i +1 ) foreachi=0,1, ...,N-2
X'+1(t'+I)=X,(t;+I) for each i=O,l,...,N 2
X'+I(tl+J
Xi(t,+J for each i = O,l,...,N - 2
One of the following set of boundary conditions is satisfied:
X(t o ) = X;(tN) =
°
(free or natural boundary)
x
or X(t o) = Xo and X(t N) = N (clamped boundary)
Chapter 3
100
TRAJECTORY-PLANNING THROUGH INTERPOLATION BY OVERLAPPING CUBIC ARCS AND CUBIC SPLINES
When the free boundary conditions are prescribed, the spline is called a natural spline, and its form
approximates the shape that a long flexible rod would assume if forced to go through each of the nodal
points.
For the interpol ants X(t) and yet) to be constructed here, however, clamped boundary conditions are
accurate approximations.
Assume for the moment that the derivatives at the end points
(xo
and
x
N)
are accurately known. To
construct the cubic spline interpolant for x as a function of t, the conditions listed above are applied to
the cubic polynomials of the following form:
(3.26)
The unknown coefficients a)(j, b)(j'
C Xi
and d xi , i::::: O,l,...,N -1, may easily be determined as shown
in .
If not explicitly known, the initial derivative Xo may be accurately approximated using Taylor
where Xi corresponds to x(t,) and At, ::::: t, - to' i::::: 1,2,3,4. Ignoring the higher order terms in the
above expressions leads to the following set oflinear equations:
Atl
XI -x o
x2
Xo
X3 -x o
x 4 -xo
At z
:::::
At3
At4
(Atl)3
(Aty
24
(AtJ4
(Aty
2
(At2)2
(At 2 )3
2
(At 3)2
24
( At 3)4
Xo
(At] )3
2
(At4)2
6
(At 4)3
24
(At4)4
Xo
2
6
24
6
6
Xo
Xo
(3.27)
Expression (3.27) is a linear system of the form Ax::::: b , and can be solved directly using a scheme such
as LU-factorization . The solution vector x contains the sought after initial derivative
Chapter 3
xo'
101
TRAJECfORY ·PLANNING THROUGH INTERPOLATION BY OVERLAPPING CUBIC ARCS AND CUBIC SPLINES
The final derivative
xN
may be obtained in a similar manner, where the set of linear equations in matrix
form is given by:
~tl
XN-1 -XN
-~t2
XN_2 -XN
=
XN_3 XN
-~t3
XN_4 -XN
~t4
with
~tj
tN
(~tl )2
2
(~t2 )2
_ (~tl)3
6
_ (~t2)3
2
(~tJ2
2
6
_ (~tJ3
6
(~tlt
24
(~t2t
24
xN
(~t3 )4
XN
24
(~t4 )2
(~t4 )3
(~t4 )4
2
6
24
XN
(3.28)
xN
tN_i> i =1, 2, 3, 4.
The advantage of using a cubic spline interpolation for X(t), t E [0, T] is that each constituent cubic
polynomial Xj(t) involves four coefficients, so there is sufficient flexibility in the cubic spline
procedure to ensure that the interpolant is not only continuously differentiable, but also has a continuous
second derivative over the whole interval [0, T] (see ).
Once ax;, b Xj ' cx;, and dx;, i
X;(t)
=O,l,...,N -1
(see (3.26» are determined, X(t) and X(t) are given by:
bx; +2c x;(t-t;)+3d xi (t-tY, i=0,1, ... ,N-1
X;(t)=2c x; +6d x;(t-t;), i
O,l,...,N-l
(3.29)
(3.30)
which are continuous functions over the interval [0, T] (see objective (c) Section 3.1).
It follows that Y(t), yet) and yet) can be found in a similar manner.
3.2 Practical problem of determining dy at Po and PN
dx
In general the set of given nodal points {Pi
(xi' Yi)' i =0,1,..., N} may represent a curve for which no
y
explicit analytical expression exists, and therefore the exact initial and final gradients (d at Po and
dx
PN) will not be known.
In this case approximations to these gradients with respect to x may be
determined using Taylor expansions in an analogous manner as done for the time derivatives Xo and
in Section 3.1.2 (see expressions (3.27) and (3.28».
Chapter 3 102
xN
TRAJECTORY-PLANNING THROUGH INTERPOLATION BY OVERLAPPING CUBIC ARCS AND CUBIC SPLINES
In Section 3.1.1.1 it is apparent that the nodal points representing a curve must be chosen in such a way
that certain conditions are satisfied (see Table 3.1 and Table 3.3). These conditions ensure that each
consecutive cubic arc can be expressed as a function of either x or y.
Returning to the illustrative example curve of Figure 3.3, it was shown that 5 nodal points along the
curve are adequate in terms of the conditions listed in Table 3.1 and Table 3.3, provided that the exact
initial and final gradients are known. However, if the exact values of the initial and final gradients are
not known, the nodal points should be specified in such a way that sufficiently accurate approximations
for these gradients may be calculated.
y
~-------------------------------------.x
Figure 3.7: Additional nodal points specified.
Figure 3.7 shows the same illustrative example curve, but with four additional nodal points.
y
approximate value for the initial gradient (d at Po, i.e.
dx
y~)
An
may then be determined using the
following Taylor expansions:
YI ;:;;; Y0 + Ax I y'0 +
Y2
with 8x j
= Xj
x o' i
= Y0 + ~ 2 y'0 +
( ~ )2
2!1
y" +
0
(Ax )3
3!I y"0 + O(Ax I )4
(Ax )3
( Ax )2
2!2 y"0 + 3!2 y"'0 + O(~ 2 )4
= 1,2,3.
The clustered distribution of nodal points Po, PI' P2 and P3 shown in Figure 3.7 is justified considering
the fact that the fourth order error term in the above Taylor expansions becomes smaller using smaller
step sizes Ax l , Ax z and Ax 3 • Neglecting these error terms an approximate initial gradient y~ is
obtained by solving the following linear system:
Chapter 3
103
TRAJECTORY-PLANNING THROUGH INTERPOLATION BY OVERLAPPING CUBIC ARCS AND CUBIC SPLINES
~XI
y
r
Y2'
Y} Yo ­ ~X2
Y3 -Yo
~X3
(~Xl )2
4
(~xy
6
(~X2 )2
(~X2)3
4
6
(~XJ2
(~3)3
4
6
(3.31 )
rm
Similarly, an approximate final gradient y~ may be obtained by solving the linear system:
(~xy
-~Xl
YN
r
N
YN-2' Y
YN
y}/-3-YN
with
~Xi
=x
N
-X N _ i , i
1
~X2
-~X3
(~XI)3
4
6
(~X2 )2
(~X2)3
4
6
(~X,)2
(~X3)3
4
6
n
(3.32)
y:
y)/
=1,2,3.
Depending on the particular circumstances, approximations for dx at Po and PN may be determined in a
dy
similar manner using Taylor expansions. The choice between determining dy or dx at Po and PN
dx
dy
depends on which of the following conditions are satisfied, and which are violated:
Condition 4(a):
Condition 4(b):
Condition 4(c):
r - - - -..- ­
.--,-----r------------',
Condition 4(d):
I
Condition 5(a):
Condition 5(b):
Condition 5(c):
YN-3 > YN-2 > YN-I > YN
Condition 5(d):
L-___________
~
__________
~
Table 3.5
Since four node points are involved in each condition, the clustered distributions of the nodal points Po,
PI' P2 and P3, as well as Ps ' P6 , P7 and Ps in Figure 3.7 are further justified considering the
requirement that at least two of the conditions listed in Table 3.5 must be satisfied for any set of nodal
points. In particular, at least one of conditions 4(a), 4(b), 5(a) or 5(b), and one of conditions 4(c), 4(d),
5(c) or 5(d) must be satisfied for the purposes of determining the sought after initial and final gradients.
Chapter 3
104
TRAJECTORY -PLANNING TIIROUGH INTERPOLATION BY OVERLAPPING CUBIC ARCS AND CUBIC SPLINES
For nodal points Po, PI' Pz and P3 shown in Figure 3.7 it is clear that both conditions 4(a) and 5(a) are
satisfied, implying that either dy or dx can be determined at Po. Similarly for nodal points Ps , P6
dx
dy
,
P7 and Pg , both conditions 4(c) and 5(c) are satisfied, implying that either dy or dx can be determined
dx
at PN
= Pg •
dy
In these eventualities, the appropriate choice is made by testing which of the additional
conditions listed in Table 3.6 are satisfied.
Condition 6(a):
\x 3 ­
X0 I~ Iy 3 - Yo!
Condition 6(b):
IX
xol < IY3
3
i
I
-Yo\
Condition 7(a):
IXN XN_31~ IYN YN-31
Condition 7(b):
IXN -X N_3 !< IYN YN-3!
Table 3.6
For example, since for nodes Po and P3 shown in Figure 3.7, condition 6(a) is satisfied, the obvious
choice is to determine dy at node Po' Similarly, since condition 7(a) is satisfied for nodes Ps and Pg ,
dx
dy is to be determined at node PN
dx
Pg •
The following truth table shows how the conditions from Table 3.5 and Table 3.6 determine the form of
the gradient to be used at node Po' As before, the rows show different combinations of satisfied and
violated conditions, where a I indicates that the specific condition is satisfied, and a 0 that it is violated.
Condition
4(a)
Condition
4(b)
Condition
5(a)
Condition
5(b)
I
Condition
Condition
6(a)
6(b)
Po
i
1
0
0
0
I
i
dy
dx
0
1
0
0
dy
dx
0
0
1
0
dx
-
0
0
0
1
-
i
Chapter 3
dy
dx
dy
105
i
TRAJECTORY-PLANNING THROUGH INTERPOLATION BY OVERLAPPING CUBIC ARCS AND CUBIC SPLINES
I
!
1
1
0
0
I
!
1
I
I
0
1
dx
I
I
I
I
0
i
I
dx
I
1
0
0
dx
­
dy
dy
0
1
1
dy
0
dx
dy
-
1
i
i
I
1
1
0
1
1
I
dy
dx
0
0
i
0
0
1
dx
dy
0
dy
dx
1
0
dx
i
I
I
Table 3.7
l
l
-
the form of the gradient to be used at PN •
Of course, with dy (or dx ) determined at nodes Po and PN using the above strategy, dx (or dy) at
dy
dx
dx
dy
nodes Po and PN is simply given by the reciprocal relationship:
Chapter 3
[dY(X,.)]-1
dy(x,.)
dx
or ~
I
I
I
I
I
I
A similar truth table may be drawn up showing how the different combinations of conditions determine
dX(Yr·)
dy
I
1
[dx(y)J­
dx i
for i = 1, N
106
TRAJECTORY -PLANNING THROUGH INTERPOLATION BY OVERLAPPING CUBIC ARCS AND CUBIC SPLINES
3.3
Synthesis of more general curves y
"X
Figure 3.8: General curve where the prescribed speed is achieved.
Section 3.1.1.3 deals with the simplified situation of tangential acceleration along the prescribed curve
until a specified tangential speed v· is achieved at time tN = T. A more realistic example is shown in
Figure 3.8 where the motion is executed in three segments: tangential acceleration along curve C 1 ,
constant tangential speed along curve C n , and tangential deceleration along curve C m • The set of nodal
points associated with the general curve of Figure 3.8 is {Pj
(xi' Yj ),i
0,1,... , NI'".,N II ,,,., N Ill} .
This Section proposes a methodology by means of which node numbers N I and N II may automatically
be assigned, given the prescribed constant tangential speed v· along C II and the maximum allowable
tangential acceleration
SALLOW'
In what follows "speed" and "acceleration" refer to tangential speed and tangential acceleration, unless
otherwise specified.
3.3.1 Linear segment with cubic blends
Assume that node numbers N I and N II are given. Node number N III is automatically known, since it
is the final node in the specified data series. Path lengths SI' SII and SIll' corresponding to the
respective curves C 1 , Cli and C m , may then be determined by following the procedure outlined in
Sections 3.1.1.1 and 3.1.1.2.
Chapter 3
107
TRAJECTORY-PLANNING THROUGH INTERPOLATION BY OVERLAPPING CUBIC ARCS AND CUBIC SPLINES
The acceleration curve C r in Figure 3.8, corresponds to the situation already discussed in Section
3.1.1.3, i.e. a gradual increase in speed is required over the time interval t E [0, Tj ] , such that the
prescribed speed v' is attained at time Tj
For curve C! the distance along curve
Sl
•
at any instant t is represented by the cubic polynomial in time
(see expression (3.15»:
SI(O == a,l+ b sl t+c si
e +d e, t
sl
E
[O,TI]
with quadratic expression 8] (t) =b Si + 2c s1 t + 3d si
and linear form Si (t)
e, for the speed
2c Sl + 6d sl t, for the acceleration
(3.33)
(3.34)
(3.35)
The motion along the acceleration curve C 1 is associated with boundary conditions that are similar to the
ones given in Section 3.1.1.3; namely
SI(O)=SO (usually So =0)
sl(T1)=SI
SI(O)=V O (usually Vo =0)
8j (T1 )=v'
sl(TJ == 0
The unknown coefficients a sl ' b Sj ' c s1 and d S] , as well as T j are determined in exactly the same manner
as explained in Section 3.1.1.1 (expressions (3.18) - (3.24». With the coefficients and the time instant
T] known, the nodal times t;, i == 1,2, ... ,N 1 may be obtained using Newton's method (see Section
3.1.1.3).
Along the constant speed segment C ll in Figure 3.8 the distance sl! along the curve from nodal point
PN 1 is given by the following linear relationship in time:
(3.36) For each node Pi == (x" y;), i
N I + 1, N 1 + 2, ... , Nil' the total path length S; from Po up to the specific node Pi may be determined from (3.25): S, =
~>j
j=1
The corresponding nodal times t;, i
,
i = N] + 1, N j + 2, ... , N II
(3.37)
N 1+ 1, N 1+ 2, ..., N II then follow from (3.36):
Sj -S] + T I ' 1' == N I +,
1 N 1 + 2,.. " N II
-'--.---'-
(3,38)
V
Chapter 3
, 'I
108
TRAJECTORY-PLANNING THROUGH INTERPOLATION BY OVERLAPPING CUBIC ARCS AND CUBIC SPLINES
For the final deceleration segment C m in Figure 3.8, the distance along the curve
Sill
from nodal point
PN is also given by a cubic polynomial function of time:
"
slll(t)=asm+bsm(t-TII)+csm(t Tu)2+dsm(t-Tn)3, tE[TII,Tm]
(3.39)
sm(t) == b SIll + 2c slIl ( t - Til) + 3d sm ( t - TII)2
(3.40)
Sm(t) = 2c sm + 6d slll (t - Tu)
(3.41)
Since a decrease in speed is required along the deceleration segment C m , the following boundary
conditions are enforced:
Sill (Tu) = v·
sm(Tm) == 0
sllI(Tu ) = 0
from which the coefficients a.lII , b slll ' c slII ' d slII and the final time instant T III can be determined:
:. c sIll
0
(3.42)
(3.43)
and with b.m = v· it follows from (3.43):
v
.
(3.44)
and substituting (3.44) into (3.42):
(3.45) Newton's method is then used to determine the corresponding nodal times t" i
=Nil + 1, N II + 2, ... ,N m
in the manner already described in Section 3.1.1.3.
It follows that the motion, described in terms of the distance set) along the general curve shown in
Figure 3.8, is a linear segment with cubic blends (LSCB), since the cubic motions along curves C 1 and
Chapter 3
109
TRAJECTORY-PLANNING THROUGH INTERPOLATION BY OVERLAPPING CUBIC ARCS AND CUBIC SPLINES
CII! are merged or blended with the constant velocity motion along curve CII at the respective blend
times TI and Til as shown in Figure 3.9 (see ).
s
o~~------~----~------~--------~~t
TJ
Till
T
Tn
Till
Figure 3.9: Linear segment with cubic blends (LSCB).
Blend time T( is given by (3.24):
(3.46) which reduces to T(
3S~
2v
with
So
=0 and vo =0.
Similarly, blend time TIl is given by (3.45):
T -T = 3(Sm- S u)
III
II
2v'
(3.47)
The latter two equations indicate that if the nodal points N ( and N II are chosen in such a way that
Sm
Su
S( , then time spans (T( - 0) and (T[II - Til) are equal. In this case the LSCB, as shown in
Figure 3.9, is "centro-symmetric" with respect to the midpoint
[T~I1 ,S~II] (see ).
Another important and desirable feature of the proposed LSCB motion, is the fact that the corresponding
speed and acceleration curves are continuous as depicted in Figure 3.10.
Chapter 3
110
TRAJECTORY-PLANNING THROUGH INTERPOLATION BY OVERLAPPING CUBIC ARCS AND CUBIC SPLINES
s
s
v'
o '-------.,----.\-.t
s
Or--~------~---.t
Figure 3.10: LSCB motion with its accompanying speed and acceleration curves.
The initial acceleration 81 (0) is obtained by substituting (3.46) into (3.23) giving
which, after substituting into (3.35) yields
2 Col
_ 2( v· - v 0 )( v 0 + 2 v • )
-
Expression (3.48) simplifies further if v 0
3(SI -so)
=0
"j(O)
S
Further, by substituting d,m
=
and
So
4(V·)2
3S]
(3.48)
0, as is usually the case, to give
(3.49)
.
-v
2
as well as (3.47) into (3.41), the final acceleration is given
3(Tm - Til)
by
Chapter 3
111
TRAJECTORY -PLANNING THROUGH INTERPOLATION BY OVERLAPPING CUBIC ARCS AND CUBIC SPLINES
.. (T)
SIll
III
-4(V')2
-S )
= 3(S
III
(3.50)
II
If SIll - SIl = SI , the magnitude of the initial acceleration SI (0) is equal to the magnitude of the final
acceleration SIII (Till) due to the symmetry that exists in this case (compare expressions (3.49) and
(3.50». It is of particular interest to note that for this symmetrical situation, the magnitudes of the
accelerations at the initial and final times are not only equal but correspond to the maximum acceleration
magnitude that occurs along the total curve.
3.3.2 Treatment of constraint on acceleration
The practical requirement that the prescribed motion is to be executed as fast as possible, is usually
subject to a prescribed maximum allowable acceleration magnitude SALLOW'
A procedure is now
described that may be used to select appropriate node numbers N 1 and N II such that the following
specific acceleration constraints are not violated:
(3.51 )
(3.52) Since by (3.35) and (3.41) the acceleration along both C 1 and CIl! vary linearly with time, it follows that
if the specific constraints (3.51) and (3.52) are satisfied, that the constraint on the acceleration is also
satisfied at each instant along the whole path.
As before, for each node Pi = (xPy;), i
O,I,... ,N IlP the total path length up to the specific node may
be detennined using expression (3.25).
A special nodal point N MID' corresponding to a point
approximately halfway along the total curve (i.e. for which SN
MID
""" Sm ) can be identified using the
2
following criterion:
(3.53) As a first iteration in finding the appropriate choices for nodes N I and N II , assume that nodes N 1 and
N 1I coincide with node N MID as shown in Figure 3.11.
Chapter 3
112
TRAJECTORY-PLANNING THROUGH INTERPOLATION BY OVERLAPPING CUBIC ARCS AND CUBIC SPLINES y
TI = T MID = Tn
Figure 3.11: General curve showing the midpoint NMID•
From (3.53) it follows that Sm
SMID;?: SMID' and with nodes N I and Nil coinciding with node N MID' it
further follows that Sm - Sn ;?: SI which, when substituted into (3.49) and (3.50), gives
(3.54) Consequently, for the situation depicted in Figure 3.11, if the initial acceleration S1(0) satisfies
constraint (3.51), the final acceleration sm(Tm) will also satisfy constraint (3.52) and, indeed, the
constraint on the magnitude of the acceleration will be satisfied at each instant along the total curve.
3.3.2.1 Attainment of central speed v·
If the initial acceleration for the situation shown in Figure 3.11 satisfies constraint (3.51), the desired
central speed v· may be reached at node N MID . For practical purposes the ideal objectives are,
however:
1. to attain the desired central speed v· in the shortest possible time T J without exceeding the
allowable maximum acceleration magnitude
SALLOW'
and 2. to maintain the desired speed v· for the longest possible time span (Tn - T j ) provided that, over the
final time interval [Tn' Till] , the maximum deceleration magnitude does not exceed the allowable
magnitude 5ALLOW'
With reference to objective 1 above, expressions (3.46) and (3.48) respectively indicate that for a specific
speed v· , a shorter path length SI will result in a shorter time TI and a larger initial acceleration 8) (0) .
By shifting, for example, node N I to coincide with node N MID - 1 the associated path length Sl and
corresponding time T j will be reduced, while the associated initial acceleration 51 (0) will become
larger. This increased initial acceleration is then tested against constraint (3.51), and if the constraint is
Chapter 3 113
TRAJECTORY-PLANNING THROUGH INTERPOLATION BY OVERLAPPING CUBIC ARCS AND CUBIC SPLINES
still satisfied, node N I is shifted further to coincide with node N MID
-
2. These integer shifts are
continued until, after p integer shifts, the choice of node N I , coinciding with node N MlD
that the initial acceleration satisfies constraint (3.51), while the choice N I = N MID
-
-
P , is such
(p + 1) violates the
constraint.
With regard to objective 2, inspection of expressions (3.47) and (3.50), indicates that a decrease in the
path length (SIll - SlI) will result in a shorter time span (TIll - TIl), and a larger final acceleration
magnitude iSm(Tm
)1·
For example, if node N n is shifted to coincide with node N MID + 1 and thus
shortens the path length (Sm
SII) and the time span (Till - Til), it will result in a larger final
acceleration magnitude ISm(Tm)l. If the larger ISllI(Tm)1 satisfies constraint (3.52), node Nu may be
shifted further to coincide with node N MID + 2 ,etc. These integer shifts may be continued until, after q
shifts, node Nil' coinciding with node N MID + q is such that the final acceleration magnitude
Is III (Till )1
satisfies constraint (3.52), while the choice Nil = N MID + (q + 1) violates the constraint.
3.3.2.2 Violation of maximum allowable acceleration
If the initial acceleration, for the situation depicted in Figure 3.11 with specified central velocity v·,
violates constraint (3.51), a different strategy is proposed which ensures that the maximum allowable
acceleration is not exceeded. For this case, the motion along the curve shown in Figure 3.11, consists of
two blended cubic polynomials in time. The blend time is TMID as shown in Figure 3.12:
s
Figure 3.12: Blended cubic polynomials.
Chapter 3
114
TRAJECTORY-PLANNING THROUGH INTERPOLATION BY OVERLAPPING CUBIC ARCS AND CUBIC SPLINES
More specifically, the two objectives set in this case are:
1.
to accelerate as fast as possible along curve C [ without violating constraint (3.51), and
2.
to decelerate as fast as possible along curve Cil without violating constraint (3.52).
Objective 1 implies a different set of boundary conditions for determining the coefficients of the cubic
polynomial function representing the path length along curve C[ (see expression (3.33». These new
boundary conditions are
s[ (0) =
So
(usually
So
0)
from which a s[' b sl ' col' dOl' and TMID may be determined as follow:
sI
(0):::
a ::: S
sO
S[ (O):::b s =v 0
and therefore c :::
sl
SALLOW
2'
(3.55)
(3.56)
and thus
d :::
sf
-SALLOW
6T
(3.57)
MID
Substituting (3.57) into (3.55): sl(TMID )::: SMID
SALLOW
So +VO T MID + - -
3
(TMID )2
which gives TMID
and which reduces to
T
MID
=+ /3S MID
-Vi .
(3.58)
SALLOW
Chapter 3
115
TRAJECTORY-PLANNING THROUGH INTERPOLATION BY OVERLAPPING CUBIC ARCS AND CUBIC SPLfNES
The positive root gives the sought after time instant TMID . From (3.34) and substituting the values ofthe coefficients determined above, the speed at node N MID is V
MID
. ()
=SI
SALLOW
TMID =v o +-2-™1D
which reduces to
~3SMIDS ALLOW
(3.59)
2
if So
=0
and v 0
O. The cubic polynomial function of time representing the deceleration segment C III in Figure 3.11 is given
by expression (3.39). The unknown coefficients a,lII' b,m'
C,III
and d'lI! as well as T III are solved for
by using similar boundary conditions as before:
slll(TIl )
SMID
slll(Tm )=0
sm(TIJ = 0
Substituting these conditions in the corresponding general expressions (3.39), (3.40) and (3.41) for s,
and
s
s , and following the identical steps represented by equations (3.42) to (3.45) finally yields:
b SIlI =
V M1D
c sIIl
0
As with the LSCB motion (see Figure 3.10), the blended cubic polynomials also result in continuous
speed and acceleration curves as shown in Figure 3.13.
Chapter 3
116
TRAJECTORY-PLANNING THROUGH INTERPOLATION BY OVERLAPPING CUBIC ARCS AND CUBIC SPLINES
s
~~----+-------~t
O~------~------~t
o f-------=-'''<:--------++t
Figure 3.13: Blended cubic polynomials motion with its associated speed and acceleration
curves.
From the above acceleration curve, it is clear that the initial acceleration is equal to the allowable
maximum acceleration, i.e. 5JO)
SALLOW'
Since the final acceleration magnitude ISm(Tm)1 is either
smaller than or equal to the initial acceleration magnitude 51 (0) (see expression (3.54)), ISn/TIll)1
automatically satisfies acceleration constraint (3.52).
3.4
Incorporation of an orientation angle
~.
For certain applications of planar motion, the time parametric curves X(t) and yet) that interpolate the
given set of nodal points {Pi
= (xi' Yi)' i
0,1, ..., N}, are insufficient for the control of the particular
mechanism, e.g. for a planar mechanism where the end-effector is to be orientated in a prescribed
manner with respect to the tangent of the given path.
Chapter 3
117
TRAJECTORY-PLANNING THROUGH INTERPOLATION BY OVERLAPPING CUBIC ARCS AND CUBIC SPLINES
As an example, consider the situation where the orientation of the end-effector is to be exactly tangential
to the curve, as the working point on the end-effector progresses along the prescribed curve. The gradient
angle e at any point along the curve is given by
tane = dy
dx
~ dpJx) or [dP;(y)]-l
dx
(3.60)
dy
Another convenient form of expression (3.60) is
(3.61) With the time parametric curves X(t) and yet) known, the respective derivatives X(t) and YCt) are
also known. Expression (3.61) may therefore be evaluated at nodal points Pi' i
=O,I,...,N
to find the
corresponding values of e;, i = 0,1,... , N. As will be described below, the actual orientation angle <p of
the end-effector is related to the angle e in a non-straightforward manner as the working point executes
the path.
In computing e, care must be taken at nodes Po and PN , since the respective speeds at these nodes are
often zero, i.e. sr(O)=vo=O and Sill (Tm) =0.
X(Tm) = Y(TllI )
With this being the case, X(O)
Y(O)=O and
0, and consequently expression (3.60) must be used instead of (3.61) to determine
eo and eN' since dy is known at nodes Po and PN (see Section 3.2).
dx
Since the arc tan function, through which e is determined via (3.60) or (3.61), only assumes values
between
90° and + 90° , a special procedure must be adopted to determine the exact orientation angle
<p as the working point on the orientated end-effector tangentially follows the prescribed curve.
Chapter 3
118
TRAJECTORY -PLANNING THROUGH INTERPOLATION BY OVERLAPPING CUBIC ARCS AND CUBIC SPLINES
e = 0°
l,rep =
-:::::---=----""
0°
B
e =+ 90°
~C
----~~-----------------------~~.x
e ~
90
D
Figure 3.14: Circular prescribed curve.
Consider, for example, a prescribed circular curve as shown in Figure 3.14 with zero orientation angle
~
e and ~ -90° and progressing counterclockwise (CCW) around the
angle e computed via (3.60) or (3.61) gradually increases with ~ from
at B. Starting at point A with
90° at A, to 0° at B, to + 90° at C. The CCW crossing of the x-axis at point C is however associated
with a jump of -180° in the computed gradient angle
e although
~
is still clearly increasing
continuously. Progressing CCW along the lower half of the circle again sees a gradual increase in the
gradient angle e from - 90 0 at C, to 0 0 at D, to + 90 0 at A, while
0
270 at A. At A where another jump of
~
continues to increase from 90° to
180 0 occurs in the computed value
e if the X-axis is crossed
CCW.
A similar pattern is recognized using a clockwise (CW) tracing of the circular curve. Starting at point A,
the computed gradient angle e gradually decreases from + 90° at A, to 0° at D, to
decreases from
90° at A to
270 0 at C. The + 180 0 jump that occurs in
90° at C while ~
e with the CW crossing of
the x-axis at C is followed by another gradual decrease in the gradient angle starting from + 90° at C, to
0 0 at B, to
90° at A, while clearly ~ continues to decrease from - 270° at C to - 450° at A.
The above-explained behavior of the gradient angle
e is used in establishing a procedure to determine
the orientation angle ~:
If the jump between any two successive gradient angles
i.e.
e e
j
i-I
Chapter 3
:S;
-90
0
,
e
i-!
and
e i = 1,2,... , N
j'
is smaller than - 90° ,
a CCW-counter Iccw is incremented. Further reflection indicates that Iccw should
119
TRAJECTORY -PLANNING THROUGH INTERPOLATION BY OVERLAPPING CUBIC ARCS AND CUBIC SPLINES
0
also be incremented if 0 < 0;
0i-J < 90°. Similarly if 0; - OJ-! 290 or
counter lew is incremented.
0
90 < OJ - OJ-l < 0, a CW-
Using these counters, the true associated orientation angles
~i'
i::: 1,2,...,N are given by
(3.62) (3.63) In general expressions (3.62) and (3.63) respectively are given by:
~i
0; +(Ieew)(1800)+~offset
(3.64)
(3.65) The term ~offset in expressions (3.64) and (3.65) is defined as the offset orientation angle of the end­
effector measured in a CCW convention from the tangent to the prescribed path at the point of contact as
shown in Figure 3.15.
e =+ 90°
~c
D
e 0° Figure 3.15: Circular prescribed curve with an angular offset added to the end-effector
orientation.
Since the time instants tp i
0,1,2, ... , N are known (see Section 3.1.1.3), a cubic spline representation
<1:>(t) may now be determined in exactly the same way as the cubic spline representations X(t) and
Chapter 3
, 'I
120
TRAJECTORY -PLANNING THROUGH INTERPOLATION BY OVERLAPPING CUBIC ARCS AND CUBIC SPLINES
yet) (see Section 3.1.2).
With the cubic spline <1>(t) known, the continuous first and second
derivatives <D(t) and <I>(t) follow automatically.
Any prescribed path may also be traced with the end-effector posed in a specified fixed orientation ~fix
,
thus eliminating the need to determine the cubic spline <1>(t) with its fITst and second derivatives <D(t)
and <I>(t).
3.5
Test problems
The proposed trajectory planning methodology using Overlapping Cubic Arcs and (cubic) Splines
(OCAS) is tested here on five different test functions. Appendix B contains a flow chart of the OCAS
trajectory planning methodology.
3.5.1 Parabolic test function
The first test function is a parabola, where y is the quadratic function of x:
y(x) =
x2
(3.66)
2
The accuracy of the approximating fit is dependent on the number and distribution of the given nodal
points {Pj
x
E [~2,2]
=(x" Yi)' i == 0,1,... , N}.
The parabolic test function is approximated over the x-interval
using 29 points spaced in such a way that they are more densely distributed at the beginning
and end of the interval (see Figure 3.16). This is done to increase the accuracy of the calculation of the
initial gradient dy at Po and final gradient dy at PN as explained in Section 3.2. The nodal points are,
dx
dx
for the obvious reason of greater accuracy, chosen more densely in neighborhood of the turning point
where the greatest change in direction occurs.
...
••
•
•
2.00
1.50
•
100
•
-2.00
-1.00
0.00
1.00
2.00
3.00
x
Figure 3.16: Nodal points used to approximate the parabolic test function.
Chapter 3
121
TRAJECTORY-PLANNING TIlROUGH INTERPOLATION BY OVERLAPPING CUBIC ARCS AND CUBIC SPLINES
In order to determine its accuracy, the fitted approximation is evaluated at chosen intermediate points
between the pairs of consecutive nodes shown in Figure 3.16. Since the proposed OCAS-approach
results in cubic spline representations for X(t) and yet) (see Section 3.1.2) over the time span between
each pair of consecutive nodes, i.e. over tl - t l_l , i
1,2,... ,28, each interval may be subdivided to
obtain a specified number of equally spaced additional intermediate time instants. The respective cubic
spline representations X(t) and yet) are then evaluated at the intermediate time instants to give the
corresponding approximated intermediate x- and y-values along the curve.
Each approximated
intermediate x-value lies in the interval x E [-2,2], and may therefore be substituted into (3.66) to find
the corresponding actual y-value. For each intermediate x-value, the y-error is taken as the absolute
difference between the approximated y-value and the actual y-value.
-3.00
-2.00
-1.00
0.00
x
1.00
2.00
3.00
Figure 3.17: OCAS-approximation of parabolic test function.
Figure 3.17 shows the x-y profile obtained from the OCAS-approximation. Each consecutive time span
t; - tj-J' i
1,2,...,28 is divided into 10 equal subintervals and the curve in Figure 3.17 therefore
represent a plot at 281 division points. The maximum absolute y-error over this set of points is
4.470 x 10-5 with an average error of 5.222 x 10-6 •
The trajectory planning along the parabolic profile was done by specifying a maximum al10wable
acceleration magnitude of 0.7 meter per second square, i.e. 8ALLOW
=0.7, and a specified central speed
of 1.0 meter per second, i.e. v· = 1.0. These specifications resulted in the LSCB-motion (see Section
3.3.1) shown in Figure 3.18. The blend times are TJ
Till = 7.864s. The initial acceleration is 81 (0)
2.922 s and Til
4.942 s with the final time
0.684 and the final acceleration is 8m (TIll) = -0.684
the magnitudes of which are both less than the specified maximum allowable acceleration magnitude
SALLOW
0.7.
Chapter 3
.
'I
122
TRAJECTORY -PLANNING THROUGH INTERPOLATION BY OVERLAPPING CUBIC ARCS AND CUBIC SPLINES
Tangef1tlalaccea&ratlon
Tangential .peed
LSCB Molion sit)
0.80
700
6,00
060
SOD
040
0.20
400
000
3.00
-t120
200
·040
1,00
~O.60
0,00
~
8
8
8
-
"
8
N
8
...
..
8
8.,;
8
<ri
~
8
<ri
8m
·060
Figure 3.18: LSCB-motion for parabolic test function with its associated speed and acceleration
curves.
The cubic spline representations X(t) and yet) that represent the x- and y-coordinate positions as
functions of time, and plotted at the 281 time instants, are shown in Figure 3.19, together with the graphs
of the continuous x- and y- velocities (X(t) and
Yet)
designated as Vx and Vy) and accelerations
(:X(t) and yet) designated as Ax and Ay) over the total time interval [0,7.864]. Note that the start
point and end point of the prescribed trajectory follows from the cubic spline representations X(t) and
Y(t) , Le. (-2,2) at t
0 and (2,2) at t
2SO
1,20
200
LOO
lSO
7.864s.
1
12
°1
1.00 .1
0,60
080
1,00
0,60
oso
---.1"
040
020 ~"-,
8
·0,50
., 00
0>
0.00
l~--~-'r--~--">C"-'~"'~-~
'0'209~ ~/1
·1.50
-0.40
·200
.0.60
·2.50
-Q50
//
'
~
/'
Figure 3.19: Plots of approximate coordinate positions, -velocities and -accelerations versus time for the parabolic test function. With the specification that the end-effector be tangentially orientated with respect to the prescribed curve, (see Section 3.4), the cubic spline approximation cD(t) representing the orientation angle <\l [degrees] over the time interval [0,7.864] may be determined and is shown in Figure 3.20. Also shown in Figure 3.20 are the continuous orientation angular velocity curve <D(t) [rad/s] as well as the continuous orientation angular acceleration curve &( t) [rad/s
Orientation angle
2
].
Orientation angular acceleration
Orientation angular velocity
5000
100
12O
060
60.00
100
0.60'
4000
0.40
0.60
2000
0,20
060
-20.00
8.-'
8
N
8
M
..
8
8
'"
8
'"
8
8
~
8
'!'
000
8
8
'"
N
·0.40
020
·40.00
8
-020
0.40
·060
·6000
0.00
8
0
~
8
N
~
.
8
-O.BO
8
'"
8
<ri
8
~
8
'"
8
0>
·1 00
~
Figure 3.20: Plots of approximate orientation angle, orientation angular velocity and ­
acceleration for the parabolic test function.
Chapter 3
123
TRAJECTORY-PLANNING THROUGH INTERPOLATION BY OVERLAPPING CUBIC ARCS AND CUBIC SPLINES
3.5.2 Spike test function
The second test function is a spike test function taken from : 1
y(x)=-12
+x
(3.67)
This spike test function is also approximated over the x-interval x E [-2,2]. The initial and final nodal
points are Po =(x o,yo)=(2,0.2) and PN (XN'YN)
(-2,0.2) respectively. In total 27 nodal points
are specified as shown in Figure 3.21, with denser distributions of nodal points at the end points of the
interval, as well as at the midpoint as is evident in Figure 3.21.
1.20
l
.. .. 100·t·
I
8. 80 1
....
-3.00
-2.00
-1.00
• 0.60
i
0.40
J
0.20
i
0.00
....
1.00
2.00
3.00
x
Figure 3.21: Nodal points used to specify the spike test function.
As in the case of the parabolic test function of Section 3.5.1, each consecutive time span t;
tl-l'
i = 1,2,...,26 is divided into 10 subintervals by equally spaced additional time instants. The approximate
x-y profile is drawn by plotting the values at the 261 division points as shown in Figure 3.22. Here the
absolute maximum y-error resulting from this approximation is 1.359 x 10-4 with average error
1.201 x 10-5 •
~I
0.60
j
OAO
0.20
g
o
o
ci
g
N
x
Fignre 3.22: OCAS-approximation of spike test function.
Chapter 3
124
TRAJECTORY-PLANNING THROUGH INTERPOLATION BY OVERLAPPING CUBIC ARCS AND CUBIC SPLINES
With a specified central speed of v'
acceleration
SALLOW
= 1.0
meter per second, and a specified maximum allowable
= 0.5 ml S2 , the resulting motion is described by two blended cubic polynomials (see
Section 3.3.2.2), as shown in Figure 3.23. The associated blend time is TMID
= 3.6148 s, and from the
tangential speeds graph in Figure 3.23 it is clear that the speed at the blend time, v MID = 0.9037 mis, is
1.0 m Is. This is due to the fact that the magnitudes of both
slightly less that the desired speed of v·
the initial and final curvature accelerations are equal to the maximum allowable acceleration of
0.5 ml S2 •
SALLOW
Tangential acceleration
Tangential speed
Blended Cubic PoIynonUlo Motion s(t)
5.00 ]
450
0.60
100
090
400 '
350
300
0.40
080
070
080
050
2SO
2.00
LSO
040
0,30
1.00
OSO
0.20
0.10
000
8
0
~
~
8
'"
..
8
t
000
~
8
'"
~
~
8
"
§
8
'"
8
ri
..
8
t
8
"'
8
"'
8
...
8
'"
Figure 3.23: Blended cubic polynomials motion for spike test function with its associated speed
and acceleration curves.
Figure 3.24 shows plots of the x- and y-positions X(t) and Y(t) (with start point (2,0.2) at t
end point (-2,0.2) at t
=0
and
7.230 s), the x- and y-velocities X(t) and yet) (designated as Vx and Vy),
and the x- and y-accelerations X(t) and yet) (designated as Ax and Ay) at the 261 time instants
distributed over the time interval [0,7.230].
[~- XI~:::yw]
2.SO
080
200·
1.00
0.40
050
150
0.20
100
050
000
0.00
~O20
-0.50
-1.00
8
"
8
'"
-150
-200
-250
~
-040
::1
-100' "
0.00
.,8
8
'"
-050
-1.00
·150 .,
-200
j
Figure 3.24: Plots of the approximate coordinates, velocities and accelerations versus time for
the spike test function.
The tangentially orientated end-effector specification results in the cubic spline approximation <I>(t)
[degrees] plotted in Figure 3.25, together with the corresponding plots of the continuous orientation
angular velocity curve <P(t) [rad/s], and the continuous orientation angular acceleration curve <D(t)
Chapter 3
125
TRAJECTORY -PLANNING THROUGH INTERPOLATION BY OVERLAPPING CUBIC ARCS AND CUBIC SPLINES
Orientation angle
Orientation angular acceleration
Orientation angular velocity
4000
J
500
200
3000
400
150
2000
10_00
100
100
000
8
·1000
.,8
'"
·20.00
8
'"
j
-30 00
_
050
N
\
M
~
~
~
~
.,8
-2.00
-300
000
8
...
...4000
~
::,"r!T
300
2.00
-0.50
8
'"
400
-500
Figure 3.25: Plots of approximate orientation angle, orientation angular velocity and
acceleration versus time for the spike test function.
In order to verify expressions (3.64) and (3.65), an orientation angle offset <PoffSet = 30° is specified, and
the associated cubic spline approximation <p(t) [degrees] is plotted in Figure 3.26. Also shown in
Figure 3.26 are the corresponding plots of the continuous orientation angular velocity curve ci>(t)
[ rad! s ], and the continuous angular acceleration curve
OffHt nrl.nutlo" anole
<l>( t)
Orientation angular acceleration
Orlentatlon angular vetoctty
500
j
7000
400
6000
150
50_00
4000
)
100·
3000
2000
200
100
000
-100
ooor···~··8
1000
000
-1000
300
-
8
8
"
....
888
~
t
'"
'"
·050
N
cry
"It
8
-2_00
8
---,
-3.00
8
-400
"
8
8
'"
"
8
'"
8
~
-5_00
Figure 3.26: Plots of approximate offset orientatiou angle, orientation angular velocity and
acceleration versus time for the spike test function.
Comparing the offset cubic spline approximation <p( t) of Figure 3.26 with the tangential cubic spline
approximation <p(t) of Figure 3.25, it is clear that the specified offset (<PoffSe!
= 30°)
resulted in an
upward shift of the orientation angle curve. As expected, the orientation angular velocity and orientation
angular acceleration curves of Figures 3.25 and 3.26 are in exact agreement.
3.5.3 Circular test curve
The ability of the proposed OCAS-approach to approximate curves that cannot be represented by
unique-valued functions of one coordinate variable in terms of the other, is demonstrated here for the
circle:
(3.68)
The nodal points
{Pi
= (xi' yJ, i 0,1, ... ,N} are specified usmg corresponding sweep angles
0; E [0° ,720°], where the sweep angle is measured clockwise from the positive x-axis.
Chapter 3
For any specific
126
8
"
TRAJECTORY-PLANNING THROUGH INTERPOLATION BY OVERLAPPING CUBIC ARCS AND CUBIC SPLINES
sweep angle
~i'
i
O,l, ... ,N the corresponding x- and y-values are given by
Xi
= 3cos ~i'
and
y, = 3sin~j respectively. Here the sequence of sweep angles {~J is chosen such that starting at ~ = 0 0
,
two complete CCW revolutions are followed.
The spacing of the nodal points is again clustered around the beginning and end of the total interval as
can be seen in Figure 3.27. A total of 79 nodal points cover the two revolutions of the circular path. The
nodes for each revolution are shown separately in Figure 3.27.
In particular, the first revolution
(f:3E [0, 360 0 ] ) is shown on the left-hand side of Figure 3.27, and the second revolution
(i3 E [360°,720
0
])
is shown on the right-hand side of Figure 3.27.
4.001
3000
3000
2.00
2.00
..
1.00
~ -4;"~"~~;~1-'~ ~~~~ ~
•
1.00
-200
-1.00 .
1
I
-:roo'
~
I
-1.00
O'r
-1.00
-200
-2.00
100
200
.
.
•
•
-4:00]
x
Figure 3.27: Nodal points used to approximate two revolutions of the circular curve.
As before, each time span tj
tl-l' i
1,2,...,78 is divided into 10 equal subintervals for the purpose of
mapping the computed approximation at the division points (see Section 3.5.1). The x-y-profile obtained
from the OCAS-approximation at the specified division points is shown in Figure 3.28. For any given
time instant the approximated x- and y-coordinates correspond to an approximated radius, from which
the absolute radius error can be calculated. The maximum absolute radius error computed over the 781
division points is 5.27 x 10-5 with an average error of 6.27 x 10'6 .
Chapter 3
127
TRAJECTORY-PLANNING THROUGH INTERPOLATION BY OVERLAPPING CUBIC ARCS AND CUBIC SPLINES
200]
0
0
0
C!
<:i
1.00
j
600
I
~
-1.00
0
0
0
C!
oJ
~
J
i
I
-2.00:
)(
Figure 3.28: OCAS-approximation of circular test curve.
The trajectory planning is done by limiting the maximum allowable acceleration to 0.5 meter per second
square, i.e.
v'
SALLOW
=
0.5 m/s2, and by specifying a central speed of 1.0 meter per second, i.e.
These specifications resulted in a LSCB- motion with corresponding blend times
1.0 m/ s.
TI = 4.712 s and Tn = 36.127 s (see Figure 3.29) and total path time TIll
40.841 s. The initial and
final tangential accelerations are sI(0)=0.424m/s 2 and s m CTm )=-0.424m/s 2 both satisfying the
bound on the magnitude of accelerations.
2500 j
'500
j
1000 -;
1
010
0.60
040
i
020
000
8
·010
o
8
~
888
~
~
~
I
000
0
j
030
,80
0.20 I
5001
8
040
100 !
30 00 .
1
0.50
""I
::: 1
2000
Tangential acceleration
Tangential sJ:MMKI
LSCB MOIlon .(t)
8
'"
8
0
8
'"
8
Iil
8
I'l
8
8
8
lil
ill
~
~
'''' 8
8
~
8
s>
8
8
!l
8
8
;;:
8
:;;
8
~
8
1/
-040 ~
-050
j
Figure 3.29: LSCB-motion for the circular curve with its accompanying speed and acceleration
curves.
Figure 3.30 shows the x- and y-positions X(t) and yet) (with start point (3,0) at t = 0 and end point
(3,0) at t = 40.841 s), the x- and y-velocities X(t) and yet) (designated as Vx and Vy), and the x­
andy-accelerations X(t) and Yet) (designated as Ax and Ay) over the time interval [0,40.841].
Chapter 3
128
TRAJECTORY -PLANNING THROUGH INTERPOLA nON BY OVERLAPPING CUBIC ARCS AND CUBIC SPLINES
1.SO
05<)
0,40
100
030
Q,20
OSO
0.00
0.00
~
.0.50
-u~
-040 ]
-1.50
-050
Figure 3.30: Plots of the approximate coordinates, velocities and accelerations versus time for
the circular curve.
For the circular test curve, the situation depicted in Figure 3.14 occurs, where the specification of a
tangentially orientated end-effector necessitates the monitoring of the behavior of the calculated gradient
angle e to keep track of the end-effector orientation angle ~ (see Section 3.4). The corresponding cubic
spline approximation <I>(t) [degrees] that results from the monitoring procedure outlined in Section 3.4
is shown in Figure 3.31.
Since the nodal points were specified within the sweep angle range of
[0° ,720°], the orientation angle varies from - 90° to 630°. The corresponding orientation angular
velocity curve
<bet) [rad/s] and the orientation angular accelerations <D(t) [rad/s 2 ] are also shown in
Figure 3.31.
Orientation angle
Orientation angular acceleration
Orientation anQuiar velOcity
700.00
80000
020
,
015
500.00 .
010
40000
0.25
30000
020
OOS
0.00
200.00
-0,'0
0.05
0.00
-10000
~O.05
0.10
10000
8
on
8
8
Q
'"
~20000
8
;1l
8
:(j
8
lil
8
:!l
8
i
8
;?
000
-0.15
8
<>
8
'"
8
<>
8
on
8
<>
N
8
i(l
8
<>
M
8
:!l
8
i
8
;?
-0.20
Figure 3.31: Plots of the approximate orientation angle, orientation angular velocity and
orientation angular acceleration versus time for the circular test curve.
The unexpected spikes at the beginning- and endpoints of the angular acceleration curve, as opposed to
the smooth behavior of both X(t) and yet) in the same regions, are probably due to the slight
inaccuracies introduced in the calculation of the gradient angle via expressions (3.60) and (3.61).
Expression (3.60) uses the approximated gradient, while expression (3.61) uses the approximated x- and
y-velocities to find the gradient angle e.
For the circular test curve under consideration, the exact gradient angle
e (and orientation
angle
corresponding to a given sweep angle J3 may be determined (see Figure 3.32).
Chapter 3
129
~)
TRAJECTORY-PLANNING THROUGH INTERPOLATION BY OVERLAPPING CUBIC ARCS AND CUBIC SPLINES
y
!
,//e
l;;
\/3
\
---+----------------~------~------_r_.x
o
Figure 3.32: Exact gradient angle for a given sweep angle.
The figure clearly shows that the relationship tan/3 =1. applies, from which it follows that the exact
x
e = /3
90°, which also allows for the exact determination of <p. With this information
available a comparison can be made between the approximated and exact orientation angle at any point
along the curve.
Corresponding to the determination of the absolute radius error, the absolute difference between the
approximated orientation angle, and the exact orientation angle is referred to as the orientation angle
error. For the circular test curve, the maximum absolute orientation angle error over all the 781 division
points is 5.714 x 10-4 rad, i.e. 0.03274° with an average error 3.742 x 10-5 rad, i.e. 0.00214°.
3.5.4 Logarithmic spiral test curve
The final analytical test curve is the logarithmic spiral taken from :
(3.69)
which specifies the relationship between the polar coordinates (p, 13) of any point on the curve.
As in the case of the circular curve of Section 3.5.3, the spiral curve can also not be expressed as a
function of one coordinate variable in terms of the other. The nodal points {PI = (XpYi)' i == O,l, ...N} are
generated via (3.69) where the polar angle plays the role of a sweep angle
the sweep angle ranges from
n to
5n , i.e.
13 E [ -n, -
Pi'
For the spiral test curve
5n]. Note that the sweep angle is incremented
such that the logarithmic spiral curve is traced in a CW manner, as opposed to the CCW-tracing of the
circular test curve (see Section 3.5.3). The 79 nodal points specified are shown in Figure 3.33
Chapter 3
130
TRAJECTORY-PLANNING THROUGH INTERPOLATION BY OVERLAPPING CUBIC ARCS AND CUBIC SPLINES
0.60
0.60
l
I
. . . ' r'j . ..
0.40
.
~.
~
.
•
0.20
.
li
.
..
•
.
-O;:~~;--~.;--~~.
.. ~-~~-~-:~L
.
.
.
. ... •
,
•
-
i
-8~0 ~t
•
-·········..().60"-­
x
Figure 3.33: Nodal points used to approximate the spiral test curve.
Using the same procedure as outlined before, the x-y profile may be mapped at 781 division points as
shown in Figure 3.34. For each interpolated point the x and y coordinates yield an interpolated radius,
i.e. r
= ~x 2 + y2
. Furthermore, for each interpolated point the relationship tan 13
x
applies as it does
for the circular test curve (see Figure 3.32). The calculated l3-angle is used in a special procedure similar
to the one explained in Section 3.4, to determine the exact l3-polar coordinate associated with the
interpolated point. Hence, by substituting the exact l3-polar coordinate into expression (3.69), the p-polar
coordinate as well as the absolute radius error Ip - rl associated with the interpolated point may be
determined. The maximum absolute radius error for the spiral test curve over the 781 division points is
1.361 x 10-5 with average error 7.258 x 10-7 •
x
Figure 3.34: OCAS-approximation of spiral test curve.
Chapter 3 131
TRAJECTORY-PLANNING THROUGH INTERPOLATION BY OVERLAPPING CUBIC ARCS AND CUBIC SPLINES Here the bound on the maximum allowable acceleration is 0.7 meter per second square, and a central
=1mls
speed of v'
is specified. The resulting LSCB motion is shown in Figure 3.35, for which the
respective blend times are T I = 2.968 s and T Il = 4.275 s with total path time Till = 7.224 s.
The
s(O) = 0.674 < 0.7
and
tangential
accelerations
are
within
the
specified
limits,
SInce
Tangential acceteratlon
Tangentlal.peed
LSCB Motion sU) 600 120
5{)()
1.00
400 080
3{)()
080
200 040
~.I
100 020
-0.40
0.80
1
060
040
J
020 ,
.,
!
!
~!
~!
8
<0
Ii
-060 "
0.00 8
0
8
~
8
N
8
M
.
8
~
8
8
'"
~
.,8
8
8
N
8
<0
~
.0.80'
Figure 3.35: LSCB-motion for the spiral test curve with its accompanying speed and
acceleration curves.
The corresponding approximated x- and y-positions X(t) and yet) (with start point (-0.730,0) at
t
0 and end point (-0.208,0) at t
7.224s), x- and y-velocities X(t) and yet) (designated as Vx
and Vy), and x- and y-accelerations X(t) and V(t) (designated as Ax and Ay ) computed at the 781 division points are plotted in Figure 3.36. 100]
1.00
I
050
I
8
8
<ri
'"
I
"., 1 oj
50 Figure 3.36: Plots of the approximate coordinates, velocities and accelerations versus time for the spiral test curve. As for the circular test curve, the orientation angles of the spiral test curve are determined using the
monitoring procedure outlined in Section 3.4. The end-effector orientation angle of the spiral test curve
varies between 84.29° and
Chapter 3
635.71° as shown in Figure 3.37.
132 TRAJECTORY -PLANNING THROUGH INTERPOLATION BY OVERLAPPING CUBIC ARCS AND CUBIC SPLINES
Orientation angle
Orientation angular ac~leratlon
Orientation angular V&kx:lty
200,00
400
0,00
100.00
~O.SO
000
8
~100.00
'"
-20000
-300.00 ~
.,8
8
"
8.;
~
9
8oj
8
M
8
<i
-1.00
8
'"
8
'"
8
3.50
'"
300
2,50
2,00
·150
1,50
H.)
'2,00
::1
MO
-2,50
0,00
~O.50
-3_00
.fIOO.OO .
~1.00
-1,50
-3,50
-700.00
Figure 3.37: Plots of the approximate orientation angle, orientation angular velocity and
orientation angular acceleration versus time for the spiral test curve.
The orientation angular velocities [rad/s] and the orientation angular accelerations [rad/ S2] are also
shown in Figure 3.37. Again the spikes at end point of the angular acceleration curve are probably due
to slight inaccuracies in the detennination of the orientation angle (see Section 3.5.3).
Although specifYing an angular offset
<Poffset
=
-45° shifts the orientation angle curve down as shown in
Figure 3.38, the orientation angular velocity and orientation angular acceleration curves remain
unchanged. In particular, the offset orientation angle of the spiral test curve varies between 39.29° and
-680.71° .
011...1 orientation angle
Orientation angular acceleration
OrMmtation angular velocity
100.00 -i
8
000
·100.00
'"
8
~
-200,00
-300.00
400j
350
3JJO ,
2,50
2,00
i
150
1,00
0,50
400.00
-500,00
0.00 ..- .. ~,-'~~"'~T'
4600.00
-3.00
-70000
",50,
8
8
.,
8
~
-3.50
·60000
Figure 3.38: Plots of the approximate offset orientation angle, orientation angular velocity and
orientation angular acceleration versus time for the spiral test curve.
3.5.5 Non-analytical test curve
The final test curve is the non-analytical treble clef shown in Figure 3.39. The particular curve is a Non­
Unifonn Rational B-Spline (NURBS) generated with commercial Computer Aided Design (CAD)
software, and fitted through the 42 nodal points also indicated in Figure 3.39. For trajectory planning
purposes, the start- and end points are as indicated in Figure 3.39.
This illustrative example is typical of a real life situation where the prescribed curve is an arbitrary
smooth curve for which no analytical expression exists.
Chapter 3
133
8
'"
TRAJECTORY-PLANNING THROUGH INTERPOLATION BY OVERLAPPING CUBIC ARCS AND CUBIC SPLINES
y
140
120
100
80
60­
40
20
o+--,---i-=::O::;i---r---+X
10
20
30
40
Figure 3.39: CAD-spline drawing through specified points of the treble clef test curve.
The OCAS trajectory planning methodology is tested by specifying different sets of nodal points, where
the nodal points of each set are differently spread along the CAD spline treble clef. In particular, three
different sets of nodal points are chosen with respectively 22, 31 and 49 nodal points. The approximated
OCAS x-y profiles computed at respectively 211, 301 and 481 division points are shown in Figure 3.40.
31 IIb:tiI Alirts
100.00 ' - - - ' - - ' - " - - ' - - - " ,
1;>100
120.00 I
10000 i
!
00.001
I
I
000
,~
!§;
0
·;>100
x
0
0
;:!
8
0
'"
8
?!
X
..
8
0
8
iii
'"0
"'
0
0.00 :
:5
0
-~
0
0
0
~
..."­
8
8
0
N
0
8
... '"0
Figure 3.40: Three OCAS approximations of treble clef test curve.
Chapter 3
134
TRAJECTORY-PLANNING THROUGH INTERPOLATION BY OVERLAPPING CUBIC ARCS AND CUBIC SPLINES
By comparing Figure 3.40 with Figure 3.39, it is evident that the approximation capability of the OCAS
trajectory planning methodology is accurate with relatively few specified nodal points. Using only 31
nodal points yields a reasonably accurate fit, while using 49 nodal points yields an approximation which
by inspection shows no deviation from the original CAD-spline treble clef. However, specifYing too few
nodal points, and in an injudicious manner, results in a poor approximation as can be seen from the x-y
profile computed for the 22 chosen nodal points in Figure 3.40.
The results presented in Figure 3.40 also emphasize the importance of clustered distributions of nodal
points near the extreme points (especially the end point) of the prescribed curve, which ensure accurate
~~
approximations to
at Po and PN •
Since the choice of 49 nodal points gives the best approximation, its associated trajectory planning
results are also shown. Here the bound on the maximum allowable acceleration is 5 mm/ S2 , and a
central speed of v'
=10mm/s
is specified. The resulting LSCB motion is shown in Figure 3.41, for
which the respective blend times are T,::=5.l26s
and TI/=40.1435s
with total path time
T III =44.693 s. The tangential accelerations are within the specified limits, since Sf (0) = 3.834 < 5 and
LSCB Motion
Tang*ntial acc••ration
Tangentlal.peed
.(~
450 00 :
1200
500
40000
35000
300.00
1
4JlO
1000
j
300
200
600
25000 i
6.00
2(XtOQ ~
8
15000
400
10000
2001
5000
0.00
'"
3.00
0.00
8
0
8
'"
8
~
8
\!i
8
l'l
8
8
g
~
t
~ ~
8
':Ij
8
iii
1
100
8
0
8
'"
8
~
8
~
8
~
8
l'l
8
:;j
8
~
~
8
':Ij
8
iii
8
"
8
'"
8
~
8
~
8
g
8
ill
- 4.00
·5.00
Figure 3.41: LSCB-motion for the treble clef test curve with its accompanying speed and
acceleration curves.
The corresponding approximated x- and y-positions X(t) and yet) (with start point (27.0,44.0) at
t
0 and end point (13.0,13.0) at t=44.693s),x-andy-velocities X(t) and yet) (designated as Vx
and Vy), and x- and y-accelerations X(t) and yet) (designated as Ax and Ay) computed at the 481
division points are plotted in Figure 3.42.
Chapter 3
135
TRAJECTORY-PLANNING THROUGH INTERPOLATION BY OVERLAPPING CUBIC ARCS AND CUBIC SPLINES
[_
xii)~~viiJJ
160,00 ";
14000
15.00
j
10.00
5.00
0.00
0.00
8
8
Sl
g
·10.00
·15.00
·20.00
·25.00
Figure 3.42: Plots of the approximate coordinates, velocities and accelerations versus time for
the treble clef test curve.
The treble clef prescribed curve is traced with the end-effector in a fixed horizontal orientation <jl
hence the orientation angle curves are omitted here.
Chapter 3
136
TRAJECTORY -PLANNING THROUGH INTERPOLATION BY OVERLAPPING CUBIC ARCS AND CUBIC SPLINES
Chapter 3
137 ```