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 With the given final gradient 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 [55]: 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 [55]. 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 [55] 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 used, since they contain more information about the respective functions, and therefore lead to more 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 [55]. 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 [55]. 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 [55]). 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) Gradient at I 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 [60]). 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 [60]). 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 circular curve the gradient 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 [68]: 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) [rad / s 2 ]. 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 gradient angle is 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 [69]: (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

* Your assessment is very important for improving the work of artificial intelligence, which forms the content of this project

Download PDF

advertisement